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>
36 const auto readVersion = in.ReadAs<uint8_t>();
45 mrpt::serialization::CArchive&
out,
const Pairings& obj)
52 mrpt::serialization::CArchive& in,
Pairings& obj)
58 std::tuple<mrpt::math::TPoint3D, mrpt::math::TPoint3D>
62 using mrpt::math::TPoint3D;
71 const double wcPoints = 1.0 / (nPt2Pt - outliers.
point2point.size());
75 TPoint3D ct_local(0, 0, 0), ct_global(0, 0, 0);
78 auto it_next_outlier = outliers.
point2point.begin();
82 if (it_next_outlier != outliers.
point2point.end() &&
83 i == *it_next_outlier)
90 ct_global += pair.global;
91 ct_local += pair.local;
95 ASSERT_EQUAL_(cnt, nPt2Pt - outliers.
point2point.size());
98 ct_global *= wcPoints;
101 return {ct_local, ct_global};
104 template <
typename T>
107 me.insert(me.end(), o.begin(), o.end());
109 template <
typename T>
113 me.end(), std::make_move_iterator(o.begin()),
114 std::make_move_iterator(o.end()));
141 template <
typename CONTAINER>
145 using namespace std::string_literals;
147 if (c.empty())
return;
148 if (!ret.empty()) ret +=
", "s;
149 ret += std::to_string(c.size()) +
" "s + name;
154 using namespace std::string_literals;
156 if (
empty())
return {
"none"s};
169 const mrpt::poses::CPose3D& localWrtGlobal,
171 -> std::shared_ptr<mrpt::opengl::CSetOfObjects>
174 auto o = mrpt::opengl::CSetOfObjects::Create();
185 mrpt::opengl::CSetOfObjects& o,
const mrpt::poses::CPose3D& localWrtGlobal,
190 auto lns = mrpt::opengl::CSetOfLines::Create();
191 lns->setColor_u8(p.
color);
196 const auto ptLocalTf = localWrtGlobal.composePoint(pair.local);
197 lns->appendLine(ptLocalTf, pair.global);
204 mrpt::opengl::CSetOfObjects& o,
const mrpt::poses::CPose3D& localWrtGlobal,
209 auto lns = mrpt::opengl::CSetOfLines::Create();
216 const auto globalPlanePose =
217 mrpt::poses::CPose3D(pair.pl_global.plane.getAsPose3DForcingOrigin(
218 pair.pl_global.centroid));
220 const auto ptLocal = pair.pt_local;
221 const auto ptLocalTf = localWrtGlobal.composePoint(ptLocal);
224 lns->appendLine(ptLocalTf, globalPlanePose.translation());
227 auto glPlane = mrpt::opengl::CTexturedPlane::Create();
228 glPlane->setPlaneCorners(-L, L, -L, L);
231 glPlane->setPose(globalPlanePose);
240 mrpt::opengl::CSetOfObjects& o,
const mrpt::poses::CPose3D& localWrtGlobal,
245 auto pairingLines = mrpt::opengl::CSetOfLines::Create();
248 auto globalLines = mrpt::opengl::CSetOfLines::Create();
255 const auto& globalLine = pair.ln_global;
257 const auto ptLocal = pair.pt_local;
258 const auto ptLocalTf = localWrtGlobal.composePoint(ptLocal);
261 pairingLines->appendLine(ptLocalTf, globalLine.pBase);
264 globalLines->appendLine(
265 globalLine.pBase - globalLine.director * L,
266 globalLine.pBase + globalLine.director * L);
269 o.insert(pairingLines);
270 o.insert(globalLines);
277 out.WriteAs<uint8_t>(0);
286 in.ReadAs<uint8_t>();
294 out.WriteAs<uint8_t>(0);
301 in.ReadAs<uint8_t>();
309 out.WriteAs<uint8_t>(0);
316 in.ReadAs<uint8_t>();
324 out.WriteAs<uint8_t>(0);
331 in.ReadAs<uint8_t>();