15 #include <mrpt/core/exceptions.h>
16 #include <mrpt/poses/Lie/SE.h>
17 #include <mrpt/tfest/se3.h>
27 const mrpt::math::TPose3D& initialGuessLocalWrtGlobal,
const Parameters& p,
28 Results& result,
const mrpt::optional_ref<LogRecord>& outputDebugInfo)
30 using namespace std::string_literals;
41 ASSERT_(!pcGlobal.
empty());
42 ASSERT_(!pcLocal.
empty());
51 std::optional<LogRecord> currentLog;
53 const bool generateDebugRecord =
56 if (generateDebugRecord)
61 currentLog->initialGuessLocalWrtGlobal = initialGuessLocalWrtGlobal;
62 currentLog->icpParameters = p;
69 if (currentLog) state.
log = ¤tLog.value();
72 mrpt::poses::CPose3D(initialGuessLocalWrtGlobal);
115 const mrpt::math::CVectorFixed<double, 6> dSol =
116 mrpt::poses::Lie::SE<3>::log(deltaSol);
117 const double delta_xyz = dSol.blockCopy<3, 1>(0, 0).norm();
118 const double delta_rot = dSol.blockCopy<3, 1>(3, 0).norm();
123 "[ICP] Iter=%3u Delta_xyz=%9.02e, Delta_rot=%6.03f deg, "
124 "(xyzypr)=%s pairs=%s\n",
126 std::abs(delta_xyz), mrpt::RAD2DEG(std::abs(delta_rot)),
167 if (currentLog) currentLog->icpResult = result;
173 if (currentLog && outputDebugInfo.has_value())
174 outputDebugInfo.value().get() = std::move(currentLog.value());
181 using namespace std::string_literals;
186 static unsigned int logFileCounter = 0;
187 static std::mutex counterMtx;
188 unsigned int RECORD_UNIQUE_ID;
191 RECORD_UNIQUE_ID = logFileCounter++;
199 const auto value = mrpt::format(
"%05u", RECORD_UNIQUE_ID);
205 const auto value = mrpt::format(
206 "%05u",
static_cast<unsigned int>(
222 const auto value = mrpt::format(
223 "%05u",
static_cast<unsigned int>(
232 const auto value = (log.
pcLocal && log.
pcLocal->label.has_value())
241 std::cerr <<
"[ERROR] Could not save icp log file to '" <<
filename
250 for (
const auto& solver :
solvers)
253 if (solver->optimal_pose(pairings,
out, sc))
return true;
268 ASSERT_(params.isSequence());
269 for (
const auto& entry : params.asSequence())
271 const auto& e = entry.asMap();
273 if (e.count(
"enabled") && e.at(
"enabled").as<
bool>() ==
false)
continue;
275 const auto sClass = e.at(
"class").as<
std::string>();
276 auto o = mrpt::rtti::classFactory(sClass);
279 auto m = std::dynamic_pointer_cast<Solver>(o);
282 "`%s` class seems not to be derived from Solver",
285 m->initialize(e.at(
"params"));
300 ASSERT_(params.isSequence());
301 for (
const auto& entry : params.asSequence())
303 const auto& e = entry.asMap();
305 if (e.count(
"enabled") && e.at(
"enabled").as<
bool>() ==
false)
continue;
307 const auto sClass = e.at(
"class").as<
std::string>();
308 auto o = mrpt::rtti::classFactory(sClass);
311 auto m = std::dynamic_pointer_cast<Matcher>(o);
314 "`%s` class seems not to be derived from Matcher",
317 m->initialize(e.at(
"params"));
327 ASSERT_(params.isSequence());
328 const auto numEntries = params.asSequence().size();
330 for (
const auto& entry : params.asSequence())
332 const auto& e = entry.asMap();
334 if (e.count(
"enabled") && e.at(
"enabled").as<
bool>() ==
false)
continue;
336 const auto sClass = e.at(
"class").as<
std::string>();
337 auto o = mrpt::rtti::classFactory(sClass);
340 auto m = std::dynamic_pointer_cast<QualityEvaluator>(o);
343 "`%s` class seems not to be derived from QualityEvaluator",
346 m->initialize(e.at(
"params"));
349 if (numEntries > 0 && e.count(
"weight") > 0)
350 weight = e.at(
"weight").as<
double>();
351 lst.emplace_back(m, weight);
362 const metric_map_t& pcLocal,
const mrpt::poses::CPose3D& localPose,
365 ASSERT_(!evaluators.empty());
367 double sumW = .0, sumEvals = .0;
368 for (
const auto& e : evaluators)
370 const double w = e.relativeWeight;
373 e.obj->evaluate(pcGlobal, pcLocal, localPose, finalPairings);
374 sumEvals += w * eval;
379 return sumEvals / sumW;