From 86170c573a33ed4a71138b19a19d108a6fe51568 Mon Sep 17 00:00:00 2001
From: PostRed <s21v_kolomnikova@179.ru>
Date: Wed, 5 Feb 2025 12:57:16 +0000
Subject: [PATCH 1/8] add normals and outliers

---
 packages/lidar_map/README.md                  | 12 ++++
 .../lidar_map/include/lidar_map/builder.h     |  9 ++-
 packages/lidar_map/include/lidar_map/common.h | 10 ++++
 .../include/lidar_map/serialization.h         |  4 ++
 packages/lidar_map/src/builder.cpp            | 43 +++++++++++++-
 packages/lidar_map/src/main.cpp               | 23 +++++++-
 packages/lidar_map/src/serialization.cpp      | 59 +++++++++++++++++++
 7 files changed, 156 insertions(+), 4 deletions(-)

diff --git a/packages/lidar_map/README.md b/packages/lidar_map/README.md
index 3406b1c0..ca3ab5e1 100644
--- a/packages/lidar_map/README.md
+++ b/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
 ```
diff --git a/packages/lidar_map/include/lidar_map/builder.h b/packages/lidar_map/include/lidar_map/builder.h
index fb36df2d..cc86e7d3 100644
--- a/packages/lidar_map/include/lidar_map/builder.h
+++ b/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_;
diff --git a/packages/lidar_map/include/lidar_map/common.h b/packages/lidar_map/include/lidar_map/common.h
index 6213214f..91e01c5e 100644
--- a/packages/lidar_map/include/lidar_map/common.h
+++ b/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
diff --git a/packages/lidar_map/include/lidar_map/serialization.h b/packages/lidar_map/include/lidar_map/serialization.h
index 7ee4b2a8..4f45a5b4 100644
--- a/packages/lidar_map/include/lidar_map/serialization.h
+++ b/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_;
diff --git a/packages/lidar_map/src/builder.cpp b/packages/lidar_map/src/builder.cpp
index 5e4eadfb..a18a46d6 100644
--- a/packages/lidar_map/src/builder.cpp
+++ b/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(
diff --git a/packages/lidar_map/src/main.cpp b/packages/lidar_map/src/main.cpp
index 08647743..05e305c1 100644
--- a/packages/lidar_map/src/main.cpp
+++ b/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);
+        }
     }
 }
diff --git a/packages/lidar_map/src/serialization.cpp b/packages/lidar_map/src/serialization.cpp
index e3d9dafb..0742e5c9 100644
--- a/packages/lidar_map/src/serialization.cpp
+++ b/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

From 37723066f56690fd0662a326cbf87737217cecb5 Mon Sep 17 00:00:00 2001
From: PostRed <s21v_kolomnikova@179.ru>
Date: Tue, 11 Feb 2025 15:18:40 +0000
Subject: [PATCH 2/8] start pr fixes

---
 packages/lidar_map/README.md                  | 12 +----
 .../lidar_map/include/lidar_map/builder.h     |  7 ++-
 .../include/lidar_map/serialization.h         |  3 +-
 packages/lidar_map/src/builder.cpp            | 50 ++++++++-----------
 packages/lidar_map/src/main.cpp               | 33 ++++--------
 packages/lidar_map/src/serialization.cpp      | 10 ++--
 6 files changed, 44 insertions(+), 71 deletions(-)

diff --git a/packages/lidar_map/README.md b/packages/lidar_map/README.md
index ca3ab5e1..b88c259a 100644
--- a/packages/lidar_map/README.md
+++ b/packages/lidar_map/README.md
@@ -23,12 +23,6 @@ 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
@@ -46,15 +40,11 @@ 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
 ```
+### Debug
diff --git a/packages/lidar_map/include/lidar_map/builder.h b/packages/lidar_map/include/lidar_map/builder.h
index 40c97046..173e4a26 100644
--- a/packages/lidar_map/include/lidar_map/builder.h
+++ b/packages/lidar_map/include/lidar_map/builder.h
@@ -60,8 +60,7 @@ class Builder {
         const geom::Poses& poses, const Clouds& clouds, double poses_min_dist) const;
 
     void initPoseGraph(
-        const geom::Poses& poses, const Clouds& clouds,
-        const bool get_clouds_with_attributes = false);
+        const geom::Poses& poses, const Clouds& clouds);
 
     geom::Poses optimizePoseGraph(size_t iterations = 1);
 
@@ -78,8 +77,8 @@ 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);
+    CloudWithAttributes calculateAttributesForReferenceCloud(
+    const Cloud& reference_cloud, const Cloud& reading_cloud);
 
   private:
     ICP icp_;
diff --git a/packages/lidar_map/include/lidar_map/serialization.h b/packages/lidar_map/include/lidar_map/serialization.h
index f53c25e0..74ff5a43 100644
--- a/packages/lidar_map/include/lidar_map/serialization.h
+++ b/packages/lidar_map/include/lidar_map/serialization.h
@@ -65,7 +65,8 @@ class MCAPWriter {
 
     void writeCloudWithAttributes(
         const std::string& mcap_path, const CloudWithAttributes& cloud_with_attributes,
-        const double percent);
+        const double percent,
+        std::string frame_name = "");
 
   private:
     size_t msg_id_ = 0;
