add normals and outliers

packages/lidar_map/README.md

@@ -46,3 +46,31 @@ ros2 run lidar_map lidar_map_executable
   --mcap-log data/logs/mcap/cloud_atrium_XX_YY_ZZ_log
   --json-log data/logs/json/cloud_atrium_XX_YY_ZZ_log.json
 ```
+### Debug
+
+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 = {
+        .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
+```

packages/lidar_map/include/lidar_map/builder.h

@@ -52,6 +52,8 @@ struct BuilderParams {
 
 class Builder {
   public:
+    std::vector<CloudWithAttributes> clouds_with_attributes;
+
     Builder(const BuilderParams& params);
 
     std::pair<geom::Poses, Clouds> sliceDataByPosesProximity(
@@ -74,6 +76,11 @@ 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;
 
+    Eigen::VectorXf calculateWeightsForReadingCloud(
+        const Cloud& reading_cloud, const Cloud& reference_cloud);
+
+    Eigen::Matrix3Xf calculateNormalsForReferenceCloud(const Cloud& reference_cloud);
+
   private:
     ICP icp_;
     g2o::SparseOptimizer optimizer_;

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,4 +19,13 @@ using Cloud = Eigen::Matrix4Xf;
  */
 using Clouds = std::vector<Cloud>;
 
+struct CloudWithAttributes {
+    Cloud cloud;
+    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

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

packages/lidar_map/include/lidar_map/serialization.h

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

packages/lidar_map/src/builder.cpp

@@ -371,6 +371,37 @@ Cloud Builder::mergeClouds(const Clouds& clouds) {
     return merged_cloud;
 }
 
+/**
+ * get outliers weights for 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);
+
+    icp_.referenceDataPointsFilters.apply(reference_dp);
+    icp_.matcher->init(reference_dp);
+    Matcher::Matches matches = icp_.matcher->findClosests(reading_dp);
+    Matcher::OutlierWeights outlierWeights =
+        icp_.outlierFilters.compute(reading_dp, reference_dp, matches);
+
+    Eigen::VectorXf weights(outlierWeights.cols());
+    for (size_t i = 0; i < outlierWeights.cols(); i++) {
+        weights(i) = outlierWeights(0, i);
+    }
+
+    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 {
 
 std::vector<size_t> findNearestIdsInsideBox(

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

packages/lidar_map/src/serialization.cpp

@@ -345,6 +345,54 @@ 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,
+    double normals_ratio, std::string frame_name) {
+    rosbag2_cpp::Writer writer;
+    writer.open(mcap_path);
+
+    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();
+        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 = 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);
+            msg_.pose.orientation = geom::msg::toQuaternion(truck::geom::Angle(yaw));
+
+            msg_array.markers.push_back(msg_);
+        }
+
+        writer.write(msg_array, "normals", getTime());
+    }
+
+    if (cloud_with_attributes.weights.has_value()) {
+        Cloud cloud = cloud_with_attributes.cloud;
+        const Eigen::VectorXf& weights = cloud_with_attributes.weights.value();
+        writer.write(msg::toPointCloud2(cloud, weights), "cloud_with_weights", getTime());
+    }
+}
+
 }  // namespace writer
 
 }  // namespace truck::lidar_map::serialization