Keypoints, descriptors and feature matching for robotics

import cv2
import numpy as np
from matplotlib import pyplot as plt
class FeatureMatcher:
    def __init__(self, detector_type='ORB'):
        """Initialize feature detector and matcher"""
        if detector_type == 'ORB':
            self.detector = cv2.ORB_create(nfeatures=1000, scaleFactor=1.2, nlevels=8)
            self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
        elif detector_type == 'SIFT':
            self.detector = cv2.SIFT_create(nfeatures=1000)
            self.matcher = cv2.BFMatcher(cv2.NORM_L2, crossCheck=False)
        self.detector_type = detector_type
        self.ratio_threshold = 0.75  # Lowe's ratio test threshold
    def detect_and_compute(self, image):
        """Detect keypoints and compute descriptors"""
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if len(image.shape) == 3 else image
        keypoints, descriptors = self.detector.detectAndCompute(gray, None)
        return keypoints, descriptors
    def match_features(self, desc1, desc2, method='knn'):
        """Match descriptors using specified method"""
        if method == 'knn':
            # k-Nearest Neighbors with ratio test
            matches = self.matcher.knnMatch(desc1, desc2, k=2)
            # Apply ratio test
            good_matches = []
            for m, n in matches:
                if m.distance < self.ratio_threshold * n.distance:
                    good_matches.append(m)
            return good_matches
        elif method == 'bf':
            # Brute-force matching with cross-check
            bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
            matches = bf.match(desc1, desc2)
            matches = sorted(matches, key=lambda x: x.distance)
            return matches
    def visualize_matches(self, img1, kp1, img2, kp2, matches):
        """Visualize matched features between two images"""
        # Draw matches
        match_img = cv2.drawMatches(
            img1, kp1, img2, kp2, matches[:50], None,
            flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS
        )
        # Add statistics
        cv2.putText(match_img, f'Matches: {len(matches)}', (10, 30),
                   cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
        return match_img
# Example usage
if __name__ == "__main__":
    # Load images
    img1 = cv2.imread('frame1.jpg')
    img2 = cv2.imread('frame2.jpg')
    # Initialize matcher
    matcher = FeatureMatcher('ORB')
    # Detect features
    kp1, desc1 = matcher.detect_and_compute(img1)
    kp2, desc2 = matcher.detect_and_compute(img2)
    print(f"Image 1: {len(kp1)} keypoints")
    print(f"Image 2: {len(kp2)} keypoints")
    # Match features
    matches = matcher.match_features(desc1, desc2, method='knn')
    print(f"Good matches: {len(matches)}")
    # Visualize
    result = matcher.visualize_matches(img1, kp1, img2, kp2, matches)
    cv2.imshow('Feature Matches', result)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

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")