API to pass mean and normals directly for ground alignment

cpp/map_closures/GroundAlign.cpp

@@ -174,4 +174,22 @@ Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &pointcloud, const doubl
     }
     return T.matrix();
 }
+
+Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &voxel_means,
+                                   const Vector3dVector &voxel_normals) {
+    auto [ground_samples, T] = SampleGroundPoints(voxel_means, voxel_normals);
+    if (ground_samples.empty()) return Eigen::Matrix4d::Identity();
+    TransformPoints(T, ground_samples);
+    for (int iters = 0; iters < max_iterations; iters++) {
+        const auto [H, b] = BuildLinearSystem(ground_samples);
+        const Eigen::Vector3d dx = H.ldlt().solve(-b);
+        Eigen::Matrix<double, 6, 1> se3 = Eigen::Matrix<double, 6, 1>::Zero();
+        se3 << 0.0, 0.0, dx.x(), dx.y(), dx.z(), 0.0;
+        const Sophus::SE3d estimation(Sophus::SE3d::exp(se3));
+        TransformPoints(estimation, ground_samples);
+        T = estimation * T;
+        if (dx.norm() < convergence_threshold) break;
+    }
+    return T.matrix();
+}
 }  // namespace map_closures

cpp/map_closures/GroundAlign.hpp

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

cpp/map_closures/MapClosures.cpp

@@ -103,6 +103,45 @@ void MapClosures::MatchAndAddToDatabase(const int id,
     ground_alignments_.emplace(id, std::move(T_ground));
 }
 
+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) {
+    Eigen::Matrix4d T_ground = AlignToLocalGround(voxel_means, voxel_normals);
+    DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution,
+                                                config_.density_threshold);
+    cv::Mat orb_descriptors;
+    std::vector<cv::KeyPoint> orb_keypoints;
+    orb_keypoints.reserve(nfeatures);
+    orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints,
+                                     orb_descriptors);
+
+    std::vector<std::vector<cv::DMatch>> self_matches;
+    self_matches.reserve(orb_keypoints.size());
+    self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2);
+
+    std::vector<Matchable *> hbst_matchable;
+    hbst_matchable.reserve(orb_descriptors.rows);
+    std::for_each(
+        self_matches.cbegin(), self_matches.cend(), [&](const std::vector<cv::DMatch> &self_match) {
+            if (self_match[1].distance > self_similarity_threshold) {
+                const int index_descriptor = self_match[0].queryIdx;
+                cv::KeyPoint keypoint = orb_keypoints[index_descriptor];
+                keypoint.pt.x = keypoint.pt.x + static_cast<float>(density_map.lower_bound.y());
+                keypoint.pt.y = keypoint.pt.y + static_cast<float>(density_map.lower_bound.x());
+                hbst_matchable.emplace_back(
+                    new Matchable(keypoint, orb_descriptors.row(index_descriptor), id));
+            }
+        });
+
+    hbst_binary_tree_->matchAndAdd(hbst_matchable, descriptor_matches_,
+                                   config_.hamming_distance_threshold,
+                                   srrg_hbst::SplittingStrategy::SplitEven);
+
+    density_maps_.emplace(id, std::move(density_map));
+    ground_alignments_.emplace(id, std::move(T_ground));
+}
+
 void MapClosures::Match(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,
@@ -134,6 +173,39 @@ void MapClosures::Match(const std::vector<Eigen::Vector3d> &local_map) {
                              config_.hamming_distance_threshold);
 }
 
