16 #include <mrpt/core/exceptions.h>
17 #include <mrpt/maps/CPointsMap.h>
18 #include <mrpt/poses/Lie/SE.h>
19 #include <mrpt/tfest/se3.h>
24 #if defined(MP2P_HAS_LIBPOINTMATCHER)
37 #if defined(MP2P_HAS_LIBPOINTMATCHER)
44 #if defined(MP2P_HAS_LIBPOINTMATCHER)
50 for (
const auto& ly : pc.
layers)
53 auto pts = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(ly.second);
56 const auto xs = pts->getPointsBufferRef_x();
57 const auto ys = pts->getPointsBufferRef_y();
58 const auto zs = pts->getPointsBufferRef_z();
59 for (
size_t i = 0; i < xs.size(); i++)
60 ss << mrpt::format(
"%f %f %f\n", xs[i], ys[i], zs[i]);
68 const mrpt::containers::yaml& params)
71 params.printAsYAML(ss);
78 [[maybe_unused]]
const mrpt::math::TPose3D& initialGuessLocalWrtGlobal,
80 [[maybe_unused]]
const mrpt::optional_ref<LogRecord>& outputDebugInfo)
82 using namespace std::string_literals;
85 #if defined(MP2P_HAS_LIBPOINTMATCHER)
87 ASSERT_EQUAL_(pcLocal.layers.size(), pcGlobal.layers.size());
88 ASSERT_(!pcLocal.empty() && !pcGlobal.empty());
92 "You must call initialize_derived() first, or initialize from a YAML "
93 "file with a `derived` section with the LibPointMathcer-specific "
100 mrpt::poses::CPose3D(initialGuessLocalWrtGlobal);
107 std::optional<LogRecord> currentLog;
109 const bool generateDebugRecord =
110 outputDebugInfo.has_value() || p.generateDebugFiles;
112 if (generateDebugRecord)
114 currentLog.emplace();
115 currentLog->pcGlobal = pcGlobal.get_shared_from_this_or_clone();
116 currentLog->pcLocal = pcLocal.get_shared_from_this_or_clone();
117 currentLog->initialGuessLocalWrtGlobal = initialGuessLocalWrtGlobal;
118 currentLog->icpParameters = p;
122 ASSERT_(pcLocal.size() > 0);
123 ASSERT_(pcGlobal.size() > 0);
126 using DP = PM::DataPoints;
129 const DP ptsLocal = pointsToPM(pcLocal);
130 const DP ptsGlobal = pointsToPM(pcGlobal);
140 std::stringstream ss;
143 icp.loadFromYaml(ss);
147 ASSERT_EQUAL_(cloudDimension, 3U);
151 initialGuessLocalWrtGlobal.getHomogeneousMatrix().asEigen();
158 "Initial transformation is not rigid, SE(3) identity will be used");
159 initTransfo = PM::TransformationParameters::Identity(
160 cloudDimension + 1, cloudDimension + 1);
163 const DP ptsLocalTf = rigidTrans.
compute(ptsLocal, initTransfo);
169 T = icp(ptsLocalTf, ptsGlobal);
174 mrpt::poses::CPose3D(initialGuessLocalWrtGlobal) +
175 mrpt::poses::CPose3D(mrpt::math::CMatrixDouble44(T));
179 catch (
const PM::ConvergenceError&)
186 if (!icp.transformationCheckers.empty())
188 icp.transformationCheckers.at(0)->getConditionVariables()[0];
190 result.nIterations = 1;
198 result.optimalScale = 1.0;
204 result.finalPairings, result.optimal_tf.mean, covParams);
210 if (currentLog) currentLog->icpResult = result;
216 if (currentLog && outputDebugInfo.has_value())
217 outputDebugInfo.value().get() = std::move(currentLog.value());
220 THROW_EXCEPTION(
"This method requires MP2P built against libpointmatcher");