fix: Small regression from manuscript results with different point types. Reverting. (#4)

Author: contagon

form/feature/extraction.tpp

@@ -108,8 +108,8 @@ FeatureExtractor::extract(const std::vector<Point> &scan, size_t scan_idx) const
           compute_normal(idx, scan, valid_mask);
       if (normal.has_value()) {
         result_planar_tbb.emplace_back(
-            static_cast<double>(point.x()), static_cast<double>(point.y()),
-            static_cast<double>(point.z()), static_cast<double>(normal.value().x()),
+            static_cast<double>(point.x), static_cast<double>(point.y),
+            static_cast<double>(point.z), static_cast<double>(normal.value().x()),
             static_cast<double>(normal.value().y()),
             static_cast<double>(normal.value().z()), static_cast<size_t>(scan_idx));
       }
@@ -124,8 +124,8 @@ FeatureExtractor::extract(const std::vector<Point> &scan, size_t scan_idx) const
   for (const size_t &idx : point_indices) {
     const Point &point = scan[idx];
     result_point.emplace_back(
-        static_cast<double>(point.x()), static_cast<double>(point.y()),
-        static_cast<double>(point.z()), static_cast<size_t>(scan_idx));
+        static_cast<double>(point.x), static_cast<double>(point.y),
+        static_cast<double>(point.z), static_cast<size_t>(scan_idx));
   }
 
   return std::make_tuple(result_planar, result_point);
