Pairings.cpp
Go to the documentation of this file.
1 /* -------------------------------------------------------------------------
2  * A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
3  * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
4  * See LICENSE for license information.
5  * ------------------------------------------------------------------------- */
13 #include <mp2p_icp/Pairings.h>
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>
19 
20 #include <iterator> // std::make_move_iterator
21 
22 using namespace mp2p_icp;
23 
24 static const uint8_t SERIALIZATION_VERSION = 1;
25 
26 Pairings::~Pairings() = default;
27 
28 void Pairings::serializeTo(mrpt::serialization::CArchive& out) const
29 {
30  out.WriteAs<uint8_t>(SERIALIZATION_VERSION);
31  out << paired_pt2pt;
33  << point_weights;
34  out << potential_pairings; // v1
35 }
36 
37 void Pairings::serializeFrom(mrpt::serialization::CArchive& in)
38 {
39  const auto readVersion = in.ReadAs<uint8_t>();
40 
41  ASSERT_LE_(readVersion, SERIALIZATION_VERSION);
42  in >> paired_pt2pt;
45  if (readVersion >= 1) in >> potential_pairings;
46 }
47 
48 mrpt::serialization::CArchive& mp2p_icp::operator<<(
49  mrpt::serialization::CArchive& out, const Pairings& obj)
50 {
51  obj.serializeTo(out);
52  return out;
53 }
54 
55 mrpt::serialization::CArchive& mp2p_icp::operator>>(
56  mrpt::serialization::CArchive& in, Pairings& obj)
57 {
58  obj.serializeFrom(in);
59  return in;
60 }
61 
62 std::tuple<mrpt::math::TPoint3D, mrpt::math::TPoint3D>
64  const Pairings& in, const OutlierIndices& outliers)
65 {
66  using mrpt::math::TPoint3D;
67 
68  const auto nPt2Pt = in.paired_pt2pt.size();
69 
70  // We need more points than outliers (!)
71  ASSERT_GT_(nPt2Pt, outliers.point2point.size());
72 
73  // Normalized weights for centroids.
74  // Discount outliers.
75  const double wcPoints = 1.0 / (nPt2Pt - outliers.point2point.size());
76 
77  // Add global coordinate of points for now, we'll convert them later to
78  // unit vectors relative to the centroids:
79  TPoint3D ct_local(0, 0, 0), ct_global(0, 0, 0);
80  {
81  std::size_t cnt = 0;
82  auto it_next_outlier = outliers.point2point.begin();
83  for (std::size_t i = 0; i < in.paired_pt2pt.size(); i++)
84  {
85  // Skip outlier?
86  if (it_next_outlier != outliers.point2point.end() &&
87  i == *it_next_outlier)
88  {
89  ++it_next_outlier;
90  continue;
91  }
92  const auto& pair = in.paired_pt2pt[i];
93 
94  ct_global += pair.global;
95  ct_local += pair.local;
96  cnt++;
97  }
98  // Sanity check:
99  ASSERT_EQUAL_(cnt, nPt2Pt - outliers.point2point.size());
100 
101  ct_local *= wcPoints;
102  ct_global *= wcPoints;
103  }
104 
105  return {ct_local, ct_global};
106 }
107 
108 template <typename T>
109 static void push_back_copy(const T& o, T& me)
110 {
111  me.insert(me.end(), o.begin(), o.end());
112 }
113 template <typename T>
114 static void push_back_move(T&& o, T& me)
115 {
116  me.insert(
117  me.end(), std::make_move_iterator(o.begin()),
118  std::make_move_iterator(o.end()));
119 }
120 
122 {
129 }
130 
132 {
133  push_back_move(std::move(o.paired_pt2pt), paired_pt2pt);
134  push_back_move(std::move(o.paired_pt2ln), paired_pt2ln);
135  push_back_move(std::move(o.paired_pt2pl), paired_pt2pl);
136  push_back_move(std::move(o.paired_ln2ln), paired_ln2ln);
137  push_back_move(std::move(o.paired_pl2pl), paired_pl2pl);
138  potential_pairings = o.potential_pairings;
139 }
140 
141 size_t Pairings::size() const
142 {
143  return paired_pt2pt.size() + paired_pt2ln.size() + paired_pt2pl.size() +
144  paired_ln2ln.size() + paired_pl2pl.size();
145 }
146 
147 template <typename CONTAINER>
149  const CONTAINER& c, const std::string& name, std::string& ret)
150 {
151  using namespace std::string_literals;
152 
153  if (c.empty()) return;
154  if (!ret.empty()) ret += ", "s;
155  ret += std::to_string(c.size()) + " "s + name;
156 }
157 
159 {
160  using namespace std::string_literals;
161 
162  if (empty()) return {"none"s};
163 
164  std::string ret;
165  append_container_size(paired_pt2pt, "point-point", ret);
166  append_container_size(paired_pt2ln, "point-line", ret);
167  append_container_size(paired_pt2pl, "point-plane", ret);
168  append_container_size(paired_ln2ln, "line-line", ret);
169  append_container_size(paired_pl2pl, "plane-plane", ret);
170  ret += " out of "s + std::to_string(potential_pairings);
171 
172  return ret;
173 }
174 
176  const mrpt::poses::CPose3D& localWrtGlobal,
177  const pairings_render_params_t& p) const
178  -> std::shared_ptr<mrpt::opengl::CSetOfObjects>
179 {
180  MRPT_START
181  auto o = mrpt::opengl::CSetOfObjects::Create();
182 
183  get_visualization_pt2pt(*o, localWrtGlobal, p.pt2pt);
184  get_visualization_pt2pl(*o, localWrtGlobal, p.pt2pl);
185  get_visualization_pt2ln(*o, localWrtGlobal, p.pt2ln);
186 
187  return o;
188  MRPT_END
189 }
190 
192  mrpt::opengl::CSetOfObjects& o, const mrpt::poses::CPose3D& localWrtGlobal,
193  const render_params_pairings_pt2pt_t& p) const
194 {
195  if (!p.visible) return;
196 
197  auto lns = mrpt::opengl::CSetOfLines::Create();
198  lns->setColor_u8(p.color);
199 
200  // this: global, other: local
201  for (const auto& pair : paired_pt2pt)
202  {
203  const auto ptLocalTf = localWrtGlobal.composePoint(pair.local);
204  lns->appendLine(ptLocalTf, pair.global);
205  }
206 
207  o.insert(lns);
208 }
209 
211  mrpt::opengl::CSetOfObjects& o, const mrpt::poses::CPose3D& localWrtGlobal,
212  const render_params_pairings_pt2pl_t& p) const
213 {
214  if (!p.visible) return;
215 
216  auto lns = mrpt::opengl::CSetOfLines::Create();
217  lns->setColor_u8(p.segmentColor);
218 
219  const double L = 0.5 * p.planePatchSize;
220 
221  for (const auto& pair : paired_pt2pl)
222  {
223  const auto globalPlanePose =
224  mrpt::poses::CPose3D(pair.pl_global.plane.getAsPose3DForcingOrigin(
225  pair.pl_global.centroid));
226 
227  const auto ptLocal = pair.pt_local;
228  const auto ptLocalTf = localWrtGlobal.composePoint(ptLocal);
229 
230  // line segment:
231  lns->appendLine(ptLocalTf, globalPlanePose.translation());
232 
233  // plane patch:
234  auto glPlane = mrpt::opengl::CTexturedPlane::Create();
235  glPlane->setPlaneCorners(-L, L, -L, L);
236  glPlane->setColor_u8(p.planePatchColor);
237 
238  glPlane->setPose(globalPlanePose);
239 
240  o.insert(glPlane);
241  }
242 
243  o.insert(lns);
244 }
245 
247  mrpt::opengl::CSetOfObjects& o, const mrpt::poses::CPose3D& localWrtGlobal,
248  const render_params_pairings_pt2ln_t& p) const
249 {
250  if (!p.visible) return;
251 
252  auto pairingLines = mrpt::opengl::CSetOfLines::Create();
253  pairingLines->setColor_u8(p.segmentColor);
254 
255  auto globalLines = mrpt::opengl::CSetOfLines::Create();
256  globalLines->setColor_u8(p.lineColor);
257 
258  const double L = 0.5 * p.lineLength;
259 
260  for (const auto& pair : paired_pt2ln)
261  {
262  const auto& globalLine = pair.ln_global;
263 
264  const auto ptLocal = pair.pt_local;
265  const auto ptLocalTf = localWrtGlobal.composePoint(ptLocal);
266 
267  // line segment:
268  pairingLines->appendLine(ptLocalTf, globalLine.pBase);
269 
270  // line segment:
271  globalLines->appendLine(
272  globalLine.pBase - globalLine.director * L,
273  globalLine.pBase + globalLine.director * L);
274  }
275 
276  o.insert(pairingLines);
277  o.insert(globalLines);
278 }
279 
280 namespace mrpt::serialization
281 {
282 CArchive& operator<<(CArchive& out, const mp2p_icp::point_line_pair_t& obj)
283 {
284  out.WriteAs<uint8_t>(0);
285 
286  out << obj.ln_global << obj.pt_local;
287  return out;
288 }
289 
290 CArchive& operator>>(CArchive& in, mp2p_icp::point_line_pair_t& obj)
291 {
292  // const auto ver =
293  in.ReadAs<uint8_t>();
294 
295  in >> obj.ln_global >> obj.pt_local;
296  return in;
297 }
298 
299 CArchive& operator<<(CArchive& out, const mp2p_icp::point_plane_pair_t& obj)
300 {
301  out.WriteAs<uint8_t>(0);
302  out << obj.pl_global.centroid << obj.pl_global.plane << obj.pt_local;
303  return out;
304 }
305 
306 CArchive& operator>>(CArchive& in, mp2p_icp::point_plane_pair_t& obj)
307 {
308  in.ReadAs<uint8_t>();
309 
310  in >> obj.pl_global.centroid >> obj.pl_global.plane >> obj.pt_local;
311  return in;
312 }
313 
314 CArchive& operator<<(CArchive& out, const mp2p_icp::matched_line_t& obj)
315 {
316  out.WriteAs<uint8_t>(0);
317  out << obj.ln_local << obj.ln_global;
318  return out;
319 }
320 
321 CArchive& operator>>(CArchive& in, mp2p_icp::matched_line_t& obj)
322 {
323  in.ReadAs<uint8_t>();
324 
325  in >> obj.ln_local >> obj.ln_global;
326  return in;
327 }
328 
329 CArchive& operator<<(CArchive& out, const mp2p_icp::matched_plane_t& obj)
330 {
331  out.WriteAs<uint8_t>(0);
332  out << obj.p_local.centroid << obj.p_local.plane;
333  out << obj.p_global.centroid << obj.p_global.plane;
334  return out;
335 }
336 CArchive& operator>>(CArchive& in, mp2p_icp::matched_plane_t& obj)
337 {
338  in.ReadAs<uint8_t>();
339 
340  in >> obj.p_local.centroid >> obj.p_local.plane;
341  in >> obj.p_global.centroid >> obj.p_global.plane;
342  return in;
343 }
344 
345 } // namespace mrpt::serialization
mp2p_icp::Pairings::paired_ln2ln
MatchedLineList paired_ln2ln
Definition: Pairings.h:91
mp2p_icp::Pairings::contents_summary
virtual std::string contents_summary() const
Definition: Pairings.cpp:158
SERIALIZATION_VERSION
static const uint8_t SERIALIZATION_VERSION
Definition: Pairings.cpp:24
mp2p_icp::render_params_pairings_pt2pl_t::planePatchColor
mrpt::img::TColor planePatchColor
Definition: render_params.h:215
mp2p_icp::Pairings::get_visualization_pt2ln
virtual void get_visualization_pt2ln(mrpt::opengl::CSetOfObjects &o, const mrpt::poses::CPose3D &localWrtGlobal, const render_params_pairings_pt2ln_t &p) const
Definition: Pairings.cpp:246
mp2p_icp
Definition: covariance.h:17
mp2p_icp::operator>>
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, Pairings &obj)
Definition: Pairings.cpp:55
mp2p_icp::render_params_pairings_pt2pt_t::color
mrpt::img::TColor color
Definition: render_params.h:205
mp2p_icp::plane_patch_t::centroid
mrpt::math::TPoint3D centroid
Definition: plane_patch.h:25
mp2p_icp::matched_plane_t::p_global
plane_patch_t p_global
Definition: Pairings.h:31
s
XmlRpcServer s
mp2p_icp::render_params_pairings_pt2pl_t::visible
bool visible
Definition: render_params.h:212
mp2p_icp::point_plane_pair_t
Definition: point_plane_pair_t.h:26
mp2p_icp::point_plane_pair_t::pl_global
plane_patch_t pl_global
Definition: point_plane_pair_t.h:28
mp2p_icp::render_params_pairings_pt2ln_t::segmentColor
mrpt::img::TColor segmentColor
Definition: render_params.h:225
mp2p_icp::Pairings
Definition: Pairings.h:78
mp2p_icp::render_params_pairings_pt2pl_t::segmentColor
mrpt::img::TColor segmentColor
Definition: render_params.h:214
mp2p_icp::render_params_pairings_pt2pt_t::visible
bool visible
Definition: render_params.h:203
mp2p_icp::Pairings::empty
virtual bool empty() const
Definition: Pairings.h:111
mp2p_icp::matched_plane_t::p_local
plane_patch_t p_local
Definition: Pairings.h:31
mp2p_icp::Pairings::serializeFrom
virtual void serializeFrom(mrpt::serialization::CArchive &in)
Definition: Pairings.cpp:37
mp2p_icp::point_line_pair_t::ln_global
mrpt::math::TLine3D ln_global
Definition: Pairings.h:55
mp2p_icp::render_params_pairings_pt2pl_t
Definition: render_params.h:208
mp2p_icp::Pairings::get_visualization_pt2pt
virtual void get_visualization_pt2pt(mrpt::opengl::CSetOfObjects &o, const mrpt::poses::CPose3D &localWrtGlobal, const render_params_pairings_pt2pt_t &p) const
Definition: Pairings.cpp:191
mp2p_icp::render_params_pairings_pt2pl_t::planePatchSize
double planePatchSize
Definition: render_params.h:216
testing::internal::string
::std::string string
Definition: gtest.h:1979
mp2p_icp::Pairings::potential_pairings
uint64_t potential_pairings
Definition: Pairings.h:98
mp2p_icp::OutlierIndices::point2point
std::vector< std::size_t > point2point
Definition: Pairings.h:182
mp2p_icp::render_params_pairings_pt2ln_t::visible
bool visible
Definition: render_params.h:223
mp2p_icp::Pairings::point_weights
std::vector< std::pair< std::size_t, double > > point_weights
Definition: Pairings.h:104
kitti-batch-convert.out
string out
Definition: kitti-batch-convert.py:7
mp2p_icp::matched_line_t::ln_global
mrpt::math::TLine3D ln_global
Definition: Pairings.h:46
mp2p_icp::matched_line_t
Definition: Pairings.h:44
append_container_size
void append_container_size(const CONTAINER &c, const std::string &name, std::string &ret)
Definition: Pairings.cpp:148
mp2p_icp::Pairings::size
virtual size_t size() const
Definition: Pairings.cpp:141
Pairings.h
Common types for all SE(3) optimal transformation methods.
mp2p_icp::Pairings::serializeTo
virtual void serializeTo(mrpt::serialization::CArchive &out) const
Definition: Pairings.cpp:28
mp2p_icp::render_params_pairings_pt2ln_t::lineColor
mrpt::img::TColor lineColor
Definition: render_params.h:226
mrpt::serialization
Definition: LogRecord.h:100
mp2p_icp::Pairings::push_back
virtual void push_back(const Pairings &o)
Definition: Pairings.cpp:121
mp2p_icp::point_line_pair_t
Definition: Pairings.h:53
mp2p_icp::render_params_pairings_pt2ln_t::lineLength
double lineLength
Definition: render_params.h:227
mp2p_icp::eval_centroids_robust
std::tuple< mrpt::math::TPoint3D, mrpt::math::TPoint3D > eval_centroids_robust(const Pairings &in, const OutlierIndices &outliers)
Definition: Pairings.cpp:63
mp2p_icp::Pairings::paired_pl2pl
MatchedPlaneList paired_pl2pl
Definition: Pairings.h:92
mp2p_icp::Pairings::get_visualization
virtual auto get_visualization(const mrpt::poses::CPose3D &localWrtGlobal, const pairings_render_params_t &p=pairings_render_params_t()) const -> std::shared_ptr< mrpt::opengl::CSetOfObjects >
Definition: Pairings.cpp:175
mp2p_icp::render_params_pairings_pt2pt_t
Definition: render_params.h:199
mp2p_icp::Pairings::~Pairings
virtual ~Pairings()
mp2p_icp::operator<<
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &out, const Pairings &obj)
Definition: Pairings.cpp:48
mp2p_icp::point_line_pair_t::pt_local
mrpt::math::TPoint3D pt_local
Definition: Pairings.h:56
mp2p_icp::Pairings::get_visualization_pt2pl
virtual void get_visualization_pt2pl(mrpt::opengl::CSetOfObjects &o, const mrpt::poses::CPose3D &localWrtGlobal, const render_params_pairings_pt2pl_t &p) const
Definition: Pairings.cpp:210
mp2p_icp::Pairings::paired_pt2ln
MatchedPointLineList paired_pt2ln
Definition: Pairings.h:89
mp2p_icp::point_plane_pair_t::pt_local
mrpt::math::TPoint3Df pt_local
Definition: point_plane_pair_t.h:29
push_back_copy
static void push_back_copy(const T &o, T &me)
Definition: Pairings.cpp:109
push_back_move
static void push_back_move(T &&o, T &me)
Definition: Pairings.cpp:114
mp2p_icp::matched_plane_t
Definition: Pairings.h:29
mp2p_icp::matched_line_t::ln_local
mrpt::math::TLine3D ln_local
Definition: Pairings.h:46
mp2p_icp::render_params_pairings_pt2ln_t
Definition: render_params.h:219
mp2p_icp::OutlierIndices
Definition: Pairings.h:180
mp2p_icp::pairings_render_params_t
Definition: render_params.h:231
mp2p_icp::plane_patch_t::plane
mrpt::math::TPlane plane
Definition: plane_patch.h:24
mp2p_icp::Pairings::paired_pt2pt
mrpt::tfest::TMatchingPairList paired_pt2pt
Definition: Pairings.h:88
mp2p_icp::Pairings::paired_pt2pl
MatchedPointPlaneList paired_pt2pl
Definition: Pairings.h:90


mp2p_icp
Author(s):
autogenerated on Thu Dec 26 2024 03:48:12