MRPT  2.0.4
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-2020, 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 <cmath>
17 #include <cstdio>
18 #include <fstream>
19 #include <iostream>
20 #include <numeric> // accumulate()
21 
22 using namespace mrpt;
23 using namespace mrpt::math;
24 using namespace mrpt::tfest;
25 using namespace mrpt::poses;
26 using namespace mrpt::system;
27 using namespace std;
28 
29 void TMatchingPairList::dumpToFile(const std::string& fileName) const
30 {
31  std::ofstream f(fileName);
32  ASSERT_(f.is_open());
33  for (const auto& it : *this)
34  {
35  f << mrpt::format(
36  "%u %u %f %f %f %f %f %f %f\n", it.this_idx, it.other_idx,
37  it.this_x, it.this_y, it.this_z, it.other_x, it.other_y, it.other_z,
38  it.errorSquareAfterTransformation);
39  }
40 }
41 
42 void TMatchingPairList::saveAsMATLABScript(const std::string& filName) const
43 {
44  FILE* f = os::fopen(filName.c_str(), "wt");
45 
46  fprintf(f, "%% ----------------------------------------------------\n");
47  fprintf(f, "%% File generated automatically by the MRPT method:\n");
48  fprintf(f, "%% saveAsMATLABScript \n");
49  fprintf(
50  f, "%% Before calling this script, define the color of lines, eg:\n");
51  fprintf(f, "%% colorLines=[1 1 1]");
52  fprintf(f, "%% J.L. Blanco (C) 2005-2012 \n");
53  fprintf(f, "%% ----------------------------------------------------\n\n");
54 
55  fprintf(f, "axis equal; hold on;\n");
56  for (const auto& it : *this)
57  {
58  fprintf(
59  f, "line([%f %f],[%f %f],'Color',colorLines);\n", it.this_x,
60  it.other_x, it.this_y, it.other_y);
61  fprintf(
62  f,
63  "set(plot([%f %f],[%f "
64  "%f],'.'),'Color',colorLines,'MarkerSize',15);\n",
65  it.this_x, it.other_x, it.this_y, it.other_y);
66  }
67  os::fclose(f);
68 }
69 
70 /*---------------------------------------------------------------
71  indexOtherMapHasCorrespondence
72  ---------------------------------------------------------------*/
73 bool TMatchingPairList::indexOtherMapHasCorrespondence(size_t idx) const
74 {
75  for (const auto& it : *this)
76  {
77  if (it.other_idx == idx) return true;
78  }
79  return false;
80 }
81 
83 {
84  if (a.this_idx == b.this_idx)
85  return (a.this_idx < b.this_idx);
86  else
87  return (a.other_idx < b.other_idx);
88 }
89 
91 {
92  return (a.this_idx == b.this_idx) && (a.other_idx == b.other_idx);
93 }
94 
96  const TMatchingPairList& a, const TMatchingPairList& b)
97 {
98  if (a.size() != b.size()) return false;
99  for (auto it1 = a.begin(), it2 = b.begin(); 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(), .0f);
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(), .0f);
117 }
118 
119 /*---------------------------------------------------------------
120  TMatchingPairList::contains
121  ---------------------------------------------------------------*/
122 bool TMatchingPairList::contains(const TMatchingPair& p) const
123 {
124  for (const auto& corresp : *this)
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 = d2f(std::cos(q.phi()));
139  const float csin = d2f(std::sin(q.phi()));
140  const float qx = d2f(q.x());
141  const float qy = d2f(q.y());
142 
143  const_iterator corresp;
144  vector<float>::iterator e_i;
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 = d2f(cos(q.phi()));
168  const float csin = d2f(sin(q.phi()));
169  const float qx = d2f(q.x());
170  const float qy = d2f(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 }
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:282
STL namespace.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
float d2f(const double d)
shortcut for static_cast<float>(double)
This base provides a set of functions for maths stuff.
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
A list of TMatchingPair.
Definition: TMatchingPair.h:70
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
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:415
return_t square(const num_t x)
Inline function for the square of a number.
const_iterator end() const
Definition: ts_hash_map.h:246
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:264
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.
Functions for estimating the optimal transformation between two frames of references given measuremen...



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 7b5ddf9de Fri May 29 14:02:56 2020 +0200 at vie may 29 14:15:09 CEST 2020