compute mean and normals inside the alignment function (#95)

Author: saurabh1002

cpp/map_closures/GroundAlign.cpp

@@ -33,6 +33,8 @@
 #include <utility>
 #include <vector>
 
+#include "VoxelMap.hpp"
+
 namespace {
 using Vector3dVector = std::vector<Eigen::Vector3d>;
 using LinearSystem = std::pair<Eigen::Matrix3d, Eigen::Vector3d>;
@@ -151,8 +153,11 @@ static constexpr int max_iterations = 10;
 }  // namespace
 
 namespace map_closures {
-Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &voxel_means,
-                                   const Vector3dVector &voxel_normals) {
+Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &pointcloud, const double resolution) {
+    VoxelMap voxel_map(resolution, 100.0);
+    voxel_map.AddPoints(pointcloud);
+    const auto &[voxel_means, voxel_normals] = voxel_map.PerVoxelMeanAndNormal();
+
     auto [ground_samples, T] = SampleGroundPoints(voxel_means, voxel_normals);
     TransformPoints(T, ground_samples);
     for (int iters = 0; iters < max_iterations; iters++) {

cpp/map_closures/GroundAlign.hpp

@@ -27,6 +27,6 @@
 #include <vector>
 
 namespace map_closures {
-Eigen::Matrix4d AlignToLocalGround(const std::vector<Eigen::Vector3d> &voxel_means,
-                                   const std::vector<Eigen::Vector3d> &voxel_normals);
+Eigen::Matrix4d AlignToLocalGround(const std::vector<Eigen::Vector3d> &pointcloud,
+                                   const double resolution);
 }  // namespace map_closures

cpp/map_closures/MapClosures.cpp

@@ -68,10 +68,8 @@ MapClosures::MapClosures(const Config &config) : config_(config) {
 }
 
 void MapClosures::MatchAndAddToDatabase(const int id,
-                                        const std::vector<Eigen::Vector3d> &local_map,
-                                        const std::vector<Eigen::Vector3d> &voxel_means,
-                                        const std::vector<Eigen::Vector3d> &voxel_normals) {
-    const Eigen::Matrix4d T_ground = AlignToLocalGround(voxel_means, voxel_normals);
+                                        const std::vector<Eigen::Vector3d> &local_map) {
+    const Eigen::Matrix4d T_ground = AlignToLocalGround(local_map, config_.density_map_resolution);
     DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution,
                                                 config_.density_threshold);
     cv::Mat orb_descriptors;
@@ -143,12 +141,8 @@ ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int
 }
 
 std::vector<ClosureCandidate> MapClosures::GetTopKClosures(
-    const int query_id,
-    const std::vector<Eigen::Vector3d> &local_map,
-    const std::vector<Eigen::Vector3d> &voxel_means,
-    const std::vector<Eigen::Vector3d> &voxel_normals,
-    const int k) {
-    MatchAndAddToDatabase(query_id, local_map, voxel_means, voxel_normals);
+    const int query_id, const std::vector<Eigen::Vector3d> &local_map, const int k) {
+    MatchAndAddToDatabase(query_id, local_map);
     auto compare_closure_candidates = [](const ClosureCandidate &a, const ClosureCandidate &b) {
         return a.number_of_inliers >= b.number_of_inliers;
     };

cpp/map_closures/MapClosures.hpp

@@ -61,25 +61,19 @@ class MapClosures {
     ~MapClosures() = default;
 
     ClosureCandidate GetBestClosure(const int query_id,
-                                    const std::vector<Eigen::Vector3d> &local_map,
-                                    const std::vector<Eigen::Vector3d> &voxel_means,
-                                    const std::vector<Eigen::Vector3d> &voxel_normals) {
-        const auto &closures = GetTopKClosures(query_id, local_map, voxel_means, voxel_normals, 1);
+                                    const std::vector<Eigen::Vector3d> &local_map) {
+        const auto &closures = GetTopKClosures(query_id, local_map, 1);
         if (closures.empty()) {
             return ClosureCandidate();
         }
         return closures.front();
     }
     std::vector<ClosureCandidate> GetTopKClosures(const int query_id,
                                                   const std::vector<Eigen::Vector3d> &local_map,
-                                                  const std::vector<Eigen::Vector3d> &voxel_means,
-                                                  const std::vector<Eigen::Vector3d> &voxel_normals,
                                                   const int k);
     std::vector<ClosureCandidate> GetClosures(const int query_id,
-                                              const std::vector<Eigen::Vector3d> &local_map,
-                                              const std::vector<Eigen::Vector3d> &voxel_means,
-                                              const std::vector<Eigen::Vector3d> &voxel_normals) {
-        return GetTopKClosures(query_id, local_map, voxel_means, voxel_normals, -1);
+                                              const std::vector<Eigen::Vector3d> &local_map) {
+        return GetTopKClosures(query_id, local_map, -1);
     }
 
     const DensityMap &getDensityMapFromId(const int map_id) const {
@@ -95,10 +89,7 @@ class MapClosures {
     }
 
 protected:
-    void MatchAndAddToDatabase(const int id,
-                               const std::vector<Eigen::Vector3d> &local_map,
-                               const std::vector<Eigen::Vector3d> &voxel_means,
-                               const std::vector<Eigen::Vector3d> &voxel_normals);
+    void MatchAndAddToDatabase(const int id, const std::vector<Eigen::Vector3d> &local_map);
     ClosureCandidate ValidateClosure(const int reference_id, const int query_id) const;
 
     Config config_;

python/map_closures/map_closures.py

@@ -37,51 +37,18 @@ def __init__(self, config: MapClosuresConfig = MapClosuresConfig()):
         self._config = config
         self._pipeline = map_closures_pybind._MapClosures(self._config.model_dump())
 
-    def get_best_closure(
-        self,
-        query_idx: int,
-        local_map: np.ndarray,
-        voxel_means: np.ndarray,
-        voxel_normals: np.ndarray,
-    ) -> ClosureCandidate:
-        closure = self._pipeline._GetBestClosure(
-            query_idx,
-            Vector3dVector(local_map),
-            Vector3dVector(voxel_means),
-            Vector3dVector(voxel_normals),
-        )
+    def get_best_closure(self, query_idx: int, local_map: np.ndarray) -> ClosureCandidate:
+        closure = self._pipeline._GetBestClosure(query_idx, Vector3dVector(local_map))
         return closure
 
     def get_top_k_closures(
-        self,
-        query_idx: int,
-        local_map: np.ndarray,
-        voxel_means: np.ndarray,
-        voxel_normals: np.ndarray,
-        k: int,
+        self, query_idx: int, local_map: np.ndarray, k: int
     ) -> List[ClosureCandidate]:
-        top_k_closures = self._pipeline._GetTopKClosures(
-            query_idx,
-            Vector3dVector(local_map),
-            Vector3dVector(voxel_means),
-            Vector3dVector(voxel_normals),
-            k,
-        )
+        top_k_closures = self._pipeline._GetTopKClosures(query_idx, Vector3dVector(local_map), k)
         return top_k_closures
 
-    def get_closures(
-        self,
-        query_idx: int,
-        local_map: np.ndarray,
-        voxel_means: np.ndarray,
-        voxel_normals: np.ndarray,
-    ) -> List[ClosureCandidate]:
-        closures = self._pipeline._GetClosures(
-            query_idx,
-            Vector3dVector(local_map),
-            Vector3dVector(voxel_means),
-            Vector3dVector(voxel_normals),
-        )
+    def get_closures(self, query_idx: int, local_map: np.ndarray) -> List[ClosureCandidate]:
+        closures = self._pipeline._GetClosures(query_idx, Vector3dVector(local_map))
         return closures
 
     def get_density_map_from_id(self, map_id: int) -> np.ndarray:

python/map_closures/pipeline.py

@@ -127,10 +127,7 @@ def _run_pipeline(self):
                 scan_idx == self._n_scans - 1
             ):
                 local_map_pointcloud = self.voxel_local_map.point_cloud()
-                points, normals = self.voxel_local_map.per_voxel_mean_and_normal()
-                closures = self.map_closures.get_closures(
-                    map_idx, local_map_pointcloud, points, normals
-                )
+                closures = self.map_closures.get_closures(map_idx, local_map_pointcloud)
 
                 density_map = self.map_closures.get_density_map_from_id(map_idx)
                 self.data.append_localmap(

python/map_closures/pybind/map_closures_pybind.cpp

@@ -78,12 +78,9 @@ PYBIND11_MODULE(map_closures_pybind, m) {
                  return density_map_eigen;
              })
         .def("_getGroundAlignmentFromId", &MapClosures::getGroundAlignmentFromId, "map_id"_a)
-        .def("_GetBestClosure", &MapClosures::GetBestClosure, "query_id"_a, "local_map"_a,
-             "voxel_means"_a, "voxel_normals"_a)
-        .def("_GetTopKClosures", &MapClosures::GetTopKClosures, "query_id"_a, "local_map"_a,
-             "voxel_means"_a, "voxel_normals"_a, "k"_a)
-        .def("_GetClosures", &MapClosures::GetClosures, "query_id"_a, "local_map"_a,
-             "voxel_means"_a, "voxel_normals"_a)
+        .def("_GetBestClosure", &MapClosures::GetBestClosure, "query_id"_a, "local_map"_a)
+        .def("_GetTopKClosures", &MapClosures::GetTopKClosures, "query_id"_a, "local_map"_a, "k"_a)
+        .def("_GetClosures", &MapClosures::GetClosures, "query_id"_a, "local_map"_a)
         .def("_SaveHbstDatabase", &MapClosures::SaveHbstDatabase, "database_path"_a);
 
     py::class_<VoxelMap> internal_map(m, "_VoxelMap", "Don't use this");