Initial Commit

src/vision/visual_odometry.py

@@ -0,0 +1,173 @@
+#!/usr/bin/env python3
+"""Visual Odometry - Camera-based position estimation without GPS."""
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image, CameraInfo
+from geometry_msgs.msg import PoseStamped, TwistStamped
+from cv_bridge import CvBridge
+import cv2
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+class VisualOdometryNode(Node):
+    
+    def __init__(self):
+        super().__init__('visual_odometry_node')
+        
+        self.bridge = CvBridge()
+        
+        self.prev_frame = None
+        self.prev_keypoints = None
+        self.prev_descriptors = None
+        
+        self.camera_matrix = None
+        self.dist_coeffs = None
+        
+        self.current_pose = np.eye(4)
+        self.position = np.zeros(3)
+        self.orientation = np.eye(3)
+        self.prev_time = None
+        self.velocity = np.zeros(3)
+        
+        self.detector_type = self.declare_parameter('detector_type', 'ORB').value
+        self.min_features = self.declare_parameter('min_features', 100).value
+        self.feature_quality = self.declare_parameter('feature_quality', 0.01).value
+        
+        if self.detector_type == 'ORB':
+            self.detector = cv2.ORB_create(nfeatures=500)
+            self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
+        elif self.detector_type == 'SIFT':
+            self.detector = cv2.SIFT_create(nfeatures=500)
+            self.matcher = cv2.BFMatcher(cv2.NORM_L2, crossCheck=False)
+        else:
+            self.detector = cv2.ORB_create(nfeatures=500)
+            self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
+        
+        self.image_sub = self.create_subscription(
+            Image, '/uav/camera/forward/image_raw', self.image_callback, 10)
+        
+        self.camera_info_sub = self.create_subscription(
+            CameraInfo, '/uav/camera/forward/camera_info', self.camera_info_callback, 10)
+        
+        self.pose_pub = self.create_publisher(PoseStamped, '/uav/visual_odometry/pose', 10)
+        self.velocity_pub = self.create_publisher(TwistStamped, '/uav/visual_odometry/velocity', 10)
+        
+        self.timer = self.create_timer(0.033, self.publish_pose)
+        
+        self.get_logger().info(f'Visual Odometry Node Started - {self.detector_type} detector')
+    
+    def camera_info_callback(self, msg):
+        if self.camera_matrix is None:
+            self.camera_matrix = np.array(msg.k).reshape(3, 3)
+            self.dist_coeffs = np.array(msg.d)
+            self.get_logger().info('Camera calibration received')
+    
+    def image_callback(self, msg):
+        try:
+            frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
+            gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+            
+            keypoints, descriptors = self.detector.detectAndCompute(gray, None)
+            
+            current_time = self.get_clock().now()
+            
+            if self.prev_frame is not None and len(keypoints) >= self.min_features:
+                matches = self.match_features(self.prev_descriptors, descriptors)
+                
+                if len(matches) >= self.min_features:
+                    self.estimate_motion(self.prev_keypoints, keypoints, matches, current_time)
+            
+            self.prev_frame = gray
+            self.prev_keypoints = keypoints
+            self.prev_descriptors = descriptors
+            self.prev_time = current_time
+            
+        except Exception as e:
+            self.get_logger().error(f'Visual odometry error: {e}')
+    
+    def match_features(self, desc1, desc2):
+        if desc1 is None or desc2 is None:
+            return []
+        
+        matches = self.matcher.knnMatch(desc1, desc2, k=2)
+        
+        good_matches = []
+        for match_pair in matches:
+            if len(match_pair) == 2:
+                m, n = match_pair
+                if m.distance < 0.7 * n.distance:
+                    good_matches.append(m)
+        
+        return good_matches
+    
+    def estimate_motion(self, prev_kp, curr_kp, matches, current_time):
+        if self.camera_matrix is None or len(matches) < 5:
+            return
+        
+        pts1 = np.float32([prev_kp[m.queryIdx].pt for m in matches])
+        pts2 = np.float32([curr_kp[m.trainIdx].pt for m in matches])
+        
+        E, mask = cv2.findEssentialMat(
+            pts1, pts2, self.camera_matrix,
+            method=cv2.RANSAC, prob=0.999, threshold=1.0)
+        
+        if E is None:
+            return
+        
+        _, R, t, mask = cv2.recoverPose(E, pts1, pts2, self.camera_matrix, mask=mask)
+        
+        scale = 1.0
+        translation = scale * t.flatten()
+        
+        if self.prev_time is not None:
+            dt = (current_time - self.prev_time).nanoseconds / 1e9
+            if dt > 0:
+                self.velocity = translation / dt
+        
+        self.position += self.orientation @ translation
+        self.orientation = R @ self.orientation
+    
+    def publish_pose(self):
+        pose_msg = PoseStamped()
+        pose_msg.header.stamp = self.get_clock().now().to_msg()
+        pose_msg.header.frame_id = 'odom'
+        
+        pose_msg.pose.position.x = float(self.position[0])
+        pose_msg.pose.position.y = float(self.position[1])
+        pose_msg.pose.position.z = float(self.position[2])
+        
+        rotation = Rotation.from_matrix(self.orientation)
+        quat = rotation.as_quat()
+        pose_msg.pose.orientation.x = quat[0]
+        pose_msg.pose.orientation.y = quat[1]
+        pose_msg.pose.orientation.z = quat[2]
+        pose_msg.pose.orientation.w = quat[3]
+        
+        self.pose_pub.publish(pose_msg)
+        
+        vel_msg = TwistStamped()
+        vel_msg.header.stamp = self.get_clock().now().to_msg()
+        vel_msg.header.frame_id = 'odom'
+        vel_msg.twist.linear.x = float(self.velocity[0])
+        vel_msg.twist.linear.y = float(self.velocity[1])
+        vel_msg.twist.linear.z = float(self.velocity[2])
+        
+        self.velocity_pub.publish(vel_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VisualOdometryNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()