Skip to content

Commit c6fc1bd

Browse files
saurabh1002saurabh1002
committed
make types explicit, fix alias declarations inside namespaces
1 parent 9595d74 commit c6fc1bd

File tree

8 files changed

+71
-68
lines changed

8 files changed

+71
-68
lines changed

‎cpp/map_closures/AlignRansac2D.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ Eigen::Isometry2d KabschUmeyamaAlignment2D(
3838
const std::vector<map_closures::PointPair> &keypoint_pairs) {
3939
map_closures::PointPair mean =
4040
std::reduce(keypoint_pairs.cbegin(), keypoint_pairs.cend(), map_closures::PointPair(),
41-
[](auto lhs, const auto &rhs) {
41+
[](map_closures::PointPair lhs, const map_closures::PointPair &rhs) {
4242
lhs.ref += rhs.ref;
4343
lhs.query += rhs.query;
4444
return lhs;
@@ -47,7 +47,7 @@ Eigen::Isometry2d KabschUmeyamaAlignment2D(
4747
mean.ref /= static_cast<double>(keypoint_pairs.size());
4848
Eigen::Matrix2d covariance_matrix = std::transform_reduce(
4949
keypoint_pairs.cbegin(), keypoint_pairs.cend(), Eigen::Matrix2d().setZero(),
50-
std::plus<Eigen::Matrix2d>(), [&](const auto &keypoint_pair) {
50+
std::plus<Eigen::Matrix2d>(), [&](const map_closures::PointPair &keypoint_pair) {
5151
return (keypoint_pair.ref - mean.ref) *
5252
((keypoint_pair.query - mean.query).transpose());
5353
});
@@ -93,11 +93,11 @@ std::pair<Eigen::Isometry2d, std::size_t> RansacAlignment2D(
9393

9494
std::sample(keypoint_pairs.begin(), keypoint_pairs.end(), sample_keypoint_pairs.begin(), 2,
9595
std::mt19937{std::random_device{}()});
96-
const auto &T = KabschUmeyamaAlignment2D(sample_keypoint_pairs);
96+
const Eigen::Isometry2d &T = KabschUmeyamaAlignment2D(sample_keypoint_pairs);
9797

9898
int index = 0;
9999
std::for_each(keypoint_pairs.cbegin(), keypoint_pairs.cend(),
100-
[&](const auto &keypoint_pair) {
100+
[&](const PointPair &keypoint_pair) {
101101
if ((T * keypoint_pair.ref - keypoint_pair.query).norm() <
102102
inliers_distance_threshold)
103103
inlier_indices.emplace_back(index);

‎cpp/map_closures/DensityMap.cpp‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -65,26 +65,26 @@ DensityMap GenerateDensityMap(const std::vector<Eigen::Vector3d> &pcd,
6565
};
6666
std::vector<Eigen::Array2i> pixels(pcd.size());
6767
std::transform(pcd.cbegin(), pcd.cend(), pixels.begin(), [&](const Eigen::Vector3d &point) {
68-
const auto &pixel = Discretize2D(point);
68+
const Eigen::Array2i &pixel = Discretize2D(point);
6969
lower_bound_coordinates = lower_bound_coordinates.min(pixel);
7070
upper_bound_coordinates = upper_bound_coordinates.max(pixel);
7171
return pixel;
7272
});
73-
const auto rows_and_columns = upper_bound_coordinates - lower_bound_coordinates;
74-
const auto n_rows = rows_and_columns.x() + 1;
75-
const auto n_cols = rows_and_columns.y() + 1;
73+
const Eigen::Array2i rows_and_columns = upper_bound_coordinates - lower_bound_coordinates;
74+
const int n_rows = rows_and_columns.x() + 1;
75+
const int n_cols = rows_and_columns.y() + 1;
7676

7777
cv::Mat counting_grid(n_rows, n_cols, CV_64FC1, 0.0);
78-
std::for_each(pixels.cbegin(), pixels.cend(), [&](const auto &pixel) {
79-
const auto px = pixel - lower_bound_coordinates;
78+
std::for_each(pixels.cbegin(), pixels.cend(), [&](const Eigen::Array2i &pixel) {
79+
const Eigen::Array2i px = pixel - lower_bound_coordinates;
8080
counting_grid.at<double>(px.x(), px.y()) += 1;
8181
max_points = std::max(max_points, counting_grid.at<double>(px.x(), px.y()));
8282
min_points = std::min(min_points, counting_grid.at<double>(px.x(), px.y()));
8383
});
8484

8585
DensityMap density_map(n_rows, n_cols, density_map_resolution, lower_bound_coordinates);
8686
counting_grid.forEach<double>([&](const double count, const int pos[]) {
87-
auto density = (count - min_points) / (max_points - min_points);
87+
double density = (count - min_points) / (max_points - min_points);
8888
density = density > density_threshold ? density : 0.0;
8989
density_map(pos[0], pos[1]) = static_cast<uint8_t>(255 * density);
9090
});

‎cpp/map_closures/DensityMap.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ struct DensityMap {
3838
DensityMap(DensityMap &&other) = default;
3939
DensityMap &operator=(DensityMap &&other) = default;
4040
DensityMap &operator=(const DensityMap &other) = delete;
41-
inline auto &operator()(const int x, const int y) { return grid.at<uint8_t>(x, y); }
41+
inline uint8_t &operator()(const int x, const int y) { return grid.at<uint8_t>(x, y); }
4242
Eigen::Vector2i lower_bound;
4343
double resolution;
4444
cv::Mat grid;

‎cpp/map_closures/GroundAlign.cpp‎

Lines changed: 18 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,9 @@
3434
#include <vector>
3535

3636
namespace {
37+
using Vector3dVector = std::vector<Eigen::Vector3d>;
38+
using LinearSystem = std::pair<Eigen::Matrix3d, Eigen::Vector3d>;
39+
3740
struct PixelHash {
3841
size_t operator()(const Eigen::Vector2i &pixel) const {
3942
const uint32_t *vec = reinterpret_cast<const uint32_t *>(pixel.data());
@@ -43,7 +46,7 @@ struct PixelHash {
4346

4447
void TransformPoints(const Sophus::SE3d &T, Vector3dVector &pointcloud) {
4548
std::transform(pointcloud.cbegin(), pointcloud.cend(), pointcloud.begin(),
46-
[&](const auto &point) { return T * point; });
49+
[&](const Eigen::Vector3d &point) { return T * point; });
4750
}
4851

4952
struct VoxelMeanAndNormal {
@@ -78,10 +81,11 @@ std::pair<Vector3dVector, Sophus::SE3d> SampleGroundPoints(const Vector3dVector
7881
low_lying_voxels.begin(), [](const auto &entry) { return entry.second; });
7982

8083
const Eigen::Matrix3d covariance_matrix =
81-
std::transform_reduce(
82-
low_lying_voxels.cbegin(), low_lying_voxels.cend(), Eigen::Matrix3d().setZero(),
83-
std::plus<Eigen::Matrix3d>(),
84-
[&](const auto &voxel) { return voxel.normal * voxel.normal.transpose(); }) /
84+
std::transform_reduce(low_lying_voxels.cbegin(), low_lying_voxels.cend(),
85+
Eigen::Matrix3d().setZero(), std::plus<Eigen::Matrix3d>(),
86+
[&](const VoxelMeanAndNormal &voxel) {
87+
return voxel.normal * voxel.normal.transpose();
88+
}) /
8589
static_cast<double>(low_lying_voxels.size() - 1);
8690

8791
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(covariance_matrix);
@@ -99,12 +103,13 @@ std::pair<Vector3dVector, Sophus::SE3d> SampleGroundPoints(const Vector3dVector
99103
Eigen::Vector3d ground_centroid(0.0, 0.0, 0.0);
100104
Vector3dVector ground_samples;
101105
ground_samples.reserve(low_lying_voxels.size());
102-
std::for_each(low_lying_voxels.cbegin(), low_lying_voxels.cend(), [&](const auto &voxel) {
103-
if (std::abs(voxel.normal.dot(largest_eigenvector)) > 0.95) {
104-
ground_centroid += voxel.mean;
105-
ground_samples.emplace_back(voxel.mean);
106-
}
107-
});
106+
std::for_each(low_lying_voxels.cbegin(), low_lying_voxels.cend(),
107+
[&](const VoxelMeanAndNormal &voxel) {
108+
if (std::abs(voxel.normal.dot(largest_eigenvector)) > 0.95) {
109+
ground_centroid += voxel.mean;
110+
ground_samples.emplace_back(voxel.mean);
111+
}
112+
});
108113
ground_samples.shrink_to_fit();
109114
ground_centroid /= static_cast<double>(ground_samples.size());
110115

@@ -113,9 +118,8 @@ std::pair<Vector3dVector, Sophus::SE3d> SampleGroundPoints(const Vector3dVector
113118
return std::make_pair(ground_samples, T_init);
114119
}
115120

116-
using LinearSystem = std::pair<Eigen::Matrix3d, Eigen::Vector3d>;
117121
LinearSystem BuildLinearSystem(const Vector3dVector &points) {
118-
auto compute_jacobian_and_residual = [](const auto &point) {
122+
auto compute_jacobian_and_residual = [](const Eigen::Vector3d &point) {
119123
const double residual = point.z();
120124
Eigen::Matrix<double, 1, 3> J;
121125
J(0, 0) = 1.0;
@@ -133,7 +137,7 @@ LinearSystem BuildLinearSystem(const Vector3dVector &points) {
133137
const auto &[H, b] =
134138
std::transform_reduce(points.cbegin(), points.cend(),
135139
LinearSystem(Eigen::Matrix3d::Zero(), Eigen::Vector3d::Zero()),
136-
sum_linear_systems, [&](const auto &point) {
140+
sum_linear_systems, [&](const Eigen::Vector3d &point) {
137141
const auto &[J, residual] = compute_jacobian_and_residual(point);
138142
const double w = std::exp(-1.0 * residual * residual);
139143
return LinearSystem(J.transpose() * w * J, // JTJ

‎cpp/map_closures/GroundAlign.hpp‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -26,9 +26,7 @@
2626
#include <Eigen/Core>
2727
#include <vector>
2828

29-
using Vector3dVector = std::vector<Eigen::Vector3d>;
30-
3129
namespace map_closures {
32-
Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &voxel_means,
33-
const Vector3dVector &voxel_normals);
30+
Eigen::Matrix4d AlignToLocalGround(const std::vector<Eigen::Vector3d> &voxel_means,
31+
const std::vector<Eigen::Vector3d> &voxel_normals);
3432
} // namespace map_closures

‎cpp/map_closures/MapClosures.cpp‎

Lines changed: 14 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -88,16 +88,17 @@ void MapClosures::MatchAndAddToDatabase(const int id,
8888

8989
std::vector<Matchable *> hbst_matchable;
9090
hbst_matchable.reserve(orb_descriptors.rows);
91-
std::for_each(self_matches.cbegin(), self_matches.cend(), [&](const auto &self_match) {
92-
if (self_match[1].distance > self_similarity_threshold) {
93-
const auto index_descriptor = self_match[0].queryIdx;
94-
auto &keypoint = orb_keypoints[index_descriptor];
95-
keypoint.pt.x = keypoint.pt.x + static_cast<float>(density_map.lower_bound.y());
96-
keypoint.pt.y = keypoint.pt.y + static_cast<float>(density_map.lower_bound.x());
97-
hbst_matchable.emplace_back(
98-
new Matchable(keypoint, orb_descriptors.row(index_descriptor), id));
99-
}
100-
});
91+
std::for_each(
92+
self_matches.cbegin(), self_matches.cend(), [&](const std::vector<cv::DMatch> &self_match) {
93+
if (self_match[1].distance > self_similarity_threshold) {
94+
const auto index_descriptor = self_match[0].queryIdx;
95+
cv::KeyPoint &keypoint = orb_keypoints[index_descriptor];
96+
keypoint.pt.x = keypoint.pt.x + static_cast<float>(density_map.lower_bound.y());
97+
keypoint.pt.y = keypoint.pt.y + static_cast<float>(density_map.lower_bound.x());
98+
hbst_matchable.emplace_back(
99+
new Matchable(keypoint, orb_descriptors.row(index_descriptor), id));
100+
}
101+
});
101102
hbst_matchable.shrink_to_fit();
102103

103104
hbst_binary_tree_->matchAndAdd(hbst_matchable, descriptor_matches_,
@@ -117,10 +118,10 @@ ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int
117118
std::vector<PointPair> keypoint_pairs(num_matches);
118119
std::transform(matches.cbegin(), matches.cend(), keypoint_pairs.begin(),
119120
[&](const Tree::Match &match) {
120-
const auto query_point =
121+
const Eigen::Vector2d query_point =
121122
Eigen::Vector2d(match.object_query.pt.y, match.object_query.pt.x);
122-
const auto ref_point = Eigen::Vector2d(match.object_references[0].pt.y,
123-
match.object_references[0].pt.x);
123+
const Eigen::Vector2d ref_point = Eigen::Vector2d(
124+
match.object_references[0].pt.y, match.object_references[0].pt.x);
124125
return PointPair(ref_point, query_point);
125126
});
126127

‎cpp/map_closures/VoxelMap.cpp‎

Lines changed: 20 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -32,10 +32,10 @@
3232

3333
namespace {
3434

35-
inline Voxel ToVoxelCoordinates(const Eigen::Vector3d &point, const double voxel_size) {
36-
return Voxel(static_cast<int>(std::floor(point.x() / voxel_size)),
37-
static_cast<int>(std::floor(point.y() / voxel_size)),
38-
static_cast<int>(std::floor(point.z() / voxel_size)));
35+
inline Eigen::Vector3i ToVoxelCoordinates(const Eigen::Vector3d &point, const double voxel_size) {
36+
return Eigen::Vector3i(static_cast<int>(std::floor(point.x() / voxel_size)),
37+
static_cast<int>(std::floor(point.y() / voxel_size)),
38+
static_cast<int>(std::floor(point.z() / voxel_size)));
3939
}
4040

4141
static constexpr unsigned int min_points_for_covariance_computation = 10;
@@ -79,22 +79,22 @@ VoxelMap::VoxelMap(const double voxel_size, const double max_distance)
7979

8080
void VoxelMap::IntegrateFrame(const Vector3dVector &points, const Eigen::Matrix4d &pose) {
8181
Vector3dVector points_transformed(points.size());
82-
const auto &R = pose.block<3, 3>(0, 0);
83-
const auto &t = pose.block<3, 1>(0, 3);
82+
const Eigen::Matrix3d &R = pose.block<3, 3>(0, 0);
83+
const Eigen::Vector3d &t = pose.block<3, 1>(0, 3);
8484
std::transform(points.cbegin(), points.cend(), points_transformed.begin(),
8585
[&](const auto &point) { return R * point + t; });
8686
AddPoints(points_transformed);
8787
}
8888

8989
void VoxelMap::AddPoints(const Vector3dVector &points) {
90-
std::for_each(points.cbegin(), points.cend(), [&](const auto &point) {
91-
const auto voxel = ToVoxelCoordinates(point, voxel_size_);
90+
std::for_each(points.cbegin(), points.cend(), [&](const Eigen::Vector3d &point) {
91+
const Voxel voxel = ToVoxelCoordinates(point, voxel_size_);
9292
const auto &[it, inserted] = map_.insert({voxel, VoxelBlock()});
9393
if (!inserted) {
94-
const auto &voxel_points = it->second;
95-
if (voxel_points.size() == max_points_per_normal_computation ||
96-
std::any_of(voxel_points.cbegin(), voxel_points.cend(),
97-
[&](const auto &voxel_point) {
94+
const VoxelBlock &voxel_block = it->second;
95+
if (voxel_block.size() == max_points_per_normal_computation ||
96+
std::any_of(voxel_block.cbegin(), voxel_block.cend(),
97+
[&](const Eigen::Vector3d &voxel_point) {
9898
return (voxel_point - point).norm() < map_resolution_;
9999
})) {
100100
return;
@@ -108,9 +108,10 @@ Vector3dVector VoxelMap::Pointcloud() const {
108108
Vector3dVector points;
109109
points.reserve(map_.size() * max_points_per_normal_computation);
110110
std::for_each(map_.cbegin(), map_.cend(), [&](const auto &map_element) {
111-
const auto &voxel_points = map_element.second;
112-
std::for_each(voxel_points.cbegin(), voxel_points.cend(),
113-
[&](const auto &p) { points.emplace_back(p.template cast<double>()); });
111+
const VoxelBlock &voxel_block = map_element.second;
112+
std::for_each(voxel_block.cbegin(), voxel_block.cend(), [&](const Eigen::Vector3d &p) {
113+
points.emplace_back(p.template cast<double>());
114+
});
114115
});
115116
points.shrink_to_fit();
116117
return points;
@@ -121,8 +122,8 @@ std::tuple<Vector3dVector, Vector3dVector> VoxelMap::PerVoxelMeanAndNormal() con
121122
voxel_means.reserve(map_.size());
122123
Vector3dVector voxel_normals;
123124
voxel_normals.reserve(map_.size());
124-
std::for_each(map_.cbegin(), map_.cend(), [&](const auto &inner_block) {
125-
const auto &voxel_block = inner_block.second;
125+
std::for_each(map_.cbegin(), map_.cend(), [&](const auto &map_element) {
126+
const VoxelBlock &voxel_block = map_element.second;
126127
if (voxel_block.size() >= min_points_for_covariance_computation) {
127128
const auto &[mean, normal] = ComputeMeanAndNormal(voxel_block);
128129
voxel_means.emplace_back(mean);
@@ -135,10 +136,10 @@ std::tuple<Vector3dVector, Vector3dVector> VoxelMap::PerVoxelMeanAndNormal() con
135136
}
136137

137138
void VoxelMap::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) {
138-
const auto max_distance2 = max_distance_ * max_distance_;
139+
const double max_distance2 = max_distance_ * max_distance_;
139140
for (auto it = map_.begin(); it != map_.end();) {
140141
const auto &[voxel, voxel_points] = *it;
141-
const auto &pt = voxel_points.front();
142+
const Eigen::Vector3d &pt = voxel_points.front();
142143
if ((pt - origin).squaredNorm() >= (max_distance2)) {
143144
it = map_.erase(it);
144145
} else {

‎cpp/map_closures/VoxelMap.hpp‎

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -29,12 +29,9 @@
2929
#include <unordered_map>
3030
#include <vector>
3131

32-
using Vector3dVector = std::vector<Eigen::Vector3d>;
33-
34-
using Voxel = Eigen::Vector3i;
3532
template <>
36-
struct std::hash<Voxel> {
37-
std::size_t operator()(const Voxel &voxel) const {
33+
struct std::hash<Eigen::Vector3i> {
34+
std::size_t operator()(const Eigen::Vector3i &voxel) const {
3835
const uint32_t *vec = reinterpret_cast<const uint32_t *>(voxel.data());
3936
return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791);
4037
}
@@ -44,13 +41,15 @@ struct std::hash<Voxel> {
4441
static constexpr unsigned int max_points_per_normal_computation = 20;
4542

4643
namespace map_closures {
44+
using Voxel = Eigen::Vector3i;
45+
using Vector3dVector = std::vector<Eigen::Vector3d>;
4746

4847
struct VoxelBlock {
4948
void emplace_back(const Eigen::Vector3d &point);
5049
inline constexpr size_t size() const { return num_points; }
5150
auto cbegin() const { return points.cbegin(); }
5251
auto cend() const { return std::next(points.cbegin(), num_points); }
53-
auto front() const { return points.front(); }
52+
Eigen::Vector3d front() const { return points.front(); }
5453
std::array<Eigen::Vector3d, max_points_per_normal_computation> points;
5554
size_t num_points = 0;
5655
};

0 commit comments

Comments
 (0)