2424#include " GroundAlign.hpp"
2525
2626#include < Eigen/Core>
27+ #include < Eigen/Eigenvalues>
28+ #include < Eigen/Geometry>
2729#include < algorithm>
2830#include < numeric>
2931#include < sophus/se3.hpp>
3234#include < vector>
3335
3436namespace {
37+ using Vector3dVector = std::vector<Eigen::Vector3d>;
38+ using LinearSystem = std::pair<Eigen::Matrix3d, Eigen::Vector3d>;
39+
3540struct PixelHash {
3641 size_t operator ()(const Eigen::Vector2i &pixel) const {
3742 const uint32_t *vec = reinterpret_cast <const uint32_t *>(pixel.data ());
3843 return (vec[0 ] * 73856093 ^ vec[1 ] * 19349669 );
3944 }
4045};
4146
42- void TransformPoints (const Sophus::SE3d &T, std::vector<Eigen::Vector3d> &pointcloud) {
47+ void TransformPoints (const Sophus::SE3d &T, Vector3dVector &pointcloud) {
4348 std::transform (pointcloud.cbegin (), pointcloud.cend (), pointcloud.begin (),
44- [&](const auto &point) { return T * point; });
49+ [&](const Eigen::Vector3d &point) { return T * point; });
4550}
4651
47- using LinearSystem = std::pair<Eigen::Matrix3d, Eigen::Vector3d>;
48- LinearSystem BuildLinearSystem (const std::vector<Eigen::Vector3d> &points,
49- const double resolution) {
50- auto compute_jacobian_and_residual = [](const auto &point) {
52+ struct VoxelMeanAndNormal {
53+ Eigen::Vector3d mean;
54+ Eigen::Vector3d normal;
55+ };
56+
57+ auto PointToPixel = [](const Eigen::Vector3d &pt) -> Eigen::Vector2i {
58+ return Eigen::Vector2i (static_cast <int >(std::floor (pt.x ())),
59+ static_cast <int >(std::floor (pt.y ())));
60+ };
61+
62+ std::pair<Vector3dVector, Sophus::SE3d> SampleGroundPoints (const Vector3dVector &voxel_means,
63+ const Vector3dVector &voxel_normals) {
64+ std::unordered_map<Eigen::Vector2i, VoxelMeanAndNormal, PixelHash> lowest_voxel_hash_map;
65+
66+ for (size_t index = 0 ; index < voxel_means.size (); ++index) {
67+ const Eigen::Vector3d &mean = voxel_means[index];
68+ const Eigen::Vector3d &normal = voxel_normals[index];
69+ const Eigen::Vector2i pixel = PointToPixel (mean);
70+
71+ auto it = lowest_voxel_hash_map.find (pixel);
72+ if (it == lowest_voxel_hash_map.end ()) {
73+ lowest_voxel_hash_map.emplace (pixel, VoxelMeanAndNormal{mean, normal});
74+ } else if (mean.z () < it->second .mean .z ()) {
75+ it->second = VoxelMeanAndNormal{mean, normal};
76+ }
77+ }
78+
79+ std::vector<VoxelMeanAndNormal> low_lying_voxels (lowest_voxel_hash_map.size ());
80+ std::transform (lowest_voxel_hash_map.cbegin (), lowest_voxel_hash_map.cend (),
81+ low_lying_voxels.begin (), [](const auto &entry) { return entry.second ; });
82+
83+ const Eigen::Matrix3d covariance_matrix =
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+ }) /
89+ static_cast <double >(low_lying_voxels.size () - 1 );
90+
91+ Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver (covariance_matrix);
92+ Eigen::Vector3d largest_eigenvector = eigensolver.eigenvectors ().col (2 );
93+ largest_eigenvector =
94+ (largest_eigenvector.z () < 0 ) ? -1.0 * largest_eigenvector : largest_eigenvector;
95+ const Eigen::Vector3d axis = largest_eigenvector.cross (Eigen::Vector3d (0.0 , 0.0 , 1.0 ));
96+ const double angle = std::acos (std::clamp (largest_eigenvector.z (), -1.0 , 1.0 ));
97+ const double axis_norm = axis.norm ();
98+
99+ const Eigen::Matrix3d R = axis_norm > 1e-3
100+ ? Eigen::AngleAxisd (angle, axis / axis_norm).toRotationMatrix ()
101+ : Eigen::Matrix3d::Identity ();
102+
103+ Eigen::Vector3d ground_centroid (0.0 , 0.0 , 0.0 );
104+ Vector3dVector ground_samples;
105+ ground_samples.reserve (low_lying_voxels.size ());
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+ });
113+ ground_samples.shrink_to_fit ();
114+ ground_centroid /= static_cast <double >(ground_samples.size ());
115+
116+ const double z_shift = R.row (2 ) * ground_centroid;
117+ const Sophus::SE3d T_init (R, Eigen::Vector3d (0.0 , 0.0 , -1.0 * z_shift));
118+ return std::make_pair (ground_samples, T_init);
119+ }
120+
121+ LinearSystem BuildLinearSystem (const Vector3dVector &points) {
122+ auto compute_jacobian_and_residual = [](const Eigen::Vector3d &point) {
51123 const double residual = point.z ();
52124 Eigen::Matrix<double , 1 , 3 > J;
53125 J (0 , 0 ) = 1.0 ;
@@ -65,55 +137,31 @@ LinearSystem BuildLinearSystem(const std::vector<Eigen::Vector3d> &points,
65137 const auto &[H, b] =
66138 std::transform_reduce (points.cbegin (), points.cend (),
67139 LinearSystem (Eigen::Matrix3d::Zero (), Eigen::Vector3d::Zero ()),
68- sum_linear_systems, [&](const auto &point) {
140+ sum_linear_systems, [&](const Eigen::Vector3d &point) {
69141 const auto &[J, residual] = compute_jacobian_and_residual (point);
70- const double w = std::abs (residual) <= resolution ? 1.0 : 0.0 ;
142+ const double w = std::exp (- 1.0 * residual * residual) ;
71143 return LinearSystem (J.transpose () * w * J, // JTJ
72144 J.transpose () * w * residual); // JTr
73145 });
74146 return {H, b};
75147}
76148
77149static constexpr double convergence_threshold = 1e-3 ;
78- static constexpr int max_iterations = 20 ;
79-
80- std::vector<Eigen::Vector3d> ComputeLowestPoints (const std::vector<Eigen::Vector3d> &pointcloud,
81- const double resolution) {
82- std::unordered_map<Eigen::Vector2i, Eigen::Vector3d, PixelHash> lowest_point_hash_map;
83- auto PointToPixel = [&resolution](const Eigen::Vector3d &pt) -> Eigen::Vector2i {
84- return Eigen::Vector2i (static_cast <int >(std::floor (pt.x () / resolution)),
85- static_cast <int >(std::floor (pt.y () / resolution)));
86- };
87-
88- std::for_each (pointcloud.cbegin (), pointcloud.cend (), [&](const Eigen::Vector3d &point) {
89- const auto &pixel = PointToPixel (point);
90- if (lowest_point_hash_map.find (pixel) == lowest_point_hash_map.cend ()) {
91- lowest_point_hash_map.emplace (pixel, point);
92- } else if (point.z () < lowest_point_hash_map[pixel].z ()) {
93- lowest_point_hash_map[pixel] = point;
94- }
95- });
96-
97- std::vector<Eigen::Vector3d> low_lying_points (lowest_point_hash_map.size ());
98- std::transform (lowest_point_hash_map.cbegin (), lowest_point_hash_map.cend (),
99- low_lying_points.begin (), [](const auto &entry) { return entry.second ; });
100- return low_lying_points;
101- }
150+ static constexpr int max_iterations = 10 ;
102151} // namespace
103152
104153namespace map_closures {
105- Eigen::Matrix4d AlignToLocalGround (const std::vector<Eigen::Vector3d> &pointcloud,
106- const double resolution) {
107- Sophus::SE3d T = Sophus::SE3d (Eigen::Matrix3d::Identity (), Eigen::Vector3d::Zero ());
108- auto low_lying_points = ComputeLowestPoints (pointcloud, resolution);
109-
154+ Eigen::Matrix4d AlignToLocalGround (const Vector3dVector &voxel_means,
155+ const Vector3dVector &voxel_normals) {
156+ auto [ground_samples, T] = SampleGroundPoints (voxel_means, voxel_normals);
157+ TransformPoints (T, ground_samples);
110158 for (int iters = 0 ; iters < max_iterations; iters++) {
111- const auto &[H, b] = BuildLinearSystem (low_lying_points, resolution );
159+ const auto &[H, b] = BuildLinearSystem (ground_samples );
112160 const Eigen::Vector3d &dx = H.ldlt ().solve (-b);
113161 Eigen::Matrix<double , 6 , 1 > se3 = Eigen::Matrix<double , 6 , 1 >::Zero ();
114162 se3.block <3 , 1 >(2 , 0 ) = dx;
115163 Sophus::SE3d estimation (Sophus::SE3d::exp (se3));
116- TransformPoints (estimation, low_lying_points );
164+ TransformPoints (estimation, ground_samples );
117165 T = estimation * T;
118166 if (dx.norm () < convergence_threshold) break ;
119167 }
0 commit comments