diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index f0b5c34..2d94ccd 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/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 se3 = Eigen::Matrix::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 diff --git a/cpp/map_closures/GroundAlign.hpp b/cpp/map_closures/GroundAlign.hpp index 98ef956..14773a8 100644 --- a/cpp/map_closures/GroundAlign.hpp +++ b/cpp/map_closures/GroundAlign.hpp @@ -28,4 +28,6 @@ namespace map_closures { Eigen::Matrix4d AlignToLocalGround(const std::vector &pointcloud, const double resolution); +Eigen::Matrix4d AlignToLocalGround(const std::vector &voxel_means, + const std::vector &voxel_normals); } // namespace map_closures diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index 7b1d24f..71a77e6 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/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 &local_map, + const std::vector &voxel_means, + const std::vector &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 orb_keypoints; + orb_keypoints.reserve(nfeatures); + orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints, + orb_descriptors); + + std::vector> self_matches; + self_matches.reserve(orb_keypoints.size()); + self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2); + + std::vector hbst_matchable; + hbst_matchable.reserve(orb_descriptors.rows); + std::for_each( + self_matches.cbegin(), self_matches.cend(), [&](const std::vector &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(density_map.lower_bound.y()); + keypoint.pt.y = keypoint.pt.y + static_cast(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 &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 &local_map) { config_.hamming_distance_threshold); } +void MapClosures::Match(const std::vector &local_map, + const std::vector &voxel_means, + const std::vector &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 orb_keypoints; + orb_keypoints.reserve(nfeatures); + orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints, + orb_descriptors); + + std::vector> self_matches; + self_matches.reserve(orb_keypoints.size()); + self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2); + + std::vector hbst_matchable; + hbst_matchable.reserve(orb_descriptors.rows); + std::for_each( + self_matches.cbegin(), self_matches.cend(), [&](const std::vector &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(density_map.lower_bound.y()); + keypoint.pt.y = keypoint.pt.y + static_cast(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 MapClosures::GetTopKClosures( return closures; } +std::vector MapClosures::GetTopKClosures( + const int query_id, + const std::vector &local_map, + const std::vector &voxel_means, + const std::vector &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 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(closures.size())); + if (top_k < static_cast(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 diff --git a/cpp/map_closures/MapClosures.hpp b/cpp/map_closures/MapClosures.hpp index eaa07c0..e1bfd33 100644 --- a/cpp/map_closures/MapClosures.hpp +++ b/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 &local_map, + const std::vector &voxel_means, + const std::vector &voxel_normals) { + std::vector closures = + GetTopKClosures(query_id, local_map, voxel_means, voxel_normals, 1); + if (closures.empty()) { + return ClosureCandidate(); + } + return std::move(closures.front()); + } + std::vector GetTopKClosures(const int query_id, const std::vector &local_map, + const std::vector &voxel_means, + const std::vector &voxel_normals, const int k); + std::vector GetTopKClosures(const int query_id, + const std::vector &local_map, + const int k); + std::vector GetClosures(const int query_id, const std::vector &local_map) { return GetTopKClosures(query_id, local_map, -1); } + std::vector GetClosures(const int query_id, + const std::vector &local_map, + const std::vector &voxel_means, + const std::vector &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 &local_map); + void MatchAndAddToDatabase(const int id, + const std::vector &local_map, + const std::vector &voxel_means, + const std::vector &voxel_normals); void Match(const std::vector &local_map); + void Match(const std::vector &local_map, + const std::vector &voxel_means, + const std::vector &voxel_normals); ClosureCandidate ValidateClosure(const int reference_id, const int query_id) const; Config config_; diff --git a/python/map_closures/map_closures.py b/python/map_closures/map_closures.py index 04f704f..02f2520 100644 --- a/python/map_closures/map_closures.py +++ b/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: diff --git a/python/map_closures/pybind/map_closures_pybind.cpp b/python/map_closures/pybind/map_closures_pybind.cpp index cafac30..c2e83d6 100644 --- a/python/map_closures/pybind/map_closures_pybind.cpp +++ b/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 &>( + &MapClosures::GetBestClosure), + "query_id"_a, "local_map"_a) + .def("_GetBestClosure", + py::overload_cast &, + const std::vector &, + const std::vector &>(&MapClosures::GetBestClosure), + "query_id"_a, "local_map"_a, "voxel_means"_a, "voxel_normals"_a) + .def("_GetTopKClosures", + py::overload_cast &, int>( + &MapClosures::GetTopKClosures), + "query_id"_a, "local_map"_a, "k"_a) + .def("_GetTopKClosures", + py::overload_cast &, + const std::vector &, + const std::vector &, int>( + &MapClosures::GetTopKClosures), + "query_id"_a, "local_map"_a, "voxel_means"_a, "voxel_normals"_a, "k"_a) + .def( + "_GetClosures", + py::overload_cast &>(&MapClosures::GetClosures), + "query_id"_a, "local_map"_a) + .def("_GetClosures", + py::overload_cast &, + const std::vector &, + const std::vector &>(&MapClosures::GetClosures), + "query_id"_a, "local_map"_a, "voxel_means"_a, "voxel_normals"_a) .def("_SaveHbstDatabase", &MapClosures::SaveHbstDatabase, "database_path"_a); py::class_ internal_map(m, "_VoxelMap", "Don't use this"); diff --git a/python/pyproject.toml b/python/pyproject.toml index 07e690f..6f06602 100644 --- a/python/pyproject.toml +++ b/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