Main MRPT website > C++ reference
MRPT logo
data_association.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (c) 2005-2013, Individual contributors, see AUTHORS file |
7  | Copyright (c) 2005-2013, MAPIR group, University of Malaga |
8  | Copyright (c) 2012-2013, University of Almeria |
9  | All rights reserved. |
10  | |
11  | Redistribution and use in source and binary forms, with or without |
12  | modification, are permitted provided that the following conditions are |
13  | met: |
14  | * Redistributions of source code must retain the above copyright |
15  | notice, this list of conditions and the following disclaimer. |
16  | * Redistributions in binary form must reproduce the above copyright |
17  | notice, this list of conditions and the following disclaimer in the |
18  | documentation and/or other materials provided with the distribution. |
19  | * Neither the name of the copyright holders nor the |
20  | names of its contributors may be used to endorse or promote products |
21  | derived from this software without specific prior written permission.|
22  | |
23  | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
24  | 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
25  | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR|
26  | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE |
27  | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL|
28  | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR|
29  | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
30  | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, |
31  | STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
32  | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
33  | POSSIBILITY OF SUCH DAMAGE. |
34  +---------------------------------------------------------------------------+ */
35 #ifndef data_association_H
36 #define data_association_H
37 
38 #include <mrpt/utils/utils_defs.h>
41 #include <mrpt/utils/TEnumType.h>
42 
43 #include <mrpt/slam/link_pragmas.h>
44 
45 namespace mrpt
46 {
47  namespace slam
48  {
49  /** \addtogroup data_assoc_grp Data association
50  * \ingroup mrpt_slam_grp
51  * @{ */
52 
53  /** @name Data association
54  @{
55  */
56 
57  /** Different algorithms for data association, used in mrpt::slam::data_association
58  */
60  {
61  assocNN = 0, //!< Nearest-neighbor.
62  assocJCBB //!< JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001].
63  };
64 
65  /** Different metrics for data association, used in mrpt::slam::data_association
66  * For a comparison of both methods see paper:
67  * * J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "An alternative to the Mahalanobis distance for determining optimal correspondences in data association", IEEE Transactions on Robotics (T-RO), (2012) DOI: 10.1109/TRO.2012.2193706 Draft: http://mapir.isa.uma.es/~jlblanco/papers/blanco2012amd.pdf
68  */
70  {
71  metricMaha= 0, //!< Mahalanobis distance
72  metricML //!< Matching likelihood (See TDataAssociationMetric for a paper explaining this metric)
73  };
74 
75  typedef size_t observation_index_t; //!< Used in mrpt::slam::TDataAssociationResults
76  typedef size_t prediction_index_t; //!< Used in mrpt::slam::TDataAssociationResults
77 
78  /** The results from mrpt::slam::data_association
79  */
81  {
83  associations(),
84  distance(0),
85  indiv_distances(0,0),
86  indiv_compatibility(0,0),
87  indiv_compatibility_counts(),
88  nNodesExploredInJCBB(0)
89  {}
90 
91  void clear()
92  {
93  associations.clear();
94  distance = 0;
95  indiv_distances.setSize(0,0);
96  indiv_compatibility.setSize(0,0);
97  indiv_compatibility_counts.clear();
98  nNodesExploredInJCBB = 0;
99  }
100 
101  /** For each observation (with row index IDX_obs in the input "Z_observations"), its association in the predictions, as the row index in the "Y_predictions_mean" input (or it's mapping to a custom ID, if it was provided).
102  * Note that not all observations may have an associated prediction.
103  * An observation with index "IDX_obs" corresponds to the prediction number "associations[IDX_obs]", or it may not correspond to anyone if it's not present
104  * in the std::map (Tip: Use associations.find(IDX_obs)!=associations.end() )
105  * \note The types observation_index_t and prediction_index_t are just used for clarity, use normal size_t's.
106  */
107  std::map<observation_index_t,prediction_index_t> associations;
108  double distance; //!< The Joint Mahalanobis distance or matching likelihood of the best associations found.
109 
110  /** Individual mahalanobis distances (or matching likelihood, depending on the selected metric) between predictions (row indices) & observations (column indices).
111  * Indices are for the appearing order in the arguments "Y_predictions_mean" & "Z_observations", they are NOT landmark IDs.
112  */
114  mrpt::math::CMatrixBool indiv_compatibility; //!< The result of a chi2 test for compatibility using mahalanobis distance - Indices are like in "indiv_distances".
115  vector_uint indiv_compatibility_counts; //!< The sum of each column of indiv_compatibility, that is, the number of compatible pairings for each observation.
116 
117  size_t nNodesExploredInJCBB; //!< Only for the JCBB method,the number of recursive calls expent in the algorithm.
118  };
119 
120 
121  /** Computes the data-association between the prediction of a set of landmarks and their observations, all of them with covariance matrices - Generic version with prediction full cross-covariances.
122  * Implemented methods include (see TDataAssociation)
123  * - NN: Nearest-neighbor
124  * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]
125  *
126  * With both a Mahalanobis-distance or Matching-likelihood metric. For a comparison of both methods, see paper:
127  * * J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "An alternative to the Mahalanobis distance for determining optimal correspondences in data association", IEEE Transactions on Robotics (T-RO), (2012) DOI: 10.1109/TRO.2012.2193706 Draft: http://mapir.isa.uma.es/~jlblanco/papers/blanco2012amd.pdf
128  *
129  * \param Z_observations_mean [IN] An MxO matrix with the M observations, each row containing the observation "mean".
130  * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each row containing the mean of one prediction.
131  * \param Y_predictions_cov [IN] An NOxNO matrix with the full covariance matrix of all the N predictions.
132  * \param results [OUT] The output data association hypothesis, and other useful information.
133  * \param method [IN, optional] The selected method to make the associations.
134  * \param chi2quantile [IN, optional] The threshold for considering a match between two close Gaussians for two landmarks, in the range [0,1]. It is used to call mrpt::math::chi2inv
135  * \param use_kd_tree [IN, optional] Build a KD-tree to speed-up the evaluation of individual compatibility (IC). It's perhaps more efficient to disable it for a small number of features. (default=true).
136  * \param predictions_IDs [IN, optional] (default:none) An N-vector. If provided, the resulting associations in "results.associations" will not contain prediction indices "i", but "predictions_IDs[i]".
137  *
138  * \sa data_association_independent_predictions, data_association_independent_2d_points, data_association_independent_3d_points
139  */
141  const mrpt::math::CMatrixDouble &Z_observations_mean,
142  const mrpt::math::CMatrixDouble &Y_predictions_mean,
143  const mrpt::math::CMatrixDouble &Y_predictions_cov,
144  TDataAssociationResults &results,
145  const TDataAssociationMethod method = assocJCBB,
146  const TDataAssociationMetric metric = metricMaha,
147  const double chi2quantile = 0.99,
148  const bool DAT_ASOC_USE_KDTREE = true,
149  const std::vector<prediction_index_t> &predictions_IDs = std::vector<prediction_index_t>(),
150  const TDataAssociationMetric compatibilityTestMetric = metricMaha,
151  const double log_ML_compat_test_threshold = 0.0
152  );
153 
154  /** Computes the data-association between the prediction of a set of landmarks and their observations, all of them with covariance matrices - Generic version with NO prediction cross-covariances.
155  * Implemented methods include (see TDataAssociation)
156  * - NN: Nearest-neighbor
157  * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]
158  *
159  * With both a Mahalanobis-distance or Matching-likelihood metric. For a comparison of both methods, see paper:
160  * * J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "An alternative to the Mahalanobis distance for determining optimal correspondences in data association", IEEE Transactions on Robotics (T-RO), (2012) DOI: 10.1109/TRO.2012.2193706 Draft: http://mapir.isa.uma.es/~jlblanco/papers/blanco2012amd.pdf
161  *
162  * \param Z_observations_mean [IN] An MxO matrix with the M observations, each row containing the observation "mean".
163  * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each row containing the mean of one prediction.
164  * \param Y_predictions_cov [IN] An NOxO matrix: A vertical stack of N covariance matrix, one for each of the N prediction.
165  * \param results [OUT] The output data association hypothesis, and other useful information.
166  * \param method [IN, optional] The selected method to make the associations.
167  * \param chi2quantile [IN, optional] The threshold for considering a match between two close Gaussians for two landmarks, in the range [0,1]. It is used to call mrpt::math::chi2inv
168  * \param use_kd_tree [IN, optional] Build a KD-tree to speed-up the evaluation of individual compatibility (IC). It's perhaps more efficient to disable it for a small number of features. (default=true).
169  * \param predictions_IDs [IN, optional] (default:none) An N-vector. If provided, the resulting associations in "results.associations" will not contain prediction indices "i", but "predictions_IDs[i]".
170  *
171  * \sa data_association_full_covariance, data_association_independent_2d_points, data_association_independent_3d_points
172  */
174  const mrpt::math::CMatrixDouble &Z_observations_mean,
175  const mrpt::math::CMatrixDouble &Y_predictions_mean,
176  const mrpt::math::CMatrixDouble &Y_predictions_cov,
177  TDataAssociationResults &results,
178  const TDataAssociationMethod method = assocJCBB,
179  const TDataAssociationMetric metric = metricMaha,
180  const double chi2quantile = 0.99,
181  const bool DAT_ASOC_USE_KDTREE = true,
182  const std::vector<prediction_index_t> &predictions_IDs = std::vector<prediction_index_t>(),
183  const TDataAssociationMetric compatibilityTestMetric = metricMaha,
184  const double log_ML_compat_test_threshold = 0.0
185  );
186 
187 
188  /** @} */
189 
190  /** @} */ // end of grouping
191 
192  } // end namespace
193 
194  // Specializations MUST occur at the same namespace:
195  namespace utils
196  {
197  template <>
199  {
201  static void fill(bimap<enum_t,std::string> &m_map)
202  {
203  m_map.insert(slam::assocNN, "assocNN");
204  m_map.insert(slam::assocJCBB, "assocJCBB");
205  }
206  };
207 
208  template <>
210  {
212  static void fill(bimap<enum_t,std::string> &m_map)
213  {
214  m_map.insert(slam::metricMaha, "metricMaha");
215  m_map.insert(slam::metricML, "metricML");
216  }
217  };
218 
219  } // End of namespace
220 
221 } // End of namespace
222 
223 #endif
Nearest-neighbor.
Mahalanobis distance.
size_t observation_index_t
Used in mrpt::slam::TDataAssociationResults.
void SLAM_IMPEXP data_association_full_covariance(const mrpt::math::CMatrixDouble &Z_observations_mean, const mrpt::math::CMatrixDouble &Y_predictions_mean, const mrpt::math::CMatrixDouble &Y_predictions_cov, TDataAssociationResults &results, const TDataAssociationMethod method=assocJCBB, const TDataAssociationMetric metric=metricMaha, const double chi2quantile=0.99, const bool DAT_ASOC_USE_KDTREE=true, const std::vector< prediction_index_t > &predictions_IDs=std::vector< prediction_index_t >(), const TDataAssociationMetric compatibilityTestMetric=metricMaha, const double log_ML_compat_test_threshold=0.0)
Computes the data-association between the prediction of a set of landmarks and their observations...
vector_uint indiv_compatibility_counts
The sum of each column of indiv_compatibility, that is, the number of compatible pairings for each ob...
mrpt::math::CMatrixDouble indiv_distances
Individual mahalanobis distances (or matching likelihood, depending on the selected metric) between p...
static void fill(bimap< enum_t, std::string > &m_map)
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:49
std::map< observation_index_t, prediction_index_t > associations
For each observation (with row index IDX_obs in the input "Z_observations"), its association in the p...
JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001].
double distance
The Joint Mahalanobis distance or matching likelihood of the best associations found.
std::vector< uint32_t > vector_uint
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:55
mrpt::math::CMatrixBool indiv_compatibility
The result of a chi2 test for compatibility using mahalanobis distance - Indices are like in "indiv_d...
This template class provides the basic functionality for a general 2D any-size, resizable container o...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
static void fill(bimap< enum_t, std::string > &m_map)
size_t prediction_index_t
Used in mrpt::slam::TDataAssociationResults.
size_t nNodesExploredInJCBB
Only for the JCBB method,the number of recursive calls expent in the algorithm.
The results from mrpt::slam::data_association.
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:96
Matching likelihood (See TDataAssociationMetric for a paper explaining this metric) ...
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
void SLAM_IMPEXP data_association_independent_predictions(const mrpt::math::CMatrixDouble &Z_observations_mean, const mrpt::math::CMatrixDouble &Y_predictions_mean, const mrpt::math::CMatrixDouble &Y_predictions_cov, TDataAssociationResults &results, const TDataAssociationMethod method=assocJCBB, const TDataAssociationMetric metric=metricMaha, const double chi2quantile=0.99, const bool DAT_ASOC_USE_KDTREE=true, const std::vector< prediction_index_t > &predictions_IDs=std::vector< prediction_index_t >(), const TDataAssociationMetric compatibilityTestMetric=metricMaha, const double log_ML_compat_test_threshold=0.0)
Computes the data-association between the prediction of a set of landmarks and their observations...
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.



Page generated by Doxygen 1.8.14 for MRPT 1.0.2 SVN: at lun oct 28 00:52:41 CET 2019 Hosted on:
SourceForge.net Logo