diff --git a/packages/lidar_map/README.md b/packages/lidar_map/README.md index 3406b1c0..479488de 100644 --- a/packages/lidar_map/README.md +++ b/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 +``` diff --git a/packages/lidar_map/include/lidar_map/builder.h b/packages/lidar_map/include/lidar_map/builder.h index 6d330ad6..2fb32a5b 100644 --- a/packages/lidar_map/include/lidar_map/builder.h +++ b/packages/lidar_map/include/lidar_map/builder.h @@ -52,6 +52,8 @@ struct BuilderParams { class Builder { public: + std::vector clouds_with_attributes; + Builder(const BuilderParams& params); std::pair 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_; diff --git a/packages/lidar_map/include/lidar_map/common.h b/packages/lidar_map/include/lidar_map/common.h index 6213214f..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 +#include namespace truck::lidar_map { @@ -18,4 +19,13 @@ using Cloud = Eigen::Matrix4Xf; */ using Clouds = std::vector; +struct CloudWithAttributes { + Cloud cloud; + std::optional 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 normals = std::nullopt; +}; + } // namespace truck::lidar_map 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/include/lidar_map/serialization.h b/packages/lidar_map/include/lidar_map/serialization.h index 6e0665fd..b1d5f788 100644 --- a/packages/lidar_map/include/lidar_map/serialization.h +++ b/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_; diff --git a/packages/lidar_map/src/builder.cpp b/packages/lidar_map/src/builder.cpp index bd9e1431..9f0d6c13 100644 --- a/packages/lidar_map/src/builder.cpp +++ b/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 findNearestIdsInsideBox( 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 d8ef397f..89e80426 100644 --- a/packages/lidar_map/src/serialization.cpp +++ b/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(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