Main MRPT website > C++ reference
MRPT logo

data_association.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef data_association_H
00029 #define data_association_H
00030 
00031 #include <mrpt/utils/utils_defs.h>
00032 #include <mrpt/poses/CPoint2DPDFGaussian.h>
00033 #include <mrpt/poses/CPointPDFGaussian.h>
00034 
00035 #include <mrpt/slam/link_pragmas.h>
00036 
00037 namespace mrpt
00038 {
00039         namespace slam
00040         {
00041                 /** @name Data association
00042                         @{
00043                   */
00044 
00045                 /** Different algorithms for data association, used in mrpt::slam::data_association
00046                   */
00047                 enum TDataAssociationMethod
00048                 {
00049                         assocNN = 0,    //!< Nearest-neighbor.
00050                         assocJCBB               //!< JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001].
00051                 };
00052 
00053                 /** Different metrics for data association, used in mrpt::slam::data_association
00054                   */
00055                 enum TDataAssociationMetric
00056                 {
00057                         metricMaha= 0,  //!< Mahalanobis distance
00058                         metricML                //!< Matching likelihood (See paper: http://www.mrpt.org/Paper:Matching_Likelihood )
00059                 };
00060 
00061                 typedef size_t observation_index_t; //!< Used in mrpt::slam::TDataAssociationResults
00062                 typedef size_t prediction_index_t; //!< Used in mrpt::slam::TDataAssociationResults
00063 
00064                 /** The results from mrpt::slam::data_association
00065                   */
00066                 struct SLAM_IMPEXP TDataAssociationResults
00067                 {
00068                         TDataAssociationResults() :
00069                                 associations(),
00070                                 distance(0),
00071                                 indiv_distances(0,0),
00072                                 indiv_compatibility(0,0),
00073                                 indiv_compatibility_counts(),
00074                                 nNodesExploredInJCBB(0)
00075                         {}
00076 
00077                         void clear()
00078                         {
00079                                 associations.clear();
00080                                 distance = 0;
00081                                 indiv_distances.setSize(0,0);
00082                                 indiv_compatibility.setSize(0,0);
00083                                 indiv_compatibility_counts.clear();
00084                                 nNodesExploredInJCBB = 0;
00085                         }
00086 
00087                         /** 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).
00088                           * Note that not all observations may have an associated prediction.
00089                           * 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
00090                           *  in the std::map (Tip: Use associations.find(IDX_obs)!=associations.end() )
00091                           * \note The types observation_index_t and prediction_index_t are just used for clarity, use normal size_t's.
00092                           */
00093                         std::map<observation_index_t,prediction_index_t> associations;
00094                         double               distance; //!< The Joint Mahalanobis distance or matching likelihood of the best associations found.
00095 
00096                         /** Individual mahalanobis distances (or matching likelihood, depending on the selected metric) between predictions (row indices) & observations (column indices).
00097                           *  Indices are for the appearing order in the arguments "Y_predictions_mean" & "Z_observations", they are NOT landmark IDs.
00098                           */
00099                         mrpt::math::CMatrixDouble       indiv_distances;
00100                         mrpt::math::CMatrixBool         indiv_compatibility;  //!< The result of a chi2 test for compatibility using mahalanobis distance - Indices are like in "indiv_distances".
00101                         vector_uint                                     indiv_compatibility_counts; //!< The sum of each column of indiv_compatibility, that is, the number of compatible pairings for each observation.
00102 
00103                         size_t          nNodesExploredInJCBB; //!< Only for the JCBB method,the number of recursive calls expent in the algorithm.
00104                 };
00105 
00106 
00107                 /** 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.
00108                   * Implemented methods include (see TDataAssociation)
00109                   *             - NN: Nearest-neighbor
00110                   *             - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]
00111                   *
00112                   *  With both a Mahalanobis-distance or Matching-likelihood metric (See paper: http://www.mrpt.org/Paper:Matching_Likelihood )
00113                   *
00114                   * \param Z_observations_mean [IN] An MxO matrix with the M observations, each row containing the observation "mean".
00115                   * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each row containing the mean of one prediction.
00116                   * \param Y_predictions_cov [IN] An N·OxN·O matrix with the full covariance matrix of all the N predictions.
00117                   * \param results [OUT] The output data association hypothesis, and other useful information.
00118                   * \param method [IN, optional] The selected method to make the associations.
00119                   * \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
00120                   * \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).
00121                   * \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]".
00122                   *
00123                   * \sa data_association_independent_predictions, data_association_independent_2d_points, data_association_independent_3d_points
00124                   */
00125                 void SLAM_IMPEXP data_association_full_covariance(
00126                         const mrpt::math::CMatrixDouble         &Z_observations_mean,
00127                         const mrpt::math::CMatrixDouble         &Y_predictions_mean,
00128                         const mrpt::math::CMatrixDouble         &Y_predictions_cov,
00129                         TDataAssociationResults                         &results,
00130                         const TDataAssociationMethod            method = assocJCBB,
00131                         const TDataAssociationMetric            metric  = metricMaha,
00132                         const double                                            chi2quantile = 0.99,
00133                         const bool                                                      DAT_ASOC_USE_KDTREE = true,
00134                         const std::vector<prediction_index_t>           &predictions_IDs = std::vector<prediction_index_t>(),
00135                         const TDataAssociationMetric            compatibilityTestMetric  = metricMaha,
00136                         const double                                            log_ML_compat_test_threshold = 0.0
00137                         );
00138 
00139                 /** 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.
00140                   * Implemented methods include (see TDataAssociation)
00141                   *             - NN: Nearest-neighbor
00142                   *             - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]
00143                   *
00144                   *  With both a Mahalanobis-distance or Matching-likelihood metric (See paper: http://www.mrpt.org/Paper:Matching_Likelihood )
00145                   *
00146                   * \param Z_observations_mean [IN] An MxO matrix with the M observations, each row containing the observation "mean".
00147                   * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each row containing the mean of one prediction.
00148                   * \param Y_predictions_cov [IN] An N·OxO matrix: A vertical stack of N covariance matrix, one for each of the N prediction.
00149                   * \param results [OUT] The output data association hypothesis, and other useful information.
00150                   * \param method [IN, optional] The selected method to make the associations.
00151                   * \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
00152                   * \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).
00153                   * \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]".
00154                   *
00155                   * \sa data_association_full_covariance, data_association_independent_2d_points, data_association_independent_3d_points
00156                   */
00157                 void SLAM_IMPEXP data_association_independent_predictions(
00158                         const mrpt::math::CMatrixDouble         &Z_observations_mean,
00159                         const mrpt::math::CMatrixDouble         &Y_predictions_mean,
00160                         const mrpt::math::CMatrixDouble         &Y_predictions_cov,
00161                         TDataAssociationResults                         &results,
00162                         const TDataAssociationMethod            method = assocJCBB,
00163                         const TDataAssociationMetric            metric  = metricMaha,
00164                         const double                                            chi2quantile = 0.99,
00165                         const bool                                                      DAT_ASOC_USE_KDTREE = true,
00166                         const std::vector<prediction_index_t>   &predictions_IDs = std::vector<prediction_index_t>(),
00167                         const TDataAssociationMetric            compatibilityTestMetric = metricMaha,
00168                         const double                                            log_ML_compat_test_threshold = 0.0
00169                         );
00170 
00171 
00172                 /** @} */
00173 
00174         } // End of namespace
00175 } // End of namespace
00176 
00177 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011