00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://www.mrpt.org/ | 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 #include <mrpt/utils/TEnumType.h> 00035 00036 #include <mrpt/slam/link_pragmas.h> 00037 00038 namespace mrpt 00039 { 00040 namespace slam 00041 { 00042 /** \addtogroup data_assoc_grp Data association 00043 * \ingroup mrpt_slam_grp 00044 * @{ */ 00045 00046 /** @name Data association 00047 @{ 00048 */ 00049 00050 /** Different algorithms for data association, used in mrpt::slam::data_association 00051 */ 00052 enum TDataAssociationMethod 00053 { 00054 assocNN = 0, //!< Nearest-neighbor. 00055 assocJCBB //!< JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]. 00056 }; 00057 00058 /** Different metrics for data association, used in mrpt::slam::data_association 00059 */ 00060 enum TDataAssociationMetric 00061 { 00062 metricMaha= 0, //!< Mahalanobis distance 00063 metricML //!< Matching likelihood (See paper: http://www.mrpt.org/Paper:Matching_Likelihood ) 00064 }; 00065 00066 typedef size_t observation_index_t; //!< Used in mrpt::slam::TDataAssociationResults 00067 typedef size_t prediction_index_t; //!< Used in mrpt::slam::TDataAssociationResults 00068 00069 /** The results from mrpt::slam::data_association 00070 */ 00071 struct SLAM_IMPEXP TDataAssociationResults 00072 { 00073 TDataAssociationResults() : 00074 associations(), 00075 distance(0), 00076 indiv_distances(0,0), 00077 indiv_compatibility(0,0), 00078 indiv_compatibility_counts(), 00079 nNodesExploredInJCBB(0) 00080 {} 00081 00082 void clear() 00083 { 00084 associations.clear(); 00085 distance = 0; 00086 indiv_distances.setSize(0,0); 00087 indiv_compatibility.setSize(0,0); 00088 indiv_compatibility_counts.clear(); 00089 nNodesExploredInJCBB = 0; 00090 } 00091 00092 /** 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). 00093 * Note that not all observations may have an associated prediction. 00094 * 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 00095 * in the std::map (Tip: Use associations.find(IDX_obs)!=associations.end() ) 00096 * \note The types observation_index_t and prediction_index_t are just used for clarity, use normal size_t's. 00097 */ 00098 std::map<observation_index_t,prediction_index_t> associations; 00099 double distance; //!< The Joint Mahalanobis distance or matching likelihood of the best associations found. 00100 00101 /** Individual mahalanobis distances (or matching likelihood, depending on the selected metric) between predictions (row indices) & observations (column indices). 00102 * Indices are for the appearing order in the arguments "Y_predictions_mean" & "Z_observations", they are NOT landmark IDs. 00103 */ 00104 mrpt::math::CMatrixDouble indiv_distances; 00105 mrpt::math::CMatrixBool indiv_compatibility; //!< The result of a chi2 test for compatibility using mahalanobis distance - Indices are like in "indiv_distances". 00106 vector_uint indiv_compatibility_counts; //!< The sum of each column of indiv_compatibility, that is, the number of compatible pairings for each observation. 00107 00108 size_t nNodesExploredInJCBB; //!< Only for the JCBB method,the number of recursive calls expent in the algorithm. 00109 }; 00110 00111 00112 /** 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. 00113 * Implemented methods include (see TDataAssociation) 00114 * - NN: Nearest-neighbor 00115 * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001] 00116 * 00117 * With both a Mahalanobis-distance or Matching-likelihood metric (See paper: http://www.mrpt.org/Paper:Matching_Likelihood ) 00118 * 00119 * \param Z_observations_mean [IN] An MxO matrix with the M observations, each row containing the observation "mean". 00120 * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each row containing the mean of one prediction. 00121 * \param Y_predictions_cov [IN] An N·OxN·O matrix with the full covariance matrix of all the N predictions. 00122 * \param results [OUT] The output data association hypothesis, and other useful information. 00123 * \param method [IN, optional] The selected method to make the associations. 00124 * \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 00125 * \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). 00126 * \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]". 00127 * 00128 * \sa data_association_independent_predictions, data_association_independent_2d_points, data_association_independent_3d_points 00129 */ 00130 void SLAM_IMPEXP data_association_full_covariance( 00131 const mrpt::math::CMatrixDouble &Z_observations_mean, 00132 const mrpt::math::CMatrixDouble &Y_predictions_mean, 00133 const mrpt::math::CMatrixDouble &Y_predictions_cov, 00134 TDataAssociationResults &results, 00135 const TDataAssociationMethod method = assocJCBB, 00136 const TDataAssociationMetric metric = metricMaha, 00137 const double chi2quantile = 0.99, 00138 const bool DAT_ASOC_USE_KDTREE = true, 00139 const std::vector<prediction_index_t> &predictions_IDs = std::vector<prediction_index_t>(), 00140 const TDataAssociationMetric compatibilityTestMetric = metricMaha, 00141 const double log_ML_compat_test_threshold = 0.0 00142 ); 00143 00144 /** 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. 00145 * Implemented methods include (see TDataAssociation) 00146 * - NN: Nearest-neighbor 00147 * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001] 00148 * 00149 * With both a Mahalanobis-distance or Matching-likelihood metric (See paper: http://www.mrpt.org/Paper:Matching_Likelihood ) 00150 * 00151 * \param Z_observations_mean [IN] An MxO matrix with the M observations, each row containing the observation "mean". 00152 * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each row containing the mean of one prediction. 00153 * \param Y_predictions_cov [IN] An N·OxO matrix: A vertical stack of N covariance matrix, one for each of the N prediction. 00154 * \param results [OUT] The output data association hypothesis, and other useful information. 00155 * \param method [IN, optional] The selected method to make the associations. 00156 * \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 00157 * \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). 00158 * \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]". 00159 * 00160 * \sa data_association_full_covariance, data_association_independent_2d_points, data_association_independent_3d_points 00161 */ 00162 void SLAM_IMPEXP data_association_independent_predictions( 00163 const mrpt::math::CMatrixDouble &Z_observations_mean, 00164 const mrpt::math::CMatrixDouble &Y_predictions_mean, 00165 const mrpt::math::CMatrixDouble &Y_predictions_cov, 00166 TDataAssociationResults &results, 00167 const TDataAssociationMethod method = assocJCBB, 00168 const TDataAssociationMetric metric = metricMaha, 00169 const double chi2quantile = 0.99, 00170 const bool DAT_ASOC_USE_KDTREE = true, 00171 const std::vector<prediction_index_t> &predictions_IDs = std::vector<prediction_index_t>(), 00172 const TDataAssociationMetric compatibilityTestMetric = metricMaha, 00173 const double log_ML_compat_test_threshold = 0.0 00174 ); 00175 00176 00177 /** @} */ 00178 00179 /** @} */ // end of grouping 00180 00181 } // end namespace 00182 00183 // Specializations MUST occur at the same namespace: 00184 namespace utils 00185 { 00186 template <> 00187 struct TEnumTypeFiller<slam::TDataAssociationMethod> 00188 { 00189 typedef slam::TDataAssociationMethod enum_t; 00190 static void fill(bimap<enum_t,std::string> &m_map) 00191 { 00192 m_map.insert(slam::assocNN, "assocNN"); 00193 m_map.insert(slam::assocJCBB, "assocJCBB"); 00194 } 00195 }; 00196 00197 template <> 00198 struct TEnumTypeFiller<slam::TDataAssociationMetric> 00199 { 00200 typedef slam::TDataAssociationMetric enum_t; 00201 static void fill(bimap<enum_t,std::string> &m_map) 00202 { 00203 m_map.insert(slam::metricMaha, "metricMaha"); 00204 m_map.insert(slam::metricML, "metricML"); 00205 } 00206 }; 00207 00208 } // End of namespace 00209 00210 } // End of namespace 00211 00212 #endif
| Page generated by Doxygen 1.7.5 for MRPT 0.9.5 SVN: at Thu Oct 13 21:25:36 UTC 2011 |