15 #include <mrpt/containers/yaml.h>
16 #include <mrpt/maps/CSimplePointsMap.h>
17 #include <mrpt/math/TPose3D.h>
18 #include <mrpt/obs/CObservationPointCloud.h>
26 const mrpt::containers::yaml& c,
FilterMerge& parent)
32 if (c.has(
"robot_pose"))
35 c[
"robot_pose"].isSequence() &&
36 c[
"robot_pose"].asSequence().size() == 6);
38 auto cc = c[
"robot_pose"].asSequence();
40 for (
int i = 0; i < 6; i++)
48 mrpt::system::COutputLogger::setLoggerName(
"FilterMerge");
55 MRPT_LOG_DEBUG_STREAM(
"Loading these params:\n" << c);
71 "Input layer '%s' not found.",
80 "Input point cloud layer '%s' could not be converted into a "
81 "point cloud (class='%s')",
83 mapPtr->GetRuntimeClass()->className));
95 mrpt::obs::CObservationPointCloud obs;
96 auto pts = mrpt::maps::CSimplePointsMap::Create();
104 pts->insertAnotherMap(pcPtr, mrpt::poses::CPose3D::Identity());
108 const auto invRobotPose = -robotPose;
109 pts->insertAnotherMap(pcPtr, invRobotPose);
113 out->insertObservation(obs, robotPose);