Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 "base-precomp.h" // Precompiled headers
11 
14 #include <mrpt/utils/utils_defs.h>
15 #include <mrpt/poses/CPose2D.h>
16 #include <mrpt/system/os.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::utils;
21 using namespace mrpt::math;
22 using namespace mrpt::poses;
23 using namespace mrpt::system;
24 using namespace std;
25 
26 /*---------------------------------------------------------------
27  dumpToFile
28  ---------------------------------------------------------------*/
29 void TMatchingPairList::dumpToFile(const std::string &fileName) const
30 {
31  CFileOutputStream f(fileName);
33 
34  for (const_iterator it=begin();it!=end();++it)
35  {
36  f.printf("%u %u %f %f %f %f %f %f %f\n",
37  it->this_idx,
38  it->other_idx,
39  it->this_x,
40  it->this_y,
41  it->this_z,
42  it->other_x,
43  it->other_y,
44  it->other_z,
45  it->errorSquareAfterTransformation);
46  }
47 }
48 
49 /*---------------------------------------------------------------
50  saveAsMATLABScript
51  ---------------------------------------------------------------*/
53 {
54  FILE *f = os::fopen(filName.c_str(),"wt");
55 
56  fprintf(f,"%% ----------------------------------------------------\n");
57  fprintf(f,"%% File generated automatically by the MRPT method:\n");
58  fprintf(f,"%% saveAsMATLABScript \n");
59  fprintf(f,"%% Before calling this script, define the color of lines, eg:\n");
60  fprintf(f,"%% colorLines=[1 1 1]");
61  fprintf(f,"%% J.L. Blanco (C) 2005-2012 \n");
62  fprintf(f,"%% ----------------------------------------------------\n\n");
63 
64  fprintf(f,"axis equal; hold on;\n");
65  for (const_iterator it=begin();it!=end();++it)
66  {
67  fprintf(f,"line([%f %f],[%f %f],'Color',colorLines);\n",
68  it->this_x,
69  it->other_x,
70  it->this_y,
71  it->other_y );
72  fprintf(f,"set(plot([%f %f],[%f %f],'.'),'Color',colorLines,'MarkerSize',15);\n",
73  it->this_x,
74  it->other_x,
75  it->this_y,
76  it->other_y );
77  }
78  os::fclose(f);
79 }
80 
81 /*---------------------------------------------------------------
82  indexOtherMapHasCorrespondence
83  ---------------------------------------------------------------*/
85 {
86  for (const_iterator it=begin();it!=end();++it) {
87  if (it->other_idx == idx)
88  return true;
89  }
90  return false;
91 }
92 
94 {
95  if (a.this_idx==b.this_idx)
96  return (a.this_idx<b.this_idx);
97  else return (a.other_idx<b.other_idx);
98 }
99 
100 
102 {
103  return (a.this_idx==b.this_idx) && (a.other_idx==b.other_idx);
104 }
105 
107 {
108  if (a.size()!=b.size())
109  return false;
110  for (TMatchingPairList::const_iterator it1=a.begin(),it2=b.begin();it1!=a.end();++it1,++it2)
111  if (! ( (*it1)==(*it2)))
112  return false;
113  return true;
114 }
115 
116 
117 /*---------------------------------------------------------------
118  overallSquareError
119  ---------------------------------------------------------------*/
121 {
122  vector<float> errs( size() );
123  squareErrorVector(q,errs);
124  return math::sum( errs );
125 }
126 
127 /*---------------------------------------------------------------
128  overallSquareErrorAndPoints
129  ---------------------------------------------------------------*/
131  const CPose2D &q,
132  vector<float> &xs,
133  vector<float> &ys ) const
134 {
135  vector<float> errs( size() );
136  squareErrorVector(q,errs,xs,ys);
137  return math::sum( errs );
138 }
139 
140 /*---------------------------------------------------------------
141  TMatchingPairList::contains
142  ---------------------------------------------------------------*/
144 {
145  for (const_iterator corresp=begin();corresp!=end();++corresp)
146  if ( *corresp == p )
147  return true;
148  return false;
149 }
150 
151 /*---------------------------------------------------------------
152  squareErrorVector
153  ---------------------------------------------------------------*/
154 void TMatchingPairList::squareErrorVector(const CPose2D &q, vector<float> &out_sqErrs ) const
155 {
156  out_sqErrs.resize( size() );
157  // * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f]
158 
159  const float ccos = cos(q.phi());
160  const float csin = sin(q.phi());
161  const float qx = q.x();
162  const float qy = q.y();
163 
164  const_iterator corresp;
166  for (corresp=begin(), e_i = out_sqErrs.begin();corresp!=end();++corresp, ++e_i)
167  {
168  float xx = qx + ccos * corresp->other_x - csin * corresp->other_y;
169  float yy = qy + csin * corresp->other_x + ccos * corresp->other_y;
170  *e_i = square( corresp->this_x - xx ) + square( corresp->this_y - yy );
171  }
172 }
173 
174 /*---------------------------------------------------------------
175  squareErrorVector
176  ---------------------------------------------------------------*/
178  const CPose2D &q,
179  vector<float> &out_sqErrs,
180  vector<float> &xs,
181  vector<float> &ys ) const
182 {
183  out_sqErrs.resize( size() );
184  xs.resize( size() );
185  ys.resize( size() );
186 
187  // * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f]
188 
189  const float ccos = cos(q.phi());
190  const float csin = sin(q.phi());
191  const float qx = q.x();
192  const float qy = q.y();
193 
194  const_iterator corresp;
195  vector<float>::iterator e_i, xx, yy;
196  for (corresp=begin(), e_i = out_sqErrs.begin(), xx = xs.begin(), yy = ys.begin();corresp!=end();++corresp, ++e_i, ++xx,++yy)
197  {
198  *xx = qx + ccos * corresp->other_x - csin * corresp->other_y;
199  *yy = qy + csin * corresp->other_x + ccos * corresp->other_y;
200  *e_i = square( corresp->this_x - *xx ) + square( corresp->this_y - *yy );
201  }
202 }
203 
204 void TMatchingPairList::filterUniqueRobustPairs(const size_t num_elements_this_map, TMatchingPairList &out_filtered_list) const
205 {
206  std::vector<TMatchingPairConstPtr> bestMatchForThisMap(num_elements_this_map, TMatchingPairConstPtr(nullptr));
207  out_filtered_list.clear();
208 
209  // 1) Go through all the correspondences and keep the best corresp.
210  // for each "global map" (this) point.
211  for (auto &c : *this)
212  {
213  if (bestMatchForThisMap[c.this_idx] == nullptr || // first one
214  c.errorSquareAfterTransformation < bestMatchForThisMap[c.this_idx]->errorSquareAfterTransformation // or better
215  )
216  {
217  bestMatchForThisMap[c.this_idx] = &c;
218  }
219  }
220 
221  // 2) Go again through the list of correspondences and remove those
222  // who are not the best one for their corresponding global map.
223  for (auto &c : *this) {
224  if (bestMatchForThisMap[c.this_idx] == &c)
225  out_filtered_list.push_back(c); // Add to the output
226  }
227 }
228 
229 
GLboolean GLboolean GLboolean GLboolean a
Definition: glew.h:5406
bool fileOpenCorrectly()
Returns true if the file was open without errors.
void dumpToFile(const std::string &fileName) const
Saves the correspondences to a text file.
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
void filterUniqueRobustPairs(const size_t num_elements_this_map, TMatchingPairList &out_filtered_list) const
Creates a filtered list of pairings with those ones which have a single correspondence which coincide...
bool contains(const TMatchingPair &p) const
Test whether the given pair "p" is within the pairings.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
const GLfloat * c
Definition: glew.h:10088
Scalar * iterator
Definition: eigen_plugins.h:23
This file implements several operations that operate element-wise on individual or pairs of container...
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
const Scalar * const_iterator
Definition: eigen_plugins.h:24
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
void squareErrorVector(const mrpt::poses::CPose2D &q, std::vector< float > &out_sqErrs) const
Returns a vector with the square error between each pair of correspondences in the list...
void saveAsMATLABScript(const std::string &filName) const
Saves the correspondences as a MATLAB script which draws them.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
bool operator<(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:281
This CStream derived class allow using a file as a write-only, binary stream.
bool indexOtherMapHasCorrespondence(size_t idx) const
Checks if the given index from the "other" map appears in the list.
A list of TMatchingPair.
Definition: TMatchingPair.h:78
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
GLfloat GLfloat p
Definition: glew.h:10113
TMatchingPair const * TMatchingPairConstPtr
Definition: TMatchingPair.h:73
GLsizeiptr size
Definition: glew.h:1586
const_iterator begin() const
Definition: ts_hash_map.h:151
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const_iterator end() const
Definition: ts_hash_map.h:152
#define ASSERT_(f)
bool operator==(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:277
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
float overallSquareError(const mrpt::poses::CPose2D &q) const
Computes the overall square error between the 2D points in the list of correspondences, given the 2D transformation "q" Where are the elements of the square error vector as computed by computeSquareErrorVector.
float overallSquareErrorAndPoints(const mrpt::poses::CPose2D &q, std::vector< float > &xs, std::vector< float > &ys) const
Computes the overall square error between the 2D points in the list of correspondences, given the 2D transformation "q", and return the transformed points as well.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018