@@ -244,14 +244,14 @@ FeatureExtractor::compute_curvature(const std::vector<Point> &scan,
       // If valid compute the curvature
       else {
         // Initialize with the difference term
-        double dx = -(2.0 * params.neighbor_points) * scan[idx].x();
-        double dy = -(2.0 * params.neighbor_points) * scan[idx].y();
-        double dz = -(2.0 * params.neighbor_points) * scan[idx].z();
+        double dx = -(2.0 * params.neighbor_points) * scan[idx].x;
+        double dy = -(2.0 * params.neighbor_points) * scan[idx].y;
+        double dz = -(2.0 * params.neighbor_points) * scan[idx].z;
         // Iterate over neighbors and accumulate
         for (size_t n = 1; n <= params.neighbor_points; n++) {
-          dx = dx + scan[idx - n].x() + scan[idx + n].x();
-          dy = dy + scan[idx - n].y() + scan[idx + n].y();
-          dz = dz + scan[idx - n].z() + scan[idx + n].z();
+          dx = dx + scan[idx - n].x + scan[idx + n].x;
+          dy = dy + scan[idx - n].y + scan[idx + n].y;
+          dz = dz + scan[idx - n].z + scan[idx + n].z;
         }
         curvature.emplace_back(idx, dx * dx + dy * dy + dz * dz);
       }
@@ -313,7 +313,7 @@ FeatureExtractor::compute_normal(
   // std::printf("---- Found %zu neighbors\n", neighbors.size());
   Eigen::Matrix<T, Eigen::Dynamic, 3> A(neighbors.size(), 3);
   for (size_t j = 0; j < neighbors.size(); ++j) {
-    A.row(j) = neighbors[j] - point;
+    A.row(j) = neighbors[j].vec3() - point.vec3();
   }
   A /= neighbors.size();
   Eigen::Matrix<T, 3, 3> Cov = A.transpose() * A;
@@ -410,7 +410,7 @@ FeatureExtractor::find_closest(const Point &point, const size_t &start,
     if (!valid_mask[idx]) {
       continue;
     }
-    const double dist2 = (scan[idx] - point).squaredNorm();
+    const double dist2 = (scan[idx].vec4() - point.vec4()).squaredNorm();
     if (dist2 < min_dist2) {
       min_dist2 = dist2;
       closest_point = idx;
@@ -427,7 +427,7 @@ void FeatureExtractor::find_neighbors(const size_t &idx,
   const auto &point = scan[idx];
   for (size_t i = 1; i <= params.neighbor_points; i++) {
     const auto &neighbor = scan[idx + i];
-    const double range2 = (neighbor - point).squaredNorm();
+    const double range2 = (neighbor.vec4() - point.vec4()).squaredNorm();
     if (range2 < params.radius * params.radius) {
       out.push_back(neighbor);
     } else {
@@ -438,7 +438,7 @@ void FeatureExtractor::find_neighbors(const size_t &idx,
   // search in the negative direction
   for (size_t i = 1; i <= params.neighbor_points; i++) {
     const auto &neighbor = scan[idx - i];
-    const double range2 = (neighbor - point).squaredNorm();
+    const double range2 = (neighbor.vec4() - point.vec4()).squaredNorm();
     if (range2 < params.radius * params.radius) {
       out.push_back(neighbor);
     } else {

form/form.cpp

@@ -38,7 +38,7 @@ Estimator::Estimator(const Estimator::Params &params) noexcept
                      KeypointMap<PointFeat>(m_params.map)} {}
 
 std::tuple<std::vector<PlanarFeat>, std::vector<PointFeat>>
-Estimator::register_scan(const std::vector<Eigen::Vector3f> &scan) noexcept {
+Estimator::register_scan(const std::vector<PointXYZf> &scan) noexcept {
   constexpr auto SEQ = std::make_index_sequence<2>{};
 
   //

form/form.hpp

@@ -80,7 +80,7 @@ struct Estimator {
 
   /// @brief Register a new scan and return the extracted features
   std::tuple<std::vector<PlanarFeat>, std::vector<PointFeat>>
-  register_scan(const std::vector<Eigen::Vector3f> &scan) noexcept;
+  register_scan(const std::vector<PointXYZf> &scan) noexcept;
 };
 
 } // namespace form

form/utils.hpp

@@ -29,8 +29,67 @@
 #include <tbb/task_arena.h>
 #include <tuple>
 
+#include <Eigen/Dense>
+#include <gtsam/geometry/Pose3.h>
+
 namespace form {
 
+/// @brief Simple 3D point structure for loading in points
+struct PointXYZf {
+  using Scalar = float;
+  float x;
+  float y;
+  float z;
+  float _ = static_cast<float>(0);
+
+  PointXYZf(float x, float y, float z) : x(x), y(y), z(z), _(0) {}
+
+  // ------------------------- Getters ------------------------- //
+  [[nodiscard]] inline Eigen::Map<Eigen::Matrix<float, 3, 1>> vec3() noexcept {
+    return Eigen::Map<Eigen::Matrix<float, 3, 1>>(&x);
+  }
+
+  [[nodiscard]] inline Eigen::Map<const Eigen::Matrix<float, 3, 1>>
+  vec3() const noexcept {
+    return Eigen::Map<const Eigen::Matrix<float, 3, 1>>(&x);
+  }
+
+  [[nodiscard]] inline Eigen::Map<const Eigen::Matrix<float, 4, 1>>
+  vec4() const noexcept {
+    return Eigen::Map<const Eigen::Matrix<float, 4, 1>>(&x);
+  }
+
+  [[nodiscard]] inline Eigen::Map<Eigen::Array<float, 3, 1>> array() noexcept {
+    return Eigen::Map<Eigen::Array<float, 3, 1>>(&x);
+  }
+
+  [[nodiscard]] inline Eigen::Map<const Eigen::Array<float, 3, 1>>
+  array() const noexcept {
+    return Eigen::Map<const Eigen::Array<float, 3, 1>>(&x);
+  }
+
+  // ------------------------- Misc ------------------------- //
+  inline void transform_in_place(const gtsam::Pose3 &pose) noexcept {
+    vec3() = (pose * vec3().template cast<double>()).template cast<float>();
+  }
+
+  [[nodiscard]] inline PointXYZf transform(const gtsam::Pose3 &pose) const noexcept {
+    auto point = *this;
+    point.transform_in_place(pose);
+    return point;
+  }
+
+  [[nodiscard]] inline float squaredNorm() const noexcept {
+    return vec4().squaredNorm();
+  }
+
+  [[nodiscard]] inline float norm() const noexcept { return vec4().norm(); }
+
+  [[nodiscard]] constexpr bool operator==(const PointXYZf &other) const noexcept {
+    return x == other.x && y == other.y && z == other.z;
+  }
+};
+
 namespace tuple {
 // Some helpers ot make iterating over tuples easier
 // https://www.cppstories.com/2022/tuple-iteration-apply/

python/bindings.cpp

@@ -40,8 +40,8 @@ template <typename Point> evalio::Point point_to_evalio(const Point &point) {
   };
 }
 
-Eigen::Vector3f point_to_form(const evalio::Point &point) {
-  return Eigen::Vector3f(point.x, point.y, point.z);
+form::PointXYZf point_to_form(const evalio::Point &point) {
+  return form::PointXYZf(point.x, point.y, point.z);
 }
 
 // ------------------------- Pipeline ------------------------- //
@@ -147,7 +147,7 @@ class FORM : public evalio::Pipeline {
   std::map<std::string, std::vector<evalio::Point>>
   add_lidar(evalio::LidarMeasurement mm) override {
     // convert to evalio
-    std::vector<Eigen::Vector3f> scan;
+    std::vector<form::PointXYZf> scan;
     auto start = mm.stamp;
     auto end = mm.stamp + delta_time_;
 
@@ -214,9 +214,15 @@ NB_MODULE(_core, m) {
   m.def("extract_keypoints", [](const std::vector<Eigen::Vector3d> &points,
                                 const form::FeatureExtractor::Params &params,
                                 evalio::LidarParams &lidar_params) {
+    // Convert to form points
+    std::vector<form::PointXYZf> points_form;
+    for (const auto &point : points) {
+      points_form.emplace_back(point.x(), point.y(), point.z());
+    }
+
     // Call the keypoint extraction function from the Tclio class
     auto [planar_keypoints, point_keypoints] =
-        form::FeatureExtractor(params, 0).extract(points, 0);
+        form::FeatureExtractor(params, 0).extract(points_form, 0);
 
     // return a tuple of (planar_points, normals, point_points)
     std::vector<Eigen::Vector3d> planar_points;