Feature Matching Fundamentals for Robotics

import numpy as np
import cv2
from scipy.spatial import KDTree
from typing import List, Tuple, Optional

class VisualSLAMFeatureTracker:
    """A simplified visual feature tracker for SLAM applications"""
    
    def __init__(self):
        self.orb = cv2.ORB_create(nfeatures=2000)
        self.flann = cv2.FlannBasedMatcher(
            dict(algorithm=6, table_number=6, key_size=12, multi_probe_level=1),
            dict(checks=50)
        )
        
        # Database of past features
        self.feature_database = {
            'keypoints': [],      # List of keypoint objects
            'descriptors': [],    # List of descriptor arrays
            'positions': [],      # 3D positions in world coordinates
            'frames': []          # Frame indices where features were seen
        }
        
        self.frame_count = 0
        self.min_matches_for_tracking = 10
    
    def process_frame(self, frame: np.ndarray) -> dict:
        """Process a new frame and return tracking results"""
        self.frame_count += 1
        
        # 1. Feature detection
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        keypoints, descriptors = self.orb.detectAndCompute(gray, None)
        
        if descriptors is None:
            return {'status': 'no_features', 'matches': 0}
        
        # 2. Convert descriptors to float32 for FLANN
        descriptors_float = descriptors.astype(np.float32)
        
        # 3. Match with previous frame (if exists)
        matches = []
        if len(self.feature_database['descriptors']) > 0:
            # Convert database descriptors to float32
            db_descriptors = np.array(self.feature_database['descriptors']).astype(np.float32)
            
            # kNN matching with k=2 for ratio test
            knn_matches = self.flann.knnMatch(descriptors_float, db_descriptors, k=2)
            
            # Apply Lowe's ratio test
            matches = []
            for match_pair in knn_matches:
                if len(match_pair) == 2:
                    m, n = match_pair
                    if m.distance < 0.7 * n.distance:
                        matches.append(m)
        
        # 4. Estimate camera motion (simplified - using essential matrix)
        motion_estimate = None
        if len(matches) >= self.min_matches_for_tracking:
            # Extract matched points
            src_pts = np.float32([keypoints[m.queryIdx].pt for m in matches])
            dst_pts = np.float32([self.feature_database['keypoints'][m.trainIdx].pt 
                                 for m in matches])
            
            # Estimate essential matrix
            E, mask = cv2.findEssentialMat(
                src_pts, dst_pts, 
                focal=1.0, pp=(0, 0),
                method=cv2.RANSAC, prob=0.999, threshold=1.0
            )
            
            if E is not None:
                # Recover pose from essential matrix
                _, R, t, mask = cv2.recoverPose(E, src_pts, dst_pts)
                motion_estimate = {'rotation': R, 'translation': t}
        
        # 5. Update feature database (add new features)
        if len(matches) < 100:  # Add new features if we don't have enough matches
            # Select unmatched keypoints
            matched_indices = set([m.queryIdx for m in matches])
            unmatched_indices = [i for i in range(len(keypoints)) 
                               if i not in matched_indices]
            
            # Add top N unmatched features
            n_to_add = min(50, len(unmatched_indices))
            for i in unmatched_indices[:n_to_add]:
                self.feature_database['keypoints'].append(keypoints[i])
                self.feature_database['descriptors'].append(descriptors[i])
                # Initialize with unknown 3D position
                self.feature_database['positions'].append(None)
                self.feature_database['frames'].append(self.frame_count)
        
        return {
            'status': 'success',
            'matches': len(matches),
            'keypoints': len(keypoints),
            'motion': motion_estimate,
            'database_size': len(self.feature_database['descriptors'])
        }
    
    def triangulate_points(self, points1, points2, P1, P2):
        """Triangulate 3D points from 2D correspondences"""
        points_4d = cv2.triangulatePoints(P1, P2, points1.T, points2.T)
        points_3d = points_4d[:3] / points_4d[3]
        return points_3d.T

# Usage example for robot navigation
if __name__ == "__main__":
    # Simulate robot camera frames
    tracker = VisualSLAMFeatureTracker()
    
    # Simulated camera intrinsic parameters
    K = np.array([[800, 0, 320],
                  [0, 800, 240],
                  [0, 0, 1]])
    
    # Process simulated frames
    for i in range(10):
        # In real application, this would be actual camera frames
        # Here we create synthetic frames for demonstration
        frame = np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8)
        
        # Add some features (simulating scene)
        cv2.circle(frame, (100 + i*10, 100), 5, (255, 0, 0), -1)
        cv2.circle(frame, (200, 150 + i*5), 5, (0, 255, 0), -1)
        
        # Process frame
        result = tracker.process_frame(frame)
        
        print(f"Frame {i}: {result['matches']} matches, "
              f"Database: {result['database_size']} features")
        
        if result['motion']:
            print(f"  Estimated motion: {result['motion']['translation'].flatten()}")
                    

import numpy as np
from typing import List, Tuple
from dataclasses import dataclass

@dataclass
class Keypoint:
    """Mathematical representation of a keypoint"""
    x: float          # x-coordinate
    y: float          # y-coordinate
    scale: float      # scale at which keypoint was detected
    orientation: float  # orientation in radians
    response: float   # detector response strength

@dataclass  
class Descriptor:
    """Mathematical representation of a descriptor"""
    vector: np.ndarray  # Feature vector
    keypoint: Keypoint  # Associated keypoint
    type: str           # Descriptor type

