use rtee for icp edges

packages/lidar_map/include/lidar_map/builder.h

@@ -2,6 +2,9 @@
 
 #include "geom/pose.h"
 #include "lidar_map/common.h"
+#include "geom/bounding_box.h"
+#include "geom/segment.h"
+#include "geom/vector.h"
 
 #include <g2o/core/sparse_optimizer.h>
 #include <g2o/core/hyper_graph.h>
@@ -36,6 +39,9 @@ struct PoseInfo {
 
 using EdgesInfo = std::vector<EdgeInfo>;
 using PosesInfo = std::vector<PoseInfo>;
+using BoundingBox = truck::geom::BoundingBox;
+using Segment = truck::geom::Segment;
+using Vec2 = truck::geom::Vec2;
 
 struct PoseGraphInfo {
     EdgesInfo edges;
@@ -45,8 +51,11 @@ 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;
+    int ktree_neighbors_clount = 10;
+    double min_poses_dist = 1.0;
     bool verbose = true;
 };
 
@@ -57,6 +66,12 @@ class Builder {
     std::pair<geom::Poses, Clouds> sliceDataByPosesProximity(
         const geom::Poses& poses, const Clouds& clouds, double poses_min_dist) const;
 
+    double segmentDistance(const Segment& seg1, const Segment& seg2);
+
+    BoundingBox computeBoundingBox(const truck::geom::Segment& segment);
+
+    bool segmentsIntersect(const Segment& s1, const Segment& s2);
+
     void initPoseGraph(const geom::Poses& poses, const Clouds& clouds);
 
     geom::Poses optimizePoseGraph(size_t iterations = 1);

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,9 @@ 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 Vec2 = truck::geom::Vec2;
+using SegmentValue = std::pair<truck::geom::BoundingBox, truck::geom::Segment>;
+
 namespace {
 
 void normalize(Cloud& cloud) {
@@ -172,6 +177,62 @@ std::pair<geom::Poses, Clouds> Builder::sliceDataByPosesProximity(
     return {filtered_poses, filtered_clouds};
 }
 
+/**
+ * Computes the shortest distance between two line segments.
+ *
+ * @param seg1 First line segment.
+ * @param seg2 Second line segment.
+ * @return Minimum Euclidean distance between the two segments.
+ */
+double Builder::segmentDistance(const Segment& seg1, const Segment& seg2) {
+    return std::min(
+        {geom::distance(seg1.begin, projection(seg1.begin, seg2)),
+         geom::distance(seg1.end, projection(seg1.end, seg2)),
+         geom::distance(seg2.begin, projection(seg2.begin, seg1)),
+         geom::distance(seg2.end, projection(seg2.end, seg1))});
+}
+
+/**
+ * Computes the bounding box for a given segment.
+ *
+ * @param segment The input line segment.
+ * @return BoundingBox object that encloses the segment.
+ */
+BoundingBox Builder::computeBoundingBox(const truck::geom::Segment& segment) {
+    return BoundingBox(
+        Vec2(std::min(segment.begin.x, segment.end.x), std::min(segment.begin.y, segment.end.y)),
+        Vec2(std::max(segment.begin.x, segment.end.x), std::max(segment.begin.y, segment.end.y)));
+}
+
+/**
+ * Determines whether two line segments intersect.
+ *
+ * @param s1 First line segment.
+ * @param s2 Second line segment.
+ * @return True if the segments intersect, false else.
+ */
+bool Builder::segmentsIntersect(const Segment& s1, const Segment& s2) {
+    auto orientation = [](const Vec2& a, const Vec2& b, const Vec2& c) {
+        return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x);
+    };
+
+    double o1 = orientation(s1.begin, s1.end, s2.begin);
+    double o2 = orientation(s1.begin, s1.end, s2.end);
+    double o3 = orientation(s2.begin, s2.end, s1.begin);
+    double o4 = orientation(s2.begin, s2.end, s1.end);
+
+    if ((o1 * o2 < 0) && (o3 * o4 < 0)) {
+        return true;
+    }
+
+    if ((s1.begin.x == s2.begin.x && s1.begin.y == s2.begin.y)
+        || (s1.end.x == s2.end.x && s1.end.y == s2.end.y)) {
+        return true;
+    }
+
+    return false;
+}
+
 /**
  * Initialize pose graph
  *
@@ -214,31 +275,57 @@ void Builder::initPoseGraph(const geom::Poses& poses, const Clouds& clouds) {
     }
 
     auto data_points_clouds = toDataPoints(clouds);
-
+    bgi::rtree<SegmentValue, bgi::quadratic<16>> rtree;
     // 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) {
+            if (geom::distance(poses[i], poses[j]) > params_.icp_edge_max_dist
+                || std::abs(static_cast<int>(i) - static_cast<int>(j))
+                       <= params_.icp_edge_max_dist / params_.min_poses_dist) {
                 continue;
             }
 
-            const Eigen::Matrix4f tf_matrix_odom = transformationMatrix(poses[i], poses[j]);
-
-            const auto& reference_cloud = data_points_clouds[i];
-            auto reading_cloud = data_points_clouds[j];
-            icp_.transformations.apply(reading_cloud, tf_matrix_odom);
-            normalize(reading_cloud);
-            const Eigen::Matrix4f tf_matrix_icp = icp_(reading_cloud, reference_cloud);
-            const Eigen::Matrix4f tf_matrix_final = tf_matrix_icp * tf_matrix_odom;
-            auto* edge = new g2o::EdgeSE2();
-            edge->setVertex(0, vertices[i]);
-            edge->setVertex(1, vertices[j]);
-            edge->setMeasurement(toSE2(tf_matrix_final));
-            edge->setInformation(Eigen::Matrix3d::Identity() * params_.icp_edge_weight);
-            auto* userData = new EdgeData(1);
-            edge->setUserData(userData);
-
-            optimizer_.addEdge(edge);
+            Vec2 start(poses[i].pos.x, poses[i].pos.y);
+            Vec2 end(poses[j].pos.x, poses[j].pos.y);
+            Segment segment(start, end);
+            BoundingBox bbox = computeBoundingBox(segment);
+
+            std::vector<SegmentValue> nearest_segments;
+            rtree.query(
+                bgi::nearest(bbox, params_.ktree_neighbors_clount),
+                std::back_inserter(nearest_segments));
+
+            bool can_add = true;
+            auto it = nearest_segments.begin();
+            while (it != nearest_segments.end() && can_add) {
+                const auto& [existing_bbox, existing_segment] = *it;
+                if (segmentDistance(segment, existing_segment) < params_.icp_edge_min_dist
+                    || segmentsIntersect(segment, existing_segment)) {
+                    can_add = false;
+                }
+                ++it;
+            }
+
+            if (can_add) {
+                const Eigen::Matrix4f tf_matrix_odom = transformationMatrix(poses[i], poses[j]);
+
+                const auto& reference_cloud = data_points_clouds[i];
+                auto reading_cloud = data_points_clouds[j];
+                icp_.transformations.apply(reading_cloud, tf_matrix_odom);
+                normalize(reading_cloud);
+                const Eigen::Matrix4f tf_matrix_icp = icp_(reading_cloud, reference_cloud);
+                const Eigen::Matrix4f tf_matrix_final = tf_matrix_icp * tf_matrix_odom;
+                auto* edge = new g2o::EdgeSE2();
+                edge->setVertex(0, vertices[i]);
+                edge->setVertex(1, vertices[j]);
+                edge->setMeasurement(toSE2(tf_matrix_final));
+                edge->setInformation(Eigen::Matrix3d::Identity() * params_.icp_edge_weight);
+                auto* userData = new EdgeData(1);
+                edge->setUserData(userData);
+
+                optimizer_.addEdge(edge);
+                rtree.insert(std::make_pair(bbox, segment));
+            }
         }
     }
 

packages/lidar_map/src/main.cpp

@@ -21,7 +21,15 @@ 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 kKtreeNeighborsCount = 10;
+const size_t kOptimizationSteps = 10;
 
 int main(int argc, char* argv[]) {
     bool enable_mcap_log = false;
@@ -74,17 +82,17 @@ 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,
+        .ktree_neighbors_clount = kKtreeNeighborsCount,
+        .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 +110,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 +143,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) {