use rtee for icp edges

packages/geom/include/geom/distance.h

@@ -27,4 +27,6 @@ double distanceSq(const MotionState& a, const MotionState& b) noexcept;
 
 double distance(const MotionState& a, const MotionState& b) noexcept;
 
+double distance(const Segment& a, const Segment& b) noexcept;
+
 }  // namespace truck::geom

packages/geom/src/distance.cpp

@@ -1,4 +1,5 @@
 #include "geom/distance.h"
+#include "geom/intersection.h"
 
 #include "common/math.h"
 
@@ -50,4 +51,15 @@ double distance(const MotionState& a, const MotionState& b) noexcept {
     return distance(a.pos, b.pos);
 }
 
+double distance(const Segment& a, const Segment& b) noexcept {
+    if (intersect(a, b)) {
+        return 0.0;
+    }
+    return std::min(
+        {distance(a.begin, projection(a.begin, b)),
+         distance(a.end, projection(a.end, b)),
+         distance(b.begin, projection(b.begin, a)),
+         distance(b.end, projection(b.end, a))});
+}
+
 }  // namespace truck::geom

packages/lidar_map/include/lidar_map/builder.h

@@ -2,12 +2,20 @@
 
 #include "geom/pose.h"
 #include "lidar_map/common.h"
+#include "geom/bounding_box.h"
+#include "geom/segment.h"
+#include "geom/vector.h"
+#include "geom/boost/box.h"
 
