Yzf/tune map node

tinynav/core/build_map_node.py

@@ -84,9 +84,12 @@ def solve_pose_graph(pose_graph_used_pose:dict, relative_pose_constraint:list, m
     constant_pose_index_dict = { min_timestamp : True }
 
     relative_pose_constraint = [
-        (curr_timestamp, prev_timestamp, T_prev_curr, np.array([10.0, 10.0, 10.0]), np.array([30.0, 30.0, 30.0]))
+        (curr_timestamp, prev_timestamp, T_prev_curr, np.array([1000.0, 1000.0, 1000.0]), np.array([3000.0, 3000.0, 3000.0]))
         for curr_timestamp, prev_timestamp, T_prev_curr in relative_pose_constraint]
+
     optimized_camera_poses = pose_graph_solve(pose_graph_used_pose, relative_pose_constraint, constant_pose_index_dict, max_iteration_num)
+    print("count of relative pose constraint: ", len(relative_pose_constraint))
+    print("count of optimized camera poses: ", len(optimized_camera_poses))
     return {t: optimized_camera_poses[t] for t in sorted(optimized_camera_poses.keys())}
 
 def find_loop(target_embedding:np.ndarray, embeddings:np.ndarray, loop_similarity_threshold:float, loop_top_k:int) -> list[tuple[int, float]]:
@@ -600,11 +603,23 @@ def find_loop_and_pose_graph(timestamp):
                             prev_matched_keypoints, curr_matched_keypoints, matches = self.match_keypoints(prev_features, curr_features)
                             success, T_prev_curr, _, _, inliers = estimate_pose(prev_matched_keypoints, curr_matched_keypoints, curr_depth, self.K)
                             if success and len(inliers) >= 100:
-                                self.relative_pose_constraint.append((curr_timestamp, prev_timestamp, T_prev_curr))
-                                print(f"Added loop relative pose constraint: {curr_timestamp} -> {prev_timestamp}")
-                                self.edges.add((prev_timestamp, curr_timestamp))
+
+                                # compare the loop constraint with the odometry constraint, if the loop constraint is much larger than the odometry constraint, we skip this loop constraint to avoid adding wrong loop constraint.
+                                # by Large, we mean large than the odometryconstraint 10%
+                                odom_T_prev_curr = np.linalg.inv(self.odom[prev_timestamp]) @ self.odom[curr_timestamp]
+                                odom_translation_diff = np.linalg.norm(odom_T_prev_curr[:3, 3] - T_prev_curr[:3, 3])
+                                odom_rotation_diff = np.arccos((np.trace(odom_T_prev_curr[:3, :3].T @ T_prev_curr[:3, :3]) - 1) / 2)
+                                odom_diff = odom_translation_diff + odom_rotation_diff
+                                if odom_diff > 0.00001 * (np.linalg.norm(odom_T_prev_curr[:3, 3])):
+                                    self.get_logger().warn(f"Loop constraint rejected due to large difference with odometry: {odom_diff} > {0.1 * (np.linalg.norm(odom_T_prev_curr[:3, 3]) + np.linalg.norm(T_prev_curr[:3, 3])) / 2}")
+                                    continue
+
+                                #self.relative_pose_constraint.append((curr_timestamp, prev_timestamp, T_prev_curr))
+                                #print(f"Added loop relative pose constraint: {curr_timestamp} -> {prev_timestamp}")
+                                #self.edges.add((prev_timestamp, curr_timestamp))
+
                     with Timer(name = "solve pose graph", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.timer_logger):
-                        self.pose_graph_used_pose = solve_pose_graph(self.pose_graph_used_pose, self.relative_pose_constraint, max_iteration_num = 5)
+                        self.pose_graph_used_pose = solve_pose_graph(self.pose_graph_used_pose, self.relative_pose_constraint, max_iteration_num = 2)
                 find_loop_and_pose_graph(keyframe_image_timestamp)
 
         with Timer(name = "publish local pointcloud", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.timer_logger):

tinynav/core/perception_node.py

@@ -1,7 +1,10 @@
 import argparse
 import json
 import logging
+import pickle
 import sys
+import os
+from datetime import datetime
 import time
 import cv2
 from message_filters import Subscriber, ApproximateTimeSynchronizer, InputAligner, SimpleFilter
@@ -73,13 +76,15 @@ class StereoPairMsg:
 class Keyframe:
     timestamp: float
     image: np.ndarray
+    right_img: np.ndarray
     disparity: np.ndarray
     depth: np.ndarray
     pose: np.ndarray
     velocity: np.ndarray
     bias: gtsam.imuBias.ConstantBias
     preintegrated_imu: gtsam.PreintegratedCombinedMeasurements
     latest_imu_timestamp: float
+    K: np.ndarray = None
 
 class PerceptionNode(Node):
     def __init__(self, verbose_timer: bool = True):
@@ -121,7 +126,7 @@ def __init__(self, verbose_timer: bool = True):
 
         self.input_aligner_imu_filter = SimpleFilter()
         self.input_aligner_stereo_filter = SimpleFilter()
-        self.input_aligner = InputAligner(Duration(seconds=1.000), self.input_aligner_imu_filter, self.input_aligner_stereo_filter)
+        self.input_aligner = InputAligner(Duration(seconds=100.000), self.input_aligner_imu_filter, self.input_aligner_stereo_filter)
         self.input_aligner.setInputPeriod(0, Duration(seconds=0.005))
         self.input_aligner.setInputPeriod(1, Duration(seconds=0.01))
         self.input_aligner.registerCallback(0, self._aligned_imu_callback)
@@ -144,8 +149,8 @@ def __init__(self, verbose_timer: bool = True):
 
         # Noise model (continuous-time)
         # for Realsense D435i
-        accel_noise_density = 0.25     # [m/s^2/√Hz]
-        gyro_noise_density = 0.00005 # [rad/s/√Hz]
+        accel_noise_density = 1.25     # [m/s^2/√Hz]
+        gyro_noise_density = 0.05 # [rad/s/√Hz]
         bias_acc_rw_sigma = 0.001
         bias_gyro_rw_sigma = 0.0001
         self.pre_integration_params = gtsam.PreintegrationCombinedParams.MakeSharedU()
@@ -168,6 +173,9 @@ def __init__(self, verbose_timer: bool = True):
         self.keyframe_queue = []
         self.logger.info("PerceptionNode initialized.")
         self.process_cnt = 0
+        self.debug_folder = f"debug/{datetime.now().strftime('%Y%m%d_%H%M%S')}"
+        os.makedirs(self.debug_folder, exist_ok=True)
+
 
     def info_callback(self, msg):
         if self.K is None:
@@ -210,7 +218,7 @@ def _aligned_stereo_callback(self, stereo_pair_msg):
         left_msg = stereo_pair_msg.left_msg
         right_msg = stereo_pair_msg.right_msg
         image_timestamp = stamp2second(left_msg.header.stamp)
-        if image_timestamp - self.last_processed_timestamp < 0.1333:
+        if image_timestamp - self.last_processed_timestamp < 0.0633:
             return
 
         self.last_processed_timestamp = image_timestamp
@@ -250,13 +258,15 @@ async def process(self, left_msg, right_msg):
                 Keyframe(
                     timestamp=current_timestamp,
                     image=left_img,
+                    right_img=right_img,
                     disparity=disparity,
                     depth=depth,
                     pose=self.T_body_last,
                     velocity=np.zeros(3),
                     bias=gtsam.imuBias.ConstantBias(),
                     preintegrated_imu=gtsam.PreintegratedCombinedMeasurements(self.pre_integration_params, gtsam.imuBias.ConstantBias()),
-                    latest_imu_timestamp=current_timestamp
+                    latest_imu_timestamp=current_timestamp,
+                    K=self.K,
                 )
             )
             return {
@@ -324,13 +334,15 @@ async def process(self, left_msg, right_msg):
             Keyframe(
                 timestamp=current_timestamp,
                 image=left_img,
+                right_img=right_img,
                 disparity=disparity,
                 depth=depth,
                 pose=self.keyframe_queue[-1].pose @ T_kf_curr,
                 velocity=self.keyframe_queue[-1].velocity,
                 bias=gtsam.imuBias.ConstantBias(),
                 preintegrated_imu=gtsam.PreintegratedCombinedMeasurements(self.pre_integration_params, gtsam.imuBias.ConstantBias()),
-                latest_imu_timestamp=current_timestamp
+                latest_imu_timestamp=current_timestamp,
+                K=self.K,
             )
         )
         if len(self.keyframe_queue) > _N:
