Skip to content

Optimize loops and implement caching improvements #117

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 2 commits into from
Aug 14, 2026
Merged

Optimize loops and implement caching improvements #117

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3dad02c
optimize loops, cache among others
saurabh1002 Aug 14, 2026
db96452
fix bug
saurabh1002 Aug 14, 2026
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
26 changes: 14 additions & 12 deletions cpp/map_closures/DensityMap.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,12 +55,11 @@ DensityMap GenerateDensityMap(const std::vector<Eigen::Vector3d> &pcd,
Eigen::Array2i lower_bound_coordinates = Eigen::Array2i::Constant(max_int);
Eigen::Array2i upper_bound_coordinates = Eigen::Array2i::Constant(min_int);

const auto R2x3 = T_ground.block<2, 3>(0, 0);
const auto t2x1 = T_ground.block<2, 1>(0, 3);
const auto inv_resolution = 1.0 / density_map_resolution;
auto Discretize2D = [&](const Eigen::Vector3d &p) -> Eigen::Array2i {
return ((T_ground.block<2, 3>(0, 0) * p + T_ground.block<2, 1>(0, 3)) /
density_map_resolution)
.array()
.floor()
.cast<int>();
return ((R2x3 * p + t2x1) * inv_resolution).array().floor().cast<int>();
};
std::vector<Eigen::Array2i> pixels(pcd.size());
std::transform(pcd.cbegin(), pcd.cend(), pixels.begin(), [&](const Eigen::Vector3d &point) {
Expand All @@ -74,20 +73,23 @@ DensityMap GenerateDensityMap(const std::vector<Eigen::Vector3d> &pcd,
const int n_cols = rows_and_columns.y() + 1;

cv::Mat counting_grid(n_rows, n_cols, CV_64FC1, 0.0);
std::for_each(pixels.cbegin(), pixels.cend(), [&](const Eigen::Array2i &pixel) {
for (const Eigen::Array2i &pixel : pixels) {
const Eigen::Array2i px = pixel - lower_bound_coordinates;
double &count = counting_grid.at<double>(px.x(), px.y());
count += 1.0;
max_points = std::max(max_points, count);
min_points = std::min(min_points, count);
});
}

DensityMap density_map(n_rows, n_cols, density_map_resolution, lower_bound_coordinates);
counting_grid.forEach<double>([&](const double count, const int pos[]) {
double density = (count - min_points) / (max_points - min_points);
density = density > density_threshold ? density : 0.0;
density_map(pos[0], pos[1]) = static_cast<uint8_t>(255 * density);
});
for (int y = 0, x = 0; y < n_rows; ++y) {
for (x = 0; x < n_cols; ++x) {
const double count = counting_grid.at<double>(y, x);
double density = (count - min_points) / (max_points - min_points);
density = density > density_threshold ? density : 0.0;
density_map(y, x) = static_cast<uint8_t>(255 * density);
}
}

return density_map;
}
Expand Down
48 changes: 25 additions & 23 deletions cpp/map_closures/GroundAlign.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,11 @@ struct PixelHash {
};

void TransformPoints(const Sophus::SE3d &T, Vector3dVector &pointcloud) {
std::transform(pointcloud.cbegin(), pointcloud.cend(), pointcloud.begin(),
[&](const Eigen::Vector3d &point) { return T * point; });
const Eigen::Matrix3d R = T.rotationMatrix();
const Eigen::Vector3d t = T.translation();
for (auto &point : pointcloud) {
point = R * point + t;
}
}

struct VoxelMeanAndNormal {
Expand All @@ -69,8 +72,8 @@ std::pair<Vector3dVector, Sophus::SE3d> SampleGroundPoints(const Vector3dVector
std::unordered_map<Eigen::Vector2i, VoxelMeanAndNormal, PixelHash> lowest_voxel_hash_map;
lowest_voxel_hash_map.reserve(voxel_means.size());
for (size_t index = 0; index < voxel_means.size(); ++index) {
const Eigen::Vector3d mean = voxel_means[index];
const Eigen::Vector3d normal = voxel_normals[index];
const Eigen::Vector3d &mean = voxel_means[index];
const Eigen::Vector3d &normal = voxel_normals[index];
const Eigen::Vector2i pixel = PointToPixel(mean);

const auto it = lowest_voxel_hash_map.find(pixel);
Expand All @@ -89,13 +92,11 @@ std::pair<Vector3dVector, Sophus::SE3d> SampleGroundPoints(const Vector3dVector
return {Vector3dVector(), Sophus::SE3d()};
}

const Eigen::Matrix3d normals_covariance_matrix =
std::transform_reduce(low_lying_voxels.cbegin(), low_lying_voxels.cend(),
Eigen::Matrix3d().setZero(), std::plus<Eigen::Matrix3d>(),
[&](const VoxelMeanAndNormal &voxel) {
return voxel.normal * voxel.normal.transpose();
}) /
static_cast<double>(low_lying_voxels.size() - 1);
Eigen::Matrix3d normals_covariance_matrix = Eigen::Matrix3d::Zero();
for (const VoxelMeanAndNormal &voxel : low_lying_voxels) {
normals_covariance_matrix += voxel.normal * voxel.normal.transpose();
}
normals_covariance_matrix /= (static_cast<double>(low_lying_voxels.size()) - 1);

const Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(normals_covariance_matrix);
Eigen::Vector3d largest_eigenvector = eigensolver.eigenvectors().col(2);
Expand All @@ -112,26 +113,25 @@ std::pair<Vector3dVector, Sophus::SE3d> SampleGroundPoints(const Vector3dVector
Eigen::Vector3d ground_centroid(0.0, 0.0, 0.0);
Vector3dVector ground_samples;
ground_samples.reserve(low_lying_voxels.size());
std::for_each(
low_lying_voxels.cbegin(), low_lying_voxels.cend(), [&](const VoxelMeanAndNormal &voxel) {
if (std::abs(voxel.normal.dot(largest_eigenvector)) > normal_filter_threshold) {
ground_centroid += voxel.mean;
ground_samples.emplace_back(voxel.mean);
}
});
for (const VoxelMeanAndNormal &voxel : low_lying_voxels) {
if (std::abs(voxel.normal.dot(largest_eigenvector)) > normal_filter_threshold) {
ground_centroid += voxel.mean;
ground_samples.emplace_back(voxel.mean);
}
}
if (ground_samples.empty()) {
return {Vector3dVector(), Sophus::SE3d()};
}
ground_centroid /= static_cast<double>(ground_samples.size());

ground_centroid /= static_cast<double>(ground_samples.size());
const double z_shift = R.row(2) * ground_centroid;
return {std::move(ground_samples), Sophus::SE3d(R, Eigen::Vector3d(0.0, 0.0, -1.0 * z_shift))};
}

LinearSystem BuildLinearSystem(const Vector3dVector &points) {
auto compute_jacobian_and_residual =
[](const Eigen::Vector3d &point) -> std::pair<Eigen::Matrix<double, 1, 3>, double> {
return {Eigen::Matrix<double, 1, 3>(1.0, point.y(), -point.x()), point.z()};
[](const Eigen::Vector3d &point) -> std::pair<Eigen::RowVector3d, double> {
return {Eigen::RowVector3d(1.0, point.y(), -point.x()), point.z()};
};

auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) -> LinearSystem {
Expand All @@ -144,7 +144,7 @@ LinearSystem BuildLinearSystem(const Vector3dVector &points) {
std::transform_reduce(points.cbegin(), points.cend(),
LinearSystem(Eigen::Matrix3d::Zero(), Eigen::Vector3d::Zero()),
sum_linear_systems, [&](const Eigen::Vector3d &point) {
const auto [J, residual] = compute_jacobian_and_residual(point);
const auto &[J, residual] = compute_jacobian_and_residual(point);
const double w = std::exp(-1.0 * residual * residual);
return LinearSystem(J.transpose() * w * J, // JTJ
J.transpose() * w * residual); // JTr
Expand All @@ -157,10 +157,11 @@ namespace map_closures {
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();
const auto &[voxel_means, voxel_normals] = voxel_map.PerVoxelMeanAndNormal();

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);
Expand All @@ -179,6 +180,7 @@ 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);
Expand Down
134 changes: 65 additions & 69 deletions cpp/map_closures/MapClosures.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@
#include "MapClosures.hpp"

#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <numeric>
#include <opencv2/core.hpp>
Expand Down Expand Up @@ -72,28 +71,27 @@ void MapClosures::MatchAndAddToDatabase(const int id,
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_keypoints_.clear();
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);
self_matches_.clear();
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));
}
});
for (const auto &self_match : self_matches_) {
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,
Expand All @@ -110,29 +108,29 @@ void MapClosures::MatchAndAddToDatabase(const int id,
Eigen::Matrix4d T_ground = AlignToLocalGround(voxel_means, voxel_normals);
DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution,
config_.density_threshold);

orb_keypoints_.clear();
orb_keypoints_.reserve(nfeatures);
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_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);
self_matches_.clear();
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));
}
});
for (const auto &self_match : self_matches_) {
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,
Expand All @@ -147,28 +145,27 @@ void MapClosures::Match(const std::vector<Eigen::Vector3d> &local_map) {
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_keypoints_.clear();
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);
self_matches_.clear();
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)));
}
});
for (const auto &self_match : self_matches_) {
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);
}
Expand All @@ -180,28 +177,27 @@ void MapClosures::Match(const std::vector<Eigen::Vector3d> &local_map,
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_keypoints_.clear();
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);
self_matches_.clear();
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)));
}
});
for (const auto &self_match : self_matches_) {
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);
}
Expand Down
7 changes: 5 additions & 2 deletions cpp/map_closures/MapClosures.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ class MapClosures {

ClosureCandidate GetBestClosure(const int query_id,
const std::vector<Eigen::Vector3d> &local_map) {
std::vector<ClosureCandidate> closures = GetTopKClosures(query_id, local_map, 1);
const std::vector<ClosureCandidate> closures = GetTopKClosures(query_id, local_map, 1);
if (closures.empty()) {
return ClosureCandidate();
}
Expand All @@ -71,7 +71,7 @@ class MapClosures {
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 =
const std::vector<ClosureCandidate> closures =
GetTopKClosures(query_id, local_map, voxel_means, voxel_normals, 1);
if (closures.empty()) {
return ClosureCandidate();
Expand Down Expand Up @@ -129,6 +129,9 @@ class MapClosures {
std::unordered_map<int, Eigen::Matrix4d> ground_alignments_;
std::unique_ptr<Tree> hbst_binary_tree_ = std::make_unique<Tree>();
cv::Ptr<cv::DescriptorExtractor> orb_extractor_;

std::vector<cv::KeyPoint> orb_keypoints_;
std::vector<std::vector<cv::DMatch>> self_matches_;
cv::BFMatcher self_matcher_ = cv::BFMatcher(cv::NORM_HAMMING);
};
} // namespace map_closures
Loading
Loading