00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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/CPointPDFGaussian.h> 00033 00034 namespace mrpt 00035 { 00036 namespace slam 00037 { 00038 /** Different algorithms for data association, used in 00039 */ 00040 enum TDataAssociationMethod 00041 { 00042 assocNN = 0, //!< Nearest-neighbor 00043 assocJCBB //!< JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001] 00044 }; 00045 00046 /** The results from mrpt::slam::data_association 00047 */ 00048 struct TDataAssociationResults 00049 { 00050 /** For each landmark ID in the observations ID_obs, its association in the predictions ID_pred = associations[ID_obs] 00051 * Note that not all observations may have an associated landmark in the predictions. 00052 */ 00053 std::map<size_t, size_t> associations; 00054 double distance_maha; //!< Mahalanobis distance of the best associations found. 00055 00056 /** Individual mahalanobis distances between predictions (row indices) & observations (column indices). 00057 * Indices are for the appearing order in the arguments "predictions" & "observations", they are NOT landmark IDs. 00058 * For helping in converting indices, see the members: obsidx2id, predidx2id, id2predidx, id2obsidx 00059 */ 00060 mrpt::math::CMatrixDouble indiv_distances; 00061 mrpt::math::CMatrixBool indiv_compatibility; //!< Like "indiv_distances" but passed thru the chi2 threshold. 00062 vector_uint indiv_compatibility_counts; //!< The sum of each column of indiv_compatibility, that is, the number of compatible pairings for each observation. 00063 00064 std::map<size_t,size_t> obsidx2id, predidx2id; //!< Maps from indices in the prediction/observation vectors to their corresponding IDs 00065 std::map<size_t,size_t> id2predidx, id2obsidx; //!< The oposite to obsidx2id & predidx2id. 00066 00067 }; 00068 00069 /** Computes the data-association between the prediction of a set of landmarks and their observations, all with uncertainty. 00070 * Implemented methods include (see TDataAssociation) 00071 * - NN: Nearest-neighbor 00072 * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001] 00073 * 00074 * \param predictions [IN] The list of predicted locations of landmarks/features, indexed by their ID. The 2D/3D locations are in the same coordinate framework than "observations". 00075 * \param observations [IN] The list of observed locations of landmarks/features, indexed by their ID. The 2D/3D locations are in the same coordinate framework than "predictions". 00076 * \param results [OUT] The output data association hypothesis, and other useful information. 00077 * \param method [IN] The selected method to make the associations. 00078 * \param chi2quantile [IN] 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 00079 */ 00080 void MRPTDLLIMPEXP data_association( 00081 const std::map<size_t, mrpt::poses::CPointPDFGaussian> & predictions, 00082 const std::map<size_t, mrpt::poses::CPointPDFGaussian> & observation, 00083 TDataAssociationResults &results, 00084 const TDataAssociationMethod method = assocJCBB, 00085 const double chi2quantile = 0.99 00086 ); 00087 00088 00089 } // End of namespace 00090 } // End of namespace 00091 00092 #endif
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 22:58:25 EDT 2009 |