MRPT  2.0.4
TMatchingPair.h
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 #pragma once
10 
11 #include <mrpt/core/common.h> // MRPT_IS_X86_AMD64
12 #include <mrpt/poses/poses_frwds.h>
13 #include <cstdint>
14 #include <iosfwd>
15 #include <string>
16 #include <vector>
17 
18 namespace mrpt::tfest
19 {
20 // Pragma defined to ensure no structure packing, so we can use SSE2
21 // vectorization on parts of this struture
22 #if defined(MRPT_IS_X86_AMD64)
23 #pragma pack(push, 1)
24 #endif
25 
26 /** A structure for holding correspondences between two sets of points or
27  * points-like entities in 2D or 3D. Using `float` to save space since large
28  * point clouds are likely stored in local coordinates using `float`.
29  * \ingroup mrpt_tfest_grp
30  */
32 {
33  TMatchingPair() = default;
34 
36  uint32_t _this_idx, uint32_t _other_idx, float _this_x, float _this_y,
37  float _this_z, float _other_x, float _other_y, float _other_z)
38  : this_idx(_this_idx),
39  other_idx(_other_idx),
40  this_x(_this_x),
41  this_y(_this_y),
42  this_z(_this_z),
43  other_x(_other_x),
44  other_y(_other_y),
45  other_z(_other_z),
47  {
48  }
49 
50  uint32_t this_idx{0};
51  uint32_t other_idx{0};
52  float this_x{0}, this_y{0}, this_z{0};
53  float other_x{0}, other_y{0}, other_z{0};
55 };
56 
57 #if defined(MRPT_IS_X86_AMD64)
58 #pragma pack(pop) // End of pack = 1
59 #endif
60 
63 
64 std::ostream& operator<<(
65  std::ostream& o, const mrpt::tfest::TMatchingPair& pair);
66 
67 /** A list of TMatchingPair
68  * \ingroup mrpt_tfest_grp
69  */
70 class TMatchingPairList : public std::vector<TMatchingPair>
71 {
72  public:
73  /** Checks if the given index from the "other" map appears in the list. */
74  bool indexOtherMapHasCorrespondence(size_t idx) const;
75  /** Saves the correspondences to a text file */
76  void dumpToFile(const std::string& fileName) const;
77  /** Saves the correspondences as a MATLAB script which draws them. */
78  void saveAsMATLABScript(const std::string& filName) const;
79 
80  /** Computes the overall square error between the 2D points in the list of
81  * correspondences, given the 2D transformation "q"
82  * \f[ \sum\limits_i e_i \f]
83  * Where \f$ e_i \f$ are the elements of the square error vector as
84  * computed by computeSquareErrorVector
85  * \sa squareErrorVector, overallSquareErrorAndPoints
86  */
87  float overallSquareError(const mrpt::poses::CPose2D& q) const;
88 
89  /** Computes the overall square error between the 2D points in the list of
90  * correspondences, given the 2D transformation "q", and return the
91  * transformed points as well.
92  * \f[ \sum\limits_i e_i \f]
93  * Where \f$ e_i \f$ are the elements of the square error vector as
94  * computed by computeSquareErrorVector
95  * \sa squareErrorVector
96  */
98  const mrpt::poses::CPose2D& q, std::vector<float>& xs,
99  std::vector<float>& ys) const;
100 
101  /** Returns a vector with the square error between each pair of
102  * correspondences in the list, given the 2D transformation "q"
103  * Each element \f$ e_i \f$ is the square distance between the "this"
104  * (global) point and the "other" (local) point transformed through "q":
105  * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f]
106  * \sa overallSquareError
107  */
108  void squareErrorVector(
109  const mrpt::poses::CPose2D& q, std::vector<float>& out_sqErrs) const;
110 
111  /** Returns a vector with the square error between each pair of
112  * correspondences in the list and the transformed "other" (local) points,
113  * given the 2D transformation "q"
114  * Each element \f$ e_i \f$ is the square distance between the "this"
115  * (global) point and the "other" (local) point transformed through "q":
116  * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f]
117  * \sa overallSquareError
118  */
119  void squareErrorVector(
120  const mrpt::poses::CPose2D& q, std::vector<float>& out_sqErrs,
121  std::vector<float>& xs, std::vector<float>& ys) const;
122 
123  /** Test whether the given pair "p" is within the pairings */
124  bool contains(const TMatchingPair& p) const;
125 
126  /** Creates a filtered list of pairings with those ones which have a single
127  * correspondence which coincides
128  * in both directions, i.e. the best pairing of element `i` in map `this`
129  * is the best match for element `j` in map `other`,
130  * and viceversa*/
132  const size_t num_elements_this_map,
133  TMatchingPairList& out_filtered_list) const;
134 };
135 
136 /** A comparison operator, for sorting lists of TMatchingPair's, first order by
137  * this_idx, if equals, by other_idx */
138 bool operator<(const TMatchingPair& a, const TMatchingPair& b);
139 
140 /** A comparison operator */
141 bool operator==(const TMatchingPair& a, const TMatchingPair& b);
142 
143 /** A comparison operator */
144 bool operator==(const TMatchingPairList& a, const TMatchingPairList& b);
145 
146 } // namespace mrpt::tfest
mrpt::tfest::TMatchingPairList::overallSquareErrorAndPoints
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,...
Definition: TMatchingPair.cpp:111
mrpt::tfest::TMatchingPairConstPtr
TMatchingPair const * TMatchingPairConstPtr
Definition: TMatchingPair.h:62
mrpt::tfest::TMatchingPair::TMatchingPair
TMatchingPair()=default
mrpt::tfest::TMatchingPair::other_z
float other_z
Definition: TMatchingPair.h:53
mrpt::tfest::TMatchingPairList::squareErrorVector
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,...
Definition: TMatchingPair.cpp:132
mrpt::tfest::TMatchingPair::other_idx
uint32_t other_idx
Definition: TMatchingPair.h:51
mrpt::tfest::TMatchingPairList::filterUniqueRobustPairs
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...
Definition: TMatchingPair.cpp:184
mrpt::tfest::TMatchingPairList::saveAsMATLABScript
void saveAsMATLABScript(const std::string &filName) const
Saves the correspondences as a MATLAB script which draws them.
Definition: TMatchingPair.cpp:42
mrpt::tfest
Functions for estimating the optimal transformation between two frames of references given measuremen...
Definition: indiv-compat-decls.h:14
mrpt::tfest::TMatchingPair
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
mrpt::tfest::TMatchingPairList::indexOtherMapHasCorrespondence
bool indexOtherMapHasCorrespondence(size_t idx) const
Checks if the given index from the "other" map appears in the list.
Definition: TMatchingPair.cpp:73
mrpt::tfest::TMatchingPairList::overallSquareError
float overallSquareError(const mrpt::poses::CPose2D &q) const
Computes the overall square error between the 2D points in the list of correspondences,...
Definition: TMatchingPair.cpp:104
mrpt::tfest::TMatchingPair::TMatchingPair
TMatchingPair(uint32_t _this_idx, uint32_t _other_idx, float _this_x, float _this_y, float _this_z, float _other_x, float _other_y, float _other_z)
Definition: TMatchingPair.h:35
mrpt::tfest::TMatchingPair::this_x
float this_x
Definition: TMatchingPair.h:52
mrpt::tfest::TMatchingPair::this_idx
uint32_t this_idx
Definition: TMatchingPair.h:50
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::tfest::TMatchingPair::other_y
float other_y
Definition: TMatchingPair.h:53
mrpt::tfest::TMatchingPairList::dumpToFile
void dumpToFile(const std::string &fileName) const
Saves the correspondences to a text file.
Definition: TMatchingPair.cpp:29
mrpt::tfest::TMatchingPair::this_y
float this_y
Definition: TMatchingPair.h:52
common.h
poses_frwds.h
mrpt::tfest::TMatchingPair::errorSquareAfterTransformation
float errorSquareAfterTransformation
Definition: TMatchingPair.h:54
mrpt::tfest::operator<
bool operator<(const TMatchingPair &a, const TMatchingPair &b)
A comparison operator, for sorting lists of TMatchingPair's, first order by this_idx,...
Definition: TMatchingPair.cpp:82
mrpt::tfest::TMatchingPair::this_z
float this_z
Definition: TMatchingPair.h:52
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:70
mrpt::tfest::operator==
bool operator==(const TMatchingPair &a, const TMatchingPair &b)
A comparison operator
Definition: TMatchingPair.cpp:90
mrpt::tfest::operator<<
std::ostream & operator<<(std::ostream &o, const mrpt::tfest::TMatchingPair &pair)
Definition: TMatchingPair.cpp:215
mrpt::tfest::TMatchingPairList::contains
bool contains(const TMatchingPair &p) const
Test whether the given pair "p" is within the pairings.
Definition: TMatchingPair.cpp:122
mrpt::tfest::TMatchingPair::other_x
float other_x
Definition: TMatchingPair.h:53



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 17:54:30 UTC 2020