+void MapClosures::Match(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);
+    DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution,
+                                                config_.density_threshold);
+    cv::Mat orb_descriptors;
+    std::vector<cv::KeyPoint> orb_keypoints;
+    orb_keypoints.reserve(nfeatures);
+    orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints,
+                                     orb_descriptors);
+
+    std::vector<std::vector<cv::DMatch>> self_matches;
+    self_matches.reserve(orb_keypoints.size());
+    self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2);
+
+    std::vector<Matchable *> hbst_matchable;
+    hbst_matchable.reserve(orb_descriptors.rows);
+    std::for_each(
+        self_matches.cbegin(), self_matches.cend(), [&](const std::vector<cv::DMatch> &self_match) {
+            if (self_match[1].distance > self_similarity_threshold) {
+                const int index_descriptor = self_match[0].queryIdx;
+                cv::KeyPoint keypoint = orb_keypoints[index_descriptor];
+                keypoint.pt.x = keypoint.pt.x + static_cast<float>(density_map.lower_bound.y());
+                keypoint.pt.y = keypoint.pt.y + static_cast<float>(density_map.lower_bound.x());
+                hbst_matchable.emplace_back(
+                    new Matchable(keypoint, orb_descriptors.row(index_descriptor)));
+            }
+        });
+    hbst_binary_tree_->match(hbst_matchable, descriptor_matches_,
+                             config_.hamming_distance_threshold);
+}
+
 ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int query_id) const {
     const auto it = descriptor_matches_.find(reference_id);
     if (it == descriptor_matches_.end()) {
@@ -197,4 +269,38 @@ std::vector<ClosureCandidate> MapClosures::GetTopKClosures(
     return closures;
 }
 
+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);
+    auto compare_closure_candidates = [](const ClosureCandidate &a, const ClosureCandidate &b) {
+        return a.number_of_inliers > b.number_of_inliers;
+    };
+
+    std::vector<ClosureCandidate> closures;
+    const int num_of_potential_closures = query_id - no_of_local_maps_to_skip;
+    if (num_of_potential_closures > 0) {
+        closures.reserve(num_of_potential_closures);
+        for (int ref_id = 0; ref_id < num_of_potential_closures; ++ref_id) {
+            ClosureCandidate closure = ValidateClosure(ref_id, query_id);
+            if (closure.number_of_inliers > min_no_of_matches) {
+                closures.emplace_back(std::move(closure));
+            }
+        }
+        if (k != -1 && !closures.empty()) {
+            const int top_k = std::min(k, static_cast<int>(closures.size()));
+            if (top_k < static_cast<int>(closures.size())) {
+                const auto kth = closures.begin() + top_k;
+                std::nth_element(closures.begin(), kth, closures.end(), compare_closure_candidates);
+                closures.resize(top_k);
+            }
+            std::sort(closures.begin(), closures.end(), compare_closure_candidates);
+        }
+    }
+    return closures;
+}
+
 }  // namespace map_closures

cpp/map_closures/MapClosures.hpp

@@ -67,13 +67,37 @@ class MapClosures {
         }
         return std::move(closures.front());
     }
