14 #include <mrpt/io/CFileGZInputStream.h>
15 #include <mrpt/io/CFileGZOutputStream.h>
16 #include <mrpt/opengl/CGridPlaneXY.h>
17 #include <mrpt/opengl/CPointCloud.h>
18 #include <mrpt/opengl/CPointCloudColoured.h>
19 #include <mrpt/opengl/CSetOfLines.h>
20 #include <mrpt/opengl/CSetOfObjects.h>
21 #include <mrpt/serialization/CArchive.h>
22 #include <mrpt/serialization/optional_serialization.h>
23 #include <mrpt/serialization/stl_serialization.h>
29 metric_map_t, mrpt::serialization::CSerializable,
mp2p_icp)
34 uint8_t metric_map_t::serializeGetVersion()
const {
return 1; }
35 void metric_map_t::serializeTo(mrpt::serialization::CArchive&
out)
const
40 for (
const auto& p :
planes)
out << p.plane << p.centroid;
43 for (
const auto& l :
lines)
out << l;
46 for (
const auto& l :
layers)
out << l.first << *l.second.get();
53 void metric_map_t::serializeFrom(
54 mrpt::serialization::CArchive& in, uint8_t version)
62 const auto nPls = in.ReadAs<uint32_t>();
64 for (
auto& pl :
planes) in >> pl.plane >> pl.centroid;
66 const auto nLins = in.ReadAs<uint32_t>();
68 for (
auto& l :
lines) in >> l;
70 const auto nPts = in.ReadAs<uint32_t>();
72 for (std::size_t i = 0; i < nPts; i++)
76 layers[name] = mrpt::ptr_cast<mrpt::maps::CMetricMap>::from(
80 if (version >= 1) { in >>
id >>
label; }
92 MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
97 -> std::shared_ptr<mrpt::opengl::CSetOfObjects>
100 auto o = mrpt::opengl::CSetOfObjects::Create();
102 get_visualization_planes(*o, p.planes);
103 get_visualization_lines(*o, p.lines);
104 get_visualization_points(*o, p.points);
118 for (
const auto& plane :
planes)
121 mrpt::opengl::CGridPlaneXY::Create(-pw, pw, -pw, pw, .0, pf);
122 gl_pl->setColor_u8(p.
color);
123 mrpt::math::TPose3D planePose;
124 plane.plane.getAsPose3DForcingOrigin(plane.centroid, planePose);
125 gl_pl->setPose(planePose);
137 auto glLin = mrpt::opengl::CSetOfLines::Create();
138 glLin->setColor_u8(p.
color);
140 for (
size_t idxLine = 0; idxLine <
lines.size(); idxLine++)
142 const auto& line =
lines[idxLine];
144 const auto halfSeg = line.director * (0.5 * linLen);
145 glLin->appendLine(line.pBase - halfSeg, line.pBase + halfSeg);
164 const auto itPts =
layers.find(kv.first);
165 if (itPts ==
layers.end())
167 "Rendering parameters given for layer '%s' which does not "
168 "exist in this metric_map_t object",
173 std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(
180 for (
const auto& kv :
layers)
183 std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(kv.second);
195 const mrpt::maps::CPointsMap::Ptr& pts)
198 if (pts->empty())
return;
203 auto glPts = mrpt::opengl::CPointCloudColoured::Create();
204 glPts->loadFromPointsMap(pts.get());
208 mrpt::math::TBoundingBoxf bb;
210 if (!p.
colorMode->colorMapMinCoord.has_value() ||
211 !p.
colorMode->colorMapMaxCoord.has_value())
212 bb = pts->boundingBox();
214 ASSERT_(p.
colorMode->recolorizeByCoordinate.has_value());
216 const unsigned int coordIdx =
static_cast<unsigned int>(
217 p.
colorMode->recolorizeByCoordinate.value());
219 const float coordMin = p.
colorMode->colorMapMinCoord.has_value()
223 const float coordMax = p.
colorMode->colorMapMaxCoord.has_value()
227 glPts->recolorizeByCoordinate(
228 coordMin, coordMax, coordIdx, p.
colorMode->colorMap);
235 auto glPts = mrpt::opengl::CPointCloud::Create();
236 glPts->loadFromPointsMap(pts.get());
239 glPts->setColor_u8(p.
color);
251 MRPT_TODO(
"Write unit test for mergeWith()")
254 const
std::optional<
mrpt::math::TPose3D>& otherRelativePose)
256 mrpt::poses::CPose3D pose;
257 if (otherRelativePose.has_value())
258 pose = mrpt::poses::CPose3D(otherRelativePose.value());
261 if (otherRelativePose.has_value())
264 otherPc.lines.begin(), otherPc.lines.end(),
265 std::back_inserter(lines), [&](
const mrpt::math::TLine3D& l) {
266 return mrpt::math::TLine3D::FromPointAndDirector(
267 pose.composePoint(l.pBase),
268 pose.rotateVector(l.getDirectorVector()));
274 otherPc.lines.begin(), otherPc.lines.end(),
275 std::back_inserter(lines));
279 if (otherRelativePose.has_value())
282 otherPc.planes.begin(), otherPc.planes.end(),
285 g.centroid = pose.composePoint(l.centroid);
286 MRPT_TODO(
"Finish rotating planes");
288 THROW_EXCEPTION(
"To-do");
295 otherPc.planes.begin(), otherPc.planes.end(),
296 std::back_inserter(planes));
300 for (
const auto& layer : otherPc.layers)
302 const auto& name = layer.first;
303 const auto& otherMap = layer.second;
306 if (layers.count(name) == 0)
309 layers[name] = std::dynamic_pointer_cast<mrpt::maps::CMetricMap>(
310 otherMap->duplicateGetSmartPtr());
312 if (otherRelativePose.has_value())
315 std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(
320 pts->changeCoordinatesReference(pose);
325 "Merging with SE(3) transform only available for "
326 "metric maps of point cloud types.");
333 if (
auto pts = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(
337 pts->insertAnotherMap(
338 std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(otherMap)
345 "Merging with SE(3) transform only available for "
346 "metric maps of point cloud types.");
365 for (
const auto& layer :
layers)
368 std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(layer.second);
370 { n += pts->size(); }
377 using namespace std::string_literals;
381 if (
id) ret +=
"id="s + std::to_string(*
id) +
" "s;
384 if (
empty())
return {ret +
"empty"s};
387 if (!ret.empty()) ret +=
", "s;
391 if (!
lines.empty()) retAppend(std::to_string(
lines.size()) +
" lines"s);
392 if (!
planes.empty()) retAppend(std::to_string(
planes.size()) +
" planes"s);
395 for (
const auto& layer :
layers)
397 ASSERT_(layer.second);
399 std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(layer.second);
401 { nPts += pts->size(); }
407 std::to_string(nPts) +
" points in "s +
408 std::to_string(
layers.size()) +
" layers ("s);
410 for (
const auto& layer :
layers)
413 "\""s + layer.first +
"\":"s + layer.second->asString() +
" "s;
423 auto f = mrpt::io::CFileGZOutputStream(fileName);
424 if (!
f.is_open())
return false;
426 auto arch = mrpt::serialization::archiveFrom(
f);
434 auto f = mrpt::io::CFileGZInputStream(fileName);
435 if (!
f.is_open())
return false;
437 auto arch = mrpt::serialization::archiveFrom(
f);
447 return shared_from_this();
449 catch (
const std::bad_weak_ptr&)
459 if (!ret) ret = std::make_shared<metric_map_t>(*
this);
467 return shared_from_this();
469 catch (
const std::bad_weak_ptr&)
479 if (!ret) ret = std::make_shared<metric_map_t>(*
this);
486 auto it =
layers.find(name);
488 THROW_EXCEPTION_FMT(
"Layer '%s' does not exist.", name.c_str());
490 const auto& ptr = it->second;
493 auto ret = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(ptr);
496 "Layer '%s' is not a point cloud (actual class:'%s').",
497 name.c_str(), ptr->GetRuntimeClass()->className);