add normals and outliers

packages/lidar_map/README.md

@@ -23,6 +23,13 @@ Parameters:
 
 4. `--json-log`: path to json file for saving map logs (optional)
 
+5. `--icp-log`: path to NON-EXISTING folder for saving ICP log file (normals and outliers) (optional)
+
+6. `--cloud-number`: Cloud number for visualizing normals and outliers for icp-log (optional)
+
+7. `--percentage`: Percentage of normals rendered [0;100] for icp-log (optional)
+
+
 Run the executable `lidar_map_executable` with the assigned `--mcap-input` and `--mcap-output` arguments:
 ```console
 ros2 run lidar_map lidar_map_executable
@@ -39,10 +46,15 @@ You can now specify logging options separately:
 
 2. `.json` file: contains poses with ICP and odom edges on each iteration of the graph
 
+3. `icp-log.mcap` file contains normals and outliers for {cloud-number} cloud, percentage of normals will be shown = {percentage}
+
 ```console
 ros2 run lidar_map lidar_map_executable
   --mcap-input data/rides/ride_atrium_XX_YY_ZZ.mcap
   --mcap-output data/clouds/cloud_atrium_XX_YY_ZZ
   --mcap-log data/logs/mcap/cloud_atrium_XX_YY_ZZ_log
   --json-log data/logs/json/cloud_atrium_XX_YY_ZZ_log.json