diff --git a/packages/lidar_map/src/builder.cpp b/packages/lidar_map/src/builder.cpp
index 11dd7544..349fd7e6 100644
--- a/packages/lidar_map/src/builder.cpp
+++ b/packages/lidar_map/src/builder.cpp
@@ -180,7 +180,7 @@ std::pair<geom::Poses, Clouds> Builder::sliceDataByPosesProximity(
  * - 'clouds': set of clouds in correspondig local frames
  */
 void Builder::initPoseGraph(
-    const geom::Poses& poses, const Clouds& clouds, const bool get_clouds_with_attributes) {
+    const geom::Poses& poses, const Clouds& clouds) {
     auto solver = new g2o::OptimizationAlgorithmLevenberg(
         g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
 
@@ -230,11 +230,6 @@ void Builder::initPoseGraph(
             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]);
@@ -380,35 +375,34 @@ Cloud Builder::mergeClouds(const Clouds& clouds) {
 /**
  * get normals and outliers for cloud
  */
-CloudWithAttributes Builder::getCloudWithAttributes(
-    const DataPoints& reference_dp, const DataPoints& reading_dp, const Cloud& cloud) {
+CloudWithAttributes  Builder::calculateAttributesForReferenceCloud(
+    const Cloud& reference_cloud, const Cloud& reading_cloud) {
+    DataPoints reference_dp = toDataPoints(reference_cloud);
+    DataPoints reading_dp = toDataPoints(reading_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);
-    }
+    cloud_with_attributes.cloud = reference_cloud;
 
-    attributes.normals = normals_matrix;
-    icp_.matcher->init(data_points_cloud);
+    icp_.referenceDataPointsFilters.apply(reference_dp);
+    
+    // normals = matrix 3 * n
+    // normals_x, normals_y, normals_z - components of the normal vector that indicate the direction perpendicular
+    // to the surface passing through the point.
+    cloud_with_attributes.attributes.normals = reference_dp.getDescriptorViewByName("normals");
+
+    icp_.matcher->init(reference_dp);
     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());
+        icp_.outlierFilters.compute(reading_dp, reference_dp, matches);
+
+    cloud_with_attributes.attributes.outliers = Cloud(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);
+        cloud_with_attributes.attributes.outliers(0, i) = reading_dp.features(0, i);
+        cloud_with_attributes.attributes.outliers(1, i) = reading_dp.features(1, i);
+        cloud_with_attributes.attributes.outliers(2, i) = reading_dp.features(2, i);
+        cloud_with_attributes.attributes.outliers(3, i) = outlierWeights(0, i);
     }
 
-    attributes.outliers = outliers;
-    cloud_with_attributes.attributes = attributes;
     return cloud_with_attributes;
 }
 
diff --git a/packages/lidar_map/src/main.cpp b/packages/lidar_map/src/main.cpp
index b9b50321..908c324d 100644
--- a/packages/lidar_map/src/main.cpp
+++ b/packages/lidar_map/src/main.cpp
@@ -20,6 +20,7 @@ const std::string kInputTopicOdom = "/ekf/odometry/filtered";
 const std::string kOutputTopicLidarMap = "/lidar_map";
 const std::string kOutputTopicPoses = "/poses";
 const std::string kOutputTopicClouds = "/clouds";
+const std::string logsFrameName = "world";
 const std::string kPkgPathLidarMap = ament_index_cpp::get_package_share_directory("lidar_map");
 
 int main(int argc, char* argv[]) {
@@ -31,9 +32,6 @@ 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");
@@ -49,16 +47,7 @@ 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")(
-            "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");
+            "path to json file for saving map logs");
 
         po::variables_map vm;
         try {
@@ -103,7 +92,8 @@ int main(int argc, char* argv[]) {
             serialization::writer::MCAPWriterParams{
                 .mcap_path = mcap_log_folder_path,
                 .poses_topic_name = kOutputTopicPoses,
-                .cloud_topic_name = kOutputTopicClouds});
+                .cloud_topic_name = kOutputTopicClouds,
+                .frame_name = logsFrameName});
     }
 
     Poses poses;
@@ -130,7 +120,7 @@ int main(int argc, char* argv[]) {
 
     // 2. Construct and optimize pose graph
     {
-        builder.initPoseGraph(poses, clouds, !icp_log_path.empty());
+        builder.initPoseGraph(poses, clouds);
 
         if (enable_mcap_log) {
             const Cloud lidar_map_on_iteration =
@@ -170,12 +160,11 @@ int main(int argc, char* argv[]) {
 
         serialization::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);
-        }
+        auto cloud_with_attributes = builder.calculateAttributesForReferenceCloud(clouds[0], clouds[1]);
+        std::cout << cloud_with_attributes.cloud.cols() << " " <<  cloud_with_attributes.attributes.outliers.rows() << " " << cloud_with_attributes.attributes.normals.cols() << '\n';
+        mcap_writer->writeCloudWithAttributes(
+                "/root/truck/packages/lidar_map/data/results/oo2",
+                cloud_with_attributes,
+                70);
     }
 }