+#include <boost/geometry.hpp>
+#include <boost/geometry/index/rtree.hpp>
 #include <g2o/core/sparse_optimizer.h>
 #include <g2o/core/hyper_graph.h>
 
 namespace truck::lidar_map {
 
+namespace bgi = boost::geometry::index;
+
 class EdgeData : public g2o::HyperGraph::Data {
   public:
     EdgeData(int value) : _value(value) {}
@@ -36,6 +44,8 @@ struct PoseInfo {
 
 using EdgesInfo = std::vector<EdgeInfo>;
 using PosesInfo = std::vector<PoseInfo>;
+using SegmentValue = std::pair<geom::BoundingBox, geom::Segment>;
+using ICPRTree = bgi::rtree<SegmentValue, bgi::quadratic<16>>;
 
 struct PoseGraphInfo {
     EdgesInfo edges;
@@ -45,8 +55,10 @@ struct PoseGraphInfo {
 struct BuilderParams {
     std::string icp_config;
     double icp_edge_max_dist = 0.6;
+    double icp_edge_min_dist = 1.0;
     double odom_edge_weight = 1.0;
     double icp_edge_weight = 3.0;
+    double min_poses_dist = 1.0;
     bool verbose = true;
 };
 
@@ -57,6 +69,8 @@ class Builder {
     std::pair<geom::Poses, Clouds> sliceDataByPosesProximity(
         const geom::Poses& poses, const Clouds& clouds, double poses_min_dist) const;
 
+    geom::BoundingBox computeBoundingBox(const geom::Segment& segment);
+
     void initPoseGraph(const geom::Poses& poses, const Clouds& clouds);
 
     geom::Poses optimizePoseGraph(size_t iterations = 1);
@@ -83,6 +97,7 @@ class Builder {
     ICP icp_;
     g2o::SparseOptimizer optimizer_;
     BuilderParams params_;
+    ICPRTree icp_edges_rtree_;
 };
 
 }  // namespace truck::lidar_map

packages/lidar_map/src/builder.cpp

@@ -7,6 +7,7 @@
 #include "geom/distance.h"
 
 #include <boost/geometry.hpp>
+#include <boost/geometry/index/rtree.hpp>
 #include <g2o/core/block_solver.h>
 #include <g2o/core/optimization_algorithm_levenberg.h>
 #include <g2o/solvers/dense/linear_solver_dense.h>
@@ -17,6 +18,7 @@
 namespace truck::lidar_map {
 
 namespace bg = boost::geometry;
+namespace bgi = boost::geometry::index;
 
 using IndexPoint = std::pair<geom::Vec2, size_t>;
 using IndexPoints = std::vector<IndexPoint>;
@@ -25,6 +27,8 @@ using RTree = bg::index::rtree<IndexPoint, bg::index::rstar<16>>;
 using BlockSolverType = g2o::BlockSolver<g2o::BlockSolverTraits<3, 3>>;
 using LinearSolverType = g2o::LinearSolverDense<BlockSolverType::PoseMatrixType>;
 
+using SegmentValue = std::pair<geom::BoundingBox, geom::Segment>;
+
 namespace {
 
 void normalize(Cloud& cloud) {
@@ -172,6 +176,20 @@ std::pair<geom::Poses, Clouds> Builder::sliceDataByPosesProximity(
     return {filtered_poses, filtered_clouds};
 }
 
+/**
+ * Computes the bounding box for a given segment.
+ *
+ * @param segment The input line segment.
+ * @return BoundingBox object that encloses the segment.
+ */
+geom::BoundingBox Builder::computeBoundingBox(const geom::Segment& segment) {
+    return geom::BoundingBox(
+        geom::Vec2(
+            std::min(segment.begin.x, segment.end.x), std::min(segment.begin.y, segment.end.y)),
+        geom::Vec2(
+            std::max(segment.begin.x, segment.end.x), std::max(segment.begin.y, segment.end.y)));
+}
+
 /**
  * Initialize pose graph
  *
@@ -214,11 +232,40 @@ void Builder::initPoseGraph(const geom::Poses& poses, const Clouds& clouds) {
     }
 
     auto data_points_clouds = toDataPoints(clouds);
-
     // Add ICP edges
     for (size_t i = 0; i < poses.size(); i++) {
         for (size_t j = i + 1; j < poses.size(); j++) {
-            if (geom::distance(poses[i], poses[j]) > params_.icp_edge_max_dist) {
+            // Distance between two poses exceeds the maximum allowed ICP edge distance
+            bool is_distance_unused =
+                geom::distance(poses[i], poses[j]) > params_.icp_edge_max_dist;
+            // Indices of the poses are close enough based on the minimum pose distance
+            double min_indices_dist = params_.icp_edge_max_dist / params_.min_poses_dist;
+            bool are_indices_close =
+                std::abs(static_cast<int>(i) - static_cast<int>(j)) <= min_indices_dist;
+
+            if (is_distance_unused || are_indices_close) {
+                continue;
+            }
+
+            geom::Vec2 start(poses[i].pos.x, poses[i].pos.y);
+            geom::Vec2 end(poses[j].pos.x, poses[j].pos.y);
+            geom::Segment segment(start, end);
+            geom::BoundingBox bbox = computeBoundingBox(segment);
+
+            std::vector<SegmentValue> nearest_segments;
+            icp_edges_rtree_.query(bgi::nearest(bbox, 1), std::back_inserter(nearest_segments));
+
+            bool can_add = true;
+            if (!nearest_segments.empty()) {
+                const auto& [existing_bbox, existing_segment] = nearest_segments.front();
+                // If the distance between segments is less than the minimum allowed value or they
+                // intersect, the new ICP edge is not added.
+                if (geom::distance(segment, existing_segment) < params_.icp_edge_min_dist) {
+                    can_add = false;
+                }
+            }
+
+            if (!can_add) {
                 continue;
             }
 
@@ -239,6 +286,7 @@ void Builder::initPoseGraph(const geom::Poses& poses, const Clouds& clouds) {
             edge->setUserData(userData);
 
             optimizer_.addEdge(edge);
+            icp_edges_rtree_.insert(std::make_pair(bbox, segment));
         }
     }
 
@@ -312,7 +360,7 @@ PoseGraphInfo Builder::calculatePoseGraphInfo() const {
             .id = vertex_se2->id(),
             .pose = {
                 .pos = geom::Vec2{estimate[0], estimate[1]},
-                .dir = truck::geom::Angle::fromRadians(estimate[2])}});
+                .dir = geom::Angle::fromRadians(estimate[2])}});
     }
     return pose_graph_info;
 }

packages/lidar_map/src/main.cpp

@@ -21,7 +21,14 @@ const std::string kOutputTopicLidarMap = "/lidar_map";
 const std::string kOutputTopicPoses = "/poses";
 const std::string kOutputTopicClouds = "/clouds";
 const std::string kPkgPathLidarMap = ament_index_cpp::get_package_share_directory("lidar_map");
+const std::string kIcpConfigPath = kPkgPathLidarMap + "/config/icp.yaml";
 const int kPointsIntensityThreshold = 15;
+const double kMinPosesDist = 1.0;
+const double kIcpEdgeMaxDist = 3.0;
+const double kIcpEdgeMinDist = 1.0;
+const double kOdomEdgeWeight = 1.0;
+const double kIcpEdgeWeight = 3.0;
+const size_t kOptimizationSteps = 10;
 
 int main(int argc, char* argv[]) {
     bool enable_mcap_log = false;
@@ -74,17 +81,16 @@ int main(int argc, char* argv[]) {
     }
 
     const BuilderParams builder_params{
-        .icp_config = kPkgPathLidarMap + "/config/icp.yaml",
-        .icp_edge_max_dist = 3,
-        .odom_edge_weight = 1.0,
-        .icp_edge_weight = 3.0,
+        .icp_config = kIcpConfigPath,
+        .icp_edge_max_dist = kIcpEdgeMaxDist,
+        .icp_edge_min_dist = kIcpEdgeMinDist,
+        .odom_edge_weight = kOdomEdgeWeight,
+        .icp_edge_weight = kIcpEdgeWeight,
+        .min_poses_dist = kMinPosesDist,
         .verbose = true};
 
     Builder builder = Builder(builder_params);
 
-    const size_t optimization_steps = 10;
-    const double min_poses_dist = 4.0;
-
     std::shared_ptr<serialization::writer::MCAPWriter> mcap_writer = nullptr;
 
     if (enable_mcap_log) {
@@ -102,7 +108,7 @@ int main(int argc, char* argv[]) {
     {
         const auto [odom_msgs, point_cloud_msgs] =
             serialization::reader::readAndSyncOdomWithPointCloud(
-                mcap_input_path, kInputTopicOdom, kInputTopicPointCloud, min_poses_dist);
+                mcap_input_path, kInputTopicOdom, kInputTopicPointCloud, kMinPosesDist);
 
         poses = toPoses(odom_msgs);
         clouds = toClouds(point_cloud_msgs);
@@ -135,7 +141,7 @@ int main(int argc, char* argv[]) {
             serialization::writer::writePoseGraphInfoToJSON(json_log_path, pose_graph_info, 0);
         }
 
-        for (size_t i = 1; i <= optimization_steps; i++) {
+        for (size_t i = 1; i <= kOptimizationSteps; i++) {
             poses = builder.optimizePoseGraph();
 
             if (enable_mcap_log) {