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) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef data_association_H
29 #define data_association_H
30 
31 #include <mrpt/utils/utils_defs.h>
32 #include <mrpt/poses/CPoint2DPDFGaussian.h>
33 #include <mrpt/poses/CPointPDFGaussian.h>
34 #include <mrpt/utils/TEnumType.h>
35 
36 #include <mrpt/slam/link_pragmas.h>
37 
38 namespace mrpt
39 {
40  namespace slam
41  {
42  /** \addtogroup data_assoc_grp Data association
43  * \ingroup mrpt_slam_grp
44  * @{ */
45 
46  /** @name Data association
47  @{
48  */
49 
50  /** Different algorithms for data association, used in mrpt::slam::data_association
51  */
52  enum TDataAssociationMethod
53  {
54  assocNN = 0, //!< Nearest-neighbor.
55  assocJCBB //!< JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001].
56  };
57 
58  /** Different metrics for data association, used in mrpt::slam::data_association
59  * For a comparison of both methods see paper:
60  * * 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
61  */
62  enum TDataAssociationMetric
63  {
64  metricMaha= 0, //!< Mahalanobis distance
65  metricML //!< Matching likelihood (See TDataAssociationMetric for a paper explaining this metric)
66  };
67 
68  typedef size_t observation_index_t; //!< Used in mrpt::slam::TDataAssociationResults
69  typedef size_t prediction_index_t; //!< Used in mrpt::slam::TDataAssociationResults
70 
71  /** The results from mrpt::slam::data_association
72  */
73  struct SLAM_IMPEXP TDataAssociationResults
74  {
75  TDataAssociationResults() :
76  associations(),
77  distance(0),
78  indiv_distances(0,0),
79  indiv_compatibility(0,0),
80  indiv_compatibility_counts(),
81  nNodesExploredInJCBB(0)
82  {}
83 
84  void clear()
85  {
86  associations.clear();
87  distance = 0;
88  indiv_distances.setSize(0,0);
89  indiv_compatibility.setSize(0,0);
90  indiv_compatibility_counts.clear();
91  nNodesExploredInJCBB = 0;
92  }
93 
94  /** 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).
95  * Note that not all observations may have an associated prediction.
96  * 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
97  * in the std::map (Tip: Use associations.find(IDX_obs)!=associations.end() )
98  * \note The types observation_index_t and prediction_index_t are just used for clarity, use normal size_t's.
99  */
100  std::map<observation_index_t,prediction_index_t> associations;
101  double distance; //!< The Joint Mahalanobis distance or matching likelihood of the best associations found.
102 
103  /** Individual mahalanobis distances (or matching likelihood, depending on the selected metric) between predictions (row indices) & observations (column indices).
104  * Indices are for the appearing order in the arguments "Y_predictions_mean" & "Z_observations", they are NOT landmark IDs.
105  */
106  mrpt::math::CMatrixDouble indiv_distances;
107  mrpt::math::CMatrixBool indiv_compatibility; //!< The result of a chi2 test for compatibility using mahalanobis distance - Indices are like in "indiv_distances".
108  vector_uint indiv_compatibility_counts; //!< The sum of each column of indiv_compatibility, that is, the number of compatible pairings for each observation.
109 
110  size_t nNodesExploredInJCBB; //!< Only for the JCBB method,the number of recursive calls expent in the algorithm.
111  };
112 
113 
114  /** 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.
115  * Implemented methods include (see TDataAssociation)
116  * - NN: Nearest-neighbor
117  * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]
118  *
119  * With both a Mahalanobis-distance or Matching-likelihood metric. For a comparison of both methods, see paper:
120  * * 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
121  *
122  * \param Z_observations_mean [IN] An MxO matrix with the M observations, each row containing the observation "mean".
123  * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each row containing the mean of one prediction.
124  * \param Y_predictions_cov [IN] An N·OxN·O matrix with the full covariance matrix of all the N predictions.
125  * \param results [OUT] The output data association hypothesis, and other useful information.
126  * \param method [IN, optional] The selected method to make the associations.
127  * \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
128  * \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).
129  * \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]".
130  *
131  * \sa data_association_independent_predictions, data_association_independent_2d_points, data_association_independent_3d_points
132  */
133  void SLAM_IMPEXP data_association_full_covariance(
134  const mrpt::math::CMatrixDouble &Z_observations_mean,
135  const mrpt::math::CMatrixDouble &Y_predictions_mean,
136  const mrpt::math::CMatrixDouble &Y_predictions_cov,
137  TDataAssociationResults &results,
138  const TDataAssociationMethod method = assocJCBB,
139  const TDataAssociationMetric metric = metricMaha,
140  const double chi2quantile = 0.99,
141  const bool DAT_ASOC_USE_KDTREE = true,
142  const std::vector<prediction_index_t> &predictions_IDs = std::vector<prediction_index_t>(),
143  const TDataAssociationMetric compatibilityTestMetric = metricMaha,
144  const double log_ML_compat_test_threshold = 0.0
145  );
146 
147  /** 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.
148  * Implemented methods include (see TDataAssociation)
149  * - NN: Nearest-neighbor
150  * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]
151  *
152  * With both a Mahalanobis-distance or Matching-likelihood metric. For a comparison of both methods, see paper:
153  * * 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
154  *
155  * \param Z_observations_mean [IN] An MxO matrix with the M observations, each row containing the observation "mean".
156  * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each row containing the mean of one prediction.
157  * \param Y_predictions_cov [IN] An N·OxO matrix: A vertical stack of N covariance matrix, one for each of the N prediction.
158  * \param results [OUT] The output data association hypothesis, and other useful information.
159  * \param method [IN, optional] The selected method to make the associations.
160  * \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
161  * \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).
162  * \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]".
163  *
164  * \sa data_association_full_covariance, data_association_independent_2d_points, data_association_independent_3d_points
165  */
166  void SLAM_IMPEXP data_association_independent_predictions(
167  const mrpt::math::CMatrixDouble &Z_observations_mean,
168  const mrpt::math::CMatrixDouble &Y_predictions_mean,
169  const mrpt::math::CMatrixDouble &Y_predictions_cov,
170  TDataAssociationResults &results,
171  const TDataAssociationMethod method = assocJCBB,
172  const TDataAssociationMetric metric = metricMaha,
173  const double chi2quantile = 0.99,
174  const bool DAT_ASOC_USE_KDTREE = true,
175  const std::vector<prediction_index_t> &predictions_IDs = std::vector<prediction_index_t>(),
176  const TDataAssociationMetric compatibilityTestMetric = metricMaha,
177  const double log_ML_compat_test_threshold = 0.0
178  );
179 
180 
181  /** @} */
182 
183  /** @} */ // end of grouping
184 
185  } // end namespace
186 
187  // Specializations MUST occur at the same namespace:
188  namespace utils
189  {
190  template <>
191  struct TEnumTypeFiller<slam::TDataAssociationMethod>
192  {
193  typedef slam::TDataAssociationMethod enum_t;
194  static void fill(bimap<enum_t,std::string> &m_map)
195  {
196  m_map.insert(slam::assocNN, "assocNN");
197  m_map.insert(slam::assocJCBB, "assocJCBB");
198  }
199  };
200 
201  template <>
202  struct TEnumTypeFiller<slam::TDataAssociationMetric>
203  {
204  typedef slam::TDataAssociationMetric enum_t;
205  static void fill(bimap<enum_t,std::string> &m_map)
206  {
207  m_map.insert(slam::metricMaha, "metricMaha");
208  m_map.insert(slam::metricML, "metricML");
209  }
210  };
211 
212  } // End of namespace
213 
214 } // End of namespace
215 
216 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013