2323#include " Registration.hpp"
2424
2525#include < tbb/blocked_range.h>
26+ #include < tbb/global_control.h>
27+ #include < tbb/info.h>
2628#include < tbb/parallel_reduce.h>
2729
2830#include < algorithm>
2931#include < cmath>
32+ #include < numeric>
3033#include < sophus/se3.hpp>
3134#include < sophus/so3.hpp>
3235#include < tuple>
3336
37+ #include " VoxelHashMap.hpp"
38+
3439namespace Eigen {
3540using Matrix6d = Eigen::Matrix<double , 6 , 6 >;
3641using Matrix3_6d = Eigen::Matrix<double , 3 , 6 >;
3742using Vector6d = Eigen::Matrix<double , 6 , 1 >;
3843} // namespace Eigen
3944
40- namespace {
45+ using Correspondences = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
46+ using LinearSystem = std::pair<Eigen::Matrix6d, Eigen::Vector6d>;
4147
48+ namespace {
4249inline double square (double x) { return x * x; }
4350
44- struct ResultTuple {
45- ResultTuple () {
46- JTJ.setZero ();
47- JTr.setZero ();
48- }
51+ void TransformPoints (const Sophus::SE3d &T, std::vector<Eigen::Vector3d> &points) {
52+ std::transform (points.cbegin (), points.cend (), points.begin (),
53+ [&](const auto &point) { return T * point; });
54+ }
4955
50- ResultTuple operator +(const ResultTuple &other) {
51- this ->JTJ += other.JTJ ;
52- this ->JTr += other.JTr ;
53- return *this ;
56+ using Voxel = mapmos::VoxelHashMap::Voxel;
57+ std::vector<Voxel> GetAdjacentVoxels (const Voxel &voxel, int adjacent_voxels = 1 ) {
58+ std::vector<Voxel> voxel_neighborhood;
59+ for (int i = voxel.x () - adjacent_voxels; i < voxel.x () + adjacent_voxels + 1 ; ++i) {
60+ for (int j = voxel.y () - adjacent_voxels; j < voxel.y () + adjacent_voxels + 1 ; ++j) {
61+ for (int k = voxel.z () - adjacent_voxels; k < voxel.z () + adjacent_voxels + 1 ; ++k) {
62+ voxel_neighborhood.emplace_back (i, j, k);
63+ }
64+ }
5465 }
66+ return voxel_neighborhood;
67+ }
5568
56- Eigen::Matrix6d JTJ;
57- Eigen::Vector6d JTr;
58- };
69+ std::tuple<Eigen::Vector3d, double > GetClosestNeighbor (const Eigen::Vector3d &point,
70+ const mapmos::VoxelHashMap &voxel_map) {
71+ // Convert the point to voxel coordinates
72+ const auto &voxel = voxel_map.PointToVoxel (point);
73+ // Get nearby voxels on the map
74+ const auto &query_voxels = GetAdjacentVoxels (voxel);
75+ // Extract the points contained within the neighborhood voxels
76+ const auto &neighbors = voxel_map.GetPoints (query_voxels);
77+
78+ // Find the nearest neighbor
79+ Eigen::Vector3d closest_neighbor;
80+ double closest_distance = std::numeric_limits<double >::max ();
81+ std::for_each (neighbors.cbegin (), neighbors.cend (), [&](const auto &neighbor) {
82+ double distance = (neighbor - point).norm ();
83+ if (distance < closest_distance) {
84+ closest_neighbor = neighbor;
85+ closest_distance = distance;
86+ }
87+ });
88+ return std::make_tuple (closest_neighbor, closest_distance);
89+ }
5990
60- void TransformPoints (const Sophus::SE3d &T, std::vector<Eigen::Vector3d> &points) {
61- std::transform (points.cbegin (), points.cend (), points.begin (),
62- [&](const auto &point) { return T * point; });
91+ Correspondences DataAssociation (const std::vector<Eigen::Vector3d> &points,
92+ const mapmos::VoxelHashMap &voxel_map,
93+ const double max_correspondance_distance) {
94+ using points_iterator = std::vector<Eigen::Vector3d>::const_iterator;
95+ Correspondences correspondences;
96+ correspondences.reserve (points.size ());
97+ correspondences = tbb::parallel_reduce (
98+ // Range
99+ tbb::blocked_range<points_iterator>{points.cbegin (), points.cend ()},
100+ // Identity
101+ correspondences,
102+ // 1st lambda: Parallel computation
103+ [&](
const tbb::blocked_range<points_iterator> &r, Correspondences res) -> Correspondences {
104+ res.reserve (r.size ());
105+ std::for_each (r.begin (), r.end (), [&](const auto &point) {
106+ const auto &[closest_neighbor, distance] = GetClosestNeighbor (point, voxel_map);
107+ if (distance < max_correspondance_distance) {
108+ res.emplace_back (point, closest_neighbor);
109+ }
110+ });
111+ return res;
112+ },
113+ // 2nd lambda: Parallel reduction
114+ [](Correspondences a, const Correspondences &b) -> Correspondences {
115+ a.insert (a.end (), //
116+ std::make_move_iterator (b.cbegin ()), //
117+ std::make_move_iterator (b.cend ()));
118+ return a;
119+ });
120+
121+ return correspondences;
63122}
64123
65- Sophus::SE3d AlignClouds (const std::vector<Eigen::Vector3d> &source,
66- const std::vector<Eigen::Vector3d> &target,
67- double th) {
68- auto compute_jacobian_and_residual = [&](auto i) {
69- const Eigen::Vector3d residual = source[i] - target[i];
124+ LinearSystem BuildLinearSystem (const Correspondences &correspondences, const double kernel_scale) {
125+ auto compute_jacobian_and_residual = [](const auto &correspondence) {
126+ const auto &[source, target] = correspondence;
127+ const Eigen::Vector3d residual = source - target;
70128 Eigen::Matrix3_6d J_r;
71129 J_r.block <3 , 3 >(0 , 0 ) = Eigen::Matrix3d::Identity ();
72- J_r.block <3 , 3 >(0 , 3 ) = -1.0 * Sophus::SO3d::hat (source[i] );
130+ J_r.block <3 , 3 >(0 , 3 ) = -1.0 * Sophus::SO3d::hat (source);
73131 return std::make_tuple (J_r, residual);
74132 };
75133
134+ auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) {
135+ a.first += b.first ;
136+ a.second += b.second ;
137+ return a;
138+ };
139+
140+ auto GM_weight = [&](const double &residual2) {
141+ return square (kernel_scale) / square (kernel_scale + residual2);
142+ };
143+
144+ using correspondence_iterator = Correspondences::const_iterator;
76145 const auto &[JTJ, JTr] = tbb::parallel_reduce (
77146 // Range
78- tbb::blocked_range<size_t >{0 , source.size ()},
147+ tbb::blocked_range<correspondence_iterator>{correspondences.cbegin (),
148+ correspondences.cend ()},
79149 // Identity
80- ResultTuple ( ),
150+ LinearSystem ( Eigen::Matrix6d::Zero (), Eigen::Vector6d::Zero () ),
81151 // 1st Lambda: Parallel computation
82- [&](const tbb::blocked_range<size_t > &r, ResultTuple J) -> ResultTuple {
83- auto Weight = [&](double residual2) { return square (th) / square (th + residual2); };
84- auto &[JTJ_private, JTr_private] = J;
85- for (auto i = r.begin (); i < r.end (); ++i) {
86- const auto &[J_r, residual] = compute_jacobian_and_residual (i);
87- const double w = Weight (residual.squaredNorm ());
88- JTJ_private.noalias () += J_r.transpose () * w * J_r;
89- JTr_private.noalias () += J_r.transpose () * w * residual;
90- }
91- return J;
152+ [&](const tbb::blocked_range<correspondence_iterator> &r, LinearSystem J) -> LinearSystem {
153+ return std::transform_reduce (
154+ r.begin (), r.end (), J, sum_linear_systems, [&](const auto &correspondence) {
155+ const auto &[J_r, residual] = compute_jacobian_and_residual (correspondence);
156+ const double w = GM_weight (residual.squaredNorm ());
157+ return LinearSystem (J_r.transpose () * w * J_r, // JTJ
158+ J_r.transpose () * w * residual); // JTr
159+ });
92160 },
93161 // 2nd Lambda: Parallel reduction of the private Jacboians
94- [&](ResultTuple a, const ResultTuple &b) -> ResultTuple { return a + b; } );
162+ sum_linear_systems );
95163
96- const Eigen::Vector6d x = JTJ.ldlt ().solve (-JTr);
97- return Sophus::SE3d::exp (x);
164+ return {JTJ, JTr};
98165}
99-
100- constexpr int MAX_NUM_ITERATIONS_ = 500 ;
101- constexpr double ESTIMATION_THRESHOLD_ = 0.0001 ;
102-
103166} // namespace
104167
105168namespace mapmos {
106169
107- Sophus::SE3d RegisterFrame (const std::vector<Eigen::Vector3d> &frame,
108- const VoxelHashMap &voxel_map,
109- const Sophus::SE3d &initial_guess,
110- double max_correspondence_distance,
111- double kernel) {
170+ Registration::Registration (int max_num_iteration, double convergence_criterion, int max_num_threads)
171+ : max_num_iterations_(max_num_iteration),
172+ convergence_criterion_ (convergence_criterion),
173+ // Only manipulate the number of threads if the user specifies something greater than 0
174+ max_num_threads_(max_num_threads > 0 ? max_num_threads : tbb::info::default_concurrency()) {
175+ // This global variable requires static duration storage to be able to manipulate the max
176+ // concurrency from TBB across the entire class
177+ static const auto tbb_control_settings = tbb::global_control (
178+ tbb::global_control::max_allowed_parallelism, static_cast <size_t >(max_num_threads_));
179+ }
180+
181+ Sophus::SE3d Registration::AlignPointsToMap (const std::vector<Eigen::Vector3d> &frame,
182+ const VoxelHashMap &voxel_map,
183+ const Sophus::SE3d &initial_guess,
184+ const double max_distance,
185+ const double kernel_scale) {
112186 if (voxel_map.Empty ()) return initial_guess;
113187
114188 // Equation (9)
@@ -117,17 +191,19 @@ Sophus::SE3d RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
117191
118192 // ICP-loop
119193 Sophus::SE3d T_icp = Sophus::SE3d ();
120- for (int j = 0 ; j < MAX_NUM_ITERATIONS_ ; ++j) {
194+ for (int j = 0 ; j < max_num_iterations_ ; ++j) {
121195 // Equation (10)
122- const auto &[src, tgt] = voxel_map. GetCorrespondences (source, max_correspondence_distance );
196+ const auto correspondences = DataAssociation (source, voxel_map, max_distance );
123197 // Equation (11)
124- auto estimation = AlignClouds (src, tgt, kernel);
198+ const auto &[JTJ, JTr] = BuildLinearSystem (correspondences, kernel_scale);
199+ const Eigen::Vector6d dx = JTJ.ldlt ().solve (-JTr);
200+ const Sophus::SE3d estimation = Sophus::SE3d::exp (dx);
125201 // Equation (12)
126202 TransformPoints (estimation, source);
127203 // Update iterations
128204 T_icp = estimation * T_icp;
129205 // Termination criteria
130- if (estimation. log (). norm () < ESTIMATION_THRESHOLD_ ) break ;
206+ if (dx. norm () < convergence_criterion_ ) break ;
131207 }
132208 // Spit the final transformation
133209 return T_icp * initial_guess;
0 commit comments