/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include "tfest-precomp.h" // Precompiled headers // #include #include #include #include #include #include #include #include #include #include // accumulate() using namespace mrpt; using namespace mrpt::math; using namespace mrpt::tfest; using namespace mrpt::poses; using namespace mrpt::system; using namespace std; template void TMatchingPairListTempl::dumpToFile(const std::string& fileName) const { std::ofstream f(fileName); ASSERT_(f.is_open()); for (const auto& it : *this) { f << mrpt::format( "%u %u %f %f %f %f %f %f %f\n", it.globalIdx, it.localIdx, it.global.x, it.global.y, it.global.z, it.local.x, it.local.y, it.local.z, it.errorSquareAfterTransformation); } } template void TMatchingPairListTempl::saveAsMATLABScript(const std::string& filName) const { FILE* f = os::fopen(filName.c_str(), "wt"); if (!f) return; fprintf(f, "%% ----------------------------------------------------\n"); fprintf(f, "%% File generated automatically by the MRPT method: \n"); fprintf(f, "%% saveAsMATLABScript \n"); fprintf(f, "%% Before calling this script, define line color: \n"); fprintf(f, "%% colorLines=[0.5 0.5 0.5] \n"); fprintf(f, "%% ----------------------------------------------------\n\n"); fprintf(f, "axis equal; hold on;\n"); for (const auto& it : *this) { fprintf( f, "line([%f %f %f],[%f %f %f],'Color',colorLines);\n", it.global.x, it.local.x, it.local.z, it.global.y, it.local.y, it.local.z); fprintf( f, "set(plot([%f %f %f],[%f %f " "%f],'.'),'Color',colorLines,'MarkerSize',15);\n", it.global.x, it.local.x, it.local.z, it.global.y, it.local.y, it.local.z); } fprintf(f, "view(3); grid on; xlabel('x'); ylabel('y'); zlabel('z');"); os::fclose(f); } template bool TMatchingPairListTempl::indexOtherMapHasCorrespondence(size_t idx) const { for (const auto& it : *this) if (it.localIdx == idx) return true; return false; } template T TMatchingPairListTempl::overallSquareError(const CPose2D& q) const { vector errs(base_t::size()); squareErrorVector(q, errs); return std::accumulate(errs.begin(), errs.end(), T(0)); } template T TMatchingPairListTempl::overallSquareError(const CPose3D& q) const { vector errs(base_t::size()); squareErrorVector(q, errs); return std::accumulate(errs.begin(), errs.end(), T(0)); } template T TMatchingPairListTempl::overallSquareErrorAndPoints( const CPose2D& q, vector& xs, vector& ys) const { vector errs(base_t::size()); squareErrorVector(q, errs, xs, ys); return std::accumulate(errs.begin(), errs.end(), T(0)); } template bool TMatchingPairListTempl::contains(const TMatchingPairTempl& p) const { for (const auto& corresp : *this) if (corresp == p) return true; return false; } template void TMatchingPairListTempl::squareErrorVector(const CPose2D& q, vector& out_sqErrs) const { out_sqErrs.resize(base_t::size()); // * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f] const T ccos = static_cast(std::cos(q.phi())); const T csin = static_cast(std::sin(q.phi())); const T qx = static_cast(q.x()); const T qy = static_cast(q.y()); typename base_t::const_iterator corresp; typename vector::iterator e_i; for (corresp = base_t::begin(), e_i = out_sqErrs.begin(); corresp != base_t::end(); ++corresp, ++e_i) { T xx = qx + ccos * corresp->local.x - csin * corresp->local.y; T yy = qy + csin * corresp->local.x + ccos * corresp->local.y; *e_i = square(corresp->global.x - xx) + square(corresp->global.y - yy); } } template void TMatchingPairListTempl::squareErrorVector(const CPose3D& q, vector& out_sqErrs) const { out_sqErrs.clear(); out_sqErrs.reserve(base_t::size()); for (const auto& c : *this) { const auto p = q.composePoint(mrpt::math::TPoint3D(c.local)); out_sqErrs.push_back(c.global.sqrDistanceTo(mrpt::math::TPoint3D_(p.x, p.y, p.z))); } } template void TMatchingPairListTempl::squareErrorVector( const CPose2D& q, vector& out_sqErrs, vector& xs, vector& ys) const { out_sqErrs.resize(base_t::size()); xs.resize(base_t::size()); ys.resize(base_t::size()); // * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f] const T ccos = static_cast(std::cos(q.phi())); const T csin = static_cast(std::sin(q.phi())); const T qx = static_cast(q.x()); const T qy = static_cast(q.y()); typename base_t::const_iterator corresp; typename vector::iterator e_i, xx, yy; for (corresp = base_t::begin(), e_i = out_sqErrs.begin(), xx = xs.begin(), yy = ys.begin(); corresp != base_t::end(); ++corresp, ++e_i, ++xx, ++yy) { *xx = qx + ccos * corresp->local.x - csin * corresp->local.y; *yy = qy + csin * corresp->local.x + ccos * corresp->local.y; *e_i = square(corresp->global.x - *xx) + square(corresp->global.y - *yy); } } template void TMatchingPairListTempl::filterUniqueRobustPairs( const size_t num_elements_this_map, TMatchingPairListTempl& out_filtered_list) const { using TMatchingPairConstPtr = TMatchingPairTempl const*; std::vector bestMatchForThisMap( num_elements_this_map, TMatchingPairConstPtr(nullptr)); out_filtered_list.clear(); // 1) Go through all the correspondences and keep the best corresp. // for each "global map" (this) point. for (auto& c : *this) { if (bestMatchForThisMap[c.globalIdx] == nullptr || // first one c.errorSquareAfterTransformation < bestMatchForThisMap[c.globalIdx]->errorSquareAfterTransformation // or better ) { bestMatchForThisMap[c.globalIdx] = &c; } } // 2) Go again through the list of correspondences and remove those // who are not the best one for their corresponding global map. for (auto& c : *this) { if (bestMatchForThisMap[c.globalIdx] == &c) out_filtered_list.push_back(c); // Add to the output } } template void TMatchingPairTempl::print(std::ostream& o) const { o << "[" << globalIdx << "->" << localIdx << "]" << ": " << "(" << global.x << "," << global.y << "," << global.z << ")" << " -> " << "(" << local.x << "," << local.y << "," << local.z << ")"; } // Explicit instantations: namespace mrpt::tfest { template class TMatchingPairListTempl; template class TMatchingPairListTempl; template struct TMatchingPairTempl; template struct TMatchingPairTempl; } // namespace mrpt::tfest