16 #include <mrpt/poses/Lie/SE.h>
18 #include <Eigen/Dense>
21 #if defined(MP2P_HAS_TBB)
22 #include <tbb/blocked_range.h>
23 #include <tbb/parallel_reduce.h>
40 "This method requires a linearization point");
53 Eigen::Vector<double, 6> g = Eigen::Vector<double, 6>::Zero();
54 Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
60 std::size_t cur_point_block_start = 0;
65 const auto dDexpe_de =
66 mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(result.
optimalPose);
68 double errNormSqr = 0;
70 #if defined(MP2P_HAS_TBB)
88 Eigen::Matrix<double, 6, 6> H;
89 Eigen::Vector<double, 6> g;
92 const auto& [H_tbb, g_tbb] = tbb::parallel_reduce(
94 tbb::blocked_range<size_t>{0, nPt2Pt},
98 [&](
const tbb::blocked_range<size_t>& r, Result
res) -> Result {
99 auto& [H_local, g_local] =
res;
100 for (
size_t idx_pt = r.begin(); idx_pt < r.end(); idx_pt++)
104 mrpt::math::CMatrixFixed<double, 3, 12> J1;
105 mrpt::math::CVectorFixedDouble<3> ret =
109 if (has_per_pt_weight)
111 if (idx_pt >= cur_point_block_start +
112 cur_point_block_weights->first)
115 cur_point_block_weights !=
117 ++cur_point_block_weights;
118 cur_point_block_start = idx_pt;
120 w.pt2pt = cur_point_block_weights->second;
124 double weight = w.pt2pt,
125 retSqrNorm = ret.asEigen().squaredNorm();
126 if (robustSqrtWeightFunc)
127 weight *= robustSqrtWeightFunc(retSqrNorm);
130 const Eigen::Vector3d err_i = ret.asEigen();
131 errNormSqr += weight * retSqrNorm;
133 const Eigen::Matrix<double, 3, 6> Ji =
134 J1.asEigen() * dDexpe_de.asEigen();
135 g_local.noalias() += weight * Ji.transpose() * err_i;
136 H_local.noalias() += weight * Ji.transpose() * Ji;
141 [](Result a,
const Result& b) -> Result {
return a + b; });
143 H = std::move(H_tbb);
144 g = std::move(g_tbb);
147 for (
size_t idx_pt = 0; idx_pt < nPt2Pt; idx_pt++)
151 mrpt::math::CMatrixFixed<double, 3, 12> J1;
152 mrpt::math::CVectorFixedDouble<3> ret =
156 if (has_per_pt_weight)
159 cur_point_block_start + cur_point_block_weights->first)
162 ++cur_point_block_weights;
163 cur_point_block_start = idx_pt;
165 w.pt2pt = cur_point_block_weights->second;
169 double weight = w.pt2pt, retSqrNorm = ret.asEigen().squaredNorm();
170 if (robustSqrtWeightFunc)
171 weight *= robustSqrtWeightFunc(retSqrNorm);
174 const Eigen::Vector3d err_i = ret.asEigen();
175 errNormSqr += weight * retSqrNorm;
177 const Eigen::Matrix<double, 3, 6> Ji =
178 J1.asEigen() * dDexpe_de.asEigen();
179 g.noalias() += weight * Ji.transpose() * err_i;
180 H.noalias() += weight * Ji.transpose() * Ji;
185 for (
size_t idx_pt = 0; idx_pt < nPt2Ln; idx_pt++)
189 mrpt::math::CMatrixFixed<double, 3, 12> J1;
190 mrpt::math::CVectorFixedDouble<3> ret =
194 double weight = w.pt2ln, retSqrNorm = ret.asEigen().squaredNorm();
195 if (robustSqrtWeightFunc)
196 weight *= robustSqrtWeightFunc(retSqrNorm);
199 const Eigen::Vector3d err_i = ret.asEigen();
200 errNormSqr += weight * weight * retSqrNorm;
202 const Eigen::Matrix<double, 3, 6> Ji =
203 J1.asEigen() * dDexpe_de.asEigen();
204 g.noalias() += weight * Ji.transpose() * err_i;
205 H.noalias() += weight * Ji.transpose() * Ji;
210 for (
size_t idx_ln = 0; idx_ln < nLn2Ln; idx_ln++)
213 mrpt::math::CMatrixFixed<double, 4, 12> J1;
214 mrpt::math::CVectorFixedDouble<4> ret =
218 double weight = w.ln2ln, retSqrNorm = ret.asEigen().squaredNorm();
219 if (robustSqrtWeightFunc)
220 weight *= robustSqrtWeightFunc(retSqrNorm);
223 const Eigen::Vector4d err_i = ret.asEigen();
224 errNormSqr += weight * weight * retSqrNorm;
226 const Eigen::Matrix<double, 4, 6> Ji =
227 J1.asEigen() * dDexpe_de.asEigen();
228 g.noalias() += weight * Ji.transpose() * err_i;
229 H.noalias() += weight * Ji.transpose() * Ji;
233 for (
size_t idx_pl = 0; idx_pl < nPt2Pl; idx_pl++)
237 mrpt::math::CMatrixFixed<double, 3, 12> J1;
238 mrpt::math::CVectorFixedDouble<3> ret =
242 double weight = w.pt2pl, retSqrNorm = ret.asEigen().squaredNorm();
243 if (robustSqrtWeightFunc)
244 weight *= robustSqrtWeightFunc(retSqrNorm);
247 const Eigen::Vector3d err_i = ret.asEigen();
248 errNormSqr += weight * weight * retSqrNorm;
250 const Eigen::Matrix<double, 3, 6> Ji =
251 J1.asEigen() * dDexpe_de.asEigen();
252 g.noalias() += weight * Ji.transpose() * err_i;
253 H.noalias() += weight * Ji.transpose() * Ji;
257 for (
size_t idx_pl = 0; idx_pl < nPl2Pl; idx_pl++)
261 mrpt::math::CMatrixFixed<double, 3, 12> J1;
262 mrpt::math::CVectorFixedDouble<3> ret =
266 double weight = w.pl2pl, retSqrNorm = ret.asEigen().squaredNorm();
267 if (robustSqrtWeightFunc)
268 weight *= robustSqrtWeightFunc(retSqrNorm);
270 const Eigen::Vector3d err_i = ret.asEigen();
271 errNormSqr += weight * weight * retSqrNorm;
273 const Eigen::Matrix<double, 3, 6> Ji =
274 J1.asEigen() * dDexpe_de.asEigen();
275 g.noalias() += weight * Ji.transpose() * err_i;
276 H.noalias() += weight * Ji.transpose() * Ji;
280 if (gnParams.
prior.has_value())
282 const auto& priorMean = gnParams.
prior->mean;
283 const auto& priorInf = gnParams.
prior->cov_inv;
290 const mrpt::poses::CPose3D P1invP2 = result.
optimalPose - priorMean;
291 const auto err_i = mrpt::poses::Lie::SE<3>::log(P1invP2);
293 mrpt::math::CMatrixDouble66 df_de2;
295 mrpt::poses::Lie::SE<3>::jacob_dDinvP1invP2_de1e2(
297 mrpt::poses::CPose3D::Identity(),
308 (df_de2.transpose() * priorInf.asEigen()) * err_i.asEigen();
311 (df_de2.transpose() * priorInf.asEigen()) * df_de2.asEigen();
315 const double errNorm = std::sqrt(errNormSqr);
317 if (errNorm <= gnParams.
maxCost)
break;
322 const Eigen::Matrix<double, 6, 1> delta = -H.ldlt().solve(g);
325 const auto dE = mrpt::poses::Lie::SE<3>::exp(
326 mrpt::math::CVectorFixed<double, 6>(delta));
332 std::cout <<
"[P2P GN] iter:" << iter <<
" err:" << errNorm
333 <<
" delta:" << delta.transpose() <<
"\n";
337 if (delta.norm() < gnParams.
minDelta)
break;