00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2010 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 namespace mrpt 00036 { 00037 namespace slam 00038 { 00039 /** @name Data association 00040 @{ 00041 */ 00042 00043 /** Different algorithms for data association, used in mrpt::slam::data_association 00044 */ 00045 enum TDataAssociationMethod 00046 { 00047 assocNN = 0, //!< Nearest-neighbor. 00048 assocJCBB //!< JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]. 00049 }; 00050 00051 /** Different metrics for data association, used in mrpt::slam::data_association 00052 */ 00053 enum TDataAssociationMetric 00054 { 00055 metricMaha= 0, //!< Mahalanobis distance 00056 metricML //!< Matching likelihood (See paper: http://babel.isa.uma.es/mrpt/index.php/Paper:Matching_Likelihood ) 00057 }; 00058 00059 typedef size_t observation_index_t; //!< Used in mrpt::slam::TDataAssociationResults 00060 typedef size_t prediction_index_t; //!< Used in mrpt::slam::TDataAssociationResults 00061 00062 /** The results from mrpt::slam::data_association 00063 */ 00064 struct MRPTDLLIMPEXP TDataAssociationResults 00065 { 00066 TDataAssociationResults() : 00067 associations(), 00068 distance(0), 00069 indiv_distances(0,0), 00070 indiv_compatibility(0,0), 00071 indiv_compatibility_counts(), 00072 nNodesExploredInJCBB(0) 00073 {} 00074 00075 void clear() 00076 { 00077 associations.clear(); 00078 distance = 0; 00079 indiv_distances.setSize(0,0); 00080 indiv_compatibility.setSize(0,0); 00081 indiv_compatibility_counts.clear(); 00082 nNodesExploredInJCBB = 0; 00083 } 00084 00085 /** 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). 00086 * Note that not all observations may have an associated prediction. 00087 * 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 00088 * in the std::map (Tip: Use associations.find(IDX_obs)!=associations.end() ) 00089 * \note The types observation_index_t and prediction_index_t are just used for clarity, use normal size_t's. 00090 */ 00091 std::map<observation_index_t,prediction_index_t> associations; 00092 double distance; //!< The Joint Mahalanobis distance or matching likelihood of the best associations found. 00093 00094 /** Individual mahalanobis distances (or matching likelihood, depending on the selected metric) between predictions (row indices) & observations (column indices). 00095 * Indices are for the appearing order in the arguments "Y_predictions_mean" & "Z_observations", they are NOT landmark IDs. 00096 */ 00097 mrpt::math::CMatrixDouble indiv_distances; 00098 mrpt::math::CMatrixBool indiv_compatibility; //!< The result of a chi2 test for compatibility using mahalanobis distance - Indices are like in "indiv_distances". 00099 vector_uint indiv_compatibility_counts; //!< The sum of each column of indiv_compatibility, that is, the number of compatible pairings for each observation. 00100 00101 size_t nNodesExploredInJCBB; //!< Only for the JCBB method,the number of recursive calls expent in the algorithm. 00102 }; 00103 00104 00105 /** 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. 00106 * Implemented methods include (see TDataAssociation) 00107 * - NN: Nearest-neighbor 00108 * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001] 00109 * 00110 * With both a Mahalanobis-distance or Matching-likelihood metric (See paper: http://babel.isa.uma.es/mrpt/index.php/Paper:Matching_Likelihood ) 00111 * 00112 * \param Z_observations_mean [IN] An MxO matrix with the M observations, each row containing the observation "mean". 00113 * \param Z_observations_covs [IN] An OxO or M·OxO matrix: M vertically-stacked matrices, each one being the covariance of its corresponding observation in "Z_observations_mean". As an optimization, if the matrix is OxO, it will be assumed that all the observations have the same covariance. 00114 * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each row containing the mean of one prediction. 00115 * \param Y_predictions_cov [IN] An N·OxN·O matrix with the full covariance matrix of all the N predictions. 00116 * \param results [OUT] The output data association hypothesis, and other useful information. 00117 * \param method [IN, optional] The selected method to make the associations. 00118 * \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 00119 * \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). 00120 * \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]". 00121 * 00122 * \sa data_association_independent_predictions, data_association_independent_2d_points, data_association_independent_3d_points 00123 */ 00124 void MRPTDLLIMPEXP data_association_full_covariance( 00125 const mrpt::math::CMatrixDouble &Z_observations_mean, 00126 const mrpt::math::CMatrixDouble &Z_observations_covs, 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 ); 00136 00137 /** 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. 00138 * Implemented methods include (see TDataAssociation) 00139 * - NN: Nearest-neighbor 00140 * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001] 00141 * 00142 * With both a Mahalanobis-distance or Matching-likelihood metric (See paper: http://babel.isa.uma.es/mrpt/index.php/Paper:Matching_Likelihood ) 00143 * 00144 * \param Z_observations_mean [IN] An MxO matrix with the M observations, each row containing the observation "mean". 00145 * \param Z_observations_covs [IN] An OxO or M·OxO matrix: M vertically-stacked matrices, each one being the covariance of its corresponding observation in "Z_observations_mean". As an optimization, if the matrix is OxO, it will be assumed that all the observations have the same covariance. 00146 * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each row containing the mean of one prediction. 00147 * \param Y_predictions_cov [IN] An N·OxO matrix: A vertical stack of N covariance matrix, one for each of the N prediction. 00148 * \param results [OUT] The output data association hypothesis, and other useful information. 00149 * \param method [IN, optional] The selected method to make the associations. 00150 * \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 00151 * \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). 00152 * \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]". 00153 * 00154 * \sa data_association_full_covariance, data_association_independent_2d_points, data_association_independent_3d_points 00155 */ 00156 void MRPTDLLIMPEXP data_association_independent_predictions( 00157 const mrpt::math::CMatrixDouble &Z_observations_mean, 00158 const mrpt::math::CMatrixDouble &Z_observations_covs, 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 ); 00168 00169 /** Computes the data-association between the prediction of a set of landmarks and their observations, all of them with covariance matrices - Specialized version for 2D points with NO prediction cross-covariances. 00170 * Implemented methods include (see TDataAssociation) 00171 * - NN: Nearest-neighbor 00172 * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001] 00173 * 00174 * With both a Mahalanobis-distance or Matching-likelihood metric (See paper: http://babel.isa.uma.es/mrpt/index.php/Paper:Matching_Likelihood ) 00175 * 00176 * \param observations [IN] A list of the M observed points. 00177 * \param predictions [IN] A list of the N predicted map points. 00178 * \param results [OUT] The output data association hypothesis, and other useful information. 00179 * \param method [IN, optional] The selected method to make the associations. 00180 * \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 00181 * \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). 00182 * \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]". 00183 * 00184 * \sa data_association_full_covariance, data_association_independent_3d_points 00185 */ 00186 void MRPTDLLIMPEXP data_association_independent_2d_points( 00187 const std::vector<mrpt::poses::CPoint2DPDFGaussian> observations, 00188 const std::vector<mrpt::poses::CPoint2DPDFGaussian> predictions, 00189 TDataAssociationResults &results, 00190 const TDataAssociationMethod method = assocJCBB, 00191 const TDataAssociationMetric metric = metricMaha, 00192 const double chi2quantile = 0.99, 00193 const bool DAT_ASOC_USE_KDTREE = true, 00194 const std::vector<prediction_index_t> &predictions_IDs = std::vector<prediction_index_t>() 00195 ); 00196 00197 /** Computes the data-association between the prediction of a set of landmarks and their observations, all of them with covariance matrices - Specialized version for 3D points with NO prediction cross-covariances. 00198 * Implemented methods include (see TDataAssociation) 00199 * - NN: Nearest-neighbor 00200 * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001] 00201 * 00202 * With both a Mahalanobis-distance or Matching-likelihood metric (See paper: http://babel.isa.uma.es/mrpt/index.php/Paper:Matching_Likelihood ) 00203 * 00204 * \param observations [IN] A list of the M observed points. 00205 * \param predictions [IN] A list of the N predicted map points. 00206 * \param results [OUT] The output data association hypothesis, and other useful information. 00207 * \param method [IN, optional] The selected method to make the associations. 00208 * \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 00209 * \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). 00210 * \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]". 00211 * 00212 * \sa data_association_full_covariance, data_association_independent_2d_points 00213 */ 00214 void MRPTDLLIMPEXP data_association_independent_3d_points( 00215 const std::vector<mrpt::poses::CPointPDFGaussian> observations, 00216 const std::vector<mrpt::poses::CPointPDFGaussian> predictions, 00217 TDataAssociationResults &results, 00218 const TDataAssociationMethod method = assocJCBB, 00219 const TDataAssociationMetric metric = metricMaha, 00220 const double chi2quantile = 0.99, 00221 const bool DAT_ASOC_USE_KDTREE = true, 00222 const std::vector<prediction_index_t> &predictions_IDs = std::vector<prediction_index_t>() 00223 ); 00224 00225 00226 /** @} */ 00227 00228 } // End of namespace 00229 } // End of namespace 00230 00231 #endif
| Page generated by Doxygen 1.6.2 for MRPT 0.8.1 SVN:exported at Mon Feb 15 22:01:07 UTC 2010 |