Tharp/macvo fix

docs/stylesheets/extra.css

@@ -11,12 +11,12 @@
 /* Ensure navigation tabs are above splash content */
 .md-tabs {
     position: relative;
-    z-index: 10;
+    z-index: 3;
 }
 
-.md-version {
+.md-version * {
     position: relative;
-    z-index: 25;
+    z-index: 5;
 }
 
 /* Splash container and background */

robot/docker/robot.env

@@ -1,2 +1,2 @@
 # These become environment variables in the robot container
-USE_MACVO="false"
+USE_MACVO="true"

robot/ros_ws/src/autonomy/2_perception/macvo/launch/macvo.launch.xml

@@ -1,6 +1,6 @@
 <launch>
     <node pkg="macvo" exec="macvo">
-        <param name="camera_config" value="$(find-pkg-share macvo)/model_config.yaml"/>
-        <param from="$(find-pkg-share macvo)/interface_config.yaml"/>
+        <param name="camera_config" value="$(find-pkg-share macvo)/config/model_config.yaml"/>
+        <param from="$(find-pkg-share macvo)/config/interface_config.yaml"/>
     </node>
 </launch>

robot/ros_ws/src/autonomy/2_perception/macvo/macvo/macvo.py

@@ -7,6 +7,8 @@
 from rclpy.node import Node
 from sensor_msgs.msg import Image, PointCloud
 from geometry_msgs.msg import PoseStamped
+from scipy.spatial.transform import Rotation
+from geometry_msgs.msg import Transform, Pose, Quaternion
 from message_filters import ApproximateTimeSynchronizer, Subscriber
 
 from pathlib import Path
@@ -38,10 +40,8 @@ def __init__(self):
         self.bridge = None
         self.time = None
         self.prev_time = None
-        self.frame = None
         self.camera_info = None
         self.baseline = None
-        self.prev_frame = None
         self.odometry = None
 
         # Declare subscriptions and publishers ----------------
@@ -111,8 +111,16 @@ def __init__(self):
         self.scale_u = float(self.camera_info.width / u_dim)
         self.scale_v = float(self.camera_info.height / v_dim)
 
-        self.rot_correction_matrix = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])
-        self.rot_correction_matrix = np.eye(3)
+        # 180 degree rotation around x-axis
+        x_rot = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])
+        # 90 degree rotation around z-axis
+        z_rot = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]])
+
+        # self.rot_correction_matrix = np.dot(x_rot, z_rot)
+        # self.rot_correction_matrix = np.dot(z_rot, x_rot)
+        self.correction_matrix = np.eye(4)
+        # self.correction_matrix[:3, :3] = 
+        self.correction_matrix[:3, :3] = np.dot(x_rot, z_rot)
 
         # self.get_logger().info(f"scale u: {self.scale_u}, scale v: {self.scale_v}, u_dim: {u_dim}, v_dim: {v_dim}, width: {self.camera_info.width}, height: {self.camera_info.height}")
 
@@ -144,8 +152,10 @@ def publish_latest_pose(self, system: MACVO):
         out_msg = to_stamped_pose(pose, frame, time)
 
         # Correction for the camera coordinate frame
-        out_msg.pose.position.x, out_msg.pose.position.y, out_msg.pose.position.z = np.dot(self.rot_correction_matrix, np.array([out_msg.pose.position.x, out_msg.pose.position.y, out_msg.pose.position.z]))
-
+        transform = self.pose_to_homogeneous(out_msg.pose)
+        transform = np.dot(self.correction_matrix, transform)
+        out_msg.pose = self.homogeneous_to_pose(transform)
+
         self.pose_pipe.publish(out_msg)
 
     def publish_latest_points(self, system: MACVO):
@@ -169,7 +179,7 @@ def publish_latest_points(self, system: MACVO):
 
         # Correction for the camera coordinate frame
         for pt in out_msg.points:
-            pt.x, pt.y, pt.z = np.dot(self.rot_correction_matrix, np.array([pt.x, pt.y, pt.z]))
+            pt.x, pt.y, pt.z, _ = np.dot(self.correction_matrix, np.array([pt.x, pt.y, pt.z, 1]))
 
         self.point_pipe.publish(out_msg)
 
@@ -216,10 +226,7 @@ def receive_frame(self, msg_L: Image, msg_R: Image) -> None:
             self.get_logger().error("MACVO Node not initialized yet, skipping frame")
             return
 
-        self.prev_frame, self.prev_time = self.frame, self.time
-
-        self.frame        = msg_L.header.frame_id
-
+        self.prev_time = self.time
         self.time = msg_L.header.stamp
         imageL = self.bridge.imgmsg_to_cv2(msg_L, desired_encoding="passthrough")
         imageR = self.bridge.imgmsg_to_cv2(msg_R, desired_encoding="passthrough")
@@ -248,6 +255,61 @@ def receive_frame(self, msg_L: Image, msg_R: Image) -> None:
         end_time = self.get_clock().now()   
         # self.get_logger().info(f"Frame {self.frame_idx} processed in {end_time - start_time}")
         self.frame_idx += 1
