autopilot/examples/transform_app.py

from pathlib import Path

from kivy.app import App
from kivy.uix.boxlayout import BoxLayout
from kivy.uix.slider import Slider
from kivy.uix.label import Label
from kivy.uix.image import Image as KivyImage
from kivy.graphics.texture import Texture
from kivy.core.window import Window
import numpy as np
from PIL import Image
import cv2
import io

class CameraTransformApp(App):
    def build(self):
        Window.size = (900, 700)

        # Главный контейнер
        main_layout = BoxLayout(orientation='vertical', padding=10, spacing=10)

        # Контейнер для изображения
        self.image_widget = KivyImage(size_hint=(1, 0.7))
        main_layout.add_widget(self.image_widget)

        # Контейнер для ползунков
        controls_layout = BoxLayout(orientation='vertical', size_hint=(1, 0.3), spacing=5)

        # Yaw (рыскание) - вращение вокруг вертикальной оси
        yaw_layout = BoxLayout(orientation='horizontal', spacing=10, size_hint=(1, None), height=40)
        yaw_layout.add_widget(Label(text='Рыскание (Yaw):', size_hint=(0.2, 1)))
        self.yaw_slider = Slider(min=-45, max=45, value=0, size_hint=(0.6, 1))
        self.yaw_slider.bind(value=self.on_slider_change)
        self.yaw_label = Label(text='0°', size_hint=(0.2, 1))
        yaw_layout.add_widget(self.yaw_slider)
        yaw_layout.add_widget(self.yaw_label)
        controls_layout.add_widget(yaw_layout)

        # Pitch (тангаж) - наклон вперед/назад
        pitch_layout = BoxLayout(orientation='horizontal', spacing=10, size_hint=(1, None), height=40)
        pitch_layout.add_widget(Label(text='Тангаж (Pitch):', size_hint=(0.2, 1)))
        self.pitch_slider = Slider(min=-45, max=45, value=0, size_hint=(0.6, 1))
        self.pitch_slider.bind(value=self.on_slider_change)
        self.pitch_label = Label(text='0°', size_hint=(0.2, 1))
        pitch_layout.add_widget(self.pitch_slider)
        pitch_layout.add_widget(self.pitch_label)
        controls_layout.add_widget(pitch_layout)

        # Roll (крен) - наклон влево/вправо
        roll_layout = BoxLayout(orientation='horizontal', spacing=10, size_hint=(1, None), height=40)
        roll_layout.add_widget(Label(text='Крен (Roll):', size_hint=(0.2, 1)))
        self.roll_slider = Slider(min=-45, max=45, value=0, size_hint=(0.6, 1))
        self.roll_slider.bind(value=self.on_slider_change)
        self.roll_label = Label(text='0°', size_hint=(0.2, 1))
        roll_layout.add_widget(self.roll_slider)
        roll_layout.add_widget(self.roll_label)
        controls_layout.add_widget(roll_layout)

        main_layout.add_widget(controls_layout)

        # Создаем тестовое изображение с сеткой
        self.original_image = self.create_test_image()
        self.update_image()

        return main_layout

    def create_test_image(self):
        """Создает тестовое изображение с сеткой и текстом"""
        img = cv2.imread(Path('images') / 'photo_610.png')

        return img

    def get_rotation_matrix(self, yaw, pitch, roll):
        """Вычисляет матрицу поворота 3D"""
        # Конвертируем в радианы
        yaw = np.radians(yaw)
        pitch = np.radians(pitch)
        roll = np.radians(roll)

        # Матрица поворота вокруг Z (yaw)
        Rz = np.array([
            [np.cos(yaw), -np.sin(yaw), 0],
            [np.sin(yaw), np.cos(yaw), 0],
            [0, 0, 1]
        ])

        # Матрица поворота вокруг Y (pitch)
        Ry = np.array([
            [np.cos(pitch), 0, -np.sin(pitch)],
            [0, 1, 0],
            [np.sin(pitch), 0, np.cos(pitch)]
        ])

        # Матрица поворота вокруг X (roll)
        Rx = np.array([
            [1, 0, 0],
            [0, np.cos(roll), -np.sin(roll)],
            [0, np.sin(roll), np.cos(roll)]
        ])

        # Комбинируем повороты: R = Rz * Ry * Rx
        R = Rx @ Ry @ Rz
        return R

    def apply_perspective_transform(self, img, yaw, pitch, roll):
        """Применяет перспективное преобразование к изображению"""
        h, w = img.shape[:2]

        # Параметры камеры
        focal_length = w
        distance = w

        # Матрица камеры
        camera_matrix = np.array([
            [focal_length / 2, 0, w / 2],
            [0, focal_length / 2, h / 2],
            [0, 0, 1]
        ])

        # Получаем матрицу поворота
        R = self.get_rotation_matrix(yaw, pitch, roll)

        T = np.array([
            [1, 0, 0.2],
            [0, 1, 0.1],
            [0, 0, 2],
        ])

        # Создаем матрицу трансформации
        H =  camera_matrix @ R @ T @ np.linalg.inv(camera_matrix)

        # Применяем преобразование
        try:
            result = cv2.warpPerspective(img, H, (w, h), 
                                        borderMode=cv2.BORDER_CONSTANT,
                                        borderValue=(50, 50, 50))
            return result
        except:
            return img

    def update_image(self):
        """Обновляет изображение с текущими параметрами"""
        yaw = self.yaw_slider.value
        pitch = self.pitch_slider.value
        roll = self.roll_slider.value

        # Применяем трансформацию
        transformed = self.apply_perspective_transform(
            self.original_image, yaw, pitch, roll
        )

        # Конвертируем в текстуру Kivy
        buf = cv2.flip(transformed, 0).tobytes()
        texture = Texture.create(size=(transformed.shape[1], transformed.shape[0]), 
                                colorfmt='bgr')
        texture.blit_buffer(buf, colorfmt='bgr', bufferfmt='ubyte')
        self.image_widget.texture = texture

    def on_slider_change(self, instance, value):
        """Обработчик изменения ползунков"""
        self.yaw_label.text = f'{int(self.yaw_slider.value)}°'
        self.pitch_label.text = f'{int(self.pitch_slider.value)}°'
        self.roll_label.text = f'{int(self.roll_slider.value)}°'
        self.update_image()

if __name__ == '__main__':
    CameraTransformApp().run()