diff --git a/packages/lidar_map/src/serialization.cpp b/packages/lidar_map/src/serialization.cpp
index 61c1ae4f..11c9a311 100644
--- a/packages/lidar_map/src/serialization.cpp
+++ b/packages/lidar_map/src/serialization.cpp
@@ -347,7 +347,7 @@ void MCAPWriter::writeCloud(
 
 void MCAPWriter::writeCloudWithAttributes(
     const std::string& mcap_path, const CloudWithAttributes& cloud_with_attributes,
-    const double percent) {
+    const double percent, std::string frame_name) {
     rosbag2_cpp::Writer writer;
     writer.open(mcap_path);
     writer.write(msg::toPointCloud2(cloud_with_attributes.cloud, "world"), "cloud", getTime());
@@ -384,13 +384,13 @@ void MCAPWriter::writeCloudWithAttributes(
         msg_.pose.position.y = cloud_with_attributes.cloud(1, i);
         msg_.pose.position.z = cloud_with_attributes.cloud(2, i);
 
+        // Get direction vector components for the normal direction
         double dir_x =
-            cloud_with_attributes.attributes.normals(4, i) - cloud_with_attributes.cloud(0, i);
+            cloud_with_attributes.attributes.normals(0, i) - cloud_with_attributes.cloud(0, i);
         double dir_y =
-            cloud_with_attributes.attributes.normals(5, i) - cloud_with_attributes.cloud(1, i);
-
+            cloud_with_attributes.attributes.normals(1, i) - cloud_with_attributes.cloud(1, i);
         msg_.scale = get_scale();
-
+        // Get yaw angle from the direction vector using atan2
         double yaw = std::atan2(dir_y, dir_x);
         msg_.pose.orientation = geom::msg::toQuaternion(truck::geom::Angle(yaw));
 

From 118704d8d3c37401cbf417a32b634865e12b25a1 Mon Sep 17 00:00:00 2001
From: PostRed <s21v_kolomnikova@179.ru>
Date: Tue, 11 Feb 2025 16:08:44 +0000
Subject: [PATCH 3/8] finish pr fixes

---
 packages/lidar_map/README.md                  | 14 +++
 .../lidar_map/include/lidar_map/builder.h     |  5 +-
 packages/lidar_map/include/lidar_map/common.h |  2 +-
 .../include/lidar_map/serialization.h         |  2 +-
 packages/lidar_map/src/builder.cpp            | 13 ++-
 packages/lidar_map/src/main.cpp               | 11 +--
 packages/lidar_map/src/serialization.cpp      | 88 ++++++++-----------
 7 files changed, 63 insertions(+), 72 deletions(-)

diff --git a/packages/lidar_map/README.md b/packages/lidar_map/README.md
index b88c259a..cb7f1c60 100644
--- a/packages/lidar_map/README.md
+++ b/packages/lidar_map/README.md
@@ -48,3 +48,17 @@ ros2 run lidar_map lidar_map_executable
   --json-log data/logs/json/cloud_atrium_XX_YY_ZZ_log.json
 ```
 ### Debug
+
+To visualize the normals, add this to main.cpp.
+
+```c++
+//Calculate attributes for the reference cloud using i and next cloud
+auto cloud_with_attributes = builder.calculateAttributesForReferenceCloud(clouds[i], clouds[i + 1]);
+mcap_writer->writeCloudWithAttributes(
+        "data/logs/icp_logs", // path where the mcap file will be saved
+        cloud_with_attributes,
+        70, // percentage of normals to be visualized
+        true, // true to write outliers
+        true, // true to write normals
+        "world"); // frame name
+```
diff --git a/packages/lidar_map/include/lidar_map/builder.h b/packages/lidar_map/include/lidar_map/builder.h
index 173e4a26..0852bbe0 100644
--- a/packages/lidar_map/include/lidar_map/builder.h
+++ b/packages/lidar_map/include/lidar_map/builder.h
@@ -59,8 +59,7 @@ class Builder {
     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);
 
     geom::Poses optimizePoseGraph(size_t iterations = 1);
 
@@ -78,7 +77,7 @@ class Builder {
         size_t min_sim_points_count, double max_sim_points_dist) const;
 
     CloudWithAttributes calculateAttributesForReferenceCloud(
-    const Cloud& reference_cloud, const Cloud& reading_cloud);
+        const Cloud& reference_cloud, const Cloud& reading_cloud);
 
   private:
     ICP icp_;
diff --git a/packages/lidar_map/include/lidar_map/common.h b/packages/lidar_map/include/lidar_map/common.h
index 91e01c5e..e3e4d217 100644
--- a/packages/lidar_map/include/lidar_map/common.h
+++ b/packages/lidar_map/include/lidar_map/common.h
@@ -20,7 +20,7 @@ using Clouds = std::vector<Cloud>;
 
 struct CloudAttributes {
     Eigen::Matrix3Xf normals;
-    Cloud outliers;
+    Eigen::Matrix4Xf outliers;
 };
 
 struct CloudWithAttributes {
diff --git a/packages/lidar_map/include/lidar_map/serialization.h b/packages/lidar_map/include/lidar_map/serialization.h
index 74ff5a43..3ed87223 100644
--- a/packages/lidar_map/include/lidar_map/serialization.h
+++ b/packages/lidar_map/include/lidar_map/serialization.h
@@ -65,7 +65,7 @@ class MCAPWriter {
 
     void writeCloudWithAttributes(
         const std::string& mcap_path, const CloudWithAttributes& cloud_with_attributes,
-        const double percent,
+        bool enable_weights = true, bool enable_normals = true, double normals_ratio = 0.5,
         std::string frame_name = "");
 
   private:
diff --git a/packages/lidar_map/src/builder.cpp b/packages/lidar_map/src/builder.cpp
index 349fd7e6..a8f26bbf 100644
--- a/packages/lidar_map/src/builder.cpp
+++ b/packages/lidar_map/src/builder.cpp
@@ -179,8 +179,7 @@ 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) {
     auto solver = new g2o::OptimizationAlgorithmLevenberg(
         g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
 
@@ -375,7 +374,7 @@ Cloud Builder::mergeClouds(const Clouds& clouds) {
 /**
  * get normals and outliers for cloud
  */
-CloudWithAttributes  Builder::calculateAttributesForReferenceCloud(
+CloudWithAttributes Builder::calculateAttributesForReferenceCloud(
     const Cloud& reference_cloud, const Cloud& reading_cloud) {
     DataPoints reference_dp = toDataPoints(reference_cloud);
     DataPoints reading_dp = toDataPoints(reading_cloud);
@@ -384,10 +383,10 @@ CloudWithAttributes  Builder::calculateAttributesForReferenceCloud(
     cloud_with_attributes.cloud = reference_cloud;
 
     icp_.referenceDataPointsFilters.apply(reference_dp);
-    
+
     // normals = matrix 3 * n
-    // normals_x, normals_y, normals_z - components of the normal vector that indicate the direction perpendicular
-    // to the surface passing through the point.
+    // normals_x, normals_y, normals_z - components of the normal vector that indicate the direction
+    // perpendicular to the surface passing through the point.
     cloud_with_attributes.attributes.normals = reference_dp.getDescriptorViewByName("normals");
 
     icp_.matcher->init(reference_dp);
@@ -395,7 +394,7 @@ CloudWithAttributes  Builder::calculateAttributesForReferenceCloud(
     Matcher::OutlierWeights outlierWeights =
         icp_.outlierFilters.compute(reading_dp, reference_dp, matches);
 
-    cloud_with_attributes.attributes.outliers = Cloud(4, outlierWeights.cols());
+    cloud_with_attributes.attributes.outliers = Eigen::Matrix4Xf(4, outlierWeights.cols());
     for (size_t i = 0; i < outlierWeights.cols(); i++) {
         cloud_with_attributes.attributes.outliers(0, i) = reading_dp.features(0, i);
         cloud_with_attributes.attributes.outliers(1, i) = reading_dp.features(1, i);
diff --git a/packages/lidar_map/src/main.cpp b/packages/lidar_map/src/main.cpp
index 908c324d..b6341591 100644
--- a/packages/lidar_map/src/main.cpp
+++ b/packages/lidar_map/src/main.cpp
@@ -20,7 +20,6 @@ const std::string kInputTopicOdom = "/ekf/odometry/filtered";
 const std::string kOutputTopicLidarMap = "/lidar_map";
 const std::string kOutputTopicPoses = "/poses";
 const std::string kOutputTopicClouds = "/clouds";
-const std::string logsFrameName = "world";
 const std::string kPkgPathLidarMap = ament_index_cpp::get_package_share_directory("lidar_map");
 
 int main(int argc, char* argv[]) {
@@ -92,8 +91,7 @@ int main(int argc, char* argv[]) {
             serialization::writer::MCAPWriterParams{
                 .mcap_path = mcap_log_folder_path,
                 .poses_topic_name = kOutputTopicPoses,
-                .cloud_topic_name = kOutputTopicClouds,
-                .frame_name = logsFrameName});
+                .cloud_topic_name = kOutputTopicClouds});
     }
 
     Poses poses;
@@ -160,11 +158,4 @@ int main(int argc, char* argv[]) {
 
         serialization::writer::MCAPWriter::writeCloud(
             mcap_output_folder_path, lidar_map, kOutputTopicLidarMap);
-        auto cloud_with_attributes = builder.calculateAttributesForReferenceCloud(clouds[0], clouds[1]);
-        std::cout << cloud_with_attributes.cloud.cols() << " " <<  cloud_with_attributes.attributes.outliers.rows() << " " << cloud_with_attributes.attributes.normals.cols() << '\n';
-        mcap_writer->writeCloudWithAttributes(
-                "/root/truck/packages/lidar_map/data/results/oo2",
-                cloud_with_attributes,
-                70);
     }
-}
diff --git a/packages/lidar_map/src/serialization.cpp b/packages/lidar_map/src/serialization.cpp
index 11c9a311..347503ca 100644
--- a/packages/lidar_map/src/serialization.cpp
+++ b/packages/lidar_map/src/serialization.cpp
@@ -347,61 +347,49 @@ void MCAPWriter::writeCloud(
 
 void MCAPWriter::writeCloudWithAttributes(
     const std::string& mcap_path, const CloudWithAttributes& cloud_with_attributes,
-    const double percent, std::string frame_name) {
+    bool enable_weights, bool enable_normals, double normals_ratio, std::string frame_name) {
     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;
-    };
+    writer.write(msg::toPointCloud2(cloud_with_attributes.cloud, frame_name), "cloud", getTime());
+    if (enable_normals) {
+        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 - (normals_ratio / 100))));
+        for (size_t i = 0; i < points_count; i += step) {
+            visualization_msgs::msg::Marker msg_;
+            msg_.header.frame_id = frame_name;
+            msg_.id = i;
+            msg_.type = visualization_msgs::msg::Marker::ARROW;
+            msg_.action = visualization_msgs::msg::Marker::ADD;
+            msg_.color = toColorRGBA(0.5, 1, 0, 0);
+
+            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);
+
+            // Get direction vector components for the normal direction
+            double dir_x =
+                cloud_with_attributes.attributes.normals(0, i) - cloud_with_attributes.cloud(0, i);
+            double dir_y =
+                cloud_with_attributes.attributes.normals(1, i) - cloud_with_attributes.cloud(1, i);
+            msg_.scale = toVector3(0.6, 0.06, 0.06);
+            // Get yaw angle from the direction vector using atan2
+            double yaw = std::atan2(dir_y, dir_x);
+            msg_.pose.orientation = geom::msg::toQuaternion(truck::geom::Angle(yaw));
+
+            msg_array.markers.push_back(msg_);
+        }
 
-    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);
-
-        // Get direction vector components for the normal direction
-        double dir_x =
-            cloud_with_attributes.attributes.normals(0, i) - cloud_with_attributes.cloud(0, i);
-        double dir_y =
-            cloud_with_attributes.attributes.normals(1, i) - cloud_with_attributes.cloud(1, i);
-        msg_.scale = get_scale();
-        // Get yaw angle from the direction vector using atan2
-        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_array, "normals", getTime());
-    writer.write(
-        msg::toPointCloud2(cloud_with_attributes.attributes.outliers, "world"),
-        "outliers",
-        getTime());
+    if (enable_weights) {
+        writer.write(
+            msg::toPointCloud2(cloud_with_attributes.attributes.outliers, frame_name),
+            "outliers",
+            getTime());
+    }
 }
 
 }  // namespace writer

From 96942c47b319401914a15e57116ef5aa134f71cb Mon Sep 17 00:00:00 2001
From: PostRed <s21v_kolomnikova@179.ru>
Date: Tue, 11 Feb 2025 16:16:38 +0000
Subject: [PATCH 4/8] remove lines

---
 packages/lidar_map/README.md    | 1 -
 packages/lidar_map/src/main.cpp | 1 +
 2 files changed, 1 insertion(+), 1 deletion(-)

diff --git a/packages/lidar_map/README.md b/packages/lidar_map/README.md
index cb7f1c60..e3fe7e6c 100644
--- a/packages/lidar_map/README.md
+++ b/packages/lidar_map/README.md
@@ -23,7 +23,6 @@ Parameters:
 
 4. `--json-log`: path to json file for saving map logs (optional)
 
-
 Run the executable `lidar_map_executable` with the assigned `--mcap-input` and `--mcap-output` arguments:
 ```console
 ros2 run lidar_map lidar_map_executable
diff --git a/packages/lidar_map/src/main.cpp b/packages/lidar_map/src/main.cpp
index b6341591..2c74f88c 100644
--- a/packages/lidar_map/src/main.cpp
+++ b/packages/lidar_map/src/main.cpp
@@ -159,3 +159,4 @@ int main(int argc, char* argv[]) {
         serialization::writer::MCAPWriter::writeCloud(
             mcap_output_folder_path, lidar_map, kOutputTopicLidarMap);
     }
+}

From 49da572e9756fed6aae1a6da45ebef849e136cf6 Mon Sep 17 00:00:00 2001
From: PostRed <s21v_kolomnikova@179.ru>
Date: Wed, 12 Feb 2025 10:43:30 +0000
Subject: [PATCH 5/8] add two functions for normals and weights

---
 .../lidar_map/include/lidar_map/builder.h     |  6 ++--
 packages/lidar_map/include/lidar_map/common.h | 12 +++----
 .../include/lidar_map/serialization.h         |  5 ++-
 packages/lidar_map/src/builder.cpp            | 33 +++++++++----------
 packages/lidar_map/src/serialization.cpp      | 27 ++++++++-------
 5 files changed, 43 insertions(+), 40 deletions(-)

diff --git a/packages/lidar_map/include/lidar_map/builder.h b/packages/lidar_map/include/lidar_map/builder.h
index 0852bbe0..2fb32a5b 100644
--- a/packages/lidar_map/include/lidar_map/builder.h
+++ b/packages/lidar_map/include/lidar_map/builder.h
@@ -76,8 +76,10 @@ 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 calculateAttributesForReferenceCloud(
-        const Cloud& reference_cloud, const Cloud& reading_cloud);
+    Eigen::VectorXf calculateWeightsForReadingCloud(
+        const Cloud& reading_cloud, const Cloud& reference_cloud);
+
+    Eigen::Matrix3Xf calculateNormalsForReferenceCloud(const Cloud& reference_cloud);
 
   private:
     ICP icp_;
diff --git a/packages/lidar_map/include/lidar_map/common.h b/packages/lidar_map/include/lidar_map/common.h
index e3e4d217..2864a9ec 100644
--- a/packages/lidar_map/include/lidar_map/common.h
+++ b/packages/lidar_map/include/lidar_map/common.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <pointmatcher/PointMatcher.h>
+#include <optional>
 
 namespace truck::lidar_map {
 
@@ -18,14 +19,13 @@ using Cloud = Eigen::Matrix4Xf;
  */
 using Clouds = std::vector<Cloud>;
 
-struct CloudAttributes {
-    Eigen::Matrix3Xf normals;
-    Eigen::Matrix4Xf outliers;
-};
-
 struct CloudWithAttributes {
     Cloud cloud;
-    CloudAttributes attributes;
+    std::optional<Eigen::VectorXf> weights = std::nullopt;
+
+    // normals_x, normals_y, normals_z - components of the normal vector that indicate the direction
+    // perpendicular to the surface passing through the point.
+    std::optional<Eigen::Matrix3Xf> normals = std::nullopt;
 };
 
 }  // namespace truck::lidar_map
diff --git a/packages/lidar_map/include/lidar_map/serialization.h b/packages/lidar_map/include/lidar_map/serialization.h
index 3ed87223..b1d5f788 100644
--- a/packages/lidar_map/include/lidar_map/serialization.h
+++ b/packages/lidar_map/include/lidar_map/serialization.h
@@ -63,10 +63,9 @@ class MCAPWriter {
         const std::string& mcap_path, const Cloud& cloud, const std::string& topic_name,
         std::string frame_name = "");
 
-    void writeCloudWithAttributes(
+    static void writeCloudWithAttributes(
         const std::string& mcap_path, const CloudWithAttributes& cloud_with_attributes,
-        bool enable_weights = true, bool enable_normals = true, double normals_ratio = 0.5,
-        std::string frame_name = "");
+        double normals_ratio = 0.5, std::string frame_name = "");
 
   private:
     size_t msg_id_ = 0;
diff --git a/packages/lidar_map/src/builder.cpp b/packages/lidar_map/src/builder.cpp
index a8f26bbf..9f0d6c13 100644
--- a/packages/lidar_map/src/builder.cpp
+++ b/packages/lidar_map/src/builder.cpp
@@ -372,37 +372,34 @@ Cloud Builder::mergeClouds(const Clouds& clouds) {
 }
 
 /**
- * get normals and outliers for cloud
+ * get outliers weights for cloud
  */
-CloudWithAttributes Builder::calculateAttributesForReferenceCloud(
-    const Cloud& reference_cloud, const Cloud& reading_cloud) {
+Eigen::VectorXf Builder::calculateWeightsForReadingCloud(
+    const Cloud& reading_cloud, const Cloud& reference_cloud) {
     DataPoints reference_dp = toDataPoints(reference_cloud);
     DataPoints reading_dp = toDataPoints(reading_cloud);
 
-    CloudWithAttributes cloud_with_attributes;
-    cloud_with_attributes.cloud = reference_cloud;
-
     icp_.referenceDataPointsFilters.apply(reference_dp);
-
-    // normals = matrix 3 * n
-    // normals_x, normals_y, normals_z - components of the normal vector that indicate the direction
-    // perpendicular to the surface passing through the point.
-    cloud_with_attributes.attributes.normals = reference_dp.getDescriptorViewByName("normals");
-
     icp_.matcher->init(reference_dp);
     Matcher::Matches matches = icp_.matcher->findClosests(reading_dp);
     Matcher::OutlierWeights outlierWeights =
         icp_.outlierFilters.compute(reading_dp, reference_dp, matches);
 
-    cloud_with_attributes.attributes.outliers = Eigen::Matrix4Xf(4, outlierWeights.cols());
+    Eigen::VectorXf weights(outlierWeights.cols());
     for (size_t i = 0; i < outlierWeights.cols(); i++) {
-        cloud_with_attributes.attributes.outliers(0, i) = reading_dp.features(0, i);
-        cloud_with_attributes.attributes.outliers(1, i) = reading_dp.features(1, i);
-        cloud_with_attributes.attributes.outliers(2, i) = reading_dp.features(2, i);
-        cloud_with_attributes.attributes.outliers(3, i) = outlierWeights(0, i);
+        weights(i) = outlierWeights(0, i);
     }
 
-    return cloud_with_attributes;
+    return weights;
+}
+
+/**
+ * get normals for cloud
+ */
+Eigen::Matrix3Xf Builder::calculateNormalsForReferenceCloud(const Cloud& reference_cloud) {
+    DataPoints reference_dp = toDataPoints(reference_cloud);
+    icp_.referenceDataPointsFilters.apply(reference_dp);
+    return reference_dp.getDescriptorViewByName("normals");
 }
 
 namespace {
diff --git a/packages/lidar_map/src/serialization.cpp b/packages/lidar_map/src/serialization.cpp
index 347503ca..ece2ddf5 100644
--- a/packages/lidar_map/src/serialization.cpp
+++ b/packages/lidar_map/src/serialization.cpp
@@ -347,11 +347,14 @@ void MCAPWriter::writeCloud(
 
 void MCAPWriter::writeCloudWithAttributes(
     const std::string& mcap_path, const CloudWithAttributes& cloud_with_attributes,
-    bool enable_weights, bool enable_normals, double normals_ratio, std::string frame_name) {
+    double normals_ratio, std::string frame_name) {
     rosbag2_cpp::Writer writer;
     writer.open(mcap_path);
     writer.write(msg::toPointCloud2(cloud_with_attributes.cloud, frame_name), "cloud", getTime());
-    if (enable_normals) {
+    std::cout << cloud_with_attributes.normals.has_value() << ' '
+              << cloud_with_attributes.weights.has_value() << '\n';
+    if (cloud_with_attributes.normals.has_value()) {
+        const Eigen::Matrix3Xf normals = cloud_with_attributes.normals.value();
         visualization_msgs::msg::MarkerArray msg_array;
         size_t points_count = cloud_with_attributes.cloud.cols();
         size_t step =
@@ -369,10 +372,8 @@ void MCAPWriter::writeCloudWithAttributes(
             msg_.pose.position.z = cloud_with_attributes.cloud(2, i);
 
             // Get direction vector components for the normal direction
-            double dir_x =
-                cloud_with_attributes.attributes.normals(0, i) - cloud_with_attributes.cloud(0, i);
-            double dir_y =
-                cloud_with_attributes.attributes.normals(1, i) - cloud_with_attributes.cloud(1, i);
+            double dir_x = normals(0, i) - cloud_with_attributes.cloud(0, i);
+            double dir_y = normals(1, i) - cloud_with_attributes.cloud(1, i);
             msg_.scale = toVector3(0.6, 0.06, 0.06);
             // Get yaw angle from the direction vector using atan2
             double yaw = std::atan2(dir_y, dir_x);
@@ -384,11 +385,15 @@ void MCAPWriter::writeCloudWithAttributes(
         writer.write(msg_array, "normals", getTime());
     }
 
-    if (enable_weights) {
-        writer.write(
-            msg::toPointCloud2(cloud_with_attributes.attributes.outliers, frame_name),
-            "outliers",
-            getTime());
+    if (cloud_with_attributes.weights.has_value()) {
+        Cloud cloud = cloud_with_attributes.cloud;
+        const Eigen::VectorXf& weights = cloud_with_attributes.weights.value();
+
+        for (size_t i = 0; i < cloud.cols(); i++) {
+            cloud(3, i) = weights(i);
+        }
+
+        writer.write(msg::toPointCloud2(cloud, frame_name), "weights", getTime());
     }
 }
 

From eb73e9a77ce68a540efc6c4bfdf500dfec03a083 Mon Sep 17 00:00:00 2001
From: PostRed <s21v_kolomnikova@179.ru>
Date: Wed, 12 Feb 2025 10:43:58 +0000
Subject: [PATCH 6/8] fix

---
 packages/lidar_map/README.md | 33 +++++++++++++++++++++++----------
 1 file changed, 23 insertions(+), 10 deletions(-)

diff --git a/packages/lidar_map/README.md b/packages/lidar_map/README.md
index e3fe7e6c..7e544b3b 100644
--- a/packages/lidar_map/README.md
+++ b/packages/lidar_map/README.md
@@ -48,16 +48,29 @@ ros2 run lidar_map lidar_map_executable
 ```
 ### Debug
 
-To visualize the normals, add this to main.cpp.
+To visualize the normals or outliers weights, add this to main.cpp.
 
 ```c++
-//Calculate attributes for the reference cloud using i and next cloud
-auto cloud_with_attributes = builder.calculateAttributesForReferenceCloud(clouds[i], clouds[i + 1]);
-mcap_writer->writeCloudWithAttributes(
-        "data/logs/icp_logs", // path where the mcap file will be saved
-        cloud_with_attributes,
-        70, // percentage of normals to be visualized
-        true, // true to write outliers
-        true, // true to write normals
-        "world"); // frame name
+//Calculate attributes for reference and reading clouds
+  CloudWithAttributes reference_cloud_with_attr = { 
+        .cloud = clouds[1],
+        .normals = builder.calculateNormalsForReferenceCloud(clouds[1]),
+        };
+
+  CloudWithAttributes reading_cloud_with_attr = { 
+        .cloud = clouds[0],
+        .weights = builder.calculateWeightsForReadingCloud(clouds[0], clouds[1]),
+        };
+
+  serialization::writer::MCAPWriter::writeCloudWithAttributes(
+      "data/logs/reference_cloud_with_attr", // path where the mcap file will be saved
+      reference_cloud_with_attr,
+      70, // percentage of normals to be visualized
+      "world"); // frame name
+
+  serialization::writer::MCAPWriter::writeCloudWithAttributes(
+      "data/logs/reading_cloud_with_attr", // path where the mcap file will be saved
+      reading_cloud_with_attr,
+      70, // percentage of normals to be visualized
+      "world"); // frame name
 ```

From 57fa33e0cd9d140bbc10c9cec820ccb86b3249af Mon Sep 17 00:00:00 2001
From: PostRed <s21v_kolomnikova@179.ru>
Date: Wed, 12 Feb 2025 10:50:51 +0000
Subject: [PATCH 7/8] formatting

---
 packages/lidar_map/README.md | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/packages/lidar_map/README.md b/packages/lidar_map/README.md
index 7e544b3b..479488de 100644
--- a/packages/lidar_map/README.md
+++ b/packages/lidar_map/README.md
@@ -52,12 +52,12 @@ To visualize the normals or outliers weights, add this to main.cpp.
 
 ```c++
 //Calculate attributes for reference and reading clouds
-  CloudWithAttributes reference_cloud_with_attr = { 
+  CloudWithAttributes reference_cloud_with_attr = {
         .cloud = clouds[1],
         .normals = builder.calculateNormalsForReferenceCloud(clouds[1]),
         };
 
-  CloudWithAttributes reading_cloud_with_attr = { 
+  CloudWithAttributes reading_cloud_with_attr = {
         .cloud = clouds[0],
         .weights = builder.calculateWeightsForReadingCloud(clouds[0], clouds[1]),
         };

From d734b328fcce514b8e51a31d963d6d7687213126 Mon Sep 17 00:00:00 2001
From: PostRed <s21v_kolomnikova@179.ru>
Date: Tue, 25 Feb 2025 15:17:24 +0000
Subject: [PATCH 8/8] add toPointCloud for cloud with weights

---
 .../lidar_map/include/lidar_map/conversion.h  |  3 ++
 packages/lidar_map/src/conversion.cpp         | 52 +++++++++++++++++++
 packages/lidar_map/src/serialization.cpp      | 14 ++---
 3 files changed, 60 insertions(+), 9 deletions(-)

diff --git a/packages/lidar_map/include/lidar_map/conversion.h b/packages/lidar_map/include/lidar_map/conversion.h
index 7dbb3701..1d76935f 100644
--- a/packages/lidar_map/include/lidar_map/conversion.h
+++ b/packages/lidar_map/include/lidar_map/conversion.h
@@ -19,6 +19,9 @@ namespace msg {
 
 sensor_msgs::msg::PointCloud2 toPointCloud2(const Cloud& cloud, std::string frame_id = "world");
 
+sensor_msgs::msg::PointCloud2 toPointCloud2(
+    const Cloud& cloud, const Eigen::VectorXf& weights, std::string frame_id = "world");
+
 }  // namespace msg
 
 }  // namespace truck::lidar_map
diff --git a/packages/lidar_map/src/conversion.cpp b/packages/lidar_map/src/conversion.cpp
index 297f946f..ad7505b2 100644
--- a/packages/lidar_map/src/conversion.cpp
+++ b/packages/lidar_map/src/conversion.cpp
@@ -93,6 +93,58 @@ sensor_msgs::msg::PointCloud2 toPointCloud2(const Cloud& cloud, std::string fram
     return result;
 }
 
+sensor_msgs::msg::PointCloud2 toPointCloud2(
+    const Cloud& cloud, const Eigen::VectorXf& weights, std::string frame_id) {
+    sensor_msgs::msg::PointCloud2 result;
+
+    result.header.frame_id = frame_id;
+    result.height = 1;
+    result.width = cloud.cols();
+    result.fields.resize(5);
+
+    result.fields[0].name = "x";
+    result.fields[0].offset = 0;
+    result.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    result.fields[0].count = 1;
+
+    result.fields[1].name = "y";
+    result.fields[1].offset = 4;
+    result.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    result.fields[1].count = 1;
+
+    result.fields[2].name = "z";
+    result.fields[2].offset = 8;
+    result.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    result.fields[2].count = 1;
+
+    result.fields[3].name = "w";
+    result.fields[3].offset = 12;
+    result.fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    result.fields[3].count = 1;
+
+    result.fields[4].name = "weights";
+    result.fields[4].offset = 16;
+    result.fields[4].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    result.fields[4].count = 1;
+
+    result.point_step = 20;
+    result.row_step = result.point_step * result.width;
+    result.is_bigendian = isBigendian();
+    result.is_dense = true;
+
+    result.data.resize(result.row_step * result.height);
+
+    for (size_t i = 0; i < cloud.cols(); i++) {
+        std::memcpy(&result.data[i * result.point_step], &cloud(0, i), sizeof(float));
+        std::memcpy(&result.data[i * result.point_step + 4], &cloud(1, i), sizeof(float));
+        std::memcpy(&result.data[i * result.point_step + 8], &cloud(2, i), sizeof(float));
+        std::memcpy(&result.data[i * result.point_step + 12], &cloud(3, i), sizeof(float));
+        std::memcpy(&result.data[i * result.point_step + 16], &weights(i), sizeof(float));
+    }
+
+    return result;
+}
+
 }  // namespace msg
 
 }  // namespace truck::lidar_map
diff --git a/packages/lidar_map/src/serialization.cpp b/packages/lidar_map/src/serialization.cpp
index ece2ddf5..89e80426 100644
--- a/packages/lidar_map/src/serialization.cpp
+++ b/packages/lidar_map/src/serialization.cpp
@@ -350,10 +350,11 @@ void MCAPWriter::writeCloudWithAttributes(
     double normals_ratio, std::string frame_name) {
     rosbag2_cpp::Writer writer;
     writer.open(mcap_path);
-    writer.write(msg::toPointCloud2(cloud_with_attributes.cloud, frame_name), "cloud", getTime());
-    std::cout << cloud_with_attributes.normals.has_value() << ' '
-              << cloud_with_attributes.weights.has_value() << '\n';
+
     if (cloud_with_attributes.normals.has_value()) {
+        writer.write(
+            msg::toPointCloud2(cloud_with_attributes.cloud, frame_name), "cloud", getTime());
+
         const Eigen::Matrix3Xf normals = cloud_with_attributes.normals.value();
         visualization_msgs::msg::MarkerArray msg_array;
         size_t points_count = cloud_with_attributes.cloud.cols();
@@ -388,12 +389,7 @@ void MCAPWriter::writeCloudWithAttributes(
     if (cloud_with_attributes.weights.has_value()) {
         Cloud cloud = cloud_with_attributes.cloud;
         const Eigen::VectorXf& weights = cloud_with_attributes.weights.value();
-
-        for (size_t i = 0; i < cloud.cols(); i++) {
-            cloud(3, i) = weights(i);
-        }
-
-        writer.write(msg::toPointCloud2(cloud, frame_name), "weights", getTime());
+        writer.write(msg::toPointCloud2(cloud, weights), "cloud_with_weights", getTime());
     }
 }