16 #include <mrpt/core/optional_ref.h>
17 #include <mrpt/math/TPoint3D.h>
30 template <
class LAMBDA,
class LAMBDA2>
33 const mrpt::math::TPoint3D& ct_local,
const mrpt::math::TPoint3D& ct_global,
35 LAMBDA2 lambda_final,
bool normalize_relative_point_vectors,
36 const mrpt::optional_ref<VisitCorrespondencesStats>& outStats =
39 using mrpt::math::TPoint3D;
40 using mrpt::math::TVector3D;
49 nPt2Ln == 0,
"This solver cannot handle point-to-line pairings.");
51 nPt2Pl == 0,
"This solver cannot handle point-to-plane pairings yet.");
53 const auto nAllMatches = nPt2Pt + nLn2Ln + nPl2Pl;
59 if (point_weights.empty())
62 point_weights.emplace_back(nPt2Pt, 1.0);
65 auto cur_point_block_weights = point_weights.begin();
66 std::size_t cur_point_block_start = 0;
69 double waPoints, waLines, waPlanes;
76 "All, point, line, plane attidude weights, are <=0 (!)");
78 const auto k = 1.0 / (wPt * nPt2Pt + wLi * nLn2Ln + wPl * nPl2Pl);
91 auto it_next_outlier = in_out_outliers.
point2point.begin();
95 for (std::size_t i = 0; i < nAllMatches; i++)
98 if (it_next_outlier != in_out_outliers.
point2point.end() &&
99 i == *it_next_outlier)
118 if (i >= cur_point_block_start + cur_point_block_weights->first)
120 ASSERT_(cur_point_block_weights != point_weights.end());
121 ++cur_point_block_weights;
122 cur_point_block_start = i;
124 wi *= cur_point_block_weights->second;
127 bi = p.global - ct_global;
128 ri = p.local - ct_local;
130 const auto bi_n = bi.norm(), ri_n = ri.norm();
132 if (bi_n < 1e-4 || ri_n < 1e-4)
141 if (normalize_relative_point_vectors)
151 const double scale_mismatch =
152 std::max(bi_n, ri_n) / std::min(bi_n, ri_n);
163 else if (i < nPt2Pt + nLn2Ln)
168 const auto idxLine = i - nPt2Pt;
170 bi = in.
paired_ln2ln[idxLine].ln_global.getDirectorVector();
171 ri = in.
paired_ln2ln[idxLine].ln_local.getDirectorVector();
173 ASSERTDEB_LT_(std::abs(bi.norm() - 1.0), 0.01);
174 ASSERTDEB_LT_(std::abs(ri.norm() - 1.0), 0.01);
181 const auto idxPlane = i - (nPt2Pt + nLn2Ln);
182 bi = in.
paired_pl2pl[idxPlane].p_global.plane.getNormalVector();
183 ri = in.
paired_pl2pl[idxPlane].p_local.plane.getNormalVector();
185 ASSERTDEB_LT_(std::abs(bi.norm() - 1.0), 0.01);
186 ASSERTDEB_LT_(std::abs(ri.norm() - 1.0), 0.01);
199 std::acos(ri2.x * bi.x + ri2.y * bi.y + ri2.z * bi.z);
204 const auto f = 1.0 / (1.0 + B * mrpt::square(ang - A));
213 lambda_each_pair(bi, ri, wi);
217 in_out_outliers = std::move(new_outliers);
222 if (outStats.has_value()) outStats.value().get() = stats;