@@ -385,6 +397,14 @@ async def process(self, left_msg, right_msg):
                 uf = uf_init(len(self.keyframe_queue[-_N:]) * _M)
 
             self.logger.debug(f"Processing {len(self.keyframe_queue)} keyframes for data association.")
+
+            # serialized self.keyframe_queue using pickle, to debug/{process_cnt}_keyframe_queue.pkl, and log the size of the serialized data
+            # create debug folder, using current datetime
+            pickle_keyframe_queue = pickle.dumps(self.keyframe_queue)
+            self.logger.debug(f"Serialized keyframe queue size: {len(pickle_keyframe_queue)} bytes")
+
+            #with open(f"{self.debug_folder}/{self.process_cnt:04d}_keyframe_queue.pkl", "wb") as f:
+            #    f.write(pickle_keyframe_queue)
 
             # Process pairs of keyframes from last _N keyframes: extract features (SuperPoint),
             # match by LightGlue, filter by geometric consistency (pose estimation), 
@@ -460,6 +480,12 @@ async def process(self, left_msg, right_msg):
                 tracks = uf_all_sets_list(uf, min_component_size=2)
                 self.logger.debug(f"Found {len(tracks)} tracks after data association.")
 
+            #match_img = np.zeros((left_img.shape[0] * 2, left_img.shape[1] * 2, 3), dtype=np.uint8)
+            #match_img[:left_img.shape[0], :left_img.shape[1]] = cv2.cvtColor(kf_prev.image, cv2.COLOR_GRAY2BGR)
+            #match_img[left_img.shape[0]:, left_img.shape[1]:] = cv2.cvtColor(kf_curr.image, cv2.COLOR_GRAY2BGR)
+            #match_img[:left_img.shape[0], left_img.shape[1]:] = cv2.cvtColor(kf_curr.right_image, cv2.COLOR_GRAY2BGR)
+            #match_img[left_img.shape[0]:, :left_img.shape[1]] = cv2.cvtColor(kf_prev.right_image, cv2.COLOR_GRAY2BGR)
+
             with Timer(name="[add track]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug):
                 for landmark in tracks[::1]:
                     # Build a smart factor per track (no explicit landmark variable)
@@ -502,6 +528,7 @@ async def process(self, left_msg, right_msg):
             params.setVerbosityLM("DEBUG")
             lm = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)
             result = lm.optimize()
+            #result = initial_estimate
 
             self.logger.info(f"ISAM optimization done with {graph.size()} factors and {initial_estimate.size()} variables.")
             self.logger.info(f"Initial error: {graph.error(initial_estimate):.4f}, Final error: {graph.error(result):.4f}")