diff --git a/tinynav/core/build_map_node.py b/tinynav/core/build_map_node.py index efeaecc..8d880b7 100644 --- a/tinynav/core/build_map_node.py +++ b/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): diff --git a/tinynav/core/perception_node.py b/tinynav/core/perception_node.py index 3e1453b..ffcc4d2 100644 --- a/tinynav/core/perception_node.py +++ b/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,6 +76,7 @@ class StereoPairMsg: class Keyframe: timestamp: float image: np.ndarray + right_img: np.ndarray disparity: np.ndarray depth: np.ndarray pose: np.ndarray @@ -80,6 +84,7 @@ class Keyframe: 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}")