28 void TMatchingPairList::dumpToFile(
const std::string& fileName)
const 30 std::ofstream f(fileName);
32 for (
auto it =
begin(); it !=
end(); ++it)
35 "%u %u %f %f %f %f %f %f %f\n", it->this_idx, it->other_idx,
36 it->this_x, it->this_y, it->this_z, it->other_x, it->other_y,
37 it->other_z, it->errorSquareAfterTransformation);
41 void TMatchingPairList::saveAsMATLABScript(
const std::string& filName)
const 43 FILE* f =
os::fopen(filName.c_str(),
"wt");
45 fprintf(f,
"%% ----------------------------------------------------\n");
46 fprintf(f,
"%% File generated automatically by the MRPT method:\n");
47 fprintf(f,
"%% saveAsMATLABScript \n");
49 f,
"%% Before calling this script, define the color of lines, eg:\n");
50 fprintf(f,
"%% colorLines=[1 1 1]");
51 fprintf(f,
"%% J.L. Blanco (C) 2005-2012 \n");
52 fprintf(f,
"%% ----------------------------------------------------\n\n");
54 fprintf(f,
"axis equal; hold on;\n");
58 f,
"line([%f %f],[%f %f],'Color',colorLines);\n", it->this_x,
59 it->other_x, it->this_y, it->other_y);
62 "set(plot([%f %f],[%f " 63 "%f],'.'),'Color',colorLines,'MarkerSize',15);\n",
64 it->this_x, it->other_x, it->this_y, it->other_y);
72 bool TMatchingPairList::indexOtherMapHasCorrespondence(
size_t idx)
const 76 if (it->other_idx == idx)
return true;
83 if (
a.this_idx ==
b.this_idx)
84 return (
a.this_idx <
b.this_idx);
86 return (
a.other_idx <
b.other_idx);
91 return (
a.this_idx ==
b.this_idx) && (
a.other_idx ==
b.other_idx);
97 if (
a.size() !=
b.size())
return false;
99 it1 !=
a.end(); ++it1, ++it2)
100 if (!((*it1) == (*it2)))
return false;
104 float TMatchingPairList::overallSquareError(
const CPose2D&
q)
const 106 vector<float> errs(
size());
107 squareErrorVector(
q, errs);
108 return std::accumulate(errs.begin(), errs.end(), 0);
111 float TMatchingPairList::overallSquareErrorAndPoints(
112 const CPose2D&
q, vector<float>& xs, vector<float>& ys)
const 114 vector<float> errs(
size());
115 squareErrorVector(
q, errs, xs, ys);
116 return std::accumulate(errs.begin(), errs.end(), 0);
125 if (*corresp ==
p)
return true;
132 void TMatchingPairList::squareErrorVector(
133 const CPose2D&
q, vector<float>& out_sqErrs)
const 135 out_sqErrs.resize(
size());
138 const float ccos = cos(
q.phi());
139 const float csin = sin(
q.phi());
140 const float qx =
q.x();
141 const float qy =
q.y();
145 for (corresp =
begin(), e_i = out_sqErrs.begin(); corresp !=
end();
148 float xx = qx + ccos * corresp->other_x - csin * corresp->other_y;
149 float yy = qy + csin * corresp->other_x + ccos * corresp->other_y;
150 *e_i =
square(corresp->this_x - xx) +
square(corresp->this_y - yy);
157 void TMatchingPairList::squareErrorVector(
158 const CPose2D&
q, vector<float>& out_sqErrs, vector<float>& xs,
159 vector<float>& ys)
const 161 out_sqErrs.resize(
size());
167 const float ccos = cos(
q.phi());
168 const float csin = sin(
q.phi());
169 const float qx =
q.x();
170 const float qy =
q.y();
174 for (corresp =
begin(), e_i = out_sqErrs.begin(), xx = xs.begin(),
176 corresp !=
end(); ++corresp, ++e_i, ++xx, ++yy)
178 *xx = qx + ccos * corresp->other_x - csin * corresp->other_y;
179 *yy = qy + csin * corresp->other_x + ccos * corresp->other_y;
180 *e_i =
square(corresp->this_x - *xx) +
square(corresp->this_y - *yy);
184 void TMatchingPairList::filterUniqueRobustPairs(
185 const size_t num_elements_this_map,
188 std::vector<TMatchingPairConstPtr> bestMatchForThisMap(
190 out_filtered_list.clear();
194 for (
auto&
c : *
this)
196 if (bestMatchForThisMap[
c.this_idx] ==
nullptr ||
197 c.errorSquareAfterTransformation <
198 bestMatchForThisMap[
c.this_idx]
199 ->errorSquareAfterTransformation
202 bestMatchForThisMap[
c.this_idx] = &
c;
208 for (
auto&
c : *
this)
210 if (bestMatchForThisMap[
c.this_idx] == &
c)
211 out_filtered_list.push_back(
c);
TMatchingPair const * TMatchingPairConstPtr
std::ostream & operator<<(std::ostream &o, const mrpt::tfest::TMatchingPair &pair)
size_t size(const MATRIXLIKE &m, const int dim)
int void fclose(FILE *f)
An OS-independent version of fclose.
GLdouble GLdouble GLdouble GLdouble q
EIGEN_STRONG_INLINE iterator begin()
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
This base provides a set of functions for maths stuff.
GLsizei const GLchar ** string
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
bool operator==(const TMatchingPair &a, const TMatchingPair &b)
A comparison operator.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
bool operator<(const TMatchingPair &a, const TMatchingPair &b)
A comparison operator, for sorting lists of TMatchingPair's, first order by this_idx, if equals, by other_idx.
GLubyte GLubyte GLubyte a
Functions for estimating the optimal transformation between two frames of references given measuremen...
const Scalar * const_iterator