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.1 for MRPT 0.9.4 SVN: at Mon Jan 10 23:33:19 UTC 2011 |