Main MRPT website > C++ reference for MRPT 1.5.7
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 
bool fileOpenCorrectly()
Returns true if the file was open without errors.
const_iterator end() const
Definition: ts_hash_map.h:152
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
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.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
const_iterator begin() const
Definition: ts_hash_map.h:151
Scalar * iterator
Definition: eigen_plugins.h:23
This file implements several operations that operate element-wise on individual or pairs of container...
bool contains(const TMatchingPair &p) const
Test whether the given pair "p" is within the pairings.
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
STL namespace.
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
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
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.
const GLubyte * c
Definition: glext.h:5590
void dumpToFile(const std::string &fileName) const
Saves the correspondences to a text file.
A list of TMatchingPair.
Definition: TMatchingPair.h:78
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
void saveAsMATLABScript(const std::string &filName) const
Saves the correspondences as a MATLAB script which draws them.
GLubyte GLubyte b
Definition: glext.h:5575
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
TMatchingPair const * TMatchingPairConstPtr
Definition: TMatchingPair.h:73
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...
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:36
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...
#define ASSERT_(f)
bool operator==(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:277
GLsizeiptr size
Definition: glext.h:3779
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
bool indexOtherMapHasCorrespondence(size_t idx) const
Checks if the given index from the "other" map appears in the list.
GLfloat GLfloat p
Definition: glext.h:5587
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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019