Skip to content

API consistency fix #95

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
saurabh1002 merged 1 commit into from
Dec 1, 2025
Merged

API consistency fix #95

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 7 additions & 2 deletions cpp/map_closures/GroundAlign.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>;
Expand Down Expand Up @@ -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++) {
Expand Down
4 changes: 2 additions & 2 deletions cpp/map_closures/GroundAlign.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
14 changes: 4 additions & 10 deletions cpp/map_closures/MapClosures.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
};
Expand Down
19 changes: 5 additions & 14 deletions cpp/map_closures/MapClosures.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -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_;
Expand Down
45 changes: 6 additions & 39 deletions python/map_closures/map_closures.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
5 changes: 1 addition & 4 deletions python/map_closures/pipeline.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
9 changes: 3 additions & 6 deletions python/map_closures/pybind/map_closures_pybind.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down
Loading