+  --icp-log /root/truck/packages/lidar_map/data/results/f_normals
+  --cloud-number 3
+  --percentage 90
 ```

packages/lidar_map/include/lidar_map/builder.h

@@ -52,12 +52,16 @@ struct BuilderParams {
 
 class Builder {
   public:
+    std::vector<CloudWithAttributes> clouds_with_attributes;
+
     Builder(const BuilderParams& params);
 
     std::pair<geom::Poses, Clouds> sliceDataByPosesProximity(
         const geom::Poses& poses, const Clouds& clouds, double poses_min_dist) const;
 
-    void initPoseGraph(const geom::Poses& poses, const Clouds& clouds);
+    void initPoseGraph(
+        const geom::Poses& poses, const Clouds& clouds,
+        const bool get_clouds_with_attributes = false);
 
     geom::Poses optimizePoseGraph(size_t iterations = 1);
 
@@ -74,6 +78,9 @@ class Builder {
         const geom::Poses& poses, const Clouds& clouds_base, double clouds_search_rad,
         size_t min_sim_points_count, double max_sim_points_dist) const;
 
+    CloudWithAttributes getCloudWithAttributes(
+        const DataPoints& reference_dp, const DataPoints& reading_dp, const Cloud& cloud);
+
   private:
     ICP icp_;
     g2o::SparseOptimizer optimizer_;

packages/lidar_map/include/lidar_map/common.h

@@ -18,4 +18,14 @@ using Cloud = Eigen::Matrix4Xf;
  */
 using Clouds = std::vector<Cloud>;
 
+struct CloudAttributes {
+    Eigen::Matrix3Xf normals;
+    Cloud outliers;
+};
+
+struct CloudWithAttributes {
+    Cloud cloud;
+    CloudAttributes attributes;
+};
+
 }  // namespace truck::lidar_map

packages/lidar_map/include/lidar_map/serialization.h

@@ -56,6 +56,10 @@ class MCAPWriter {
         const std::string& mcap_path, const Cloud& cloud, const std::string& topic_name,
         std::string frame_name = "");
 
+    void writeCloudWithAttributes(
+        const std::string& mcap_path, const CloudWithAttributes& cloud_with_attributes,
+        const double percent);
+
   private:
     size_t msg_id_ = 0;
     rosbag2_cpp::Writer writer_;

packages/lidar_map/src/builder.cpp

@@ -179,7 +179,8 @@ std::pair<geom::Poses, Clouds> Builder::sliceDataByPosesProximity(
  * - 'poses': set of clouds' poses in a world frame
  * - 'clouds': set of clouds in correspondig local frames
  */
-void Builder::initPoseGraph(const geom::Poses& poses, const Clouds& clouds) {
+void Builder::initPoseGraph(
+    const geom::Poses& poses, const Clouds& clouds, const bool get_clouds_with_attributes) {
     auto solver = new g2o::OptimizationAlgorithmLevenberg(
         g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
 
@@ -229,6 +230,11 @@ void Builder::initPoseGraph(const geom::Poses& poses, const Clouds& clouds) {
             icp_.transformations.apply(reading_cloud, tf_matrix_odom);
             normalize(reading_cloud);
             const Eigen::Matrix4f tf_matrix_icp = icp_(reading_cloud, reference_cloud);
+            if (get_clouds_with_attributes) {
+                auto cloud_with_attributes =
+                    getCloudWithAttributes(reference_cloud, reading_cloud, clouds[i]);
+                clouds_with_attributes.push_back(cloud_with_attributes);
+            }
             const Eigen::Matrix4f tf_matrix_final = tf_matrix_icp * tf_matrix_odom;
             auto* edge = new g2o::EdgeSE2();
             edge->setVertex(0, vertices[i]);
@@ -371,6 +377,41 @@ Cloud Builder::mergeClouds(const Clouds& clouds) const {
     return merged_cloud;
 }
 
+/**
+ * get normals and outliers for cloud
+ */
+CloudWithAttributes Builder::getCloudWithAttributes(
+    const DataPoints& reference_dp, const DataPoints& reading_dp, const Cloud& cloud) {
+    CloudWithAttributes cloud_with_attributes;
+    cloud_with_attributes.cloud = cloud;
+
+    DataPoints data_points_cloud(reference_dp);
+    icp_.referenceDataPointsFilters.apply(data_points_cloud);
+    BOOST_AUTO(normals, data_points_cloud.getDescriptorViewByName("normals"));
+    CloudAttributes attributes;
+    Eigen::Matrix3Xf normals_matrix(normals.cols(), normals.rows());
+    for (size_t i = 0; i < normals.cols(); i++) {
+        normals_matrix.row(i) = normals.row(i);
+    }
+
+    attributes.normals = normals_matrix;
+    icp_.matcher->init(data_points_cloud);
+    Matcher::Matches matches = icp_.matcher->findClosests(reading_dp);
+    Matcher::OutlierWeights outlierWeights =
+        icp_.outlierFilters.compute(reading_dp, data_points_cloud, matches);
+    Cloud outliers(4, outlierWeights.cols());
+    for (size_t i = 0; i < outlierWeights.cols(); i++) {
+        outliers(0, i) = reading_dp.features(0, i);
+        outliers(1, i) = reading_dp.features(1, i);
+        outliers(2, i) = reading_dp.features(2, i);
+        outliers(3, i) = outlierWeights(0, i);
+    }
+
+    attributes.outliers = outliers;
+    cloud_with_attributes.attributes = attributes;
+    return cloud_with_attributes;
+}
+
 namespace {
 
 std::vector<size_t> findNearestIdsInsideBox(

packages/lidar_map/src/main.cpp

@@ -29,6 +29,9 @@ int main(int argc, char* argv[]) {
     std::string mcap_output_folder_path;
     std::string mcap_log_folder_path;
     std::string json_log_path;
+    std::string icp_log_path;
+    int icp_cloud_number;
+    int normals_rarefaction_percentage;
 
     {
         po::options_description desc("Executable for constructing 2D LiDAR map");
@@ -44,7 +47,16 @@ int main(int argc, char* argv[]) {
             "path to NON-EXISTING folder for saving map logs")(
             "json-log",
             po::value<std::string>(&json_log_path)->default_value(""),
-            "path to json file for saving map logs");
+            "path to json file for saving map logs")(
+            "icp-log,il",
+            po::value<std::string>(&icp_log_path)->default_value(""),
+            "path to NON-EXISTING folder for saving ICP log file (normals and outliers) ")(
+            "cloud_number,cn",
+            po::value<int>(&icp_cloud_number)->default_value(1),
+            "cloud number for visualizing normals and outliers for icp-log")(
+            "percentage,p",
+            po::value<int>(&normals_rarefaction_percentage)->default_value(100),
+            "percentage of normals rendered [0;100] for icp-log");
 
         po::variables_map vm;
         try {
@@ -119,7 +131,7 @@ int main(int argc, char* argv[]) {
 
     // 2. Construct and optimize pose graph
     {
-        builder.initPoseGraph(poses, clouds);
+        builder.initPoseGraph(poses, clouds, !icp_log_path.empty());
 
         if (enable_mcap_log) {
             const Cloud lidar_map_on_iteration =
@@ -157,5 +169,12 @@ int main(int argc, char* argv[]) {
 
         const auto lidar_map = builder.mergeClouds(builder.transformClouds(poses, clouds));
         writer::MCAPWriter::writeCloud(mcap_output_folder_path, lidar_map, kOutputTopicLidarMap);
+
+        if (!icp_log_path.empty()) {
+            mcap_writer.writeCloudWithAttributes(
+                icp_log_path,
+                builder.clouds_with_attributes[icp_cloud_number],
+                normals_rarefaction_percentage);
+        }
     }
 }

packages/lidar_map/src/serialization.cpp

@@ -297,6 +297,65 @@ void MCAPWriter::writeCloud(
     writer.write(msg::toPointCloud2(cloud, frame_name), topic_name, getTime());
 }
 
+void MCAPWriter::writeCloudWithAttributes(
+    const std::string& mcap_path, const CloudWithAttributes& cloud_with_attributes,
+    const double percent) {
+    rosbag2_cpp::Writer writer;
+    writer.open(mcap_path);
+    writer.write(msg::toPointCloud2(cloud_with_attributes.cloud, "world"), "cloud", getTime());
+
+    auto get_color = [](double a = 0.5, double r = 1.0, double g = 0.0, double b = 0.0) {
+        std_msgs::msg::ColorRGBA color;
+        color.a = a;
+        color.r = r;
+        color.g = g;
+        color.b = b;
+        return color;
+    };
+
+    auto get_scale = [](double x = 0.6, double y = 0.06, double z = 0.06) {
+        geometry_msgs::msg::Vector3 scale;
+        scale.x = x;
+        scale.y = y;
+        scale.z = z;
+        return scale;
+    };
+
+    visualization_msgs::msg::MarkerArray msg_array;
+    size_t points_count = cloud_with_attributes.cloud.cols();
+    size_t step = static_cast<size_t>(points_count / (points_count * (1 - (percent / 100))));
+    for (size_t i = 0; i < points_count; i += step) {
+        visualization_msgs::msg::Marker msg_;
+        msg_.header.frame_id = "world";
+        msg_.id = i;
+        msg_.type = visualization_msgs::msg::Marker::ARROW;
+        msg_.action = visualization_msgs::msg::Marker::ADD;
+        msg_.color = get_color();
+
+        msg_.pose.position.x = cloud_with_attributes.cloud(0, i);
+        msg_.pose.position.y = cloud_with_attributes.cloud(1, i);
+        msg_.pose.position.z = cloud_with_attributes.cloud(2, i);
+
+        double dir_x =
+            cloud_with_attributes.attributes.normals(4, i) - cloud_with_attributes.cloud(0, i);
+        double dir_y =
+            cloud_with_attributes.attributes.normals(5, i) - cloud_with_attributes.cloud(1, i);
+
+        msg_.scale = get_scale();
+
+        double yaw = std::atan2(dir_y, dir_x);
+        msg_.pose.orientation = geom::msg::toQuaternion(truck::geom::Angle(yaw));
+
+        msg_array.markers.push_back(msg_);
+    }
+
+    writer.write(msg_array, "normals", getTime());
+    writer.write(
+        msg::toPointCloud2(cloud_with_attributes.attributes.outliers, "world"),
+        "outliers",
+        getTime());
+}
+
 }  // namespace writer
 
 }  // namespace truck::lidar_map