Debian Codegraph @

libs/tfest/src/TMatchingPair.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "tfest-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/core/format.h>
13 #include <mrpt/poses/CPose2D.h>
14 #include <mrpt/poses/CPose3D.h>
15 #include <mrpt/system/os.h>
16 #include <mrpt/tfest/TMatchingPair.h>
17
18 #include <cmath>
19 #include <cstdio>
20 #include <fstream>
21 #include <iostream>
22 #include <numeric> // accumulate()
23
24 using namespace mrpt;
25 using namespace mrpt::math;
26 using namespace mrpt::tfest;
27 using namespace mrpt::poses;
28 using namespace mrpt::system;
29 using namespace std;
30
31 template <typename T>
32 void TMatchingPairListTempl<T>::dumpToFile(const std::string& fileName) const
33 {
34 std::ofstream f(fileName);
35 ASSERT_(f.is_open());
36 for (const auto& it : *this)
37 {
38 f << mrpt::format(
39 "%u %u %f %f %f %f %f %f %f\n", it.globalIdx, it.localIdx, it.global.x, it.global.y,
40 it.global.z, it.local.x, it.local.y, it.local.z, it.errorSquareAfterTransformation);
41 }
42 }
43
44 template <typename T>
45 void TMatchingPairListTempl<T>::saveAsMATLABScript(const std::string& filName) const
46 {
47 FILE* f = os::fopen(filName.c_str(), "wt");
48 if (!f) return;
49
50 fprintf(f, "%% ----------------------------------------------------\n");
51 fprintf(f, "%% File generated automatically by the MRPT method: \n");
52 fprintf(f, "%% saveAsMATLABScript \n");
53 fprintf(f, "%% Before calling this script, define line color: \n");
54 fprintf(f, "%% colorLines=[0.5 0.5 0.5] \n");
55 fprintf(f, "%% ----------------------------------------------------\n\n");
56
57 fprintf(f, "axis equal; hold on;\n");
58 for (const auto& it : *this)
59 {
60 fprintf(
61 f, "line([%f %f %f],[%f %f %f],'Color',colorLines);\n", it.global.x, it.local.x, it.local.z,
62 it.global.y, it.local.y, it.local.z);
63 fprintf(
64 f,
65 "set(plot([%f %f %f],[%f %f "
66 "%f],'.'),'Color',colorLines,'MarkerSize',15);\n",
67 it.global.x, it.local.x, it.local.z, it.global.y, it.local.y, it.local.z);
68 }
69 fprintf(f, "view(3); grid on; xlabel('x'); ylabel('y'); zlabel('z');");
70 os::fclose(f);
71 }
72
73 template <typename T>
74 bool TMatchingPairListTempl<T>::indexOtherMapHasCorrespondence(size_t idx) const
75 {
76 for (const auto& it : *this)
77 if (it.localIdx == idx) return true;
78 return false;
79 }
80
81 template <typename T>
82 T TMatchingPairListTempl<T>::overallSquareError(const CPose2D& q) const
83 {
84 vector<T> errs(base_t::size());
85 squareErrorVector(q, errs);
86 return std::accumulate(errs.begin(), errs.end(), T(0));
87 }
88
89 template <typename T>
90 T TMatchingPairListTempl<T>::overallSquareError(const CPose3D& q) const
91 {
92 vector<T> errs(base_t::size());
93 squareErrorVector(q, errs);
94 return std::accumulate(errs.begin(), errs.end(), T(0));
95 }
96
97 template <typename T>
98 T TMatchingPairListTempl<T>::overallSquareErrorAndPoints(
99 const CPose2D& q, vector<T>& xs, vector<T>& ys) const
100 {
101 vector<T> errs(base_t::size());
102 squareErrorVector(q, errs, xs, ys);
103 return std::accumulate(errs.begin(), errs.end(), T(0));
104 }
105
106 template <typename T>
107 bool TMatchingPairListTempl<T>::contains(const TMatchingPairTempl<T>& p) const
108 {
109 for (const auto& corresp : *this)
110 if (corresp == p) return true;
111 return false;
112 }
113
114 template <typename T>
115 void TMatchingPairListTempl<T>::squareErrorVector(const CPose2D& q, vector<T>& out_sqErrs) const
116 {
117 out_sqErrs.resize(base_t::size());
118 // * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f]
119
120 const T ccos = static_cast<T>(std::cos(q.phi()));
121 const T csin = static_cast<T>(std::sin(q.phi()));
122 const T qx = static_cast<T>(q.x());
123 const T qy = static_cast<T>(q.y());
124
125 typename base_t::const_iterator corresp;
126 typename vector<T>::iterator e_i;
127 for (corresp = base_t::begin(), e_i = out_sqErrs.begin(); corresp != base_t::end();
128 ++corresp, ++e_i)
129 {
130 T xx = qx + ccos * corresp->local.x - csin * corresp->local.y;
131 T yy = qy + csin * corresp->local.x + ccos * corresp->local.y;
132 *e_i = square(corresp->global.x - xx) + square(corresp->global.y - yy);
133 }
134 }
135
136 template <typename T>
137 void TMatchingPairListTempl<T>::squareErrorVector(const CPose3D& q, vector<T>& out_sqErrs) const
138 {
139 out_sqErrs.clear();
140 out_sqErrs.reserve(base_t::size());
141
142 for (const auto& c : *this)
143 {
144 const auto p = q.composePoint(mrpt::math::TPoint3D(c.local));
145 out_sqErrs.push_back(c.global.sqrDistanceTo(mrpt::math::TPoint3D_<T>(p.x, p.y, p.z)));
146 }
147 }
148
149 template <typename T>
150 void TMatchingPairListTempl<T>::squareErrorVector(
151 const CPose2D& q, vector<T>& out_sqErrs, vector<T>& xs, vector<T>& ys) const
152 {
153 out_sqErrs.resize(base_t::size());
154 xs.resize(base_t::size());
155 ys.resize(base_t::size());
156
157 // * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f]
158
159 const T ccos = static_cast<T>(std::cos(q.phi()));
160 const T csin = static_cast<T>(std::sin(q.phi()));
161 const T qx = static_cast<T>(q.x());
162 const T qy = static_cast<T>(q.y());
163
164 typename base_t::const_iterator corresp;
165 typename vector<T>::iterator e_i, xx, yy;
166 for (corresp = base_t::begin(), e_i = out_sqErrs.begin(), xx = xs.begin(), yy = ys.begin();
167 corresp != base_t::end(); ++corresp, ++e_i, ++xx, ++yy)
168 {
169 *xx = qx + ccos * corresp->local.x - csin * corresp->local.y;
170 *yy = qy + csin * corresp->local.x + ccos * corresp->local.y;
171 *e_i = square(corresp->global.x - *xx) + square(corresp->global.y - *yy);
172 }
173 }
174
175 template <typename T>
176 void TMatchingPairListTempl<T>::filterUniqueRobustPairs(
177 const size_t num_elements_this_map, TMatchingPairListTempl<T>& out_filtered_list) const
178 {
179 using TMatchingPairConstPtr = TMatchingPairTempl<T> const*;
180
181 std::vector<TMatchingPairConstPtr> bestMatchForThisMap(
182 num_elements_this_map, TMatchingPairConstPtr(nullptr));
183 out_filtered_list.clear();
184
185 // 1) Go through all the correspondences and keep the best corresp.
186 // for each "global map" (this) point.
187 for (auto& c : *this)
188 {
189 if (bestMatchForThisMap[c.globalIdx] == nullptr || // first one
190 c.errorSquareAfterTransformation <
191 bestMatchForThisMap[c.globalIdx]->errorSquareAfterTransformation // or better
192 )
193 {
194 bestMatchForThisMap[c.globalIdx] = &c;
195 }
196 }
197
198 // 2) Go again through the list of correspondences and remove those
199 // who are not the best one for their corresponding global map.
200 for (auto& c : *this)
201 {
202 if (bestMatchForThisMap[c.globalIdx] == &c)
203 out_filtered_list.push_back(c); // Add to the output
204 }
205 }
206
207 template <typename T>
208 void TMatchingPairTempl<T>::print(std::ostream& o) const
209 {
210 o << "[" << globalIdx << "->" << localIdx << "]"
211 << ": "
212 << "(" << global.x << "," << global.y << "," << global.z << ")"
213 << " -> "
214 << "(" << local.x << "," << local.y << "," << local.z << ")";
215 }
216
217 // Explicit instantations:
218 namespace mrpt::tfest
219 {
220 template class TMatchingPairListTempl<float>;
221 template class TMatchingPairListTempl<double>;
222
223 template struct TMatchingPairTempl<float>;
224 template struct TMatchingPairTempl<double>;
225
226 } // namespace mrpt::tfest