+    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) {
+        std::vector<ClosureCandidate> closures =
+            GetTopKClosures(query_id, local_map, voxel_means, voxel_normals, 1);
+        if (closures.empty()) {
+            return ClosureCandidate();
+        }
+        return std::move(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> GetTopKClosures(const int query_id,
+                                                  const std::vector<Eigen::Vector3d> &local_map,
+                                                  const int k);
+
     std::vector<ClosureCandidate> GetClosures(const int query_id,
                                               const std::vector<Eigen::Vector3d> &local_map) {
         return GetTopKClosures(query_id, local_map, -1);
     }
+    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 DensityMap &getDensityMapFromId(const int map_id) const {
         return density_maps_.at(map_id);
@@ -89,7 +113,14 @@ class MapClosures {
 
 protected:
     void MatchAndAddToDatabase(const int id, const std::vector<Eigen::Vector3d> &local_map);
+    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 Match(const std::vector<Eigen::Vector3d> &local_map);
+    void Match(const std::vector<Eigen::Vector3d> &local_map,
+               const std::vector<Eigen::Vector3d> &voxel_means,
+               const std::vector<Eigen::Vector3d> &voxel_normals);
     ClosureCandidate ValidateClosure(const int reference_id, const int query_id) const;
 
     Config config_;

python/map_closures/map_closures.py

@@ -19,7 +19,7 @@
 # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
-from typing import List
+from typing import List, overload
 
 import numpy as np
 from typing_extensions import TypeAlias
@@ -36,18 +36,107 @@ def __init__(self, config: MapClosuresConfig = MapClosuresConfig()):
         self._config = config
         self._pipeline = map_closures_pybind._MapClosures(self._config.model_dump())
 
+    @overload
     def get_best_closure(self, query_idx: int, local_map: np.ndarray) -> ClosureCandidate:
-        closure = self._pipeline._GetBestClosure(query_idx, Vector3dVector(local_map))
+        ...
+
+    @overload
+    def get_best_closure(
+        self,
+        query_idx: int,
+        local_map: np.ndarray,
+        voxel_means: np.ndarray,
+        voxel_normals: np.ndarray,
+    ) -> ClosureCandidate:
+        ...
+
+    def get_best_closure(
+        self,
+        query_idx: int,
+        local_map: np.ndarray,
+        voxel_means: np.ndarray = None,
+        voxel_normals: np.ndarray = None,
+    ) -> ClosureCandidate:
+        if voxel_means is None or voxel_normals is None:
+            closure = self._pipeline._GetBestClosure(query_idx, Vector3dVector(local_map))
+        else:
+            closure = self._pipeline._GetBestClosure(
+                query_idx,
+                Vector3dVector(local_map),
+                Vector3dVector(voxel_means),
+                Vector3dVector(voxel_normals),
+            )
         return closure
 
+    @overload
     def get_top_k_closures(
         self, query_idx: int, local_map: np.ndarray, k: int
     ) -> List[ClosureCandidate]:
-        top_k_closures = self._pipeline._GetTopKClosures(query_idx, Vector3dVector(local_map), k)
+        ...
+
+    @overload
+    def get_top_k_closures(
+        self,
+        query_idx: int,
+        local_map: np.ndarray,
+        k: int,
+        voxel_means: np.ndarray,
+        voxel_normals: np.ndarray,
+    ) -> List[ClosureCandidate]:
+        ...
+
+    def get_top_k_closures(
+        self,
+        query_idx: int,
+        local_map: np.ndarray,
+        k: int,
+        voxel_means: np.ndarray = None,
+        voxel_normals: np.ndarray = None,
+    ) -> List[ClosureCandidate]:
+        if voxel_means is None or voxel_normals is None:
+            top_k_closures = self._pipeline._GetTopKClosures(
+                query_idx, Vector3dVector(local_map), k
+            )
+        else:
+            top_k_closures = self._pipeline._GetTopKClosures(
+                query_idx,
+                Vector3dVector(local_map),
+                Vector3dVector(voxel_means),
+                Vector3dVector(voxel_normals),
+                k,
+            )
         return top_k_closures
 
+    @overload
     def get_closures(self, query_idx: int, local_map: np.ndarray) -> List[ClosureCandidate]:
-        closures = self._pipeline._GetClosures(query_idx, Vector3dVector(local_map))
+        ...
+
+    @overload
+    def get_closures(
+        self,
+        query_idx: int,
+        local_map: np.ndarray,
+        voxel_means: np.ndarray,
+        voxel_normals: np.ndarray,
+    ) -> List[ClosureCandidate]:
+        ...
+
+    def get_closures(
+        self,
+        query_idx: int,
+        local_map: np.ndarray,
+        voxel_means: np.ndarray = None,
+        voxel_normals: np.ndarray = None,
+    ) -> List[ClosureCandidate]:
+        if voxel_means is None or voxel_normals is None:
+            closures = self._pipeline._GetClosures(query_idx, Vector3dVector(local_map))
+        else:
+            closures = self._pipeline._GetClosures(
+                query_idx,
+                Vector3dVector(local_map),
+                Vector3dVector(voxel_means),
+                Vector3dVector(voxel_normals),
+            )
         return closures
 
     def get_density_map_from_id(self, map_id: int) -> np.ndarray:

python/map_closures/pybind/map_closures_pybind.cpp

@@ -77,9 +77,34 @@ 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)
-        .def("_GetTopKClosures", &MapClosures::GetTopKClosures, "query_id"_a, "local_map"_a, "k"_a)
-        .def("_GetClosures", &MapClosures::GetClosures, "query_id"_a, "local_map"_a)
+        .def("_GetBestClosure",
+             py::overload_cast<int, const std::vector<Eigen::Vector3d> &>(
+                 &MapClosures::GetBestClosure),
+             "query_id"_a, "local_map"_a)
+        .def("_GetBestClosure",
+             py::overload_cast<int, const std::vector<Eigen::Vector3d> &,
+                               const std::vector<Eigen::Vector3d> &,
+                               const std::vector<Eigen::Vector3d> &>(&MapClosures::GetBestClosure),
+             "query_id"_a, "local_map"_a, "voxel_means"_a, "voxel_normals"_a)
+        .def("_GetTopKClosures",
+             py::overload_cast<int, const std::vector<Eigen::Vector3d> &, int>(
+                 &MapClosures::GetTopKClosures),
+             "query_id"_a, "local_map"_a, "k"_a)
+        .def("_GetTopKClosures",
+             py::overload_cast<int, const std::vector<Eigen::Vector3d> &,
+                               const std::vector<Eigen::Vector3d> &,
+                               const std::vector<Eigen::Vector3d> &, int>(
+                 &MapClosures::GetTopKClosures),
+             "query_id"_a, "local_map"_a, "voxel_means"_a, "voxel_normals"_a, "k"_a)
+        .def(
+            "_GetClosures",
+            py::overload_cast<int, const std::vector<Eigen::Vector3d> &>(&MapClosures::GetClosures),
+            "query_id"_a, "local_map"_a)
+        .def("_GetClosures",
+             py::overload_cast<int, const std::vector<Eigen::Vector3d> &,
+                               const std::vector<Eigen::Vector3d> &,
+                               const std::vector<Eigen::Vector3d> &>(&MapClosures::GetClosures),
+             "query_id"_a, "local_map"_a, "voxel_means"_a, "voxel_normals"_a)
         .def("_SaveHbstDatabase", &MapClosures::SaveHbstDatabase, "database_path"_a);
 
     py::class_<VoxelMap> internal_map(m, "_VoxelMap", "Don't use this");

python/pyproject.toml

@@ -58,8 +58,8 @@ Homepage = "https://github.com/PRBonn/MapClosures"
 
 [tool.scikit-build]
 build-dir = "build/{wheel_tag}"
-cmake.verbose = false
-cmake.minimum-version = "3.22"
+build.verbose = false
+cmake.version = ">=3.22"
 editable.mode = "redirect"
 editable.rebuild = true
 editable.verbose = true