class MathematicalFeatureMatcher:
    """Pure mathematical implementation of feature matching algorithms"""
    
    @staticmethod
    def compute_simple_descriptor(image_patch: np.ndarray) -> np.ndarray:
        """
        Compute a simple descriptor from an image patch
        Based on intensity and gradient statistics
        """
        h, w = image_patch.shape
        
        # 1. Intensity histogram (4 bins)
        hist, _ = np.histogram(image_patch, bins=4, range=(0, 255))
        hist = hist / (h * w)  # Normalize
        
        # 2. Gradient magnitude and orientation statistics
        # Compute gradients using Sobel operators
        sobel_x = np.array([[-1, 0, 1], [-2, 0, 2], [-1, 0, 1]])
        sobel_y = np.array([[-1, -2, -1], [0, 0, 0], [1, 2, 1]])
        
        # Ensure patch is large enough
        if h > 2 and w > 2:
            grad_x = np.abs(np.convolve(image_patch.flatten(), sobel_x.flatten(), 'valid'))
            grad_y = np.abs(np.convolve(image_patch.flatten(), sobel_y.flatten(), 'valid'))
            grad_magnitude = np.sqrt(grad_x**2 + grad_y**2)
            
            grad_mean = np.mean(grad_magnitude) if len(grad_magnitude) > 0 else 0
            grad_std = np.std(grad_magnitude) if len(grad_magnitude) > 0 else 0
        else:
            grad_mean, grad_std = 0, 0
        
        # 3. Create descriptor vector
        descriptor = np.concatenate([
            hist,                    # 4 elements
            [grad_mean, grad_std],   # 2 elements
            [np.mean(image_patch), np.std(image_patch)]  # 2 elements
        ])
        
        return descriptor
    
    @staticmethod
    def euclidean_distance(vec1: np.ndarray, vec2: np.ndarray) -> float:
        """Compute Euclidean distance between two vectors"""
        return np.sqrt(np.sum((vec1 - vec2) ** 2))
    
    @staticmethod
    def hamming_distance(bits1: np.ndarray, bits2: np.ndarray) -> int:
        """Compute Hamming distance between two binary vectors"""
        return np.sum(bits1 != bits2)
    
    @staticmethod
    def brute_force_nn(query: np.ndarray, database: List[np.ndarray]) -> Tuple[int, float]:
        """
        Brute-force nearest neighbor search
        Returns (index_of_best_match, distance)
        """
        best_idx = -1
        best_dist = float('inf')
        
        for i, db_vec in enumerate(database):
            dist = np.linalg.norm(query - db_vec)  # Euclidean distance
            if dist < best_dist:
                best_dist = dist
                best_idx = i
        
        return best_idx, best_dist
    
    @staticmethod
    def knn_search(query: np.ndarray, database: List[np.ndarray], k: int = 2) -> List[Tuple[int, float]]:
        """
        k-Nearest Neighbors search
        Returns list of (index, distance) for k closest matches
        """
        distances = []
        
        for i, db_vec in enumerate(database):
            dist = np.linalg.norm(query - db_vec)
            distances.append((i, dist))
        
        # Sort by distance and return top k
        distances.sort(key=lambda x: x[1])
        return distances[:k]
    
    @staticmethod
    def lowes_ratio_test(matches: List[Tuple[int, float]], ratio_threshold: float = 0.8) -> bool:
        """
        Apply Lowe's ratio test to determine if match is reliable
        Returns True if match passes the test
        """
        if len(matches) < 2:
            return False
        
        # matches[0] is best match, matches[1] is second best
        d1 = matches[0][1]  # distance to best match
        d2 = matches[1][1]  # distance to second best
        
        return d1 < ratio_threshold * d2

# Example: Mathematical workflow for feature matching
if __name__ == "__main__":
    matcher = MathematicalFeatureMatcher()
    
    # Create synthetic image patches (simulating regions around keypoints)
    patch1 = np.random.randint(0, 255, (16, 16))
    patch2 = np.random.randint(0, 255, (16, 16))
    patch3 = np.random.randint(0, 255, (16, 16))
    
    # Create database patches
    db_patches = [np.random.randint(0, 255, (16, 16)) for _ in range(10)]
    
    # Compute descriptors
    query_desc = matcher.compute_simple_descriptor(patch1)
    database_descs = [matcher.compute_simple_descriptor(patch) for patch in db_patches]
    
    print(f"Query descriptor shape: {query_desc.shape}")
    print(f"Descriptor values: {query_desc}")
    
    # Perform brute-force NN search
    best_idx, best_dist = matcher.brute_force_nn(query_desc, database_descs)
    print(f"\nBrute-force NN result:")
    print(f"  Best match index: {best_idx}")
    print(f"  Distance: {best_dist:.4f}")
    
    # Perform k-NN search
    knn_results = matcher.knn_search(query_desc, database_descs, k=3)
    print(f"\nk-NN results (k=3):")
    for idx, (match_idx, dist) in enumerate(knn_results):
        print(f"  Match {idx}: index={match_idx}, distance={dist:.4f}")
    
    # Apply Lowe's ratio test
    passes_test = matcher.lowes_ratio_test(knn_results, ratio_threshold=0.8)
    print(f"\nLowe's ratio test: {'PASS' if passes_test else 'FAIL'}")
    
    # Demonstrate with binary descriptors (simulated)
    print(f"\n--- Binary Descriptor Example ---")
    binary_desc1 = np.random.randint(0, 2, 256)  # 256-bit binary descriptor
    binary_desc2 = np.random.randint(0, 2, 256)
    
    hamming_dist = matcher.hamming_distance(binary_desc1, binary_desc2)
    print(f"Binary descriptor 1: {binary_desc1[:8]}... (first 8 bits)")
    print(f"Binary descriptor 2: {binary_desc2[:8]}... (first 8 bits)")
    print(f"Hamming distance: {hamming_dist} bits different")