+
+    def pose_to_homogeneous(self, pose):
+        """
+        Converts a Pose message into a 4x4 homogeneous transformation matrix.
+
+        Args:
+            Pose (Pose): The ROS Pose message to convert.
+
+        Returns:
+            np.ndarray: A 4x4 homogeneous transformation matrix.
+        """
+        assert isinstance(pose, Pose) , "Input must be a Pose message, got %s" % type(Pose)
+        # Extract translation components
+        translation = pose.position
+        translation_vector = np.array([translation.x, translation.y, translation.z])
+
+        # Extract rotation components
+        rotation = pose.orientation
+        # quaternion = np.array([rotation.x, rotation.y, rotation.z, rotation.w]) * np.array([1, -1, -1, 1])
+        quaternion = np.array([rotation.x, rotation.y, rotation.z, rotation.w])
+        rot_matrix = Rotation.from_quat(quaternion).as_matrix()
+        # assert False, f"rot_vector: {Rotation.from_quat(quaternion).as_rotvec()}"
+
+        # Combine into a homogeneous transformation matrix
+        homogeneous_matrix = np.eye(4)
+        homogeneous_matrix[:3, :3] = rot_matrix[:3, :3]
+        homogeneous_matrix[:3, 3] = translation_vector
+        assert homogeneous_matrix.shape == (4, 4), "Output must be a 4x4 numpy array, got %s" % homogeneous_matrix.shape
+        return homogeneous_matrix
+
+    def homogeneous_to_pose(self, homogeneous_matrix):
+        """
+        Converts a 4x4 homogeneous transformation matrix into a Pose message.
+
+        Args:
+            homogeneous_matrix (np.ndarray): The 4x4 homogeneous transformation matrix to convert.
+
+        Returns:
+            Pose: A ROS Pose message.
+        """
+        assert isinstance(homogeneous_matrix, np.ndarray) and homogeneous_matrix.shape == (4, 4), "Input must be a 4x4 numpy array, got %s" % type(homogeneous_matrix)
+        # Extract translation components
+        translation_vector = homogeneous_matrix[:3, 3]
+        translation = translation_vector
+
+        # Extract rotation components
+        rot_matrix = homogeneous_matrix[:3, :3]
+        quaternion = Rotation.from_matrix(rot_matrix).as_quat()
+        rotation = quaternion
+
+        # Combine into a Pose message
+        pose = Pose()
+        pose.position.x, pose.position.y, pose.position.z = translation
+        pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w = rotation
+        return pose
 
 def main():
     rclpy.init()

robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml

@@ -4,7 +4,7 @@
     <!-- State Estimation -->
     <!-- Visual Odometry -->
 
-    <node pkg="macvo" exec="macvo" if="$(env USE_MACVO)">
+    <node pkg="macvo" exec="macvo">
         <param name="camera_config" value="$(find-pkg-share macvo)/config/model_config.yaml" />
         <param from="$(find-pkg-share macvo)/config/interface_config.yaml" />
         <remap from="left/image_rect"

robot/ros_ws/src/robot_bringup/rviz/robot.rviz

@@ -13,7 +13,7 @@ Panels:
         - /Local1/Trajectory Controller1
         - /Global1
       Splitter Ratio: 0.590062141418457
-    Tree Height: 701
+    Tree Height: 365
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -103,8 +103,6 @@ Visualization Manager:
         world:
           map_FLU:
             base_link_ground_truth:
-              {}
-            map:
               base_link:
                 base_link_frd:
                   {}
@@ -115,6 +113,7 @@ Visualization Manager:
                     {}
                 ouster:
                   {}
+            map:
               base_link_stabilized:
                 {}
               look_ahead_point:
@@ -506,37 +505,37 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 17.393077850341797
+      Distance: 9.570683479309082
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -2.118461847305298
-        Y: 3.126241683959961
-        Z: -0.541254997253418
+        X: 4.359821796417236
+        Y: 5.181562900543213
+        Z: 0.06652849912643433
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3603954613208771
+      Pitch: 0.6066677570343018
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.3381178677082062
+      Yaw: 2.166358232498169
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: false
+    collapsed: true
   Front Left Depth:
     collapsed: false
   Front Left RGB:
     collapsed: false
-  Height: 1376
-  Hide Left Dock: false
+  Height: 2096
+  Hide Left Dock: true
   Hide Right Dock: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -547,6 +546,6 @@ Window Geometry:
     collapsed: false
   Visual Odometry Features:
     collapsed: false
-  Width: 2490
-  X: 1990
+  Width: 3770
+  X: 70
   Y: 27

simulation/isaac-sim/sitl_integration/drag_and_drop/Makefile

@@ -1,2 +1,2 @@
 all:
-	g++ inject.cpp -fPIC -shared -o inject.so
+	g++ inject.cpp -ggdb -fPIC -shared -o inject.so