14 #include <mrpt/opengl/CSetOfLines.h>
15 #include <mrpt/opengl/CSetOfObjects.h>
16 #include <mrpt/opengl/CTexturedPlane.h>
17 #include <mrpt/serialization/CArchive.h>
18 #include <mrpt/serialization/stl_serialization.h>
39 const auto readVersion = in.ReadAs<uint8_t>();
49 mrpt::serialization::CArchive&
out,
const Pairings& obj)
56 mrpt::serialization::CArchive& in,
Pairings& obj)
62 std::tuple<mrpt::math::TPoint3D, mrpt::math::TPoint3D>
66 using mrpt::math::TPoint3D;
75 const double wcPoints = 1.0 / (nPt2Pt - outliers.
point2point.size());
79 TPoint3D ct_local(0, 0, 0), ct_global(0, 0, 0);
82 auto it_next_outlier = outliers.
point2point.begin();
86 if (it_next_outlier != outliers.
point2point.end() &&
87 i == *it_next_outlier)
94 ct_global += pair.global;
95 ct_local += pair.local;
99 ASSERT_EQUAL_(cnt, nPt2Pt - outliers.
point2point.size());
101 ct_local *= wcPoints;
102 ct_global *= wcPoints;
105 return {ct_local, ct_global};
108 template <
typename T>
111 me.insert(me.end(), o.begin(), o.end());
113 template <
typename T>
117 me.end(), std::make_move_iterator(o.begin()),
118 std::make_move_iterator(o.end()));
147 template <
typename CONTAINER>
151 using namespace std::string_literals;
153 if (c.empty())
return;
154 if (!ret.empty()) ret +=
", "s;
155 ret += std::to_string(c.size()) +
" "s + name;
160 using namespace std::string_literals;
162 if (
empty())
return {
"none"s};
176 const mrpt::poses::CPose3D& localWrtGlobal,
178 -> std::shared_ptr<mrpt::opengl::CSetOfObjects>
181 auto o = mrpt::opengl::CSetOfObjects::Create();
192 mrpt::opengl::CSetOfObjects& o,
const mrpt::poses::CPose3D& localWrtGlobal,
197 auto lns = mrpt::opengl::CSetOfLines::Create();
198 lns->setColor_u8(p.
color);
203 const auto ptLocalTf = localWrtGlobal.composePoint(pair.local);
204 lns->appendLine(ptLocalTf, pair.global);
211 mrpt::opengl::CSetOfObjects& o,
const mrpt::poses::CPose3D& localWrtGlobal,
216 auto lns = mrpt::opengl::CSetOfLines::Create();
223 const auto globalPlanePose =
224 mrpt::poses::CPose3D(pair.pl_global.plane.getAsPose3DForcingOrigin(
225 pair.pl_global.centroid));
227 const auto ptLocal = pair.pt_local;
228 const auto ptLocalTf = localWrtGlobal.composePoint(ptLocal);
231 lns->appendLine(ptLocalTf, globalPlanePose.translation());
234 auto glPlane = mrpt::opengl::CTexturedPlane::Create();
235 glPlane->setPlaneCorners(-L, L, -L, L);
238 glPlane->setPose(globalPlanePose);
247 mrpt::opengl::CSetOfObjects& o,
const mrpt::poses::CPose3D& localWrtGlobal,
252 auto pairingLines = mrpt::opengl::CSetOfLines::Create();
255 auto globalLines = mrpt::opengl::CSetOfLines::Create();
262 const auto& globalLine = pair.ln_global;
264 const auto ptLocal = pair.pt_local;
265 const auto ptLocalTf = localWrtGlobal.composePoint(ptLocal);
268 pairingLines->appendLine(ptLocalTf, globalLine.pBase);
271 globalLines->appendLine(
272 globalLine.pBase - globalLine.director * L,
273 globalLine.pBase + globalLine.director * L);
276 o.insert(pairingLines);
277 o.insert(globalLines);
284 out.WriteAs<uint8_t>(0);
293 in.ReadAs<uint8_t>();
301 out.WriteAs<uint8_t>(0);
308 in.ReadAs<uint8_t>();
316 out.WriteAs<uint8_t>(0);
323 in.ReadAs<uint8_t>();
331 out.WriteAs<uint8_t>(0);
338 in.ReadAs<uint8_t>();