From ee8d1fa2215c34e163db8418eef5c2c5fad69163 Mon Sep 17 00:00:00 2001 From: S1ink Date: Sat, 21 Mar 2026 22:08:54 -0500 Subject: [PATCH 1/5] disable global alignment by default --- src/core/threads/localization_worker.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/threads/localization_worker.hpp b/src/core/threads/localization_worker.hpp index 54bb3d8..fe8d90f 100644 --- a/src/core/threads/localization_worker.hpp +++ b/src/core/threads/localization_worker.hpp @@ -139,7 +139,7 @@ class LocalizationWorker std::string base_frame; std::atomic threads_running{false}; - std::atomic global_alignment_enabled{true}; + std::atomic global_alignment_enabled{false}; ScanPreprocessor< OdomPointType, From c5f8eaa908ed4fc290568333a8bcdd7499fcec6f Mon Sep 17 00:00:00 2001 From: S1ink Date: Sat, 28 Mar 2026 09:41:20 -0500 Subject: [PATCH 2/5] Update mining eval to support different cross-section sizes for each query --- src/core/threads/mining_eval_worker.cpp | 25 +++++++++++++------------ src/core/threads/mining_eval_worker.hpp | 4 ++-- srv/UpdateMiningEvalMode.srv | 4 ++-- 3 files changed, 17 insertions(+), 16 deletions(-) diff --git a/src/core/threads/mining_eval_worker.cpp b/src/core/threads/mining_eval_worker.cpp index 2a08495..4b21ed4 100644 --- a/src/core/threads/mining_eval_worker.cpp +++ b/src/core/threads/mining_eval_worker.cpp @@ -98,9 +98,10 @@ void MiningEvalWorker::accept( buff = std::make_shared(); } buff->poses = req->queries; - buff->eval_width = req->eval_width; - buff->eval_height = req->eval_height; - resp->query_id = buff->id = this->query_count.load(); + buff->widths = req->query_widths; + buff->heights = req->query_heights; + resp->query_id = + static_cast(buff->id = this->query_count.load()); this->query_notifier.unlockInputAndNotify(buff); } this->query_count++; @@ -197,14 +198,6 @@ void MiningEvalWorker::mining_eval_callback(MiningEvalResources& buff) } } - Box3f eval_bound; - eval_bound.min().x() = 0.f; - eval_bound.min().y() = query_ptr->eval_width * -0.5f; - eval_bound.min().z() = query_ptr->eval_height * -0.5f; - eval_bound.max().x() = std::numeric_limits::infinity(); - eval_bound.max().y() = query_ptr->eval_width * 0.5f; - eval_bound.max().z() = query_ptr->eval_height * 0.5f; - for (size_t i = 0; i < query_ptr->poses.poses.size(); i++) { Iso3f pose_tf; @@ -212,12 +205,20 @@ void MiningEvalWorker::mining_eval_callback(MiningEvalResources& buff) pose_tf = (pose_tf << query_ptr->poses.poses[i]).inverse() * odom_to_ref_tf; + Box3f eval_bound; + eval_bound.min().x() = 0.f; + eval_bound.min().y() = query_ptr->widths[i] * -0.5f; + eval_bound.min().z() = query_ptr->heights[i] * -0.5f; + eval_bound.max().x() = std::numeric_limits::infinity(); + eval_bound.max().y() = query_ptr->widths[i] * 0.5f; + eval_bound.max().z() = query_ptr->heights[i] * 0.5f; + msg.ranges[i] = std::numeric_limits::infinity(); for (const auto& pt : buff.avoid_points) { Vec3f pt_ = (pose_tf * pt.getVector4fMap()).template head<3>(); - if(eval_bound.contains(pt_) && pt_.x() < msg.ranges[i]) + if (eval_bound.contains(pt_) && pt_.x() < msg.ranges[i]) { msg.ranges[i] = pt_.x(); } diff --git a/src/core/threads/mining_eval_worker.hpp b/src/core/threads/mining_eval_worker.hpp index 4f1f091..70f6c93 100644 --- a/src/core/threads/mining_eval_worker.hpp +++ b/src/core/threads/mining_eval_worker.hpp @@ -102,8 +102,8 @@ class MiningEvalWorker using SharedPtr = std::shared_ptr; PoseArrayMsg poses; - float eval_width; - float eval_height; + std::vector widths; + std::vector heights; uint32_t id; }; diff --git a/srv/UpdateMiningEvalMode.srv b/srv/UpdateMiningEvalMode.srv index fd45ba4..87be922 100644 --- a/srv/UpdateMiningEvalMode.srv +++ b/srv/UpdateMiningEvalMode.srv @@ -1,6 +1,6 @@ geometry_msgs/PoseArray queries -float32 eval_width -float32 eval_height +float32[] query_widths +float32[] query_heights bool completed --- int32 query_id From c58da66b1d6b662123f99f3bef500b9bf80f005e Mon Sep 17 00:00:00 2001 From: S1ink Date: Thu, 2 Apr 2026 16:46:43 -0500 Subject: [PATCH 3/5] update mining eval topics --- src/core/perception_core.cpp | 2 +- src/core/threads/mining_eval_worker.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/core/perception_core.cpp b/src/core/perception_core.cpp index 5468e8d..3a06e3c 100644 --- a/src/core/perception_core.cpp +++ b/src/core/perception_core.cpp @@ -834,7 +834,7 @@ void PerceptionNode::initPubSubs(PerceptionConfig& config) { this->path_planning_worker.accept(req, resp); }); // #endif this->mining_eval_service = this->create_service( - PERCEPTION_TOPIC("query_mining_eval"), + PERCEPTION_TOPIC("update_mining_eval"), [this]( UpdateMiningEvalSrv::Request::SharedPtr req, UpdateMiningEvalSrv::Response::SharedPtr resp) diff --git a/src/core/threads/mining_eval_worker.cpp b/src/core/threads/mining_eval_worker.cpp index 4b21ed4..b4a0164 100644 --- a/src/core/threads/mining_eval_worker.cpp +++ b/src/core/threads/mining_eval_worker.cpp @@ -189,7 +189,7 @@ void MiningEvalWorker::mining_eval_callback(MiningEvalResources& buff) .lookupTransform( header.frame_id, this->odom_frame, - util::toTf2TimePoint(header.stamp)) + tf2::TimePointZero) .transform; } catch (const std::exception& e) @@ -225,7 +225,7 @@ void MiningEvalWorker::mining_eval_callback(MiningEvalResources& buff) } } - this->pub_map.publish("mining_evaluation", msg); + this->pub_map.publish("mining_eval_results", msg); } }; // namespace perception From f2942ecfa6089a66b2ee8d37bcd0eed1b1c26cf9 Mon Sep 17 00:00:00 2001 From: Sam Richter Date: Fri, 15 May 2026 14:15:34 -0400 Subject: [PATCH 4/5] better support for turning in place with low fov --- include/modules/impl/lidar_odom_impl.hpp | 122 +++++++++++++---------- include/modules/lidar_odom.hpp | 9 +- 2 files changed, 78 insertions(+), 53 deletions(-) diff --git a/include/modules/impl/lidar_odom_impl.hpp b/include/modules/impl/lidar_odom_impl.hpp index 2d8a35c..0e848f8 100644 --- a/include/modules/impl/lidar_odom_impl.hpp +++ b/include/modules/impl/lidar_odom_impl.hpp @@ -82,7 +82,12 @@ LidarOdometry::LidarOdometryParam::LidarOdometryParam(rclcpp::Node& node) node, "dlo.keyframe.thresh_R", this->keyframe_thresh_rot_, - 1.0); + 30.0); + util::declare_param( + node, + "dlo.keyframe.max_nearby", + this->keyframe_max_nearby_, + 10); // Submap util::declare_param(node, "dlo.keyframe.submap.knn", this->submap_knn_, 10); @@ -268,6 +273,18 @@ LidarOdometry::LidarOdometry(rclcpp::Node& inst) : 0.1); this->initState(); + + if (this->param.keyframe_max_nearby_ > this->param.submap_knn_) + { + RCLCPP_WARN( + inst.get_logger(), + "[ODOMETRY]: keyframe.max_nearby (%d) > keyframe.submap.knn (%d). " + "If thresh_R is small enough to create more than knn rotation keyframes " + "at one location, the submap selection will be unstable and GICP may " + "oscillate. Either raise thresh_R or set max_nearby <= submap.knn.", + this->param.keyframe_max_nearby_, + this->param.submap_knn_); + } } @@ -706,6 +723,8 @@ void LidarOdometry::initializeInputTarget() this->gicp_s2s.calculateSourceCovariances(); this->keyframe_normals.push_back(this->gicp_s2s.getSourceCovariances()); + this->state.last_kf_rotq = this->state.rotq; + this->keyframe_kdtree.setInputCloud(this->keyframe_points); ++this->state.num_keyframes; } @@ -833,23 +852,23 @@ void LidarOdometry::getSubmapKeyframes() // K NEAREST NEIGHBORS FROM ALL KEYFRAMES // - // calculate distance between current pose and poses in keyframe set - std::vector ds; - std::vector keyframe_nn; - int i = 0; Vec3f curr_pose = this->state.T_s2s.template block<3, 1>(0, 3); - for (const auto& k : this->keyframes) + PointT query_pt; + query_pt.x = curr_pose.x(); + query_pt.y = curr_pose.y(); + query_pt.z = curr_pose.z(); + + // K nearest neighbor keyframe poses via kdtree (replaces O(N) linear scan) { - float d = static_cast((curr_pose - k.first.first).norm()); - ds.push_back(d); - keyframe_nn.push_back(i); - i++; + std::vector knn_idx; + std::vector knn_dist_sq; + this->keyframe_kdtree.nearestKSearch( + query_pt, this->param.submap_knn_, knn_idx, knn_dist_sq); + for (const auto idx : knn_idx) + this->submap_kf_idx_curr.insert(idx); } - // get indices for top K nearest neighbor keyframe poses - this->pushSubmapIndices(ds, this->param.submap_knn_, keyframe_nn); - // // K NEAREST NEIGHBORS FROM CONVEX HULL // @@ -862,7 +881,7 @@ void LidarOdometry::getSubmapKeyframes() convex_ds.reserve(this->keyframe_convex.size()); for (const auto& c : this->keyframe_convex) { - convex_ds.push_back(ds[c]); + convex_ds.push_back((curr_pose - this->keyframes[c].first.first).norm()); } // get indicies for top kNN for convex hull @@ -880,13 +899,14 @@ void LidarOdometry::getSubmapKeyframes() // get distances for each keyframe on concave hull std::vector concave_ds; - concave_ds.reserve(keyframe_concave.size()); + concave_ds.reserve(this->keyframe_concave.size()); for (const auto& c : this->keyframe_concave) { - concave_ds.push_back(ds[c]); + concave_ds.push_back( + (curr_pose - this->keyframes[c].first.first).norm()); } - // get indicies for top kNN for convex hull + // get indicies for top kNN for concave hull this->pushSubmapIndices( concave_ds, this->param.submap_kcc_, @@ -1049,45 +1069,40 @@ void LidarOdometry::transformCurrentScan() template void LidarOdometry::updateKeyframes() { - // calculate difference in pose and rotation to all poses in trajectory - double closest_d = std::numeric_limits::infinity(); - int closest_idx = 0; - int keyframes_idx = 0; - int num_nearby = 0; - - for (const auto& k : this->keyframes) - { - // calculate distance between current pose and pose in keyframes - const double delta_d = (this->state.translation - k.first.first).norm(); - - // count the number nearby current pose - { - if (delta_d <= this->state.keyframe_thresh_dist_ * 1.5) - { - num_nearby++; - } - } - // store into variable - if (delta_d < closest_d) - { - closest_d = delta_d; - closest_idx = keyframes_idx; - } - - keyframes_idx++; - } - - // calculate difference in orientation - double theta_rad = this->state.rotq.angularDistance( - this->keyframes[closest_idx].first.second); - double theta_deg = theta_rad * (180.0 / std::numbers::pi); + PointT query_pt; + query_pt.x = this->state.translation.x(); + query_pt.y = this->state.translation.y(); + query_pt.z = this->state.translation.z(); + + // nearest keyframe for distance threshold check + std::vector nn_idx(1); + std::vector nn_dist_sq(1); + this->keyframe_kdtree.nearestKSearch(query_pt, 1, nn_idx, nn_dist_sq); + const double closest_d = std::sqrt(static_cast(nn_dist_sq[0])); + + // count keyframes within 1.5x distance threshold for density cap + std::vector nearby_idx; + std::vector nearby_dist_sq; + const int num_nearby = this->keyframe_kdtree.radiusSearch( + query_pt, + static_cast(this->state.keyframe_thresh_dist_ * 1.5), + nearby_idx, + nearby_dist_sq); + + // rotation accumulated since last keyframe was added (not nearest keyframe), + // so in-place rotation near existing keyframes still triggers new keyframes + const double theta_rad = + this->state.rotq.angularDistance(this->state.last_kf_rotq); + const double theta_deg = theta_rad * (180.0 / std::numbers::pi); // update keyframe - const bool keyframe_close = - abs(closest_d) > this->state.keyframe_thresh_dist_; + const bool keyframe_close = closest_d > this->state.keyframe_thresh_dist_; + // rotation trigger uses a per-cell density cap instead of num_nearby <= 1, + // so it fires on first visit regardless of existing spatial coverage const bool theta_rotated = - abs(theta_deg) > this->param.keyframe_thresh_rot_ && num_nearby <= 1; + theta_deg > this->param.keyframe_thresh_rot_ && + num_nearby < this->param.keyframe_max_nearby_; if (keyframe_close || theta_rotated) { @@ -1109,6 +1124,9 @@ void LidarOdometry::updateKeyframes() this->state.translation.y(), this->state.translation.z()); + this->state.last_kf_rotq = this->state.rotq; + this->keyframe_kdtree.setInputCloud(this->keyframe_points); + // compute kdtree and keyframe normals (use gicp_s2s input source as temporary storage because it will be // overwritten by setInputSources()) *this->keyframe_cloud = *this->current_scan_t; diff --git a/include/modules/lidar_odom.hpp b/include/modules/lidar_odom.hpp index 5abe304..d1f3cdc 100644 --- a/include/modules/lidar_odom.hpp +++ b/include/modules/lidar_odom.hpp @@ -176,10 +176,10 @@ class LidarOdometry std::vector keyframe_convex, keyframe_concave; PointCloudPtrT keyframe_cloud, keyframe_points; - // TODO: use kdtree for positions std::vector, PointCloudPtrT>> keyframes; std::vector>> keyframe_normals; + pcl::KdTreeFLANN keyframe_kdtree; PointCloudPtrT submap_cloud; std::vector> submap_normals; @@ -208,6 +208,7 @@ class LidarOdometry Vec3f translation{Vec3f::Zero()}; Quatf rotq{Quatf::Identity()}; Quatf last_rotq{Quatf::Identity()}; + Quatf last_kf_rotq{Quatf::Identity()}; Mat4f T{Mat4f::Identity()}; Mat4f T_s2s{Mat4f::Identity()}; @@ -226,6 +227,12 @@ class LidarOdometry bool use_scan_ts_as_init_; double keyframe_thresh_rot_; + // For submap stability: max_nearby should be <= submap_knn_, OR + // thresh_R should be large enough that (max_rotation / thresh_R) < submap_knn_. + // If more rotation keyframes exist at a location than knn slots, the kdtree + // tie-breaks arbitrarily between them, causing the submap to fluctuate each + // frame and GICP to oscillate between local minima. + int keyframe_max_nearby_; int submap_knn_; int submap_kcv_; From 7732230a4d5f7ab53db64c43499f989c423fd4bb Mon Sep 17 00:00:00 2001 From: S1ink Date: Thu, 28 May 2026 16:58:05 -0500 Subject: [PATCH 5/5] fix nanogicp kdtree index out of bounds bug --- include/nano_gicp/impl/nano_gicp_impl.hpp | 9 ++++++--- include/nano_gicp/nano_gicp.hpp | 9 ++++----- include/nano_gicp/nanoflann.hpp | 7 ++++++- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/include/nano_gicp/impl/nano_gicp_impl.hpp b/include/nano_gicp/impl/nano_gicp_impl.hpp index 97e36dd..089eff5 100644 --- a/include/nano_gicp/impl/nano_gicp_impl.hpp +++ b/include/nano_gicp/impl/nano_gicp_impl.hpp @@ -137,7 +137,7 @@ template void NanoGICP::setInputSource( const PointCloudSourceConstPtr& cloud) { - if (input_ == cloud) + if (input_ == cloud && source_kdtree_->getIndexedPointCount() == cloud->size()) { return; } @@ -151,7 +151,7 @@ template void NanoGICP::setInputTarget( const PointCloudTargetConstPtr& cloud) { - if (target_ == cloud) + if (target_ == cloud && target_kdtree_->getIndexedPointCount() == cloud->size()) { return; } @@ -190,6 +190,8 @@ bool NanoGICP::calculateTargetCovariances() return calculate_covariances(target_, *target_kdtree_, target_covs_); } + + template void NanoGICP::computeTransformation( PointCloudSource& output, @@ -379,7 +381,8 @@ bool NanoGICP::calculate_covariances( std::vector>& covariances) { - if (kdtree.getInputCloud() != cloud) + if (kdtree.getInputCloud() != cloud || + kdtree.getIndexedPointCount() != cloud->size()) { kdtree.setInputCloud(cloud); } diff --git a/include/nano_gicp/nano_gicp.hpp b/include/nano_gicp/nano_gicp.hpp index 5e3ae3e..143170c 100644 --- a/include/nano_gicp/nano_gicp.hpp +++ b/include/nano_gicp/nano_gicp.hpp @@ -96,31 +96,30 @@ class NanoGICP : public LsqRegistration virtual void swapSourceAndTarget() override; virtual void clearSource() override; virtual void clearTarget() override; + virtual void registerInputSource(const PointCloudSourceConstPtr& cloud); virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; + virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; virtual void setSourceCovariances( const std::vector< Eigen::Matrix4d, Eigen::aligned_allocator>& covs); - virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; virtual void setTargetCovariances( const std::vector< Eigen::Matrix4d, Eigen::aligned_allocator>& covs); - virtual void registerInputSource(const PointCloudSourceConstPtr& cloud); - virtual bool calculateSourceCovariances(); virtual bool calculateTargetCovariances(); - const std:: + inline const std:: vector>& getSourceCovariances() const { return source_covs_; } - const std:: + inline const std:: vector>& getTargetCovariances() const { diff --git a/include/nano_gicp/nanoflann.hpp b/include/nano_gicp/nanoflann.hpp index 7909561..bd57e28 100644 --- a/include/nano_gicp/nanoflann.hpp +++ b/include/nano_gicp/nanoflann.hpp @@ -83,6 +83,11 @@ class KdTreeFLANN inline PointCloudConstPtr getInputCloud() const { return _adaptor.pcl; } + inline size_t getIndexedPointCount() const + { + return _kdtree.size_at_index_build_; + } + int nearestKSearch( const PointT& point, int k, @@ -177,7 +182,7 @@ inline int KdTreeFLANN::radiusSearch( std::vector& k_indices, std::vector& k_sqr_distances) const { - static std::vector> indices_dist; + thread_local std::vector> indices_dist; indices_dist.reserve(128); RadiusResultSet resultSet(radius, indices_dist);