MRPT  1.9.9
TMatchingPair.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, 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/system/os.h>
16 #include <cstdio>
17 #include <fstream>
18 #include <iostream>
19 #include <numeric> // accumulate()
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 (const auto& it : *this)
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, it.other_z,
37  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 auto& it : *this)
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 auto& it : *this)
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 (auto it1 = a.begin(), it2 = b.begin(); it1 != a.end(); ++it1, ++it2)
99  if (!((*it1) == (*it2))) return false;
100  return true;
101 }
102 
103 float TMatchingPairList::overallSquareError(const CPose2D& q) const
104 {
105  vector<float> errs(size());
106  squareErrorVector(q, errs);
107  return std::accumulate(errs.begin(), errs.end(), .0f);
108 }
109 
110 float TMatchingPairList::overallSquareErrorAndPoints(
111  const CPose2D& q, vector<float>& xs, vector<float>& ys) const
112 {
113  vector<float> errs(size());
114  squareErrorVector(q, errs, xs, ys);
115  return std::accumulate(errs.begin(), errs.end(), .0f);
116 }
117 
118 /*---------------------------------------------------------------
119  TMatchingPairList::contains
120  ---------------------------------------------------------------*/
121 bool TMatchingPairList::contains(const TMatchingPair& p) const
122 {
123  for (const auto& corresp : *this)
124  if (corresp == p) return true;
125  return false;
126 }
127 
128 /*---------------------------------------------------------------
129  squareErrorVector
130  ---------------------------------------------------------------*/
131 void TMatchingPairList::squareErrorVector(
132  const CPose2D& q, vector<float>& out_sqErrs) const
133 {
134  out_sqErrs.resize(size());
135  // * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f]
136 
137  const float ccos = cos(q.phi());
138  const float csin = sin(q.phi());
139  const float qx = q.x();
140  const float qy = q.y();
141 
142  const_iterator corresp;
143  vector<float>::iterator e_i;
144  for (corresp = begin(), e_i = out_sqErrs.begin(); corresp != end();
145  ++corresp, ++e_i)
146  {
147  float xx = qx + ccos * corresp->other_x - csin * corresp->other_y;
148  float yy = qy + csin * corresp->other_x + ccos * corresp->other_y;
149  *e_i = square(corresp->this_x - xx) + square(corresp->this_y - yy);
150  }
151 }
152 
153 /*---------------------------------------------------------------
154  squareErrorVector
155  ---------------------------------------------------------------*/
156 void TMatchingPairList::squareErrorVector(
157  const CPose2D& q, vector<float>& out_sqErrs, vector<float>& xs,
158  vector<float>& ys) const
159 {
160  out_sqErrs.resize(size());
161  xs.resize(size());
162  ys.resize(size());
163 
164  // * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f]
165 
166  const float ccos = cos(q.phi());
167  const float csin = sin(q.phi());
168  const float qx = q.x();
169  const float qy = q.y();
170 
171  const_iterator corresp;
172  vector<float>::iterator e_i, xx, yy;
173  for (corresp = begin(), e_i = out_sqErrs.begin(), xx = xs.begin(),
174  yy = ys.begin();
175  corresp != end(); ++corresp, ++e_i, ++xx, ++yy)
176  {
177  *xx = qx + ccos * corresp->other_x - csin * corresp->other_y;
178  *yy = qy + csin * corresp->other_x + ccos * corresp->other_y;
179  *e_i = square(corresp->this_x - *xx) + square(corresp->this_y - *yy);
180  }
181 }
182 
183 void TMatchingPairList::filterUniqueRobustPairs(
184  const size_t num_elements_this_map,
185  TMatchingPairList& out_filtered_list) const
186 {
187  std::vector<TMatchingPairConstPtr> bestMatchForThisMap(
188  num_elements_this_map, TMatchingPairConstPtr(nullptr));
189  out_filtered_list.clear();
190 
191  // 1) Go through all the correspondences and keep the best corresp.
192  // for each "global map" (this) point.
193  for (auto& c : *this)
194  {
195  if (bestMatchForThisMap[c.this_idx] == nullptr || // first one
196  c.errorSquareAfterTransformation <
197  bestMatchForThisMap[c.this_idx]
198  ->errorSquareAfterTransformation // or better
199  )
200  {
201  bestMatchForThisMap[c.this_idx] = &c;
202  }
203  }
204 
205  // 2) Go again through the list of correspondences and remove those
206  // who are not the best one for their corresponding global map.
207  for (auto& c : *this)
208  {
209  if (bestMatchForThisMap[c.this_idx] == &c)
210  out_filtered_list.push_back(c); // Add to the output
211  }
212 }
213 
215  std::ostream& o, const mrpt::tfest::TMatchingPair& pair)
216 {
217  o << "[" << pair.this_idx << "->" << pair.other_idx << "]"
218  << ": "
219  << "(" << pair.this_x << "," << pair.this_y << "," << pair.this_z << ")"
220  << " -> "
221  << "(" << pair.other_x << "," << pair.other_y << "," << pair.other_z
222  << ")";
223  return o;
224 }
TMatchingPair const * TMatchingPairConstPtr
Definition: TMatchingPair.h:62
std::ostream & operator<<(std::ostream &o, const mrpt::tfest::TMatchingPair &pair)
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
size_t size(const MATRIXLIKE &m, const int dim)
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
STL namespace.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
T square(const T x)
Inline function for the square of a number.
const GLubyte * c
Definition: glext.h:6406
GLuint GLuint end
Definition: glext.h:3532
A list of TMatchingPair.
Definition: TMatchingPair.h:70
GLubyte GLubyte b
Definition: glext.h:6372
GLsizei const GLchar ** string
Definition: glext.h:4116
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:31
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:410
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const_iterator begin() const
Definition: ts_hash_map.h:240
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
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:257
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:6372
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLfloat GLfloat p
Definition: glext.h:6398



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019