From 046339ec27139ed2f39df8cdb8ca135824107d2f Mon Sep 17 00:00:00 2001 From: YANG Zhenfei Date: Fri, 17 Apr 2026 22:36:33 +0800 Subject: [PATCH 01/14] 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 02/14] 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 03/14] 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 04/14] 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() From 391ab5d1f0434da9a86d97a22a441ec01eed4497 Mon Sep 17 00:00:00 2001 From: junlinp Date: Thu, 23 Apr 2026 23:15:47 +0800 Subject: [PATCH 05/14] feat(control): track planning path with spline+slerp and PID --- tinynav/core/planning_node.py | 8 +- tinynav/platforms/cmd_vel_control.py | 212 ++++++++++++++++++++++----- 2 files changed, 185 insertions(+), 35 deletions(-) diff --git a/tinynav/core/planning_node.py b/tinynav/core/planning_node.py index 7ccdb363..21412c0a 100644 --- a/tinynav/core/planning_node.py +++ b/tinynav/core/planning_node.py @@ -9,6 +9,7 @@ from numba import njit import message_filters from rclpy.time import Time +from rclpy.duration import Duration from sensor_msgs.msg import PointCloud2, PointCloud from geometry_msgs.msg import PoseStamped, Point32 import sensor_msgs_py.point_cloud2 as pc2 @@ -591,8 +592,10 @@ def cost_function(traj, param, score, target_pose): path = Path() path.header = depth_msg.header path.header.frame_id = "world" + traj_dt = 0.1 + base_time = Time.from_msg(depth_msg.header.stamp) for i in top_indices: - for j in range(0, len(trajectories[i]), 10): + for j in range(0, len(trajectories[i]), 1): x,y,z,qx,qy,qz,qw = trajectories[i][j] if self.poi_changed or self.target_pose is None: x,y,z,qx,qy,qz,qw = trajectories[i][0] @@ -602,7 +605,8 @@ def cost_function(traj, param, score, target_pose): self.get_logger().info("target pose is None, using first point") pose = PoseStamped() - pose.header = depth_msg.header + pose.header.frame_id = "world" + pose.header.stamp = (base_time + Duration(seconds=float(j * traj_dt))).to_msg() pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = z diff --git a/tinynav/platforms/cmd_vel_control.py b/tinynav/platforms/cmd_vel_control.py index 8ad76263..c82d35d3 100644 --- a/tinynav/platforms/cmd_vel_control.py +++ b/tinynav/platforms/cmd_vel_control.py @@ -4,6 +4,8 @@ from nav_msgs.msg import Path from nav_msgs.msg import Odometry from scipy.spatial.transform import Rotation as R +from scipy.spatial.transform import Slerp +from scipy.interpolate import CubicSpline import numpy as np import logging import time @@ -11,6 +13,32 @@ # Module-level logger for cases where self.get_logger() is not available logger = logging.getLogger(__name__) + +class PIDController: + def __init__(self, kp: float, ki: float, kd: float, integral_limit: float = 1.0): + self.kp = float(kp) + self.ki = float(ki) + self.kd = float(kd) + self.integral_limit = float(abs(integral_limit)) + self.integral = 0.0 + self.prev_error = 0.0 + self.initialized = False + + def reset(self): + self.integral = 0.0 + self.prev_error = 0.0 + self.initialized = False + + def update(self, error: float, dt: float) -> float: + dt = max(1e-3, float(dt)) + self.integral += float(error) * dt + self.integral = float(np.clip(self.integral, -self.integral_limit, self.integral_limit)) + derivative = 0.0 if not self.initialized else (float(error) - self.prev_error) / dt + self.prev_error = float(error) + self.initialized = True + return self.kp * float(error) + self.ki * self.integral + self.kd * derivative + + class CmdVelControlNode(Node): def __init__(self): super().__init__('cmd_vel_control_node') @@ -27,6 +55,16 @@ def __init__(self): self.last_path_time = 0.0 self.pose = None self.path = None + self.path_dt = 0.1 # Planning node trajectory time step. + self.path_start_time = None + self.path_duration = 0.0 + self.path_pos_spline = None + self.path_ori_slerp = None + self.path_robot_to_camera = self.T_robot_to_camera + + # PID for tracking path against /slam/odometry. + self.pos_pid = PIDController(kp=1.2, ki=0.05, kd=0.08, integral_limit=1.5) + self.yaw_pid = PIDController(kp=1.8, ki=0.03, kd=0.10, integral_limit=1.0) # === Control loop (ported from planning_node_compare style) === self.cmd_rate_hz = 20.0 @@ -47,11 +85,146 @@ def pose_callback(self, msg): def _clamp_step(self, target: float, current: float, max_delta: float) -> float: return float(np.clip(target - current, -max_delta, max_delta) + current) + def _wrap_to_pi(self, angle: float) -> float: + return float((angle + np.pi) % (2.0 * np.pi) - np.pi) + + def _yaw_from_transform(self, T: np.ndarray) -> float: + fwd = T[:3, :3] @ np.array([1.0, 0.0, 0.0], dtype=np.float64) + return float(np.arctan2(fwd[1], fwd[0])) + + def _world_robot_from_pose_quat(self, position: np.ndarray, quat: np.ndarray) -> np.ndarray: + T_world_cam = np.eye(4) + T_world_cam[:3, :3] = R.from_quat(quat).as_matrix() + T_world_cam[:3, 3] = np.asarray(position, dtype=np.float64) + return T_world_cam @ self.path_robot_to_camera + + def _current_world_robot_from_odom(self): + if self.pose is None: + return None + p = self.pose.pose.pose.position + q = self.pose.pose.pose.orientation + pos = np.array([p.x, p.y, p.z], dtype=np.float64) + quat = np.array([q.x, q.y, q.z, q.w], dtype=np.float64) + return self._world_robot_from_pose_quat(pos, quat) + + def _build_path_interpolators(self, path_msg: Path): + poses = path_msg.poses + count = len(poses) + if count < 2: + return False + + times = np.arange(count, dtype=np.float64) * self.path_dt + positions = np.empty((count, 3), dtype=np.float64) + quats = np.empty((count, 4), dtype=np.float64) + for i, pose_stamped in enumerate(poses): + p = pose_stamped.pose.position + q = pose_stamped.pose.orientation + positions[i] = np.array([p.x, p.y, p.z], dtype=np.float64) + quats[i] = np.array([q.x, q.y, q.z, q.w], dtype=np.float64) + + try: + rotations = R.from_quat(quats) + self.path_pos_spline = CubicSpline(times, positions, axis=0, bc_type='natural') + self.path_ori_slerp = Slerp(times, rotations) + self.path_duration = float(times[-1]) + self.path_start_time = time.monotonic() + self.pos_pid.reset() + self.yaw_pid.reset() + return True + except ValueError as exc: + self.logger.warning(f"Failed to build path interpolators: {exc}") + self.path_pos_spline = None + self.path_ori_slerp = None + self.path_duration = 0.0 + self.path_start_time = None + return False + + def _sample_pose_at(self, t: float): + if self.path_pos_spline is None or self.path_ori_slerp is None: + return None, None + t_clamped = float(np.clip(t, 0.0, self.path_duration)) + position = self.path_pos_spline(t_clamped) + quat = self.path_ori_slerp([t_clamped]).as_quat()[0] + return position, quat + + def _compute_cmd_from_sampled_path(self, now_mono: float): + if self.path_start_time is None or self.path_duration <= 0.0: + return None, None + + t0 = now_mono - self.path_start_time + t1 = t0 + (1.0 / self.cmd_rate_hz) + + p0, q0 = self._sample_pose_at(t0) + p1, q1 = self._sample_pose_at(t1) + if p0 is None or p1 is None: + return None, None + + T1 = np.eye(4) + T1[:3, :3] = R.from_quat(q0).as_matrix() + T1[:3, 3] = p0 + + T2 = np.eye(4) + T2[:3, :3] = R.from_quat(q1).as_matrix() + T2[:3, 3] = p1 + + T_robot_1 = T1 @ self.T_robot_to_camera + T_robot_2 = T2 @ self.T_robot_to_camera + T_robot_2_to_1 = np.linalg.inv(T_robot_1) @ T_robot_2 + + dt = max(1e-3, t1 - t0) + linear_velocity_vec = T_robot_2_to_1[:3, 3] / dt + angular_velocity_vec = R.from_matrix(T_robot_2_to_1[:3, :3]).as_rotvec() / dt + return linear_velocity_vec, angular_velocity_vec + + def _compute_tracking_pid_cmd(self, now_mono: float, dt: float): + if self.path_start_time is None or self.path_duration <= 0.0: + return None, None + T_robot_now = self._current_world_robot_from_odom() + if T_robot_now is None: + return None, None + + t_ref = now_mono - self.path_start_time + ref_pos, ref_quat = self._sample_pose_at(t_ref) + if ref_pos is None: + return None, None + T_robot_ref = self._world_robot_from_pose_quat(ref_pos, ref_quat) + + # Position error in current robot frame; control forward axis only. + T_ref_in_robot = np.linalg.inv(T_robot_now) @ T_robot_ref + pos_err_forward = float(T_ref_in_robot[0, 3]) + + yaw_now = self._yaw_from_transform(T_robot_now) + yaw_ref = self._yaw_from_transform(T_robot_ref) + yaw_err = self._wrap_to_pi(yaw_ref - yaw_now) + + vx_correction = self.pos_pid.update(pos_err_forward, dt) + wz_correction = self.yaw_pid.update(yaw_err, dt) + return vx_correction, wz_correction + def cmd_timer_callback(self): now = time.monotonic() dt = max(1e-3, now - self.last_cmd_pub_time) self.last_cmd_pub_time = now + ff_vx = 0.0 + ff_wz = 0.0 + linear_velocity_vec, angular_velocity_vec = self._compute_cmd_from_sampled_path(now) + if linear_velocity_vec is not None: + ff_vx = float(linear_velocity_vec[0]) + ff_wz = float(angular_velocity_vec[2]) + + pid_vx = 0.0 + pid_wz = 0.0 + pid_cmd = self._compute_tracking_pid_cmd(now, dt) + if pid_cmd[0] is not None: + pid_vx, pid_wz = pid_cmd + + vx = np.clip(ff_vx + pid_vx, -0.1, 0.3) + vyaw = np.clip(ff_wz + pid_wz, -0.8, 0.8) + self.latest_cmd.linear.x = float(vx) + self.latest_cmd.linear.y = 0.0 + self.latest_cmd.angular.z = float(vyaw) + # Stale-path protection: slow down, then stop if planner has not refreshed. age = float('inf') if self.last_path_update_time is None else (now - self.last_path_update_time) target_cmd = Twist() @@ -76,45 +249,18 @@ def cmd_timer_callback(self): self.prev_cmd = out def path_callback(self, msg): - if msg is None or self.pose is None: + if msg is None: return if len(msg.poses) < 2: return - self.path = msg - - current_time = self.get_clock().now().to_msg().sec + self.get_clock().now().to_msg().nanosec * 1e-9 - self.last_path_time = current_time + # Save the latest path first. + self.path = msg self.last_path_update_time = time.monotonic() - - def msg2np(msg): - T = np.eye(4) - position = msg.pose.position - rot = msg.pose.orientation - quat = [rot.x, rot.y, rot.z, rot.w] - T[:3, :3] = R.from_quat(quat).as_matrix() - T[:3, 3] = np.array([position.x, position.y, position.z]).ravel() - return T - - T1 = msg2np(self.path.poses[0]) - T2 = msg2np(self.path.poses[1]) - T_robot_1 = T1 @ self.T_robot_to_camera - T_robot_2 = T2 @ self.T_robot_to_camera - T_robot_2_to_1 = np.linalg.inv(T_robot_1) @ T_robot_2 - p = T_robot_2_to_1[:3, 3] - dt = 0.1 # Planning node trajectory time step (duration=2.0, dt=0.1) - linear_velocity_vec = p / dt - r = R.from_matrix(T_robot_2_to_1[:3, :3]) - angular_velocity_vec = r.as_rotvec() / dt - - vx = np.clip(linear_velocity_vec[0], -0.1, 0.3) - vy = 0.0 - vyaw = np.clip(angular_velocity_vec[2], -0.8, 0.8) - self.latest_cmd.linear.x = float(vx) - self.latest_cmd.linear.y = float(vy) - self.latest_cmd.angular.z = float(vyaw) + self.last_path_time = self.get_clock().now().nanoseconds * 1e-9 + self._build_path_interpolators(msg) age = 0.0 if self.last_path_update_time is None else (time.monotonic() - self.last_path_update_time) - self.logger.debug(f"cmd vx={vx:.3f} vyaw={vyaw:.3f} path_age={age:.2f}s") + self.logger.debug(f"path updated age={age:.2f}s points={len(msg.poses)}") def destroy_node(self): self.logger.info("Destroying cmd_vel_control connection.") From ff66279cb04bb1191df35c560c51622c00eec998 Mon Sep 17 00:00:00 2001 From: junlinp Date: Fri, 24 Apr 2026 00:07:05 +0800 Subject: [PATCH 06/14] fix(control): move path camera-to-body conversion to cmd_vel --- tinynav/platforms/cmd_vel_control.py | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/tinynav/platforms/cmd_vel_control.py b/tinynav/platforms/cmd_vel_control.py index c82d35d3..a51637ac 100644 --- a/tinynav/platforms/cmd_vel_control.py +++ b/tinynav/platforms/cmd_vel_control.py @@ -60,7 +60,6 @@ def __init__(self): self.path_duration = 0.0 self.path_pos_spline = None self.path_ori_slerp = None - self.path_robot_to_camera = self.T_robot_to_camera # PID for tracking path against /slam/odometry. self.pos_pid = PIDController(kp=1.2, ki=0.05, kd=0.08, integral_limit=1.5) @@ -92,11 +91,19 @@ def _yaw_from_transform(self, T: np.ndarray) -> float: fwd = T[:3, :3] @ np.array([1.0, 0.0, 0.0], dtype=np.float64) return float(np.arctan2(fwd[1], fwd[0])) - def _world_robot_from_pose_quat(self, position: np.ndarray, quat: np.ndarray) -> np.ndarray: + def _world_from_pose_quat(self, position: np.ndarray, quat: np.ndarray) -> np.ndarray: T_world_cam = np.eye(4) T_world_cam[:3, :3] = R.from_quat(quat).as_matrix() T_world_cam[:3, 3] = np.asarray(position, dtype=np.float64) - return T_world_cam @ self.path_robot_to_camera + return T_world_cam + + def _path_pose_camera_to_body(self, position: np.ndarray, quat: np.ndarray): + """Convert path pose from world-camera to world-body by right multiply.""" + T_world_cam = self._world_from_pose_quat(position, quat) + T_world_body = T_world_cam @ self.T_robot_to_camera + pos_body = T_world_body[:3, 3].copy() + quat_body = R.from_matrix(T_world_body[:3, :3]).as_quat() + return pos_body, quat_body def _current_world_robot_from_odom(self): if self.pose is None: @@ -105,7 +112,7 @@ def _current_world_robot_from_odom(self): q = self.pose.pose.pose.orientation pos = np.array([p.x, p.y, p.z], dtype=np.float64) quat = np.array([q.x, q.y, q.z, q.w], dtype=np.float64) - return self._world_robot_from_pose_quat(pos, quat) + return self._world_from_pose_quat(pos, quat) @ self.T_robot_to_camera def _build_path_interpolators(self, path_msg: Path): poses = path_msg.poses @@ -119,8 +126,11 @@ def _build_path_interpolators(self, path_msg: Path): for i, pose_stamped in enumerate(poses): p = pose_stamped.pose.position q = pose_stamped.pose.orientation - positions[i] = np.array([p.x, p.y, p.z], dtype=np.float64) - quats[i] = np.array([q.x, q.y, q.z, q.w], dtype=np.float64) + pos_cam = np.array([p.x, p.y, p.z], dtype=np.float64) + quat_cam = np.array([q.x, q.y, q.z, q.w], dtype=np.float64) + pos_body, quat_body = self._path_pose_camera_to_body(pos_cam, quat_cam) + positions[i] = pos_body + quats[i] = quat_body try: rotations = R.from_quat(quats) @@ -167,9 +177,7 @@ def _compute_cmd_from_sampled_path(self, now_mono: float): T2[:3, :3] = R.from_quat(q1).as_matrix() T2[:3, 3] = p1 - T_robot_1 = T1 @ self.T_robot_to_camera - T_robot_2 = T2 @ self.T_robot_to_camera - T_robot_2_to_1 = np.linalg.inv(T_robot_1) @ T_robot_2 + T_robot_2_to_1 = np.linalg.inv(T1) @ T2 dt = max(1e-3, t1 - t0) linear_velocity_vec = T_robot_2_to_1[:3, 3] / dt @@ -187,7 +195,7 @@ def _compute_tracking_pid_cmd(self, now_mono: float, dt: float): ref_pos, ref_quat = self._sample_pose_at(t_ref) if ref_pos is None: return None, None - T_robot_ref = self._world_robot_from_pose_quat(ref_pos, ref_quat) + T_robot_ref = self._world_from_pose_quat(ref_pos, ref_quat) # Position error in current robot frame; control forward axis only. T_ref_in_robot = np.linalg.inv(T_robot_now) @ T_robot_ref From 987a656ff39a9126dc1f64df6d1447ead8bb29c8 Mon Sep 17 00:00:00 2001 From: junlinp Date: Fri, 24 Apr 2026 18:43:08 +0800 Subject: [PATCH 07/14] feat(control): add cross-track yaw coupling in cmd_vel tracking --- tinynav/platforms/cmd_vel_control.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tinynav/platforms/cmd_vel_control.py b/tinynav/platforms/cmd_vel_control.py index a51637ac..e51c1296 100644 --- a/tinynav/platforms/cmd_vel_control.py +++ b/tinynav/platforms/cmd_vel_control.py @@ -64,6 +64,8 @@ def __init__(self): # PID for tracking path against /slam/odometry. self.pos_pid = PIDController(kp=1.2, ki=0.05, kd=0.08, integral_limit=1.5) self.yaw_pid = PIDController(kp=1.8, ki=0.03, kd=0.10, integral_limit=1.0) + # Cross-track coupling: map lateral error (robot-frame y) into yaw-rate correction. + self.cross_track_gain = 1.2 # === Control loop (ported from planning_node_compare style) === self.cmd_rate_hz = 20.0 @@ -200,13 +202,14 @@ def _compute_tracking_pid_cmd(self, now_mono: float, dt: float): # Position error in current robot frame; control forward axis only. T_ref_in_robot = np.linalg.inv(T_robot_now) @ T_robot_ref pos_err_forward = float(T_ref_in_robot[0, 3]) + pos_err_lateral = float(T_ref_in_robot[1, 3]) yaw_now = self._yaw_from_transform(T_robot_now) yaw_ref = self._yaw_from_transform(T_robot_ref) yaw_err = self._wrap_to_pi(yaw_ref - yaw_now) vx_correction = self.pos_pid.update(pos_err_forward, dt) - wz_correction = self.yaw_pid.update(yaw_err, dt) + wz_correction = self.yaw_pid.update(yaw_err, dt) + self.cross_track_gain * pos_err_lateral return vx_correction, wz_correction def cmd_timer_callback(self): From 0f494c1848c5b7334160d69491dcc173af126401 Mon Sep 17 00:00:00 2001 From: junlinp Date: Mon, 27 Apr 2026 17:31:56 +0800 Subject: [PATCH 08/14] refactor(perception): decouple imu propagator and add callback timing --- tinynav/core/imu_propagator_node.py | 62 ++++++++++++++++------------- tinynav/core/perception_node.py | 5 --- 2 files changed, 34 insertions(+), 33 deletions(-) diff --git a/tinynav/core/imu_propagator_node.py b/tinynav/core/imu_propagator_node.py index a36002f4..681521bf 100644 --- a/tinynav/core/imu_propagator_node.py +++ b/tinynav/core/imu_propagator_node.py @@ -2,6 +2,7 @@ import numpy as np import rclpy +from codetiming import Timer from message_filters import ApproximateTimeSynchronizer, Subscriber from nav_msgs.msg import Odometry from rclpy.node import Node @@ -62,34 +63,39 @@ def __init__(self): 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) - print("imu: ", timestamp) - 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]) + with Timer( + name="ImuPropagatorNode.imu_callback", + text="[{name}] Elapsed time: {milliseconds:.2f} ms", + logger=self.get_logger().info, + ): + if len(self.odom_100hz_buffer) == 0: + 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) + + if self.imu_buffer[-1][0] <= self.odom_100hz_buffer[-1][0] + 0.008: + 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) diff --git a/tinynav/core/perception_node.py b/tinynav/core/perception_node.py index 5af55ffb..c9dcd394 100644 --- a/tinynav/core/perception_node.py +++ b/tinynav/core/perception_node.py @@ -26,7 +26,6 @@ from dataclasses import dataclass from gtsam.symbol_shorthand import X, B, V -from tinynav.core.imu_propagator_node import ImuPropagatorNode _N = 5 _M = 1000 @@ -574,14 +573,10 @@ 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.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 89cb8a861b83db776d9c29cb33444770a69d9b8d Mon Sep 17 00:00:00 2001 From: junlinp Date: Tue, 28 Apr 2026 13:57:00 +0800 Subject: [PATCH 09/14] feat(control): publish pid target pose on odometry callback --- tinynav/platforms/cmd_vel_control.py | 50 ++++++++++++---------------- 1 file changed, 21 insertions(+), 29 deletions(-) diff --git a/tinynav/platforms/cmd_vel_control.py b/tinynav/platforms/cmd_vel_control.py index de820299..e605e22e 100644 --- a/tinynav/platforms/cmd_vel_control.py +++ b/tinynav/platforms/cmd_vel_control.py @@ -9,6 +9,7 @@ import numpy as np import logging import time +from tinynav.core.math_utils import np2msg # Module-level logger for cases where self.get_logger() is not available logger = logging.getLogger(__name__) @@ -44,6 +45,7 @@ def __init__(self): super().__init__('cmd_vel_control_node') self.logger = self.get_logger() # Use ROS2 logger self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10) + self.target_pose_pub = self.create_publisher(Odometry, '/control/target_pose_pid', 10) self.pose_sub = self.create_subscription(Odometry, '/slam/odometry', self.pose_callback, 10) self.create_subscription(Path, '/planning/trajectory_path', self.path_callback, 10) self.T_robot_to_camera = np.array([ @@ -68,25 +70,11 @@ def __init__(self): self.cross_track_gain = 1.2 # === Control loop (ported from planning_node_compare style) === - # Planner input is typically 7-10 Hz; over-driving cmd publish rate amplifies jitter. - self.cmd_rate_hz = 12.0 - # Use minima; actual stale thresholds are scaled by observed planner period. - self.path_stale_slow_s = 0.35 - self.path_stale_stop_s = 0.8 - self.path_stale_slow_factor = 3.5 - self.path_stale_stop_factor = 5.0 - self.max_linear_acc = 0.6 # m/s^2 + self.cmd_rate_hz = 20.0 + self.path_stale_slow_s = 0.3 + self.path_stale_stop_s = 0.6 + self.max_linear_acc = 0.4 # m/s^2 self.max_angular_acc = 0.8 # rad/s^2 - self.planner_dt = 0.1 # trajectory dt in planning_node - # planning_node publishes path with for j in range(..., step=10), so points are ~1.0 s apart. - self.path_pose_stride = 10 - self.path_period_ema = 0.12 - self.path_filter_tau = 0.30 - self.lookahead_steps = 1 - # Static-friction compensation: very small vx often cannot move the robot. - self.min_effective_linear_speed = 0.2 - self.linear_engage_threshold = 0.04 - self.fixed_reverse_speed = 0.1 self.latest_cmd = Twist() self.prev_cmd = Twist() @@ -96,6 +84,19 @@ def __init__(self): def pose_callback(self, msg): self.pose = msg + self._publish_pid_target_pose(msg.header.stamp) + + def _publish_pid_target_pose(self, stamp): + if self.path_start_time is None or self.path_duration <= 0.0: + return + now_mono = time.monotonic() + t_ref = now_mono - self.path_start_time + ref_pos, ref_quat = self._sample_pose_at(t_ref) + if ref_pos is None: + return + T_robot_ref = self._world_from_pose_quat(ref_pos, ref_quat) + target_msg = np2msg(T_robot_ref, stamp, "world", "camera_target") + self.target_pose_pub.publish(target_msg) def _clamp_step(self, target: float, current: float, max_delta: float) -> float: return float(np.clip(target - current, -max_delta, max_delta) + current) @@ -252,15 +253,13 @@ def cmd_timer_callback(self): # Stale-path protection: slow down, then stop if planner has not refreshed. age = float('inf') if self.last_path_update_time is None else (now - self.last_path_update_time) - stale_slow_s = max(self.path_stale_slow_s, self.path_period_ema * self.path_stale_slow_factor) - stale_stop_s = max(self.path_stale_stop_s, self.path_period_ema * self.path_stale_stop_factor) target_cmd = Twist() target_cmd.linear.x = self.latest_cmd.linear.x target_cmd.angular.z = self.latest_cmd.angular.z - if age > stale_stop_s: + if age > self.path_stale_stop_s: target_cmd.linear.x = 0.0 target_cmd.angular.z = 0.0 - elif age > stale_slow_s: + elif age > self.path_stale_slow_s: target_cmd.linear.x *= 0.3 target_cmd.angular.z *= 0.5 @@ -271,13 +270,6 @@ def cmd_timer_callback(self): out.linear.x = self._clamp_step(target_cmd.linear.x, self.prev_cmd.linear.x, max_dv) out.angular.z = self._clamp_step(target_cmd.angular.z, self.prev_cmd.angular.z, max_dw) out.linear.y = 0.0 - # Keep a minimum forward speed when planner requests motion and path is fresh. - if ( - age <= stale_slow_s - and abs(target_cmd.linear.x) >= self.linear_engage_threshold - and abs(out.linear.x) < self.min_effective_linear_speed - ): - out.linear.x = float(np.sign(target_cmd.linear.x) * self.min_effective_linear_speed) self.cmd_pub.publish(out) self.prev_cmd = out From 9f162ab8fe9d58d9f8630f0f7621ac759b7b63b2 Mon Sep 17 00:00:00 2001 From: junlinp Date: Tue, 28 Apr 2026 13:59:37 +0800 Subject: [PATCH 10/14] feat(tool): add poi publish action and decouple imu propagator startup --- tinynav/core/perception_node.py | 2 ++ tool/poi_editor.py | 26 ++++++++++++++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/tinynav/core/perception_node.py b/tinynav/core/perception_node.py index 7bd8c8b0..0d3fb314 100644 --- a/tinynav/core/perception_node.py +++ b/tinynav/core/perception_node.py @@ -130,6 +130,7 @@ def __init__(self, verbose_timer: bool = True): self.input_aligner_seen_imu = False self.input_aligner_seen_stereo = False self.odom_pub = self.create_publisher(Odometry, "/slam/odometry", 10) + self.odom_visual_pub = self.create_publisher(Odometry, "/slam/odometry_visual", 10) self.slam_camera_info_pub = self.create_publisher(CameraInfo, "/slam/camera_info", 10) self.depth_pub = self.create_publisher(Image, "/slam/depth", 10) self.disparity_pub_vis = self.create_publisher(Image, '/slam/disparity_vis', 10) @@ -550,6 +551,7 @@ async def process(self, left_msg, right_msg): self.V_last = result.atVector(V(len(self.keyframe_queue) - 1)) # publish odometry self.odom_pub.publish(np2msg(self.T_body_last, left_msg.header.stamp, "world", "camera", self.V_last)) + self.odom_visual_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")) diff --git a/tool/poi_editor.py b/tool/poi_editor.py index 3707fff4..fe121547 100755 --- a/tool/poi_editor.py +++ b/tool/poi_editor.py @@ -20,6 +20,7 @@ import rclpy import os from math_utils import msg2np, matrix_to_quat +from std_msgs.msg import String class SplatFile(TypedDict): centers: npt.NDArray[np.floating] @@ -187,6 +188,7 @@ class RelocalizationPose(Node): def __init__(self, viser_server: viser.ViserServer): super().__init__('relocalization_pose') self.viser_server = viser_server + self.cmd_pois_pub = self.create_publisher(String, "/mapping/cmd_pois", 10) self.relocalization_pose_sub = self.create_subscription(Odometry, '/map/relocalization', self.relocalization_pose_callback, 10) self.global_plan_sub = self.create_subscription(nav_msgs.msg.Path, '/mapping/global_plan', self.global_plan_callback, 10) self.planning_path_sub = self.create_subscription(nav_msgs.msg.Path, '/planning/trajectory_path', self.planning_path_callback, 10) @@ -196,6 +198,12 @@ def __init__(self, viser_server: viser.ViserServer): Odometry, "/mapping/current_pose_in_map", self.current_pose_in_map_callback, 10 ) + def publish_pois_to_map_node(self, payload: dict) -> None: + msg = String() + msg.data = json.dumps(payload, separators=(",", ":")) + self.cmd_pois_pub.publish(msg) + self.get_logger().info(f"Published {len(payload)} POIs to /mapping/cmd_pois") + def relocalization_pose_callback(self, msg: Odometry): position = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]) self.viser_server.scene.add_icosphere( @@ -280,6 +288,7 @@ def main( server = viser.ViserServer() server.scene.world_axes.visible = True server.scene.set_up_direction("+z") + ros_bridge: dict[str, RelocalizationPose | None] = {"node": None} # POI management poi_points = {} @@ -298,12 +307,28 @@ def main( with server.gui.add_folder("Points of Interest (POI)") as _: add_poi_button = server.gui.add_button("Add POI Point") add_save_poi_button = server.gui.add_button("Save POI") + add_send_poi_button = server.gui.add_button("Send POI to map_node") @add_save_poi_button.on_click def _(_) -> None: with open(f"{tinynav_map_path}/pois.json", "w") as f: json.dump(poi_points, f, indent=2, default=lambda x: x.tolist() if isinstance(x, np.ndarray) else x) + @add_send_poi_button.on_click + def _(_) -> None: + node = ros_bridge["node"] + if node is None: + print("ROS node is not ready yet. Cannot send POIs.") + return + payload = {} + for key, poi in poi_points.items(): + payload[str(int(key))] = { + "id": int(poi.get("id", key)), + "name": str(poi.get("name", f"POI_{key}")), + "position": [float(x) for x in poi["position"]], + } + node.publish_pois_to_map_node(payload) + poi_list_container = server.gui.add_folder("POI List") for poi_id, poi_point in poi_points.items(): @@ -667,6 +692,7 @@ def _(_, pc_handle=pc_handle, remove_button=remove_button) -> None: rclpy.init() relocalization_pose_node = RelocalizationPose(server) + ros_bridge["node"] = relocalization_pose_node try: rclpy.spin(relocalization_pose_node) relocalization_pose_node.destroy_node() From 8d6761e12de9f7a83973c00d2ecc0fda011bf62a Mon Sep 17 00:00:00 2001 From: junlinp Date: Tue, 28 Apr 2026 18:04:43 +0800 Subject: [PATCH 11/14] refactor(control): publish cmd only on odometry callback --- tinynav/platforms/cmd_vel_control.py | 45 +++++++++++++++++----------- 1 file changed, 27 insertions(+), 18 deletions(-) diff --git a/tinynav/platforms/cmd_vel_control.py b/tinynav/platforms/cmd_vel_control.py index e605e22e..c3b0718e 100644 --- a/tinynav/platforms/cmd_vel_control.py +++ b/tinynav/platforms/cmd_vel_control.py @@ -57,7 +57,7 @@ def __init__(self): self.last_path_time = 0.0 self.pose = None self.path = None - self.path_dt = 0.1 # Planning node trajectory time step. + self.path_dt_fallback = 0.1 # Fallback if path per-pose timestamps are invalid. self.path_start_time = None self.path_duration = 0.0 self.path_pos_spline = None @@ -78,13 +78,20 @@ def __init__(self): self.latest_cmd = Twist() self.prev_cmd = Twist() - self.last_cmd_pub_time = time.monotonic() + self.last_odom_time = None self.last_path_update_time = None - self.cmd_timer = self.create_timer(1.0 / self.cmd_rate_hz, self.cmd_timer_callback) def pose_callback(self, msg): self.pose = msg + now = time.monotonic() + if self.last_odom_time is None: + dt = 1.0 / self.cmd_rate_hz + else: + dt = max(1e-3, now - self.last_odom_time) + self.last_odom_time = now + self._publish_pid_target_pose(msg.header.stamp) + self._publish_cmd_on_odom(now, dt) def _publish_pid_target_pose(self, stamp): if self.path_start_time is None or self.path_duration <= 0.0: @@ -137,7 +144,20 @@ def _build_path_interpolators(self, path_msg: Path): if count < 2: return False - times = np.arange(count, dtype=np.float64) * self.path_dt + stamp_times = np.array( + [ + float(pose_stamped.header.stamp.sec) + + float(pose_stamped.header.stamp.nanosec) * 1e-9 + for pose_stamped in poses + ], + dtype=np.float64, + ) + if np.all(np.diff(stamp_times) > 0): + times = stamp_times - stamp_times[0] + else: + # Fallback for missing/invalid stamps. + times = np.arange(count, dtype=np.float64) * self.path_dt_fallback + positions = np.empty((count, 3), dtype=np.float64) quats = np.empty((count, 4), dtype=np.float64) for i, pose_stamped in enumerate(poses): @@ -227,26 +247,15 @@ def _compute_tracking_pid_cmd(self, now_mono: float, dt: float): wz_correction = self.yaw_pid.update(yaw_err, dt) + self.cross_track_gain * pos_err_lateral return vx_correction, wz_correction - def cmd_timer_callback(self): - now = time.monotonic() - dt = max(1e-3, now - self.last_cmd_pub_time) - self.last_cmd_pub_time = now - - ff_vx = 0.0 - ff_wz = 0.0 - linear_velocity_vec, angular_velocity_vec = self._compute_cmd_from_sampled_path(now) - if linear_velocity_vec is not None: - ff_vx = float(linear_velocity_vec[0]) - ff_wz = float(angular_velocity_vec[2]) - + def _publish_cmd_on_odom(self, now: float, dt: float): pid_vx = 0.0 pid_wz = 0.0 pid_cmd = self._compute_tracking_pid_cmd(now, dt) if pid_cmd[0] is not None: pid_vx, pid_wz = pid_cmd - vx = np.clip(ff_vx + pid_vx, -0.1, 0.3) - vyaw = np.clip(ff_wz + pid_wz, -0.8, 0.8) + vx = np.clip(pid_vx, -0.1, 0.3) + vyaw = np.clip(pid_wz, -0.8, 0.8) self.latest_cmd.linear.x = float(vx) self.latest_cmd.linear.y = 0.0 self.latest_cmd.angular.z = float(vyaw) From c2bb51a4bdd4d8a53a3a88f4b266f34498ecef35 Mon Sep 17 00:00:00 2001 From: junlinp Date: Tue, 28 Apr 2026 18:06:38 +0800 Subject: [PATCH 12/14] refactor(control): require valid monotonic path timestamps --- tinynav/platforms/cmd_vel_control.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/tinynav/platforms/cmd_vel_control.py b/tinynav/platforms/cmd_vel_control.py index c3b0718e..14f84d4b 100644 --- a/tinynav/platforms/cmd_vel_control.py +++ b/tinynav/platforms/cmd_vel_control.py @@ -57,7 +57,6 @@ def __init__(self): self.last_path_time = 0.0 self.pose = None self.path = None - self.path_dt_fallback = 0.1 # Fallback if path per-pose timestamps are invalid. self.path_start_time = None self.path_duration = 0.0 self.path_pos_spline = None @@ -152,11 +151,10 @@ def _build_path_interpolators(self, path_msg: Path): ], dtype=np.float64, ) - if np.all(np.diff(stamp_times) > 0): - times = stamp_times - stamp_times[0] - else: - # Fallback for missing/invalid stamps. - times = np.arange(count, dtype=np.float64) * self.path_dt_fallback + if not np.all(np.diff(stamp_times) > 0): + self.logger.warning("Path timestamps are invalid (non-monotonic); skip this path update.") + return False + times = stamp_times - stamp_times[0] positions = np.empty((count, 3), dtype=np.float64) quats = np.empty((count, 4), dtype=np.float64) From 9f5386f524cfb24005e7424a6f96baced354331d Mon Sep 17 00:00:00 2001 From: junlinp Date: Tue, 28 Apr 2026 18:15:13 +0800 Subject: [PATCH 13/14] chore(lint): clean imu_propagator_node with ruff --- tinynav/core/imu_propagator_node.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tinynav/core/imu_propagator_node.py b/tinynav/core/imu_propagator_node.py index a36002f4..d4f4514d 100644 --- a/tinynav/core/imu_propagator_node.py +++ b/tinynav/core/imu_propagator_node.py @@ -2,7 +2,6 @@ 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 0be0f1a43ed31bb16999aff9a10bee66c151cdcd Mon Sep 17 00:00:00 2001 From: junlinp Date: Tue, 28 Apr 2026 18:21:02 +0800 Subject: [PATCH 14/14] chore(core): finalize imu propagator cleanup --- tinynav/core/imu_propagator_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tinynav/core/imu_propagator_node.py b/tinynav/core/imu_propagator_node.py index d4f4514d..b8d1a4f8 100644 --- a/tinynav/core/imu_propagator_node.py +++ b/tinynav/core/imu_propagator_node.py @@ -65,12 +65,12 @@ def imu_callback(self, imu_msg: Imu): return timestamp = self._stamp_to_sec(imu_msg.header.stamp) - print("imu: ", timestamp) + #print("imu: ", timestamp) 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: + if self.imu_buffer[-1][0] <= self.odom_100hz_buffer[-1][0] + 0.010: return start_idx = None