From 046339ec27139ed2f39df8cdb8ca135824107d2f Mon Sep 17 00:00:00 2001 From: YANG Zhenfei Date: Fri, 17 Apr 2026 22:36:33 +0800 Subject: [PATCH 1/4] feat(node): add imu propagator node --- tinynav/core/imu_propagator_node.py | 118 ++++++ tinynav/core/perception_node.py | 556 ++++++++++++++-------------- 2 files changed, 399 insertions(+), 275 deletions(-) create mode 100644 tinynav/core/imu_propagator_node.py diff --git a/tinynav/core/imu_propagator_node.py b/tinynav/core/imu_propagator_node.py new file mode 100644 index 00000000..b105597e --- /dev/null +++ b/tinynav/core/imu_propagator_node.py @@ -0,0 +1,118 @@ +import logging + +import numpy as np +import rclpy +from message_filters import ApproximateTimeSynchronizer, Subscriber +from nav_msgs.msg import Odometry +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy +from scipy.spatial.transform import Rotation as R +from sensor_msgs.msg import Imu + +from tinynav.core.math_utils import msg2np, np2msg + + +def integrate(odom_prev, imu, gravity_world=np.array([0.0, 0.0, -9.80])): + t0, odom_msg = odom_prev + pose_prev, velocity_prev = msg2np(odom_msg) + rotation_prev = pose_prev[:3, :3] + translation_prev = pose_prev[:3, 3] + + t1, imu_msg = imu + gyro = np.array([ + imu_msg.angular_velocity.x, + imu_msg.angular_velocity.y, + imu_msg.angular_velocity.z, + ], dtype=float) + accel = np.array([ + imu_msg.linear_acceleration.x, + imu_msg.linear_acceleration.y, + imu_msg.linear_acceleration.z, + ], dtype=float) + + dt = t1 - t0 + delta_rotation = R.from_rotvec(gyro * dt).as_matrix() + rotation_new = rotation_prev @ delta_rotation + accel_world = rotation_prev @ accel + gravity_world + velocity_new = velocity_prev + accel_world * dt + translation_new = translation_prev + velocity_prev * dt + 0.5 * accel_world * dt * dt + + pose_new = np.eye(4) + pose_new[:3, :3] = rotation_new + pose_new[:3, 3] = translation_new + + odom_new = np2msg(pose_new, imu_msg.header.stamp, odom_msg.header.frame_id, odom_msg.child_frame_id, velocity_new) + odom_new.twist.twist.angular.x = gyro[0] + odom_new.twist.twist.angular.y = gyro[1] + odom_new.twist.twist.angular.z = gyro[2] + return t1, odom_new + + +class ImuPropagatorNode(Node): + def __init__(self): + super().__init__("imu_propagator_node") + + qos_profile = QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT, depth=1000) + self.imu_sub = self.create_subscription(Imu, "/camera/camera/imu", self.imu_callback, qos_profile) + self.odom_sub = self.create_subscription(Odometry, "/slam/odometry", self.odom_callback, qos_profile) + self.odom_pub = self.create_publisher(Odometry, "/slam/odometry_100hz", 50) + + self.imu_buffer = [] + self.odom_10hz_buffer = [] + self.odom_100hz_buffer = [] + + def imu_callback(self, imu_msg: Imu): + if len(self.odom_100hz_buffer) == 0: + return + + timestamp = self._stamp_to_sec(imu_msg.header.stamp) + self.imu_buffer.append((timestamp, imu_msg)) + if len(self.imu_buffer) > 2000: + self.imu_buffer.pop(0) + + if self.imu_buffer[-1][0] <= self.odom_100hz_buffer[-1][0] + 0.050: + return + + start_idx = None + for i in range(len(self.imu_buffer)): + if self.imu_buffer[-(i + 1)][0] > self.odom_100hz_buffer[-1][0]: + start_idx = -(i + 1) + else: + break + + if start_idx is None: + return + + for i in range(start_idx, 0): + self.odom_100hz_buffer.append(integrate(self.odom_100hz_buffer[-1], self.imu_buffer[i])) + if len(self.odom_100hz_buffer) > 1000: + self.odom_100hz_buffer.pop(0) + + self.odom_pub.publish(self.odom_100hz_buffer[-1][1]) + + def odom_callback(self, msg: Odometry): + timestamp = self._stamp_to_sec(msg.header.stamp) + self.odom_10hz_buffer.append((timestamp, msg)) + if len(self.odom_10hz_buffer) > 100: + self.odom_10hz_buffer.pop(0) + + while self.odom_100hz_buffer and self.odom_100hz_buffer[-1][0] > timestamp: + self.odom_100hz_buffer.pop() + self.odom_100hz_buffer.append((timestamp, msg)) + + @staticmethod + def _stamp_to_sec(stamp) -> float: + return stamp.sec + stamp.nanosec * 1e-9 + + +def main(args=None): + rclpy.init(args=args) + node = ImuPropagatorNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + logging.basicConfig(level=logging.INFO) + main() diff --git a/tinynav/core/perception_node.py b/tinynav/core/perception_node.py index 3e1453b2..baa1f2b3 100644 --- a/tinynav/core/perception_node.py +++ b/tinynav/core/perception_node.py @@ -266,18 +266,7 @@ async def process(self, left_msg, right_msg): with Timer(name="[Stereo Inference]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): disparity, depth = await self.stereo_engine.infer(left_img, right_img, np.array([[self.baseline]]), np.array([[self.K[0,0]]])) - kf_prev = self.keyframe_queue[-1] - prev_left_extract_result = await self.superpoint.infer(kf_prev.image) - current_left_extract_result = await self.superpoint.infer(left_img) - - match_result = await self.light_glue.infer( - prev_left_extract_result["kpts"], - current_left_extract_result["kpts"], - prev_left_extract_result["descps"], - current_left_extract_result["descps"], - prev_left_extract_result["mask"], - current_left_extract_result["mask"], - kf_prev.image.shape, + self.image_shape = ( left_img.shape) # propagate IMU measurements @@ -302,236 +291,246 @@ async def process(self, left_msg, right_msg): with Timer(name="[PnP]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): # do simple pose estimation between last keyframe and current frame - prev_keypoints = prev_left_extract_result["kpts"][0] # (n, 2) - current_keypoints = current_left_extract_result["kpts"][0] # (n, 2) - match_indices = match_result["match_indices"][0] - idx_to_origial = range(len(prev_keypoints)) - valid_mask = match_indices != -1 - kpt_pre = prev_keypoints[valid_mask] - kpt_cur = current_keypoints[match_indices[valid_mask]] - idx_valid = np.array(idx_to_origial)[valid_mask] - logging.debug(f"match cnt: {len(kpt_pre)}") - state, T_kf_curr, _, _, _ = estimate_pose( - kpt_pre, - kpt_cur, - depth, - self.K, - idx_valid - ) - self.logger.debug("Estimated T_kf_curr:\n", T_kf_curr) - # for new frame, we first add it as keyframe, if not, we pop it later - self.keyframe_queue.append( - Keyframe( - timestamp=current_timestamp, - image=left_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 + if self.last_keyframe_features is None: + kpts_curr, desc_curr = self.superpoint.detectAndCompute(left_img) + self.last_keyframe_features = (kpts_curr, desc_curr) + self.last_keyframe_img = left_img + with Timer(name=" [LightGlue]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + kpts_prev, desc_prev = self.last_keyframe_features + idx_prev, idx_curr = self.light_glue.match(desc_prev, desc_curr := self.superpoint.detectAndCompute(left_img)[1], kpts_prev, kpts_curr := self.superpoint.detectAndCompute(left_img)[0]) + with Timer(name=" [Index Features]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + kpts_prev = kpts_prev[idx_prev] + kpts_curr = kpts_curr[idx_curr] + with Timer(name=" [Estimate Pose]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + T_kf_curr, inliers, pnp_diagnostics = estimate_pose(kpts_prev, kpts_curr, self.keyframe_queue[-1].depth, self.K, idx_curr) + if pnp_diagnostics is None: + self.logger.warning("PnP failed, skipping frame.") + return { + "stats": {"process_cnt": self.process_cnt}, + "metrics": { + "num_keyframes": len(self.keyframe_queue), + "num_tracks": 0, + "num_factors": 0, + "num_variables": 0, + "initial_error": 0.0, + "final_error": 0.0, + "pnp_success": False, + "pnp_inlier_count": 0, + "pnp_inlier_ratio": 0.0, + "pnp_match_count": 0, + } + } + + metrics = { + "pnp_success": True, + "pnp_inlier_count": pnp_diagnostics["inlier_count"], + "pnp_inlier_ratio": pnp_diagnostics["inlier_ratio"], + "pnp_match_count": pnp_diagnostics["match_count"], + } + + self.logger.debug(f"estimate_pose cache info: {estimate_pose.cache_info()}") + if np.linalg.norm(T_kf_curr[:3, 3]) > 10.0: + self.logger.warning("Pose estimation failed, skipping frame.") + return + + # check if is keyframe + if keyframe_check(np.eye(4), T_kf_curr): + self.last_keyframe_features = (kpts_curr, desc_curr) + self.last_keyframe_img = left_img + self.keyframe_queue.append( + Keyframe( + timestamp=current_timestamp, + image=left_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 + ) ) - ) - if len(self.keyframe_queue) > _N: - self.keyframe_queue.pop(0) - with Timer(name="[ISAM Processing]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.info): - with Timer(name="[adding imu]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): - # we have new graph each time - graph = gtsam.NonlinearFactorGraph() - initial_estimate = gtsam.Values() - # process previous keyframes' factors - for i, keyframe in enumerate(self.keyframe_queue[-_N:]): - # per pose -- bias - initial_estimate.insert(B(i), gtsam.imuBias.ConstantBias()) - graph.add(gtsam.PriorFactorConstantBias(B(i), gtsam.imuBias.ConstantBias(), gtsam.noiseModel.Diagonal.Sigmas(np.array([1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2])))) - - initial_estimate.insert(V(i), keyframe.velocity) - initial_estimate.insert(X(i), Matrix4x4ToGtsamPose3(keyframe.pose)) - if i == 0: - ## per pose -- velocity - #graph.add(gtsam.PriorFactorVector(V(i), np.zeros(3), gtsam.noiseModel.Diagonal.Sigmas(np.array([1e-2, 1e-2, 1e-2])))) - - # per pose -- pose, could only be applied to the first keyframe - graph.add(gtsam.PriorFactorPose3(X(i), Matrix4x4ToGtsamPose3(keyframe.pose), gtsam.noiseModel.Diagonal.Sigmas(np.array([1e-1, 1e-1, 1e-1, 1e-1, 1e-1, 1e-1])))) - - # per pose -- preintegrated IMU factor, only between two keyframes - if i != len(self.keyframe_queue[-_N:]) - 1: - imu_factor = gtsam.CombinedImuFactor(X(i), V(i), X(i+1), V(i+1), B(i), B(i+1), keyframe.preintegrated_imu) - graph.add(imu_factor) - self.logger.debug(f"for frame {i} at {keyframe.timestamp}, added imufactor up to {keyframe.latest_imu_timestamp}") - - #with Timer(name="[stats]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - # self.frame_diff_t = [] - # for i in range(max(0, len(self.keyframe_queue) - _N), len(self.keyframe_queue) - 1): - # j = i + 1 - # kf_prev_timestamp, kf_prev_image, kf_prev_disparity, kf_prev_depth, kf_prev_P, kf_prev_V, kf_prev_B, kf_prev_factor, _ = astuple(self.keyframe_queue[i]) - # kf_curr_timestamp, kf_curr_image, kf_curr_disparity, kf_curr_depth, kf_curr_P, kf_curr_V, kf_curr_B, kf_curr_factor, _ = astuple(self.keyframe_queue[i + 1]) - # self.frame_diff_t.append(kf_curr_timestamp - kf_prev_timestamp) - - #for i, keyframe in enumerate(self.keyframe_queue[-_N:]): - # kf_timestamp, kf_image, kf_disparity, kf_depth, kf_P, kf_V, kf_B, kf_factor, latest_imu_timestamp = astuple(keyframe) - # if i != len(self.keyframe_queue[-_N:]) - 1: - # imu_factor = gtsam.CombinedImuFactor(X(i), V(i), X(i+1), V(i+1), B(i), B(i+1), kf_factor) - - # print("processing imu factor between ", i, " and ", i+1) - # print("error: ", imu_factor.error(initial_estimate)) - # print("frame_diff_t: ", self.frame_diff_t[i]) - # print("kf_factor: ", kf_factor) - #current_i = len(self.keyframe_queue[-_N:]) - - with Timer(name="[init extract info]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - extract_info = [await self.superpoint.infer(kf.image) for kf in self.keyframe_queue[-_N:]] - uf = uf_init(len(self.keyframe_queue[-_N:]) * _M) - - self.logger.debug(f"Processing {len(self.keyframe_queue)} keyframes for data association.") - - # Process pairs of keyframes from last _N keyframes: extract features (SuperPoint), - # match by LightGlue, filter by geometric consistency (pose estimation), - # and build tracks via Union-Find - with Timer(name="[cached result]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - for i in range(max(0, len(self.keyframe_queue) - _N), len(self.keyframe_queue) - 1): - with Timer(name="[cached result[1/3]]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): - j = i + 1 - kf_prev = self.keyframe_queue[i] - kf_curr = self.keyframe_queue[j] - - self.logger.debug("timestamp prev: ", kf_prev.timestamp) - self.logger.debug("timestamp curr: ", kf_curr.timestamp) - with Timer(name="[cached result[1.1/3]]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): - prev_left_extract_result = await self.superpoint.infer(kf_prev.image) - with Timer(name="[cached result[1.2/3]]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): - current_left_extract_result = await self.superpoint.infer(kf_curr.image) - - with Timer(name="[cached result[1.3/3]]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): - match_result = await self.light_glue.infer( - prev_left_extract_result["kpts"], - current_left_extract_result["kpts"], - prev_left_extract_result["descps"], - current_left_extract_result["descps"], - prev_left_extract_result["mask"], - current_left_extract_result["mask"], - kf_prev.image.shape, - kf_curr.image.shape) - with Timer(name="[cached result[2/3]]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): - prev_keypoints = prev_left_extract_result["kpts"][0] # (n, 2) - current_keypoints = current_left_extract_result["kpts"][0] # (n, 2) - match_indices = match_result["match_indices"][0].copy() - idx_to_origial = range(len(prev_keypoints)) - - valid_mask = match_indices != -1 - kpt_pre = prev_keypoints[valid_mask] - kpt_cur = current_keypoints[match_indices[valid_mask]] - idx_valid = np.array(idx_to_origial)[valid_mask] - - depth = kf_curr.depth - - logging.debug(f"match cnt: {len(kpt_pre)}") - state, _, _, _, inliers = estimate_pose( - kpt_pre, - kpt_cur, - depth, - self.K, - idx_valid - ) - inlier_set = set(inliers) - if len(inlier_set) > 20: - for idx in range(len(match_indices)): - if idx not in inlier_set: - match_indices[idx] = -1 - else: - for idx in range(len(match_indices)): - match_indices[idx] = -1 - self.logger.warning(f"match cnt: {len(kpt_pre)} is too small, {len(inlier_set)} inliers.enable velocity constraint") - velocity_constraint = gtsam.PriorFactorVector(V(i), np.zeros(3), gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.25]))) - graph.add(velocity_constraint) - - with Timer(name="[cached result[3/3]]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): - count = 0 - for k, match_idx in enumerate(match_indices): - if match_idx != -1: - idx_prev = i * _M + k - idx_curr = j * _M + match_idx - uf_union(idx_prev, idx_curr, uf) - count += 1 - self.logger.debug(f"{i} match {j} after Pnp filter count: {count}") - - with Timer(name="[found track]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - tracks = uf_all_sets_list(uf, min_component_size=2) - self.logger.debug(f"Found {len(tracks)} tracks after data association.") - - 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) - disparity_valid = True - observations = [] - for projection in landmark: - pose_idx = projection // _M - feature_idx = projection % _M - disparity = self.keyframe_queue[pose_idx].disparity - kpt = extract_info[pose_idx]['kpts'][0][feature_idx] - if disparity[int(kpt[1]), int(kpt[0])] < 0.1: - disparity_valid = False - break - observations.append((pose_idx, kpt, disparity)) - - if not disparity_valid or len(observations) < 2: + if len(self.keyframe_queue) > _N: + self.keyframe_queue.pop(0) + if len(self.keyframe_queue) < _N: + self.logger.info(f"Not enough keyframes yet. Current: {len(self.keyframe_queue)}, Required: {_N}") + return { + "stats": {"process_cnt": self.process_cnt}, + "metrics": { + "num_keyframes": len(self.keyframe_queue), + "num_tracks": 0, + "num_factors": 0, + "num_variables": 0, + "initial_error": 0.0, + "final_error": 0.0, + **metrics, + } + } + + graph = gtsam.NonlinearFactorGraph() + initial_values = gtsam.Values() + + self.logger.debug("Preintegration params:") + self.logger.debug(f"Accelerometer Covariance: {self.pre_integration_params.getAccelerometerCovariance()}") + self.logger.debug(f"Gyroscope Covariance: {self.pre_integration_params.getGyroscopeCovariance()}") + self.logger.debug(f"Integration Covariance: {self.pre_integration_params.getIntegrationCovariance()}") + self.logger.debug(f"Bias Acc Covariance: {self.pre_integration_params.getBiasAccCovariance()}") + self.logger.debug(f"Bias Omega Covariance: {self.pre_integration_params.getBiasOmegaCovariance()}") + self.logger.debug(f"Omega Coriolis: {self.pre_integration_params.getOmegaCoriolis()}") + + # 1. add variables, priors, imu factors + with Timer(name="[IMU Factor]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + # Prior noise models + pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01, 0.01, 0.01, 0.1, 0.1, 0.1])) + velocity_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + bias_noise = gtsam.noiseModel.Isotropic.Sigma(6, 1e-3) + + # Prior for the first pose, velocity, and bias + first_pose = self.keyframe_queue[0].pose + first_vel = self.keyframe_queue[0].velocity + first_bias = self.keyframe_queue[0].bias + + graph.add(gtsam.PriorFactorPose3(X(0), Matrix4x4ToGtsamPose3(first_pose), pose_noise)) + graph.add(gtsam.PriorFactorVector(V(0), first_vel, velocity_noise)) + graph.add(gtsam.PriorFactorConstantBias(B(0), first_bias, bias_noise)) + + # Insert initial values + for i, kf in enumerate(self.keyframe_queue): + initial_values.insert(X(i), Matrix4x4ToGtsamPose3(kf.pose)) + initial_values.insert(V(i), kf.velocity) + initial_values.insert(B(i), kf.bias) + + imu_bias_rw_sigma = np.array([0.001, 0.001, 0.001, 1e-4, 1e-4, 1e-4]) + bias_noise_model = gtsam.noiseModel.Diagonal.Sigmas(np.sqrt(kf.latest_imu_timestamp - self.keyframe_queue[i - 1].latest_imu_timestamp) * imu_bias_rw_sigma) + for i in range(1, len(self.keyframe_queue)): + prev_kf = self.keyframe_queue[i - 1] + curr_kf = self.keyframe_queue[i] + + self.logger.debug(f"Preintegrated IMU measurements between keyframes {i-1} and {i}:") + self.logger.debug(f" Delta t: {curr_kf.preintegrated_imu.deltaTij()}") + self.logger.debug(f" Delta P: {curr_kf.preintegrated_imu.deltaPij()}") + self.logger.debug(f" Delta V: {curr_kf.preintegrated_imu.deltaVij()}") + self.logger.debug(f" Delta R: {curr_kf.preintegrated_imu.deltaRij().matrix()}") + + imu_factor = gtsam.CombinedImuFactor( + X(i - 1), V(i - 1), X(i), V(i), + B(i - 1), B(i), + curr_kf.preintegrated_imu + ) + graph.add(imu_factor) + graph.add(gtsam.BetweenFactorConstantBias(B(i - 1), B(i), gtsam.imuBias.ConstantBias(), bias_noise_model)) + + # 2. feature track across keyframes + max_dist = 5.0 # meters + feature_tracks = [] + landmark_positions = [] + correspondence_info = [] + + with Timer(name="[Feature Matching]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + all_kpts = [] + all_desc = [] + for kf in self.keyframe_queue: + kpts, desc = self.superpoint.detectAndCompute(kf.image) + all_kpts.append(kpts) + all_desc.append(desc) + for i in range(len(self.keyframe_queue)): + for j in range(i + 1, len(self.keyframe_queue)): + idx_i, idx_j = self.light_glue.match(all_desc[i], all_desc[j], all_kpts[i], all_kpts[j]) + kpts_i = all_kpts[i][idx_i] + kpts_j = all_kpts[j][idx_j] + pts_3d = [] + pts_2d = [] + valid_idx_i = [] + for m, (kp_i, kp_j) in enumerate(zip(kpts_i, kpts_j)): + u, v = int(kp_i[0]), int(kp_i[1]) + if 0 <= v < self.keyframe_queue[i].depth.shape[0] and 0 <= u < self.keyframe_queue[i].depth.shape[1]: + Z = self.keyframe_queue[i].depth[v, u] + if Z > 0.1 and Z < max_dist: + pts_3d.append(depth_to_point(kp_i, Z, self.K)) + pts_2d.append(kp_j) + valid_idx_i.append(idx_i[m]) + if len(pts_3d) == 0: continue - - # Smart factors require isotropic pixel noise - noise = gtsam.noiseModel.Isotropic.Sigma(3, 1.0) - params = gtsam.SmartProjectionParams() - smart_factor = gtsam_unstable.SmartStereoProjectionPoseFactor(noise, params) - - calib = gtsam.Cal3_S2Stereo( - self.K[0, 0], self.K[1, 1], 0, self.K[0, 2], self.K[1, 2], self.baseline - ) - for pose_idx, kpt, disparity in observations: - stereo_meas = gtsam.StereoPoint2( - kpt[0], - kpt[0] - disparity[int(kpt[1]), int(kpt[0])], - kpt[1], + T_iw = self.keyframe_queue[i].pose + landmarks_i = [T_iw[:3, :3] @ pt + T_iw[:3, 3] for pt in pts_3d] + feature_tracks.append((i, j, valid_idx_i, idx_j[:len(valid_idx_i)], landmarks_i, pts_2d)) + correspondence_info.append((i, j, len(pts_3d))) + + # 3. robust union-find on landmarks with outlier rejection + track_map = {} + point_records = [] + for track_id, (i, j, idx_i_list, idx_j_list, landmarks_i, pts_2d) in enumerate(feature_tracks): + for n, (idx_i, idx_j, landmark) in enumerate(zip(idx_i_list, idx_j_list, landmarks_i)): + key_i = (i, idx_i) + key_j = (j, idx_j) + point_records.append((track_id, n, key_i, key_j, landmark)) + + uf = uf_init(len(point_records)) + key_to_indices = {} + for idx, (_, _, key_i, key_j, _) in enumerate(point_records): + key_to_indices.setdefault(key_i, []).append(idx) + key_to_indices.setdefault(key_j, []).append(idx) + for indices in key_to_indices.values(): + for k in range(1, len(indices)): + uf_union(uf, indices[0], indices[k]) + sets = uf_all_sets_list(uf) + + for s in sets: + if len(s) < 2: + continue + pts = np.array([point_records[idx][4] for idx in s]) + mean = np.mean(pts, axis=0) + dists = np.linalg.norm(pts - mean, axis=1) + inliers = [s[idx] for idx, d in enumerate(dists) if d < 0.3] + if len(inliers) < 2: + continue + landmark_positions.append(np.mean([point_records[idx][4] for idx in inliers], axis=0)) + lm_id = len(landmark_positions) - 1 + for idx in inliers: + _, _, key_i, key_j, _ = point_records[idx] + track_map[key_i] = lm_id + track_map[key_j] = lm_id + + # 4. add projection factors + fx, fy = self.K[0, 0], self.K[1, 1] + cx, cy = self.K[0, 2], self.K[1, 2] + K_cal = gtsam.Cal3_S2(fx, fy, 0.0, cx, cy) + meas_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) + + with Timer(name="[Projection Factors]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + for track_id, (i, j, idx_i_list, idx_j_list, landmarks_i, pts_2d) in enumerate(feature_tracks): + kpts_i = all_kpts[i][idx_i_list] + kpts_j = all_kpts[j][idx_j_list] + for idx_i, idx_j, pt_j in zip(idx_i_list, idx_j_list, pts_2d): + key_i = (i, idx_i) + key_j = (j, idx_j) + if key_i in track_map and key_j in track_map and track_map[key_i] == track_map[key_j]: + lm_id = track_map[key_i] + landmark = landmark_positions[lm_id] + point3 = gtsam.Point3(landmark) + if not initial_values.exists(gtsam.symbol('l', lm_id)): + initial_values.insert(gtsam.symbol('l', lm_id), point3) + factor = gtsam.GenericProjectionFactorCal3_S2( + gtsam.Point2(pt_j[0], pt_j[1]), + meas_noise, + X(j), + gtsam.symbol('l', lm_id), + K_cal, + Matrix4x4ToGtsamPose3(self.T_imu_body_to_camera) ) - smart_factor.add(stereo_meas, X(pose_idx), calib) - graph.add(smart_factor) - - with Timer(name="[Solver]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - params = gtsam.LevenbergMarquardtParams() - # set iteration limit - params.setMaxIterations(3) - params.setVerbosityLM("DEBUG") - lm = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) - result = lm.optimize() - - 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}") - - for i, keyframe in enumerate(self.keyframe_queue[-_N:]): - T_i = result.atPose3(X(i)).matrix() - keyframe.pose = T_i - keyframe.velocity = result.atVector(V(i)) - self.logger.debug(f"Keyframe {i} pose updated:\n{T_i}, at timestamp {keyframe.timestamp}") - self.logger.debug(f"Bias {i} updated:\n{result.atConstantBias(B(i))}") - #print("imu error: ", keyframe.preintegrated_imu.error(initial_estimate)) - - with Timer(text="[Depth as Color] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - disp_vis = disparity.copy().astype(np.uint8) - disp_color = cv2.applyColorMap(disp_vis * 4, cv2.COLORMAP_PLASMA) - disp_color_msg = self.bridge.cv2_to_imgmsg(disp_color, encoding='bgr8') - disp_color_msg.header = left_msg.header - self.disparity_pub_vis.publish(disp_color_msg) - - with Timer(name='[Depth as Cloud', text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - # publish depth image and camera info for depth topic (required by DepthCloud) - depth_msg = self.bridge.cv2_to_imgmsg(depth, encoding="32FC1") - depth_msg.header.stamp = left_msg.header.stamp - depth_msg.header.frame_id = "camera" # Match TF frame - self.camera_info_msg.header.stamp = left_msg.header.stamp - self.camera_info_msg.header.frame_id = "camera" # Match TF frame - self.slam_camera_info_pub.publish(self.camera_info_msg) - self.depth_pub.publish(depth_msg) - self.logger.debug(f"superpoint cache info: {self.superpoint.infer.cache_info()}") - self.logger.debug(f"lightglue cache info: {self.light_glue.infer.cache_info()}") + graph.add(factor) + + with Timer(name="[Optimize]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_values) + initial_error = graph.error(initial_values) + result = optimizer.optimize() + final_error = graph.error(result) + + self.logger.debug(f"Correspondence info (kf_i, kf_j, num_matches): {correspondence_info}") + self.logger.debug(f"Number of landmarks: {len(landmark_positions)}") + self.logger.debug(f"Number of factors: {graph.size()}") + self.logger.debug(f"Initial error: {initial_error}") + self.logger.debug(f"Final error: {final_error}") self.logger.debug(f"estimate_pose cache info: {estimate_pose.cache_info()}") with Timer(name="[Publish Odometry]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): @@ -541,48 +540,55 @@ async def process(self, left_msg, right_msg): self.odom_pub.publish(np2msg(self.T_body_last, left_msg.header.stamp, "world", "camera", self.V_last)) # publish TF self.tf_broadcaster.sendTransform(np2tf(self.T_body_last, left_msg.header.stamp, "world", "camera")) - - last_keyframe = self.keyframe_queue[-2] - current_keyframe = self.keyframe_queue[-1] - if keyframe_check(last_keyframe.pose, current_keyframe.pose) or current_keyframe.timestamp - last_keyframe.timestamp > 3.0: - self.keyframe_pose_pub.publish(np2msg(current_keyframe.pose, left_msg.header.stamp, "world", "camera", current_keyframe.velocity)) - self.keyframe_image_pub.publish(left_msg) - self.keyframe_depth_pub.publish(depth_msg) - else: - self.keyframe_queue.pop() + # publish camera info + self.camera_info_msg.header.stamp = left_msg.header.stamp + self.camera_info_msg.header.frame_id = "camera" + self.slam_camera_info_pub.publish(self.camera_info_msg) + # publish depth + depth_msg = self.bridge.cv2_to_imgmsg(depth.astype(np.float32), encoding='32FC1') + depth_msg.header.stamp = left_msg.header.stamp + depth_msg.header.frame_id = "camera" + self.depth_pub.publish(depth_msg) + # publish depth_vis 0->255, uint8 + disparity_vis = (disparity - disparity.min()) / (disparity.max() - disparity.min()) * 255 + disparity_vis = disparity_vis.astype(np.uint8) + disparity_vis = cv2.applyColorMap(disparity_vis, cv2.COLORMAP_JET) + disparity_msg = self.bridge.cv2_to_imgmsg(disparity_vis, encoding='bgr8') + disparity_msg.header.stamp = left_msg.header.stamp + disparity_msg.header.frame_id = "camera" + self.disparity_pub_vis.publish(disparity_msg) + + # publish latest keyframe pose, image, depth if available + if self.keyframe_queue: + kf = self.keyframe_queue[-1] + self.keyframe_pose_pub.publish(np2msg(kf.pose, left_msg.header.stamp, "world", "camera", kf.velocity)) + keyframe_image_msg = self.bridge.cv2_to_imgmsg(kf.image, encoding='mono8') + keyframe_image_msg.header.stamp = left_msg.header.stamp + keyframe_image_msg.header.frame_id = "camera" + self.keyframe_image_pub.publish(keyframe_image_msg) + keyframe_depth_msg = self.bridge.cv2_to_imgmsg(kf.depth.astype(np.float32), encoding='32FC1') + keyframe_depth_msg.header.stamp = left_msg.header.stamp + keyframe_depth_msg.header.frame_id = "camera" + self.keyframe_depth_pub.publish(keyframe_depth_msg) return { - "stats": { - "process_cnt": self.process_cnt, - }, + "stats": {"process_cnt": self.process_cnt}, "metrics": { "num_keyframes": len(self.keyframe_queue), - "num_tracks": len(tracks), + "num_tracks": len(feature_tracks), "num_factors": graph.size(), - "num_variables": initial_estimate.size(), - "initial_error": graph.error(initial_estimate), - "final_error": graph.error(result), - }, + "num_variables": initial_values.size(), + "initial_error": initial_error, + "final_error": final_error, + **metrics, + } } - def main(args=None): - rclpy.init(args=args) - parser = argparse.ArgumentParser() - parser.set_defaults(verbose_timer=True) - parser.add_argument("--verbose_timer", action="store_true", help="Enable verbose timer output") - parser.add_argument("--no_verbose_timer", dest="verbose_timer", action="store_false", help="Disable verbose timer output") - parser.add_argument("--log_file", type=str, default="odom.log", help="Path to the log file") - parsed_args, unknown_args = parser.parse_known_args(sys.argv[1:]) - print(f"Verbose timer: {parsed_args.verbose_timer}") - - logging.basicConfig( - level=logging.INFO, - format="%(asctime)s - %(filename)s:%(lineno)s - %(message)s", - datefmt="%Y-%m-%d %H:%M:%S", - handlers=[logging.StreamHandler(sys.stdout), logging.FileHandler(parsed_args.log_file)], - ) + parser = argparse.ArgumentParser(description='Run TinyNav perception node.') + parser.add_argument('--verbose_timer', action='store_true', help='Print timing for key pipeline stages.') + parsed_args = parser.parse_args(args=sys.argv[1:] if args is None else args) perception_node = PerceptionNode(verbose_timer=parsed_args.verbose_timer) @@ -590,8 +596,8 @@ def main(args=None): executor.add_node(perception_node) executor.spin() perception_node.destroy_node() - executor.shutdown() - + rclpy.shutdown() -if __name__ == "__main__": +if __name__ == '__main__': + logging.basicConfig(level=logging.INFO) main() From 8b85ee09bd741272ef075eead74c3982e110109d Mon Sep 17 00:00:00 2001 From: YANG Zhenfei Date: Fri, 17 Apr 2026 22:55:22 +0800 Subject: [PATCH 2/4] feat(node): run imu propagator with perception --- tinynav/core/perception_node.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/tinynav/core/perception_node.py b/tinynav/core/perception_node.py index baa1f2b3..968dcd28 100644 --- a/tinynav/core/perception_node.py +++ b/tinynav/core/perception_node.py @@ -26,6 +26,7 @@ from dataclasses import dataclass from gtsam.symbol_shorthand import X, B, V +from tinynav.core.imu_propagator_node import ImuPropagatorNode _N = 5 _M = 1000 @@ -591,11 +592,14 @@ def main(args=None): parsed_args = parser.parse_args(args=sys.argv[1:] if args is None else args) perception_node = PerceptionNode(verbose_timer=parsed_args.verbose_timer) + imu_propagator_node = ImuPropagatorNode() - executor = rclpy.executors.SingleThreadedExecutor() + executor = rclpy.executors.MultiThreadedExecutor() executor.add_node(perception_node) + executor.add_node(imu_propagator_node) executor.spin() perception_node.destroy_node() + imu_propagator_node.destroy_node() rclpy.shutdown() if __name__ == '__main__': From f4beff0ed5e749b92184f219f233deea35e3e6dc Mon Sep 17 00:00:00 2001 From: YANG Zhenfei Date: Fri, 17 Apr 2026 22:59:24 +0800 Subject: [PATCH 3/4] fix(node): restore PerceptionNode from main --- tinynav/core/perception_node.py | 532 +++++++++++++++----------------- 1 file changed, 257 insertions(+), 275 deletions(-) diff --git a/tinynav/core/perception_node.py b/tinynav/core/perception_node.py index 968dcd28..b7f9545b 100644 --- a/tinynav/core/perception_node.py +++ b/tinynav/core/perception_node.py @@ -267,7 +267,18 @@ async def process(self, left_msg, right_msg): with Timer(name="[Stereo Inference]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): disparity, depth = await self.stereo_engine.infer(left_img, right_img, np.array([[self.baseline]]), np.array([[self.K[0,0]]])) - self.image_shape = ( + kf_prev = self.keyframe_queue[-1] + prev_left_extract_result = await self.superpoint.infer(kf_prev.image) + current_left_extract_result = await self.superpoint.infer(left_img) + + match_result = await self.light_glue.infer( + prev_left_extract_result["kpts"], + current_left_extract_result["kpts"], + prev_left_extract_result["descps"], + current_left_extract_result["descps"], + prev_left_extract_result["mask"], + current_left_extract_result["mask"], + kf_prev.image.shape, left_img.shape) # propagate IMU measurements @@ -292,246 +303,236 @@ async def process(self, left_msg, right_msg): with Timer(name="[PnP]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): # do simple pose estimation between last keyframe and current frame - if self.last_keyframe_features is None: - kpts_curr, desc_curr = self.superpoint.detectAndCompute(left_img) - self.last_keyframe_features = (kpts_curr, desc_curr) - self.last_keyframe_img = left_img - with Timer(name=" [LightGlue]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - kpts_prev, desc_prev = self.last_keyframe_features - idx_prev, idx_curr = self.light_glue.match(desc_prev, desc_curr := self.superpoint.detectAndCompute(left_img)[1], kpts_prev, kpts_curr := self.superpoint.detectAndCompute(left_img)[0]) - with Timer(name=" [Index Features]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - kpts_prev = kpts_prev[idx_prev] - kpts_curr = kpts_curr[idx_curr] - with Timer(name=" [Estimate Pose]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - T_kf_curr, inliers, pnp_diagnostics = estimate_pose(kpts_prev, kpts_curr, self.keyframe_queue[-1].depth, self.K, idx_curr) - if pnp_diagnostics is None: - self.logger.warning("PnP failed, skipping frame.") - return { - "stats": {"process_cnt": self.process_cnt}, - "metrics": { - "num_keyframes": len(self.keyframe_queue), - "num_tracks": 0, - "num_factors": 0, - "num_variables": 0, - "initial_error": 0.0, - "final_error": 0.0, - "pnp_success": False, - "pnp_inlier_count": 0, - "pnp_inlier_ratio": 0.0, - "pnp_match_count": 0, - } - } - - metrics = { - "pnp_success": True, - "pnp_inlier_count": pnp_diagnostics["inlier_count"], - "pnp_inlier_ratio": pnp_diagnostics["inlier_ratio"], - "pnp_match_count": pnp_diagnostics["match_count"], - } - - self.logger.debug(f"estimate_pose cache info: {estimate_pose.cache_info()}") - if np.linalg.norm(T_kf_curr[:3, 3]) > 10.0: - self.logger.warning("Pose estimation failed, skipping frame.") - return - - # check if is keyframe - if keyframe_check(np.eye(4), T_kf_curr): - self.last_keyframe_features = (kpts_curr, desc_curr) - self.last_keyframe_img = left_img - self.keyframe_queue.append( - Keyframe( - timestamp=current_timestamp, - image=left_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 - ) + prev_keypoints = prev_left_extract_result["kpts"][0] # (n, 2) + current_keypoints = current_left_extract_result["kpts"][0] # (n, 2) + match_indices = match_result["match_indices"][0] + idx_to_origial = range(len(prev_keypoints)) + valid_mask = match_indices != -1 + kpt_pre = prev_keypoints[valid_mask] + kpt_cur = current_keypoints[match_indices[valid_mask]] + idx_valid = np.array(idx_to_origial)[valid_mask] + logging.debug(f"match cnt: {len(kpt_pre)}") + state, T_kf_curr, _, _, _ = estimate_pose( + kpt_pre, + kpt_cur, + depth, + self.K, + idx_valid ) - if len(self.keyframe_queue) > _N: - self.keyframe_queue.pop(0) - if len(self.keyframe_queue) < _N: - self.logger.info(f"Not enough keyframes yet. Current: {len(self.keyframe_queue)}, Required: {_N}") - return { - "stats": {"process_cnt": self.process_cnt}, - "metrics": { - "num_keyframes": len(self.keyframe_queue), - "num_tracks": 0, - "num_factors": 0, - "num_variables": 0, - "initial_error": 0.0, - "final_error": 0.0, - **metrics, - } - } - - graph = gtsam.NonlinearFactorGraph() - initial_values = gtsam.Values() - - self.logger.debug("Preintegration params:") - self.logger.debug(f"Accelerometer Covariance: {self.pre_integration_params.getAccelerometerCovariance()}") - self.logger.debug(f"Gyroscope Covariance: {self.pre_integration_params.getGyroscopeCovariance()}") - self.logger.debug(f"Integration Covariance: {self.pre_integration_params.getIntegrationCovariance()}") - self.logger.debug(f"Bias Acc Covariance: {self.pre_integration_params.getBiasAccCovariance()}") - self.logger.debug(f"Bias Omega Covariance: {self.pre_integration_params.getBiasOmegaCovariance()}") - self.logger.debug(f"Omega Coriolis: {self.pre_integration_params.getOmegaCoriolis()}") - - # 1. add variables, priors, imu factors - with Timer(name="[IMU Factor]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - # Prior noise models - pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01, 0.01, 0.01, 0.1, 0.1, 0.1])) - velocity_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - bias_noise = gtsam.noiseModel.Isotropic.Sigma(6, 1e-3) - - # Prior for the first pose, velocity, and bias - first_pose = self.keyframe_queue[0].pose - first_vel = self.keyframe_queue[0].velocity - first_bias = self.keyframe_queue[0].bias - - graph.add(gtsam.PriorFactorPose3(X(0), Matrix4x4ToGtsamPose3(first_pose), pose_noise)) - graph.add(gtsam.PriorFactorVector(V(0), first_vel, velocity_noise)) - graph.add(gtsam.PriorFactorConstantBias(B(0), first_bias, bias_noise)) - - # Insert initial values - for i, kf in enumerate(self.keyframe_queue): - initial_values.insert(X(i), Matrix4x4ToGtsamPose3(kf.pose)) - initial_values.insert(V(i), kf.velocity) - initial_values.insert(B(i), kf.bias) - - imu_bias_rw_sigma = np.array([0.001, 0.001, 0.001, 1e-4, 1e-4, 1e-4]) - bias_noise_model = gtsam.noiseModel.Diagonal.Sigmas(np.sqrt(kf.latest_imu_timestamp - self.keyframe_queue[i - 1].latest_imu_timestamp) * imu_bias_rw_sigma) - for i in range(1, len(self.keyframe_queue)): - prev_kf = self.keyframe_queue[i - 1] - curr_kf = self.keyframe_queue[i] - - self.logger.debug(f"Preintegrated IMU measurements between keyframes {i-1} and {i}:") - self.logger.debug(f" Delta t: {curr_kf.preintegrated_imu.deltaTij()}") - self.logger.debug(f" Delta P: {curr_kf.preintegrated_imu.deltaPij()}") - self.logger.debug(f" Delta V: {curr_kf.preintegrated_imu.deltaVij()}") - self.logger.debug(f" Delta R: {curr_kf.preintegrated_imu.deltaRij().matrix()}") - - imu_factor = gtsam.CombinedImuFactor( - X(i - 1), V(i - 1), X(i), V(i), - B(i - 1), B(i), - curr_kf.preintegrated_imu - ) - graph.add(imu_factor) - graph.add(gtsam.BetweenFactorConstantBias(B(i - 1), B(i), gtsam.imuBias.ConstantBias(), bias_noise_model)) - - # 2. feature track across keyframes - max_dist = 5.0 # meters - feature_tracks = [] - landmark_positions = [] - correspondence_info = [] - - with Timer(name="[Feature Matching]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - all_kpts = [] - all_desc = [] - for kf in self.keyframe_queue: - kpts, desc = self.superpoint.detectAndCompute(kf.image) - all_kpts.append(kpts) - all_desc.append(desc) - for i in range(len(self.keyframe_queue)): - for j in range(i + 1, len(self.keyframe_queue)): - idx_i, idx_j = self.light_glue.match(all_desc[i], all_desc[j], all_kpts[i], all_kpts[j]) - kpts_i = all_kpts[i][idx_i] - kpts_j = all_kpts[j][idx_j] - pts_3d = [] - pts_2d = [] - valid_idx_i = [] - for m, (kp_i, kp_j) in enumerate(zip(kpts_i, kpts_j)): - u, v = int(kp_i[0]), int(kp_i[1]) - if 0 <= v < self.keyframe_queue[i].depth.shape[0] and 0 <= u < self.keyframe_queue[i].depth.shape[1]: - Z = self.keyframe_queue[i].depth[v, u] - if Z > 0.1 and Z < max_dist: - pts_3d.append(depth_to_point(kp_i, Z, self.K)) - pts_2d.append(kp_j) - valid_idx_i.append(idx_i[m]) - if len(pts_3d) == 0: + self.logger.debug("Estimated T_kf_curr:\n", T_kf_curr) + # for new frame, we first add it as keyframe, if not, we pop it later + self.keyframe_queue.append( + Keyframe( + timestamp=current_timestamp, + image=left_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 + ) + ) + if len(self.keyframe_queue) > _N: + self.keyframe_queue.pop(0) + with Timer(name="[ISAM Processing]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.info): + with Timer(name="[adding imu]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): + # we have new graph each time + graph = gtsam.NonlinearFactorGraph() + initial_estimate = gtsam.Values() + # process previous keyframes' factors + for i, keyframe in enumerate(self.keyframe_queue[-_N:]): + # per pose -- bias + initial_estimate.insert(B(i), gtsam.imuBias.ConstantBias()) + graph.add(gtsam.PriorFactorConstantBias(B(i), gtsam.imuBias.ConstantBias(), gtsam.noiseModel.Diagonal.Sigmas(np.array([1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2])))) + + initial_estimate.insert(V(i), keyframe.velocity) + initial_estimate.insert(X(i), Matrix4x4ToGtsamPose3(keyframe.pose)) + if i == 0: + ## per pose -- velocity + #graph.add(gtsam.PriorFactorVector(V(i), np.zeros(3), gtsam.noiseModel.Diagonal.Sigmas(np.array([1e-2, 1e-2, 1e-2])))) + + # per pose -- pose, could only be applied to the first keyframe + graph.add(gtsam.PriorFactorPose3(X(i), Matrix4x4ToGtsamPose3(keyframe.pose), gtsam.noiseModel.Diagonal.Sigmas(np.array([1e-1, 1e-1, 1e-1, 1e-1, 1e-1, 1e-1])))) + + # per pose -- preintegrated IMU factor, only between two keyframes + if i != len(self.keyframe_queue[-_N:]) - 1: + imu_factor = gtsam.CombinedImuFactor(X(i), V(i), X(i+1), V(i+1), B(i), B(i+1), keyframe.preintegrated_imu) + graph.add(imu_factor) + self.logger.debug(f"for frame {i} at {keyframe.timestamp}, added imufactor up to {keyframe.latest_imu_timestamp}") + + #with Timer(name="[stats]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + # self.frame_diff_t = [] + # for i in range(max(0, len(self.keyframe_queue) - _N), len(self.keyframe_queue) - 1): + # j = i + 1 + # kf_prev_timestamp, kf_prev_image, kf_prev_disparity, kf_prev_depth, kf_prev_P, kf_prev_V, kf_prev_B, kf_prev_factor, _ = astuple(self.keyframe_queue[i]) + # kf_curr_timestamp, kf_curr_image, kf_curr_disparity, kf_curr_depth, kf_curr_P, kf_curr_V, kf_curr_B, kf_curr_factor, _ = astuple(self.keyframe_queue[i + 1]) + # self.frame_diff_t.append(kf_curr_timestamp - kf_prev_timestamp) + + #for i, keyframe in enumerate(self.keyframe_queue[-_N:]): + # kf_timestamp, kf_image, kf_disparity, kf_depth, kf_P, kf_V, kf_B, kf_factor, latest_imu_timestamp = astuple(keyframe) + # if i != len(self.keyframe_queue[-_N:]) - 1: + # imu_factor = gtsam.CombinedImuFactor(X(i), V(i), X(i+1), V(i+1), B(i), B(i+1), kf_factor) + + # print("processing imu factor between ", i, " and ", i+1) + # print("error: ", imu_factor.error(initial_estimate)) + # print("frame_diff_t: ", self.frame_diff_t[i]) + # print("kf_factor: ", kf_factor) + #current_i = len(self.keyframe_queue[-_N:]) + + with Timer(name="[init extract info]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + extract_info = [await self.superpoint.infer(kf.image) for kf in self.keyframe_queue[-_N:]] + uf = uf_init(len(self.keyframe_queue[-_N:]) * _M) + + self.logger.debug(f"Processing {len(self.keyframe_queue)} keyframes for data association.") + + # Process pairs of keyframes from last _N keyframes: extract features (SuperPoint), + # match by LightGlue, filter by geometric consistency (pose estimation), + # and build tracks via Union-Find + with Timer(name="[cached result]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + for i in range(max(0, len(self.keyframe_queue) - _N), len(self.keyframe_queue) - 1): + with Timer(name="[cached result[1/3]]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): + j = i + 1 + kf_prev = self.keyframe_queue[i] + kf_curr = self.keyframe_queue[j] + + self.logger.debug("timestamp prev: ", kf_prev.timestamp) + self.logger.debug("timestamp curr: ", kf_curr.timestamp) + with Timer(name="[cached result[1.1/3]]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): + prev_left_extract_result = await self.superpoint.infer(kf_prev.image) + with Timer(name="[cached result[1.2/3]]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): + current_left_extract_result = await self.superpoint.infer(kf_curr.image) + + with Timer(name="[cached result[1.3/3]]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): + match_result = await self.light_glue.infer( + prev_left_extract_result["kpts"], + current_left_extract_result["kpts"], + prev_left_extract_result["descps"], + current_left_extract_result["descps"], + prev_left_extract_result["mask"], + current_left_extract_result["mask"], + kf_prev.image.shape, + kf_curr.image.shape) + with Timer(name="[cached result[2/3]]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): + prev_keypoints = prev_left_extract_result["kpts"][0] # (n, 2) + current_keypoints = current_left_extract_result["kpts"][0] # (n, 2) + match_indices = match_result["match_indices"][0].copy() + idx_to_origial = range(len(prev_keypoints)) + + valid_mask = match_indices != -1 + kpt_pre = prev_keypoints[valid_mask] + kpt_cur = current_keypoints[match_indices[valid_mask]] + idx_valid = np.array(idx_to_origial)[valid_mask] + + depth = kf_curr.depth + + logging.debug(f"match cnt: {len(kpt_pre)}") + state, _, _, _, inliers = estimate_pose( + kpt_pre, + kpt_cur, + depth, + self.K, + idx_valid + ) + inlier_set = set(inliers) + if len(inlier_set) > 20: + for idx in range(len(match_indices)): + if idx not in inlier_set: + match_indices[idx] = -1 + else: + for idx in range(len(match_indices)): + match_indices[idx] = -1 + self.logger.warning(f"match cnt: {len(kpt_pre)} is too small, {len(inlier_set)} inliers.enable velocity constraint") + velocity_constraint = gtsam.PriorFactorVector(V(i), np.zeros(3), gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.25]))) + graph.add(velocity_constraint) + + with Timer(name="[cached result[3/3]]", text="[{name}] Elapsed time: {milliseconds:.03f} ms", logger=self.logger.debug): + count = 0 + for k, match_idx in enumerate(match_indices): + if match_idx != -1: + idx_prev = i * _M + k + idx_curr = j * _M + match_idx + uf_union(idx_prev, idx_curr, uf) + count += 1 + self.logger.debug(f"{i} match {j} after Pnp filter count: {count}") + + with Timer(name="[found track]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + tracks = uf_all_sets_list(uf, min_component_size=2) + self.logger.debug(f"Found {len(tracks)} tracks after data association.") + + 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) + disparity_valid = True + observations = [] + for projection in landmark: + pose_idx = projection // _M + feature_idx = projection % _M + disparity = self.keyframe_queue[pose_idx].disparity + kpt = extract_info[pose_idx]['kpts'][0][feature_idx] + if disparity[int(kpt[1]), int(kpt[0])] < 0.1: + disparity_valid = False + break + observations.append((pose_idx, kpt, disparity)) + + if not disparity_valid or len(observations) < 2: continue - T_iw = self.keyframe_queue[i].pose - landmarks_i = [T_iw[:3, :3] @ pt + T_iw[:3, 3] for pt in pts_3d] - feature_tracks.append((i, j, valid_idx_i, idx_j[:len(valid_idx_i)], landmarks_i, pts_2d)) - correspondence_info.append((i, j, len(pts_3d))) - - # 3. robust union-find on landmarks with outlier rejection - track_map = {} - point_records = [] - for track_id, (i, j, idx_i_list, idx_j_list, landmarks_i, pts_2d) in enumerate(feature_tracks): - for n, (idx_i, idx_j, landmark) in enumerate(zip(idx_i_list, idx_j_list, landmarks_i)): - key_i = (i, idx_i) - key_j = (j, idx_j) - point_records.append((track_id, n, key_i, key_j, landmark)) - - uf = uf_init(len(point_records)) - key_to_indices = {} - for idx, (_, _, key_i, key_j, _) in enumerate(point_records): - key_to_indices.setdefault(key_i, []).append(idx) - key_to_indices.setdefault(key_j, []).append(idx) - for indices in key_to_indices.values(): - for k in range(1, len(indices)): - uf_union(uf, indices[0], indices[k]) - sets = uf_all_sets_list(uf) - - for s in sets: - if len(s) < 2: - continue - pts = np.array([point_records[idx][4] for idx in s]) - mean = np.mean(pts, axis=0) - dists = np.linalg.norm(pts - mean, axis=1) - inliers = [s[idx] for idx, d in enumerate(dists) if d < 0.3] - if len(inliers) < 2: - continue - landmark_positions.append(np.mean([point_records[idx][4] for idx in inliers], axis=0)) - lm_id = len(landmark_positions) - 1 - for idx in inliers: - _, _, key_i, key_j, _ = point_records[idx] - track_map[key_i] = lm_id - track_map[key_j] = lm_id - - # 4. add projection factors - fx, fy = self.K[0, 0], self.K[1, 1] - cx, cy = self.K[0, 2], self.K[1, 2] - K_cal = gtsam.Cal3_S2(fx, fy, 0.0, cx, cy) - meas_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) - - with Timer(name="[Projection Factors]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - for track_id, (i, j, idx_i_list, idx_j_list, landmarks_i, pts_2d) in enumerate(feature_tracks): - kpts_i = all_kpts[i][idx_i_list] - kpts_j = all_kpts[j][idx_j_list] - for idx_i, idx_j, pt_j in zip(idx_i_list, idx_j_list, pts_2d): - key_i = (i, idx_i) - key_j = (j, idx_j) - if key_i in track_map and key_j in track_map and track_map[key_i] == track_map[key_j]: - lm_id = track_map[key_i] - landmark = landmark_positions[lm_id] - point3 = gtsam.Point3(landmark) - if not initial_values.exists(gtsam.symbol('l', lm_id)): - initial_values.insert(gtsam.symbol('l', lm_id), point3) - factor = gtsam.GenericProjectionFactorCal3_S2( - gtsam.Point2(pt_j[0], pt_j[1]), - meas_noise, - X(j), - gtsam.symbol('l', lm_id), - K_cal, - Matrix4x4ToGtsamPose3(self.T_imu_body_to_camera) + + # Smart factors require isotropic pixel noise + noise = gtsam.noiseModel.Isotropic.Sigma(3, 1.0) + params = gtsam.SmartProjectionParams() + smart_factor = gtsam_unstable.SmartStereoProjectionPoseFactor(noise, params) + + calib = gtsam.Cal3_S2Stereo( + self.K[0, 0], self.K[1, 1], 0, self.K[0, 2], self.K[1, 2], self.baseline + ) + for pose_idx, kpt, disparity in observations: + stereo_meas = gtsam.StereoPoint2( + kpt[0], + kpt[0] - disparity[int(kpt[1]), int(kpt[0])], + kpt[1], ) - graph.add(factor) - - with Timer(name="[Optimize]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_values) - initial_error = graph.error(initial_values) - result = optimizer.optimize() - final_error = graph.error(result) - - self.logger.debug(f"Correspondence info (kf_i, kf_j, num_matches): {correspondence_info}") - self.logger.debug(f"Number of landmarks: {len(landmark_positions)}") - self.logger.debug(f"Number of factors: {graph.size()}") - self.logger.debug(f"Initial error: {initial_error}") - self.logger.debug(f"Final error: {final_error}") + smart_factor.add(stereo_meas, X(pose_idx), calib) + graph.add(smart_factor) + + with Timer(name="[Solver]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + params = gtsam.LevenbergMarquardtParams() + # set iteration limit + params.setMaxIterations(3) + params.setVerbosityLM("DEBUG") + lm = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) + result = lm.optimize() + + 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}") + + for i, keyframe in enumerate(self.keyframe_queue[-_N:]): + T_i = result.atPose3(X(i)).matrix() + keyframe.pose = T_i + keyframe.velocity = result.atVector(V(i)) + self.logger.debug(f"Keyframe {i} pose updated:\n{T_i}, at timestamp {keyframe.timestamp}") + self.logger.debug(f"Bias {i} updated:\n{result.atConstantBias(B(i))}") + #print("imu error: ", keyframe.preintegrated_imu.error(initial_estimate)) + + with Timer(text="[Depth as Color] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + disp_vis = disparity.copy().astype(np.uint8) + disp_color = cv2.applyColorMap(disp_vis * 4, cv2.COLORMAP_PLASMA) + disp_color_msg = self.bridge.cv2_to_imgmsg(disp_color, encoding='bgr8') + disp_color_msg.header = left_msg.header + self.disparity_pub_vis.publish(disp_color_msg) + + with Timer(name='[Depth as Cloud', text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): + # publish depth image and camera info for depth topic (required by DepthCloud) + depth_msg = self.bridge.cv2_to_imgmsg(depth, encoding="32FC1") + depth_msg.header.stamp = left_msg.header.stamp + depth_msg.header.frame_id = "camera" # Match TF frame + self.camera_info_msg.header.stamp = left_msg.header.stamp + self.camera_info_msg.header.frame_id = "camera" # Match TF frame + self.slam_camera_info_pub.publish(self.camera_info_msg) + self.depth_pub.publish(depth_msg) + self.logger.debug(f"superpoint cache info: {self.superpoint.infer.cache_info()}") + self.logger.debug(f"lightglue cache info: {self.light_glue.infer.cache_info()}") self.logger.debug(f"estimate_pose cache info: {estimate_pose.cache_info()}") with Timer(name="[Publish Odometry]", text="[{name}] Elapsed time: {milliseconds:.0f} ms", logger=self.logger.debug): @@ -541,50 +542,31 @@ async def process(self, left_msg, right_msg): self.odom_pub.publish(np2msg(self.T_body_last, left_msg.header.stamp, "world", "camera", self.V_last)) # publish TF self.tf_broadcaster.sendTransform(np2tf(self.T_body_last, left_msg.header.stamp, "world", "camera")) - # publish camera info - self.camera_info_msg.header.stamp = left_msg.header.stamp - self.camera_info_msg.header.frame_id = "camera" - self.slam_camera_info_pub.publish(self.camera_info_msg) - # publish depth - depth_msg = self.bridge.cv2_to_imgmsg(depth.astype(np.float32), encoding='32FC1') - depth_msg.header.stamp = left_msg.header.stamp - depth_msg.header.frame_id = "camera" - self.depth_pub.publish(depth_msg) - # publish depth_vis 0->255, uint8 - disparity_vis = (disparity - disparity.min()) / (disparity.max() - disparity.min()) * 255 - disparity_vis = disparity_vis.astype(np.uint8) - disparity_vis = cv2.applyColorMap(disparity_vis, cv2.COLORMAP_JET) - disparity_msg = self.bridge.cv2_to_imgmsg(disparity_vis, encoding='bgr8') - disparity_msg.header.stamp = left_msg.header.stamp - disparity_msg.header.frame_id = "camera" - self.disparity_pub_vis.publish(disparity_msg) - - # publish latest keyframe pose, image, depth if available - if self.keyframe_queue: - kf = self.keyframe_queue[-1] - self.keyframe_pose_pub.publish(np2msg(kf.pose, left_msg.header.stamp, "world", "camera", kf.velocity)) - keyframe_image_msg = self.bridge.cv2_to_imgmsg(kf.image, encoding='mono8') - keyframe_image_msg.header.stamp = left_msg.header.stamp - keyframe_image_msg.header.frame_id = "camera" - self.keyframe_image_pub.publish(keyframe_image_msg) - keyframe_depth_msg = self.bridge.cv2_to_imgmsg(kf.depth.astype(np.float32), encoding='32FC1') - keyframe_depth_msg.header.stamp = left_msg.header.stamp - keyframe_depth_msg.header.frame_id = "camera" - self.keyframe_depth_pub.publish(keyframe_depth_msg) + + last_keyframe = self.keyframe_queue[-2] + current_keyframe = self.keyframe_queue[-1] + if keyframe_check(last_keyframe.pose, current_keyframe.pose) or current_keyframe.timestamp - last_keyframe.timestamp > 3.0: + self.keyframe_pose_pub.publish(np2msg(current_keyframe.pose, left_msg.header.stamp, "world", "camera", current_keyframe.velocity)) + self.keyframe_image_pub.publish(left_msg) + self.keyframe_depth_pub.publish(depth_msg) + else: + self.keyframe_queue.pop() return { - "stats": {"process_cnt": self.process_cnt}, + "stats": { + "process_cnt": self.process_cnt, + }, "metrics": { "num_keyframes": len(self.keyframe_queue), - "num_tracks": len(feature_tracks), + "num_tracks": len(tracks), "num_factors": graph.size(), - "num_variables": initial_values.size(), - "initial_error": initial_error, - "final_error": final_error, - **metrics, - } + "num_variables": initial_estimate.size(), + "initial_error": graph.error(initial_estimate), + "final_error": graph.error(result), + }, } + def main(args=None): rclpy.init(args=args) parser = argparse.ArgumentParser(description='Run TinyNav perception node.') From 755158aec43d571f73a3c9a117a1dd469ae4c9de Mon Sep 17 00:00:00 2001 From: dvorak Date: Mon, 20 Apr 2026 20:29:32 +0800 Subject: [PATCH 4/4] save for working version --- docs/vis.rviz | 140 +++++++++++++++++++--------- tinynav/core/imu_propagator_node.py | 1 + tinynav/core/perception_node.py | 4 +- 3 files changed, 98 insertions(+), 47 deletions(-) diff --git a/docs/vis.rviz b/docs/vis.rviz index 6293a3f3..d66a0bee 100644 --- a/docs/vis.rviz +++ b/docs/vis.rviz @@ -1,23 +1,15 @@ Panels: - Class: rviz_common/Displays - Help Height: 138 + Help Height: 114 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - - /Perception1/DepthCloud1/Auto Size1 - - /Perception1/DepthCloud1/Occlusion Compensation1 - - /Perception1/DepthCloud1/Autocompute Value Bounds1 - - /Planning1/3DGridMap1 + - /Perception1/Local Cloud1/Autocompute Value Bounds1 + - /Planning1 - /Planning1/3DGridMap1/Autocompute Value Bounds1 - - /Planning1/TrajScores1 - - /Planning1/PointCloud21 - - /Planning1/ObstacleMask1 - - /Planning1/Footprint1 - - /PointCloud21 - - /Path1 Splitter Ratio: 0.5 - Tree Height: 756 + Tree Height: 114 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -62,17 +54,71 @@ Visualization Manager: Frame Timeout: 10000 Frames: All Enabled: true - cam_depth: + base_footprint: Value: true - cam_left: + base_link: Value: true - cam_rgb: + camera: Value: true - cam_right: + camera_color_frame: Value: true - camera_imu_optical_frame: + camera_color_optical_frame: Value: true - imu: + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_imu_frame: + Value: true + camera_left_ir_frame: + Value: true + camera_left_ir_optical_frame: + Value: true + camera_link: + Value: true + camera_right_ir_frame: + Value: true + camera_right_ir_optical_frame: + Value: true + imu_link: + Value: true + lf_foot_link: + Value: true + lf_hip_link: + Value: true + lf_lower_leg_link: + Value: true + lf_upper_leg_link: + Value: true + lh_foot_link: + Value: true + lh_hip_link: + Value: true + lh_lower_leg_link: + Value: true + lh_upper_leg_link: + Value: true + map: + Value: true + odom: + Value: true + rf_foot_link: + Value: true + rf_hip_link: + Value: true + rf_lower_leg_link: + Value: true + rf_upper_leg_link: + Value: true + rh_foot_link: + Value: true + rh_hip_link: + Value: true + rh_lower_leg_link: + Value: true + rh_upper_leg_link: + Value: true + trunk: Value: true world: Value: true @@ -83,16 +129,7 @@ Visualization Manager: Show Names: false Tree: world: - imu: - cam_left: - cam_depth: - {} - cam_rgb: - {} - cam_right: - {} - camera_imu_optical_frame: - {} + {} Update Interval: 0 Value: true - Class: rviz_common/Group @@ -121,6 +158,7 @@ Visualization Manager: Min Value: -3 Value: false Axis: Z + CameraInfo Topic: /slam/camera_info Channel Name: intensity Class: rviz_default_plugins/DepthCloud Color: 255; 255; 255 @@ -190,7 +228,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /slam/odometry Value: true - Enabled: true + Enabled: false Name: Perception - Class: rviz_common/Group Displays: @@ -209,9 +247,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 0; 255; 0 - Max Intensity: 0.30000001192092896 + Max Intensity: -0.30000001192092896 Min Color: 116; 0; 0 - Min Intensity: -0.5 + Min Intensity: -1 Name: 3DGridMap Position Transformer: XYZ Selectable: true @@ -232,7 +270,7 @@ Visualization Manager: Buffer Length: 1 Class: rviz_default_plugins/Path Color: 25; 255; 0 - Enabled: false + Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 @@ -255,7 +293,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /planning/trajectory_path - Value: false + Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz_default_plugins/Odometry Covariance: @@ -273,7 +311,7 @@ Visualization Manager: Scale: 1 Value: true Value: true - Enabled: false + Enabled: true Keep: 1 Name: TargetPose Position Tolerance: 0.10000000149011612 @@ -294,7 +332,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /control/target_pose - Value: false + Value: true - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map Color Scheme: map @@ -400,12 +438,23 @@ Visualization Manager: Use Timestamp: false Value: true - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud Color: 255; 255; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 Name: Footprint Position Transformer: XYZ Selectable: true @@ -420,6 +469,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/footprint Use Fixed Frame: true + Use rainbow: true Value: true Enabled: true Name: Planning @@ -573,7 +623,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: RGB8 Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 @@ -594,7 +644,7 @@ Visualization Manager: Value: /slam/global_cloud Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path @@ -669,7 +719,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/ThirdPersonFollower - Distance: 10.385795593261719 + Distance: 4.001773357391357 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -684,22 +734,22 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6347965598106384 + Pitch: 1.289795994758606 Target Frame: camera Value: ThirdPersonFollower (rviz_default_plugins) - Yaw: 0.05539698153734207 + Yaw: 4.973613262176514 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1994 + Height: 749 HeightMap: collapsed: false Hide Left Dock: false Hide Right Dock: false Image: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -710,9 +760,9 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2331 - X: 1419 - Y: 64 + Width: 1272 + X: 1280 + Y: 782 color image: collapsed: false gray image: diff --git a/tinynav/core/imu_propagator_node.py b/tinynav/core/imu_propagator_node.py index b105597e..a36002f4 100644 --- a/tinynav/core/imu_propagator_node.py +++ b/tinynav/core/imu_propagator_node.py @@ -66,6 +66,7 @@ def imu_callback(self, imu_msg: Imu): return timestamp = self._stamp_to_sec(imu_msg.header.stamp) + print("imu: ", timestamp) self.imu_buffer.append((timestamp, imu_msg)) if len(self.imu_buffer) > 2000: self.imu_buffer.pop(0) diff --git a/tinynav/core/perception_node.py b/tinynav/core/perception_node.py index b7f9545b..5af55ffb 100644 --- a/tinynav/core/perception_node.py +++ b/tinynav/core/perception_node.py @@ -145,8 +145,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 = 0.50 # [m/s^2/√Hz] + gyro_noise_density = 0.50 # [rad/s/√Hz] bias_acc_rw_sigma = 0.001 bias_gyro_rw_sigma = 0.0001 self.pre_integration_params = gtsam.PreintegrationCombinedParams.MakeSharedU()