MRPT  2.0.1
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_base_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_base_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
void saveAsMATLABScript(const std::string &filName) const
Saves the correspondences as a MATLAB script which draws them.
TMatchingPair const * TMatchingPairConstPtr
Definition: TMatchingPair.h:62
std::ostream & operator<<(std::ostream &o, const mrpt::tfest::TMatchingPair &pair)
void dumpToFile(const std::string &fileName) const
Saves the correspondences to a text file.
bool contains(const TMatchingPair &p) const
Test whether the given pair "p" is within the pairings.
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
A list of TMatchingPair.
Definition: TMatchingPair.h:70
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...
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.
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
bool indexOtherMapHasCorrespondence(size_t idx) const
Checks if the given index from the "other" map appears in the list.
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...
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.
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.
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.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020