diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index 3e5e8c9..97004e0 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -33,6 +33,8 @@ #include #include +#include "VoxelMap.hpp" + namespace { using Vector3dVector = std::vector; using LinearSystem = std::pair; @@ -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++) { diff --git a/cpp/map_closures/GroundAlign.hpp b/cpp/map_closures/GroundAlign.hpp index d88d11a..04067db 100644 --- a/cpp/map_closures/GroundAlign.hpp +++ b/cpp/map_closures/GroundAlign.hpp @@ -27,6 +27,6 @@ #include namespace map_closures { -Eigen::Matrix4d AlignToLocalGround(const std::vector &voxel_means, - const std::vector &voxel_normals); +Eigen::Matrix4d AlignToLocalGround(const std::vector &pointcloud, + const double resolution); } // namespace map_closures diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index 85d34dd..136ef5e 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/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 &local_map, - const std::vector &voxel_means, - const std::vector &voxel_normals) { - const Eigen::Matrix4d T_ground = AlignToLocalGround(voxel_means, voxel_normals); + 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, config_.density_threshold); cv::Mat orb_descriptors; @@ -143,12 +141,8 @@ ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int } 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); + const int query_id, const std::vector &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; }; diff --git a/cpp/map_closures/MapClosures.hpp b/cpp/map_closures/MapClosures.hpp index 39b7c66..b4b3bf9 100644 --- a/cpp/map_closures/MapClosures.hpp +++ b/cpp/map_closures/MapClosures.hpp @@ -61,10 +61,8 @@ class MapClosures { ~MapClosures() = default; ClosureCandidate GetBestClosure(const int query_id, - const std::vector &local_map, - const std::vector &voxel_means, - const std::vector &voxel_normals) { - const auto &closures = GetTopKClosures(query_id, local_map, voxel_means, voxel_normals, 1); + const std::vector &local_map) { + const auto &closures = GetTopKClosures(query_id, local_map, 1); if (closures.empty()) { return ClosureCandidate(); } @@ -72,14 +70,10 @@ class MapClosures { } 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 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 std::vector &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 &local_map, - const std::vector &voxel_means, - const std::vector &voxel_normals); + void MatchAndAddToDatabase(const int id, const std::vector &local_map); 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 6f93ed2..1b6a713 100644 --- a/python/map_closures/map_closures.py +++ b/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: diff --git a/python/map_closures/pipeline.py b/python/map_closures/pipeline.py index 2a04230..6bcabe6 100644 --- a/python/map_closures/pipeline.py +++ b/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( diff --git a/python/map_closures/pybind/map_closures_pybind.cpp b/python/map_closures/pybind/map_closures_pybind.cpp index c8ceb6e..c6b9c01 100644 --- a/python/map_closures/pybind/map_closures_pybind.cpp +++ b/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_ internal_map(m, "_VoxelMap", "Don't use this");