Visual Odometry: Motion from Images

class SimpleVisualOdometry:
    """Basic visual odometry using feature matching."""

    def __init__(self, focal_length=640, baseline=0.12):
        self.focal_length = focal_length
        self.baseline = baseline
        self.prev_frame = None
        self.prev_keypoints = None
        self.trajectory = []

    def detect_features(self, frame, method='orb'):
        """Detect features in frame."""
        # Simplified feature detection (use OpenCV in practice)
        # This creates synthetic features for demonstration
        h, w = frame.shape[:2] if len(frame.shape) > 2 else frame.shape
        n_features = 500

        # Random feature positions (real code uses actual detection)
        features = np.column_stack([
            np.random.randint(50, w-50, n_features),
            np.random.randint(50, h-50, n_features)
        ])

        # Descriptors (128-dimensional for demo)
        descriptors = np.random.randn(n_features, 128).astype(np.float32)

        return features, descriptors

    def match_features(self, desc1, desc2, max_distance=0.7):
        """Match features using brute force distance."""
        matches = []
        for i, d1 in enumerate(desc1):
            distances = np.linalg.norm(desc2 - d1, axis=1)
            best_idx = np.argmin(distances)
            second_best = np.partition(distances, 1)[1]

            # Lowe's ratio test
            if distances[best_idx] < max_distance * second_best:
                matches.append((i, best_idx))

        return matches

    def estimate_motion(self, kp1, kp2, matches):
        """Estimate relative motion from matched features."""
        if len(matches) < 8:
            return None, None

        pts1 = np.array([kp1[m[0]] for m in matches], dtype=np.float32)
        pts2 = np.array([kp2[m[1]] for m in matches], dtype=np.float32)

        # Simple translation estimation (real code uses Essential matrix)
        displacement = np.mean(pts2 - pts1, axis=0)

        # Convert pixel displacement to meters (simplified)
        tx = displacement[0] * self.baseline / self.focal_length
        ty = displacement[1] * self.baseline / self.focal_length

        return np.array([tx, ty, 0]), displacement

    def process_frame(self, frame):
        """Process a new frame and estimate motion."""
        kp, desc = self.detect_features(frame)

        if self.prev_frame is not None:
            matches = self.match_features(self.prev_keypoints[1], desc)

            if len(matches) >= 8:
                position, _ = self.estimate_motion(
                    self.prev_keypoints[0], kp, matches
                )
                if position is not None:
                    if self.trajectory:
                        position += self.trajectory[-1]
                    self.trajectory.append(position)

        self.prev_frame = frame
        self.prev_keypoints = (kp, desc)

        return self.trajectory[-1] if self.trajectory else np.zeros(3)

# Demo
np.random.seed(42)
vo = SimpleVisualOdometry()

# Simulate 10 frames of forward motion
for i in range(10):
    # Create synthetic frame (real code: load camera image)
    frame = np.random.randint(0, 256, (480, 640, 3), dtype=np.uint8)
    position = vo.process_frame(frame)
    print(f"Frame {i}: Position = ({position[0]:.3f}, {position[1]:.3f}, {position[2]:.3f})")

class MonocularVO:
    """Monocular visual odometry pipeline."""

    def __init__(self):
        self.scale = 1.0  # Monocular scale ambiguity
        self.prev_frame = None
        self.prev_points = None
        self.cur_pose = np.eye(4)  # 4x4 transformation matrix
        self.trajectory = [np.zeros(3)]

    def preprocess(self, frame):
        """Convert to grayscale and equalize histogram."""
        if len(frame.shape) == 3:
            gray = np.mean(frame, axis=2).astype(np.uint8)
        else:
            gray = frame

        # Simple histogram equalization
        hist, bins = np.histogram(gray.flatten(), 256, [0, 256])
        cdf = hist.cumsum()
        cdf_normalized = cdf * 255 / cdf[-1]
        equalized = cdf_normalized[gray]
        return equalized.astype(np.uint8)

    def track_features(self, prev_frame, cur_frame, max_corners=200):
        """Track features using optical flow (simplified)."""
        h, w = prev_frame.shape

        # Generate grid of points to track
        grid_size = int(np.sqrt(max_corners))
        step_x = w // grid_size
        step_y = h // grid_size

        prev_points = []
        cur_points = []

        for i in range(grid_size):
            for j in range(grid_size):
                x = step_x * (i + 0.5)
                y = step_y * (j + 0.5)

                # Check if point has enough texture
                patch_prev = prev_frame[max(0,int(y)-5):int(y)+5,
                                       max(0,int(x)-5):int(x)+5]
                if np.std(patch_prev) > 10:  # Texture threshold
                    prev_points.append([x, y])
                    # Simulate optical flow (real: cv2.calcOpticalFlowPyrLK)
                    cur_points.append([x + np.random.normal(0, 0.5),
                                      y + np.random.normal(0, 0.5)])

        return np.array(prev_points), np.array(cur_points)

    def estimate_essential_matrix(self, pts1, pts2, camera_matrix):
        """Estimate essential matrix (simplified)."""
        # In practice: cv2.findEssentialMat(pts1, pts2, camera_matrix)
        # This returns R, t from the essential matrix
        R = np.eye(3)  # No rotation for demo
        t = np.array([[0.01], [0.0], [0.0]])  # Small forward translation
        return R, t

    def process_frame(self, frame):
        """Main processing pipeline."""
        processed = self.preprocess(frame)

        if self.prev_frame is not None:
            pts1, pts2 = self.track_features(self.prev_frame, processed)

            if len(pts1) > 30:
                # Camera intrinsics (typical drone camera)
                camera_matrix = np.array([
                    [640, 0, 320],
                    [0, 640, 240],
                    [0, 0, 1]
                ], dtype=float)

                R, t = self.estimate_essential_matrix(pts1, pts2, camera_matrix)

                # Update pose
                T = np.eye(4)
                T[:3, :3] = R
                T[:3, 3] = (t * self.scale).flatten()
                self.cur_pose = self.cur_pose @ np.linalg.inv(T)

                position = self.cur_pose[:3, 3]
                self.trajectory.append(position.copy())

        self.prev_frame = processed
        return self.cur_pose[:3, 3]

# Demo
vo = MonocularVO()
np.random.seed(42)

for i in range(5):
    frame = np.random.randint(0, 256, (480, 640), dtype=np.uint8)
    pos = vo.process_frame(frame)
    print(f"Frame {i}: x={pos[0]:.3f} y={pos[1]:.3f} z={pos[2]:.3f}")