Main MRPT website > C++ reference for MRPT 1.9.9
TMatchingPair.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "tfest-precomp.h" // Precompiled headers
11 
13 #include <mrpt/poses/CPose2D.h>
14 #include <mrpt/system/os.h>
15 #include <mrpt/core/format.h>
16 #include <iostream>
17 #include <fstream>
18 #include <numeric> // accumulate()
19 #include <cstdio>
20 
21 using namespace mrpt;
22 using namespace mrpt::math;
23 using namespace mrpt::tfest;
24 using namespace mrpt::poses;
25 using namespace mrpt::system;
26 using namespace std;
27 
28 void TMatchingPairList::dumpToFile(const std::string& fileName) const
29 {
30  std::ofstream f(fileName);
31  ASSERT_(f.is_open());
32  for (auto it = begin(); it != end(); ++it)
33  {
34  f << mrpt::format(
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);
38  }
39 }
40 
41 void TMatchingPairList::saveAsMATLABScript(const std::string& filName) const
42 {
43  FILE* f = os::fopen(filName.c_str(), "wt");
44 
45  fprintf(f, "%% ----------------------------------------------------\n");
46  fprintf(f, "%% File generated automatically by the MRPT method:\n");
47  fprintf(f, "%% saveAsMATLABScript \n");
48  fprintf(
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");
53 
54  fprintf(f, "axis equal; hold on;\n");
55  for (const_iterator it = begin(); it != end(); ++it)
56  {
57  fprintf(
58  f, "line([%f %f],[%f %f],'Color',colorLines);\n", it->this_x,
59  it->other_x, it->this_y, it->other_y);
60  fprintf(
61  f,
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);
65  }
66  os::fclose(f);
67 }
68 
69 /*---------------------------------------------------------------
70  indexOtherMapHasCorrespondence
71  ---------------------------------------------------------------*/
72 bool TMatchingPairList::indexOtherMapHasCorrespondence(size_t idx) const
73 {
74  for (const_iterator it = begin(); it != end(); ++it)
75  {
76  if (it->other_idx == idx) return true;
77  }
78  return false;
79 }
80 
82 {
83  if (a.this_idx == b.this_idx)
84  return (a.this_idx < b.this_idx);
85  else
86  return (a.other_idx < b.other_idx);
87 }
88 
90 {
91  return (a.this_idx == b.this_idx) && (a.other_idx == b.other_idx);
92 }
93 
95  const TMatchingPairList& a, const TMatchingPairList& b)
96 {
97  if (a.size() != b.size()) return false;
98  for (TMatchingPairList::const_iterator it1 = a.begin(), it2 = b.begin();
99  it1 != a.end(); ++it1, ++it2)
100  if (!((*it1) == (*it2))) return false;
101  return true;
102 }
103 
104 float TMatchingPairList::overallSquareError(const CPose2D& q) const
105 {
106  vector<float> errs(size());
107  squareErrorVector(q, errs);
108  return std::accumulate(errs.begin(), errs.end(), 0);
109 }
110 
111 float TMatchingPairList::overallSquareErrorAndPoints(
112  const CPose2D& q, vector<float>& xs, vector<float>& ys) const
113 {
114  vector<float> errs(size());
115  squareErrorVector(q, errs, xs, ys);
116  return std::accumulate(errs.begin(), errs.end(), 0);
117 }
118 
119 /*---------------------------------------------------------------
120  TMatchingPairList::contains
121  ---------------------------------------------------------------*/
122 bool TMatchingPairList::contains(const TMatchingPair& p) const
123 {
124  for (const_iterator corresp = begin(); corresp != end(); ++corresp)
125  if (*corresp == p) return true;
126  return false;
127 }
128 
129 /*---------------------------------------------------------------
130  squareErrorVector
131  ---------------------------------------------------------------*/
132 void TMatchingPairList::squareErrorVector(
133  const CPose2D& q, vector<float>& out_sqErrs) const
134 {
135  out_sqErrs.resize(size());
136  // * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f]
137 
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();
142 
143  const_iterator corresp;
145  for (corresp = begin(), e_i = out_sqErrs.begin(); corresp != end();
146  ++corresp, ++e_i)
147  {
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);
151  }
152 }
153 
154 /*---------------------------------------------------------------
155  squareErrorVector
156  ---------------------------------------------------------------*/
157 void TMatchingPairList::squareErrorVector(
158  const CPose2D& q, vector<float>& out_sqErrs, vector<float>& xs,
159  vector<float>& ys) const
160 {
161  out_sqErrs.resize(size());
162  xs.resize(size());
163  ys.resize(size());
164 
165  // * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f]
166 
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();
171 
172  const_iterator corresp;
173  vector<float>::iterator e_i, xx, yy;
174  for (corresp = begin(), e_i = out_sqErrs.begin(), xx = xs.begin(),
175  yy = ys.begin();
176  corresp != end(); ++corresp, ++e_i, ++xx, ++yy)
177  {
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);
181  }
182 }
183 
184 void TMatchingPairList::filterUniqueRobustPairs(
185  const size_t num_elements_this_map,
186  TMatchingPairList& out_filtered_list) const
187 {
188  std::vector<TMatchingPairConstPtr> bestMatchForThisMap(
189  num_elements_this_map, TMatchingPairConstPtr(nullptr));
190  out_filtered_list.clear();
191 
192  // 1) Go through all the correspondences and keep the best corresp.
193  // for each "global map" (this) point.
194  for (auto& c : *this)
195  {
196  if (bestMatchForThisMap[c.this_idx] == nullptr || // first one
197  c.errorSquareAfterTransformation <
198  bestMatchForThisMap[c.this_idx]
199  ->errorSquareAfterTransformation // or better
200  )
201  {
202  bestMatchForThisMap[c.this_idx] = &c;
203  }
204  }
205 
206  // 2) Go again through the list of correspondences and remove those
207  // who are not the best one for their corresponding global map.
208  for (auto& c : *this)
209  {
210  if (bestMatchForThisMap[c.this_idx] == &c)
211  out_filtered_list.push_back(c); // Add to the output
212  }
213 }
214 
216  std::ostream& o, const mrpt::tfest::TMatchingPair& pair)
217 {
218  o << "[" << pair.this_idx << "->" << pair.other_idx << "]"
219  << ": "
220  << "(" << pair.this_x << "," << pair.this_y << "," << pair.this_z << ")"
221  << " -> "
222  << "(" << pair.other_x << "," << pair.other_y << "," << pair.other_z
223  << ")";
224  return o;
225 }
Scalar * iterator
Definition: eigen_plugins.h:26
TMatchingPair const * TMatchingPairConstPtr
Definition: TMatchingPair.h:75
std::ostream & operator<<(std::ostream &o, const mrpt::tfest::TMatchingPair &pair)
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
size_t size(const MATRIXLIKE &m, const int dim)
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:29
STL namespace.
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This base provides a set of functions for maths stuff.
const GLubyte * c
Definition: glext.h:6313
GLuint GLuint end
Definition: glext.h:3528
A list of TMatchingPair.
Definition: TMatchingPair.h:83
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLubyte GLubyte b
Definition: glext.h:6279
GLsizei const GLchar ** string
Definition: glext.h:4101
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...
Definition: TMatchingPair.h:33
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
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...
Definition: CPose2D.h:40
bool operator==(const TMatchingPair &a, const TMatchingPair &b)
A comparison operator.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
bool operator<(const TMatchingPair &a, const TMatchingPair &b)
A comparison operator, for sorting lists of TMatchingPair&#39;s, first order by this_idx, if equals, by other_idx.
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLfloat GLfloat p
Definition: glext.h:6305
const Scalar * const_iterator
Definition: eigen_plugins.h:27



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019