<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
<title>CRangeBearingKFSLAM2D.h Source File</title>
<link href="doxygen.css" rel="stylesheet" type="text/css">
<link href="tabs.css" rel="stylesheet" type="text/css">
</head><body>
<div align="left"><a href="http://www.mrpt.org/">Main MRPT website</a> > <b>C++ reference</b> </div>
<div align="right">
<a href="index.html"><img border="0" src="mrpt_logo.png" alt="MRPT logo"></a>
</div>
<!-- Generated by Doxygen 1.7.5 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
<div id="navrow1" class="tabs">
<ul class="tablist">
<li><a href="index.html"><span>Main Page</span></a></li>
<li><a href="pages.html"><span>Related Pages</span></a></li>
<li><a href="modules.html"><span>Modules</span></a></li>
<li><a href="namespaces.html"><span>Namespaces</span></a></li>
<li><a href="annotated.html"><span>Classes</span></a></li>
<li class="current"><a href="files.html"><span>Files</span></a></li>
<li>
<div id="MSearchBox" class="MSearchBoxInactive">
<div class="left">
<form id="FSearchBox" action="search.php" method="get">
<img id="MSearchSelect" src="search/mag.png" alt=""/>
<input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S"
onfocus="searchBox.OnSearchFieldFocus(true)"
onblur="searchBox.OnSearchFieldFocus(false)"/>
</form>
</div><div class="right"></div>
</div>
</li>
</ul>
</div>
<div id="navrow2" class="tabs2">
<ul class="tablist">
<li><a href="files.html"><span>File List</span></a></li>
<li><a href="globals.html"><span>File Members</span></a></li>
</ul>
</div>
<div class="header">
<div class="headertitle">
<div class="title">CRangeBearingKFSLAM2D.h</div> </div>
</div>
<div class="contents">
<a href="_c_range_bearing_k_f_s_l_a_m2_d_8h.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/* +---------------------------------------------------------------------------+</span>
<a name="l00002"></a>00002 <span class="comment"> | The Mobile Robot Programming Toolkit (MRPT) C++ library |</span>
<a name="l00003"></a>00003 <span class="comment"> | |</span>
<a name="l00004"></a>00004 <span class="comment"> | http://www.mrpt.org/ |</span>
<a name="l00005"></a>00005 <span class="comment"> | |</span>
<a name="l00006"></a>00006 <span class="comment"> | Copyright (C) 2005-2011 University of Malaga |</span>
<a name="l00007"></a>00007 <span class="comment"> | |</span>
<a name="l00008"></a>00008 <span class="comment"> | This software was written by the Machine Perception and Intelligent |</span>
<a name="l00009"></a>00009 <span class="comment"> | Robotics Lab, University of Malaga (Spain). |</span>
<a name="l00010"></a>00010 <span class="comment"> | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |</span>
<a name="l00011"></a>00011 <span class="comment"> | |</span>
<a name="l00012"></a>00012 <span class="comment"> | This file is part of the MRPT project. |</span>
<a name="l00013"></a>00013 <span class="comment"> | |</span>
<a name="l00014"></a>00014 <span class="comment"> | MRPT is free software: you can redistribute it and/or modify |</span>
<a name="l00015"></a>00015 <span class="comment"> | it under the terms of the GNU General Public License as published by |</span>
<a name="l00016"></a>00016 <span class="comment"> | the Free Software Foundation, either version 3 of the License, or |</span>
<a name="l00017"></a>00017 <span class="comment"> | (at your option) any later version. |</span>
<a name="l00018"></a>00018 <span class="comment"> | |</span>
<a name="l00019"></a>00019 <span class="comment"> | MRPT is distributed in the hope that it will be useful, |</span>
<a name="l00020"></a>00020 <span class="comment"> | but WITHOUT ANY WARRANTY; without even the implied warranty of |</span>
<a name="l00021"></a>00021 <span class="comment"> | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |</span>
<a name="l00022"></a>00022 <span class="comment"> | GNU General Public License for more details. |</span>
<a name="l00023"></a>00023 <span class="comment"> | |</span>
<a name="l00024"></a>00024 <span class="comment"> | You should have received a copy of the GNU General Public License |</span>
<a name="l00025"></a>00025 <span class="comment"> | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |</span>
<a name="l00026"></a>00026 <span class="comment"> | |</span>
<a name="l00027"></a>00027 <span class="comment"> +---------------------------------------------------------------------------+ */</span>
<a name="l00028"></a>00028 <span class="preprocessor">#ifndef CRangeBearingKFSLAM2D_H</span>
<a name="l00029"></a>00029 <span class="preprocessor"></span><span class="preprocessor">#define CRangeBearingKFSLAM2D_H</span>
<a name="l00030"></a>00030 <span class="preprocessor"></span>
<a name="l00031"></a>00031 <span class="preprocessor">#include <<a class="code" href="_c_debug_output_capable_8h.html">mrpt/utils/CDebugOutputCapable.h</a>></span>
<a name="l00032"></a>00032 <span class="preprocessor">#include <<a class="code" href="_c_matrix_template_numeric_8h.html">mrpt/math/CMatrixTemplateNumeric.h</a>></span>
<a name="l00033"></a>00033 <span class="preprocessor">#include <<a class="code" href="_c_vector_template_8h.html">mrpt/math/CVectorTemplate.h</a>></span>
<a name="l00034"></a>00034 <span class="preprocessor">#include <<a class="code" href="_c_config_file_base_8h.html">mrpt/utils/CConfigFileBase.h</a>></span>
<a name="l00035"></a>00035 <span class="preprocessor">#include <<a class="code" href="_c_loadable_options_8h.html">mrpt/utils/CLoadableOptions.h</a>></span>
<a name="l00036"></a>00036 <span class="preprocessor">#include <<a class="code" href="opengl_8h.html">mrpt/opengl.h</a>></span>
<a name="l00037"></a>00037 <span class="preprocessor">#include <<a class="code" href="_c_kalman_filter_capable_8h.html">mrpt/bayes/CKalmanFilterCapable.h</a>></span>
<a name="l00038"></a>00038
<a name="l00039"></a>00039 <span class="preprocessor">#include <<a class="code" href="safe__pointers_8h.html">mrpt/utils/safe_pointers.h</a>></span>
<a name="l00040"></a>00040 <span class="preprocessor">#include <<a class="code" href="bimap_8h.html">mrpt/utils/bimap.h</a>></span>
<a name="l00041"></a>00041
<a name="l00042"></a>00042 <span class="preprocessor">#include <<a class="code" href="_c_sensory_frame_8h.html">mrpt/slam/CSensoryFrame.h</a>></span>
<a name="l00043"></a>00043 <span class="preprocessor">#include <<a class="code" href="_c_action_collection_8h.html">mrpt/slam/CActionCollection.h</a>></span>
<a name="l00044"></a>00044 <span class="preprocessor">#include <<a class="code" href="_c_observation_bearing_range_8h.html">mrpt/slam/CObservationBearingRange.h</a>></span>
<a name="l00045"></a>00045 <span class="preprocessor">#include <<a class="code" href="_c_point2_d_8h.html">mrpt/poses/CPoint2D.h</a>></span>
<a name="l00046"></a>00046 <span class="preprocessor">#include <<a class="code" href="_c_pose_p_d_f_gaussian_8h.html">mrpt/poses/CPosePDFGaussian.h</a>></span>
<a name="l00047"></a>00047 <span class="preprocessor">#include <<a class="code" href="_c_landmark_8h.html">mrpt/slam/CLandmark.h</a>></span>
<a name="l00048"></a>00048 <span class="preprocessor">#include <<a class="code" href="_c_simple_map_8h.html">mrpt/slam/CSimpleMap.h</a>></span>
<a name="l00049"></a>00049 <span class="preprocessor">#include <<a class="code" href="_c_incremental_map_partitioner_8h.html">mrpt/slam/CIncrementalMapPartitioner.h</a>></span>
<a name="l00050"></a>00050 <span class="preprocessor">#include <<a class="code" href="data__association_8h.html">mrpt/slam/data_association.h</a>></span>
<a name="l00051"></a>00051
<a name="l00052"></a>00052 <span class="preprocessor">#include <<a class="code" href="slam_2include_2mrpt_2slam_2link__pragmas_8h.html">mrpt/slam/link_pragmas.h</a>></span>
<a name="l00053"></a>00053
<a name="l00054"></a>00054 <span class="keyword">namespace </span>mrpt
<a name="l00055"></a>00055 {
<a name="l00056"></a>00056 <span class="keyword">namespace </span>slam
<a name="l00057"></a>00057 {
<a name="l00058"></a>00058 <span class="keyword">using namespace </span>mrpt::bayes;
<a name="l00059"></a>00059 <span class="keyword">using namespace </span>mrpt::poses;
<a name="l00060"></a>00060 <span class="comment"></span>
<a name="l00061"></a>00061 <span class="comment"> /** An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot pose, and 2D landmarks.</span>
<a name="l00062"></a>00062 <span class="comment"> * The main method is "processActionObservation" which processes pairs of action/observation.</span>
<a name="l00063"></a>00063 <span class="comment"> *</span>
<a name="l00064"></a>00064 <span class="comment"> * The following pages describe front-end applications based on this class:</span>
<a name="l00065"></a>00065 <span class="comment"> * - http://www.mrpt.org/Application:2d-slam-demo</span>
<a name="l00066"></a>00066 <span class="comment"> * - http://www.mrpt.org/Application:kf-slam</span>
<a name="l00067"></a>00067 <span class="comment"> *</span>
<a name="l00068"></a>00068 <span class="comment"> * \sa CRangeBearingKFSLAM \ingroup metric_slam_grp</span>
<a name="l00069"></a>00069 <span class="comment"> */</span>
<a name="l00070"></a>00070 <span class="keyword">class </span><a class="code" href="slam_2include_2mrpt_2slam_2link__pragmas_8h.html#a26118d9c67fb641718c989b57c9acf64">SLAM_IMPEXP</a> CRangeBearingKFSLAM2D :
<a name="l00071"></a>00071 <span class="keyword">public</span> bayes::<a class="code" href="classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html" title="Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.">CKalmanFilterCapable</a><3 <span class="comment">/* x y yaw */</span>, 2 <span class="comment">/* range yaw */</span>, 2 <span class="comment">/* x y */</span>, 3 <span class="comment">/* Ax Ay Ayaw */</span>>
<a name="l00072"></a>00072 <span class="comment">// <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, size typename kftype = double></span>
<a name="l00073"></a>00073 {
<a name="l00074"></a>00074 <span class="keyword">public</span>:
<a name="l00075"></a>00075 CRangeBearingKFSLAM2D( ); <span class="comment">//!< Default constructor</span>
<a name="l00076"></a>00076 <span class="comment"></span> <span class="keyword">virtual</span> ~CRangeBearingKFSLAM2D(); <span class="comment">//!< Destructor</span>
<a name="l00077"></a>00077 <span class="comment"></span> <span class="keywordtype">void</span> reset(); <span class="comment">//!< Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).</span>
<a name="l00078"></a>00078 <span class="comment"></span><span class="comment"></span>
<a name="l00079"></a>00079 <span class="comment"> /** Process one new action and observations to update the map and robot pose estimate. See the description of the class at the top of this page.</span>
<a name="l00080"></a>00080 <span class="comment"> * \param action May contain odometry</span>
<a name="l00081"></a>00081 <span class="comment"> * \param SF The set of observations, must contain at least one CObservationBearingRange</span>
<a name="l00082"></a>00082 <span class="comment"> */</span>
<a name="l00083"></a>00083 <span class="keywordtype">void</span> processActionObservation(
<a name="l00084"></a>00084 CActionCollectionPtr &action,
<a name="l00085"></a>00085 CSensoryFramePtr &SF );
<a name="l00086"></a>00086 <span class="comment"></span>
<a name="l00087"></a>00087 <span class="comment"> /** Returns the complete mean and cov.</span>
<a name="l00088"></a>00088 <span class="comment"> * \param out_robotPose The mean & 3x3 covariance matrix of the robot 2D pose</span>
<a name="l00089"></a>00089 <span class="comment"> * \param out_landmarksPositions One entry for each of the M landmark positions (2D).</span>
<a name="l00090"></a>00090 <span class="comment"> * \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.</span>
<a name="l00091"></a>00091 <span class="comment"> * \param out_fullState The complete state vector (3+2M).</span>
<a name="l00092"></a>00092 <span class="comment"> * \param out_fullCovariance The full (3+2M)x(3+2M) covariance matrix of the filter.</span>
<a name="l00093"></a>00093 <span class="comment"> * \sa getCurrentRobotPose</span>
<a name="l00094"></a>00094 <span class="comment"> */</span>
<a name="l00095"></a>00095 <span class="keywordtype">void</span> getCurrentState(
<a name="l00096"></a>00096 <a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 2D pose ...">CPosePDFGaussian</a> &out_robotPose,
<a name="l00097"></a>00097 <a class="code" href="classstd_1_1vector.html">std::vector<TPoint2D></a> &out_landmarksPositions,
<a name="l00098"></a>00098 <a class="code" href="classstd_1_1map.html" title="STL class.">std::map<unsigned int,CLandmark::TLandmarkID></a> &out_landmarkIDs,
<a name="l00099"></a>00099 <a class="code" href="namespacemrpt_1_1math.html#abe5956b4804ecb9ae70f0bd8ebc4d150" title="This is just another name for mrpt::vector_double (Backward compatibility with MRPT <=0...">CVectorDouble</a> &out_fullState,
<a name="l00100"></a>00100 <a class="code" href="namespacemrpt_1_1math.html#a3814c2b868f059d6a7ab0d8ecd2311d6" title="Declares a matrix of double numbers (non serializable).">CMatrixDouble</a> &out_fullCovariance
<a name="l00101"></a>00101 ) <span class="keyword">const</span>;
<a name="l00102"></a>00102 <span class="comment"></span>
<a name="l00103"></a>00103 <span class="comment"> /** Returns the mean & 3x3 covariance matrix of the robot 2D pose.</span>
<a name="l00104"></a>00104 <span class="comment"> * \sa getCurrentState</span>
<a name="l00105"></a>00105 <span class="comment"> */</span>
<a name="l00106"></a>00106 <span class="keywordtype">void</span> getCurrentRobotPose(
<a name="l00107"></a>00107 <a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 2D pose ...">CPosePDFGaussian</a> &out_robotPose ) <span class="keyword">const</span>;
<a name="l00108"></a>00108 <span class="comment"></span>
<a name="l00109"></a>00109 <span class="comment"> /** Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state.</span>
<a name="l00110"></a>00110 <span class="comment"> * \param out_objects</span>
<a name="l00111"></a>00111 <span class="comment"> */</span>
<a name="l00112"></a>00112 <span class="keywordtype">void</span> getAs3DObject( <a class="code" href="structmrpt_1_1opengl_1_1_c_set_of_objects_ptr.html">mrpt::opengl::CSetOfObjectsPtr</a> &outObj ) <span class="keyword">const</span>;
<a name="l00113"></a>00113 <span class="comment"></span>
<a name="l00114"></a>00114 <span class="comment"> /** Load options from a ini-like file/text</span>
<a name="l00115"></a>00115 <span class="comment"> */</span>
<a name="l00116"></a>00116 <span class="keywordtype">void</span> loadOptions( <span class="keyword">const</span> <a class="code" href="classmrpt_1_1utils_1_1_c_config_file_base.html" title="This class allows loading and storing values and vectors of different types from a configuration text...">mrpt::utils::CConfigFileBase</a> &ini );
<a name="l00117"></a>00117 <span class="comment"></span>
<a name="l00118"></a>00118 <span class="comment"> /** The options for the algorithm</span>
<a name="l00119"></a>00119 <span class="comment"> */</span>
<a name="l00120"></a>00120 <span class="keyword">struct </span><a class="code" href="slam_2include_2mrpt_2slam_2link__pragmas_8h.html#a26118d9c67fb641718c989b57c9acf64">SLAM_IMPEXP</a> TOptions : utils::CLoadableOptions
<a name="l00121"></a>00121 {<span class="comment"></span>
<a name="l00122"></a>00122 <span class="comment"> /** Default values</span>
<a name="l00123"></a>00123 <span class="comment"> */</span>
<a name="l00124"></a>00124 TOptions();
<a name="l00125"></a>00125 <span class="comment"></span>
<a name="l00126"></a>00126 <span class="comment"> /** Load from a config file/text</span>
<a name="l00127"></a>00127 <span class="comment"> */</span>
<a name="l00128"></a>00128 <span class="keywordtype">void</span> loadFromConfigFile(
<a name="l00129"></a>00129 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1utils_1_1_c_config_file_base.html" title="This class allows loading and storing values and vectors of different types from a configuration text...">mrpt::utils::CConfigFileBase</a> &source,
<a name="l00130"></a>00130 <span class="keyword">const</span> <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a> &section);
<a name="l00131"></a>00131 <span class="comment"></span>
<a name="l00132"></a>00132 <span class="comment"> /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream.</span>
<a name="l00133"></a>00133 <span class="comment"> */</span>
<a name="l00134"></a>00134 <span class="keywordtype">void</span> dumpToTextStream(<a class="code" href="namespacemrpt_1_1utils.html#a4aa6d0388c837d159ca801e48d601449">CStream</a> &out) <span class="keyword">const</span>;
<a name="l00135"></a>00135
<a name="l00136"></a>00136
<a name="l00137"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#a22ae065c8df9c77b554f6dc24978d853">00137</a> <a class="code" href="structmrpt_1_1dynamicsize__vector.html" title="The base class of MRPT vectors, actually, Eigen column matrices of dynamic size with specialized cons...">vector_float</a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#a22ae065c8df9c77b554f6dc24978d853" title="A 3-length vector with the std. deviation of the transition model in (x,y,phi) used only when there i...">stds_Q_no_odo</a>; <span class="comment">//!< A 3-length vector with the std. deviation of the transition model in (x,y,phi) used only when there is no odometry (if there is odo, its uncertainty values will be used instead); x y: In meters, phi: radians (but in degrees when loading from a configuration ini-file!)</span>
<a name="l00138"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#a41b68b424ec93e6ec2a393be4177a489">00138</a> <span class="comment"></span> <span class="keywordtype">float</span> std_sensor_range, <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#a41b68b424ec93e6ec2a393be4177a489" title="The std. deviation of the sensor (for the matrix R in the kalman filters), in meters and radians...">std_sensor_yaw</a>; <span class="comment">//!< The std. deviation of the sensor (for the matrix R in the kalman filters), in meters and radians.</span>
<a name="l00139"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#a1d2b86a7683615f31fa62d86438fd0c6">00139</a> <span class="comment"></span> <span class="keywordtype">float</span> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#a1d2b86a7683615f31fa62d86438fd0c6" title="Default = 3.">quantiles_3D_representation</a>; <span class="comment">//!< Default = 3</span>
<a name="l00140"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#ae9234a928b9a41f41a692ec6e4ae1b55">00140</a> <span class="comment"></span> <span class="keywordtype">bool</span> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#ae9234a928b9a41f41a692ec6e4ae1b55" title="Whether to fill m_SFs (default=false)">create_simplemap</a>; <span class="comment">//!< Whether to fill m_SFs (default=false)</span>
<a name="l00141"></a>00141 <span class="comment"></span>
<a name="l00142"></a>00142 <span class="comment">// Data association:</span>
<a name="l00143"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#a92a94e39d6c5fcecd4bef9b577975e7f">00143</a> <a class="code" href="group__data__assoc__grp.html#gaf66b40e2929429a8b181039f447caf84" title="Different algorithms for data association, used in mrpt::slam::data_association.">TDataAssociationMethod</a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#a92a94e39d6c5fcecd4bef9b577975e7f">data_assoc_method</a>;
<a name="l00144"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#a979ab6aa5b5693a99cb1e1e6beb10c58">00144</a> <a class="code" href="group__data__assoc__grp.html#gaba3986ed0165435285d91f15552abbc5" title="Different metrics for data association, used in mrpt::slam::data_association.">TDataAssociationMetric</a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#a979ab6aa5b5693a99cb1e1e6beb10c58">data_assoc_metric</a>;
<a name="l00145"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#ad7b2dff7a66e37df96755ae006d89993">00145</a> <span class="keywordtype">double</span> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#ad7b2dff7a66e37df96755ae006d89993" title="Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...">data_assoc_IC_chi2_thres</a>; <span class="comment">//!< Threshold in [0,1] for the chi2square test for individual compatibility between predictions and observations (default: 0.99)</span>
<a name="l00146"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#a97fe3497e9cbfebaaca22ce574153fd8">00146</a> <span class="comment"></span> <a class="code" href="group__data__assoc__grp.html#gaba3986ed0165435285d91f15552abbc5" title="Different metrics for data association, used in mrpt::slam::data_association.">TDataAssociationMetric</a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#a97fe3497e9cbfebaaca22ce574153fd8" title="Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.">data_assoc_IC_metric</a>; <span class="comment">//!< Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.</span>
<a name="l00147"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#af72baa99d012948804961a453cb1aee3">00147</a> <span class="comment"></span> <span class="keywordtype">double</span> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html#af72baa99d012948804961a453cb1aee3" title="Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)">data_assoc_IC_ml_threshold</a>;<span class="comment">//!< Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)</span>
<a name="l00148"></a>00148 <span class="comment"></span>
<a name="l00149"></a>00149 };
<a name="l00150"></a>00150
<a name="l00151"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#aaac671183aab10c91682e180b1d4dddf">00151</a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_options.html" title="The options for the algorithm.">TOptions</a> <a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#aaac671183aab10c91682e180b1d4dddf" title="The options for the algorithm.">options</a>; <span class="comment">//!< The options for the algorithm</span>
<a name="l00152"></a>00152 <span class="comment"></span>
<a name="l00153"></a>00153 <span class="comment"></span>
<a name="l00154"></a>00154 <span class="comment"> /** Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D</span>
<a name="l00155"></a>00155 <span class="comment"> */</span>
<a name="l00156"></a>00156 <span class="keywordtype">void</span> saveMapAndPath2DRepresentationAsMATLABFile(
<a name="l00157"></a>00157 <span class="keyword">const</span> <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a> &fil,
<a name="l00158"></a>00158 <span class="keywordtype">float</span> stdCount=3.0f,
<a name="l00159"></a>00159 <span class="keyword">const</span> <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a> &styleLandmarks = <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a>(<span class="stringliteral">"b"</span>),
<a name="l00160"></a>00160 <span class="keyword">const</span> <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a> &stylePath = <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a>(<span class="stringliteral">"r"</span>),
<a name="l00161"></a>00161 <span class="keyword">const</span> <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a> &styleRobot = <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a>(<span class="stringliteral">"r"</span>) ) <span class="keyword">const</span>;
<a name="l00162"></a>00162
<a name="l00163"></a>00163 <span class="comment"></span>
<a name="l00164"></a>00164 <span class="comment"> /** Information for data-association:</span>
<a name="l00165"></a>00165 <span class="comment"> * \sa getLastDataAssociation</span>
<a name="l00166"></a>00166 <span class="comment"> */</span>
<a name="l00167"></a>00167 <span class="keyword">struct </span><a class="code" href="slam_2include_2mrpt_2slam_2link__pragmas_8h.html#a26118d9c67fb641718c989b57c9acf64">SLAM_IMPEXP</a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html" title="Information for data-association:">TDataAssocInfo</a>
<a name="l00168"></a>00168 {
<a name="l00169"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html#ad1268240f863005ae933b04919942537">00169</a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html" title="Information for data-association:">TDataAssocInfo</a>() :
<a name="l00170"></a>00170 Y_pred_means(0,0),
<a name="l00171"></a>00171 Y_pred_covs(0,0)
<a name="l00172"></a>00172 {
<a name="l00173"></a>00173 }
<a name="l00174"></a>00174
<a name="l00175"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html#a467983b626c2c3846dfa057f41dc674c">00175</a> <span class="keywordtype">void</span> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html#a467983b626c2c3846dfa057f41dc674c">clear</a>() {
<a name="l00176"></a>00176 results.clear();
<a name="l00177"></a>00177 predictions_IDs.clear();
<a name="l00178"></a>00178 newly_inserted_landmarks.clear();
<a name="l00179"></a>00179 }
<a name="l00180"></a>00180
<a name="l00181"></a>00181 <span class="comment">// Predictions from the map:</span>
<a name="l00182"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html#abc8aa5231cc29c2cbee506404d5d8c17">00182</a> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">CMatrixTemplateNumeric<kftype></a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html#abc8aa5231cc29c2cbee506404d5d8c17">Y_pred_means</a>,Y_pred_covs;
<a name="l00183"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html#a64dafc6c0a7128506035fbb247fed046">00183</a> mrpt<a class="code" href="classstd_1_1vector.html">::vector_size_t</a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html#a64dafc6c0a7128506035fbb247fed046">predictions_IDs</a>;
<a name="l00184"></a>00184 <span class="comment"></span>
<a name="l00185"></a>00185 <span class="comment"> /** Map from the 0-based index within the last observation and the landmark 0-based index in the map (the robot-map state vector)</span>
<a name="l00186"></a>00186 <span class="comment"> Only used for stats and so. */</span>
<a name="l00187"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html#a4eab962c75b430495bae16f9f1b0d97e">00187</a> std<a class="code" href="classstd_1_1map.html">::map<size_t,size_t></a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html#a4eab962c75b430495bae16f9f1b0d97e" title="Map from the 0-based index within the last observation and the landmark 0-based index in the map (the...">newly_inserted_landmarks</a>;
<a name="l00188"></a>00188
<a name="l00189"></a>00189 <span class="comment">// DA results:</span>
<a name="l00190"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html#a5e09af842d411139d7f6baa368982737">00190</a> <a class="code" href="structmrpt_1_1slam_1_1_t_data_association_results.html" title="The results from mrpt::slam::data_association.">TDataAssociationResults</a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html#a5e09af842d411139d7f6baa368982737">results</a>;
<a name="l00191"></a>00191 };
<a name="l00192"></a>00192 <span class="comment"></span>
<a name="l00193"></a>00193 <span class="comment"> /** Returns a read-only reference to the information on the last data-association */</span>
<a name="l00194"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#a1cd426f4152e3f4fb08fe394010038ab">00194</a> <span class="keyword">const</span> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html" title="Information for data-association:">TDataAssocInfo</a> & <a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#a1cd426f4152e3f4fb08fe394010038ab" title="Returns a read-only reference to the information on the last data-association.">getLastDataAssociation</a>()<span class="keyword"> const </span>{
<a name="l00195"></a>00195 <span class="keywordflow">return</span> m_last_data_association;
<a name="l00196"></a>00196 }
<a name="l00197"></a>00197
<a name="l00198"></a>00198 <span class="keyword">protected</span>:
<a name="l00199"></a>00199 <span class="comment"></span>
<a name="l00200"></a>00200 <span class="comment"> /** @name Virtual methods for Kalman Filter implementation</span>
<a name="l00201"></a>00201 <span class="comment"> @{</span>
<a name="l00202"></a>00202 <span class="comment"> */</span>
<a name="l00203"></a>00203 <span class="comment"></span>
<a name="l00204"></a>00204 <span class="comment"> /** Must return the action vector u.</span>
<a name="l00205"></a>00205 <span class="comment"> * \param out_u The action vector which will be passed to OnTransitionModel</span>
<a name="l00206"></a>00206 <span class="comment"> */</span>
<a name="l00207"></a>00207 <span class="keywordtype">void</span> OnGetAction( KFArray_ACT &out_u ) <span class="keyword">const</span>;
<a name="l00208"></a>00208 <span class="comment"></span>
<a name="l00209"></a>00209 <span class="comment"> /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$</span>
<a name="l00210"></a>00210 <span class="comment"> * \param in_u The vector returned by OnGetAction.</span>
<a name="l00211"></a>00211 <span class="comment"> * \param inout_x At input has \f[ \hat{x}_{k-1|k-1} \f] , at output must have \f$ \hat{x}_{k|k-1} \f$ .</span>
<a name="l00212"></a>00212 <span class="comment"> * \param out_skip Set this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false</span>
<a name="l00213"></a>00213 <span class="comment"> */</span>
<a name="l00214"></a>00214 <span class="keywordtype">void</span> OnTransitionModel(
<a name="l00215"></a>00215 <span class="keyword">const</span> KFArray_ACT &in_u,
<a name="l00216"></a>00216 KFArray_VEH &inout_x,
<a name="l00217"></a>00217 <span class="keywordtype">bool</span> &out_skipPrediction
<a name="l00218"></a>00218 ) <span class="keyword">const</span>;
<a name="l00219"></a>00219 <span class="comment"></span>
<a name="l00220"></a>00220 <span class="comment"> /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$</span>
<a name="l00221"></a>00221 <span class="comment"> * \param out_F Must return the Jacobian.</span>
<a name="l00222"></a>00222 <span class="comment"> * The returned matrix must be \f$V \times V\f$ with V being either the size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for SLAM problems).</span>
<a name="l00223"></a>00223 <span class="comment"> */</span>
<a name="l00224"></a>00224 <span class="keywordtype">void</span> OnTransitionJacobian( KFMatrix_VxV &out_F ) <span class="keyword">const</span>;
<a name="l00225"></a>00225 <span class="comment"></span>
<a name="l00226"></a>00226 <span class="comment"> /** Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.</span>
<a name="l00227"></a>00227 <span class="comment"> */</span>
<a name="l00228"></a>00228 <span class="keywordtype">void</span> OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) <span class="keyword">const</span>;
<a name="l00229"></a>00229
<a name="l00230"></a>00230 <span class="comment"></span>
<a name="l00231"></a>00231 <span class="comment"> /** Implements the transition noise covariance \f$ Q_k \f$</span>
<a name="l00232"></a>00232 <span class="comment"> * \param out_Q Must return the covariance matrix.</span>
<a name="l00233"></a>00233 <span class="comment"> * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian</span>
<a name="l00234"></a>00234 <span class="comment"> */</span>
<a name="l00235"></a>00235 <span class="keywordtype">void</span> OnTransitionNoise( KFMatrix_VxV &out_Q ) <span class="keyword">const</span>;
<a name="l00236"></a>00236 <span class="comment"></span>
<a name="l00237"></a>00237 <span class="comment"> /** This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map.</span>
<a name="l00238"></a>00238 <span class="comment"> *</span>
<a name="l00239"></a>00239 <span class="comment"> * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N being the number of "observations": how many observed landmarks for a map, or just one if not applicable.</span>
<a name="l00240"></a>00240 <span class="comment"> * \param out_data_association An empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector (in the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and we want to insert it at the end of this KF iteration.</span>
<a name="l00241"></a>00241 <span class="comment"> * \param in_S The full covariance matrix of the observation predictions (i.e. the "innovation covariance matrix"). This is a M·O x M·O matrix with M=length of "in_lm_indices_in_S".</span>
<a name="l00242"></a>00242 <span class="comment"> * \param in_lm_indices_in_S The indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.</span>
<a name="l00243"></a>00243 <span class="comment"> *</span>
<a name="l00244"></a>00244 <span class="comment"> * This method will be called just once for each complete KF iteration.</span>
<a name="l00245"></a>00245 <span class="comment"> * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.</span>
<a name="l00246"></a>00246 <span class="comment"> */</span>
<a name="l00247"></a>00247 <span class="keywordtype">void</span> OnGetObservationsAndDataAssociation(
<a name="l00248"></a>00248 vector_KFArray_OBS &out_z,
<a name="l00249"></a>00249 <a class="code" href="classstd_1_1vector.html" title="STL class.">vector_int</a> &out_data_association,
<a name="l00250"></a>00250 <span class="keyword">const</span> vector_KFArray_OBS &in_all_predictions,
<a name="l00251"></a>00251 <span class="keyword">const</span> KFMatrix &in_S,
<a name="l00252"></a>00252 <span class="keyword">const</span> <a class="code" href="classstd_1_1vector.html">vector_size_t</a> &in_lm_indices_in_S,
<a name="l00253"></a>00253 <span class="keyword">const</span> KFMatrix_OxO &in_R
<a name="l00254"></a>00254 );
<a name="l00255"></a>00255
<a name="l00256"></a>00256 <span class="keywordtype">void</span> OnObservationModel(
<a name="l00257"></a>00257 <span class="keyword">const</span> <a class="code" href="classstd_1_1vector.html">vector_size_t</a> &idx_landmarks_to_predict,
<a name="l00258"></a>00258 vector_KFArray_OBS &out_predictions
<a name="l00259"></a>00259 ) <span class="keyword">const</span>;
<a name="l00260"></a>00260 <span class="comment"></span>
<a name="l00261"></a>00261 <span class="comment"> /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.</span>
<a name="l00262"></a>00262 <span class="comment"> * \param idx_landmark_to_predict The index of the landmark in the map whose prediction is expected as output. For non SLAM-like problems, this will be zero and the expected output is for the whole state vector.</span>
<a name="l00263"></a>00263 <span class="comment"> * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.</span>
<a name="l00264"></a>00264 <span class="comment"> * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$.</span>
<a name="l00265"></a>00265 <span class="comment"> */</span>
<a name="l00266"></a>00266 <span class="keywordtype">void</span> OnObservationJacobians(
<a name="l00267"></a>00267 <span class="keyword">const</span> <span class="keywordtype">size_t</span> &idx_landmark_to_predict,
<a name="l00268"></a>00268 KFMatrix_OxV &Hx,
<a name="l00269"></a>00269 KFMatrix_OxF &Hy
<a name="l00270"></a>00270 ) <span class="keyword">const</span>;
<a name="l00271"></a>00271 <span class="comment"></span>
<a name="l00272"></a>00272 <span class="comment"> /** Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.</span>
<a name="l00273"></a>00273 <span class="comment"> */</span>
<a name="l00274"></a>00274 <span class="keywordtype">void</span> OnObservationJacobiansNumericGetIncrements(
<a name="l00275"></a>00275 KFArray_VEH &out_veh_increments,
<a name="l00276"></a>00276 KFArray_FEAT &out_feat_increments ) <span class="keyword">const</span>;
<a name="l00277"></a>00277
<a name="l00278"></a>00278 <span class="comment"></span>
<a name="l00279"></a>00279 <span class="comment"> /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).</span>
<a name="l00280"></a>00280 <span class="comment"> */</span>
<a name="l00281"></a>00281 <span class="keywordtype">void</span> OnSubstractObservationVectors(KFArray_OBS &A, <span class="keyword">const</span> KFArray_OBS &B) <span class="keyword">const</span>;
<a name="l00282"></a>00282 <span class="comment"></span>
<a name="l00283"></a>00283 <span class="comment"> /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.</span>
<a name="l00284"></a>00284 <span class="comment"> * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.</span>
<a name="l00285"></a>00285 <span class="comment"> */</span>
<a name="l00286"></a>00286 <span class="keywordtype">void</span> OnGetObservationNoise(KFMatrix_OxO &out_R) <span class="keyword">const</span>;
<a name="l00287"></a>00287 <span class="comment"></span>
<a name="l00288"></a>00288 <span class="comment"> /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.</span>
<a name="l00289"></a>00289 <span class="comment"> * For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.</span>
<a name="l00290"></a>00290 <span class="comment"> * \param in_all_prediction_means The mean of each landmark predictions; the computation or not of the corresponding covariances is what we're trying to determined with this method.</span>
<a name="l00291"></a>00291 <span class="comment"> * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.</span>
<a name="l00292"></a>00292 <span class="comment"> * \note This is not a pure virtual method, so it should be implemented only if desired. The default implementation returns a vector with all the landmarks in the map.</span>
<a name="l00293"></a>00293 <span class="comment"> * \sa OnGetObservations, OnDataAssociation</span>
<a name="l00294"></a>00294 <span class="comment"> */</span>
<a name="l00295"></a>00295 <span class="keywordtype">void</span> OnPreComputingPredictions(
<a name="l00296"></a>00296 <span class="keyword">const</span> vector_KFArray_OBS &in_all_prediction_means,
<a name="l00297"></a>00297 <a class="code" href="classstd_1_1vector.html">vector_size_t</a> &out_LM_indices_to_predict ) <span class="keyword">const</span>;
<a name="l00298"></a>00298 <span class="comment"></span>
<a name="l00299"></a>00299 <span class="comment"> /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".</span>
<a name="l00300"></a>00300 <span class="comment"> * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservations().</span>
<a name="l00301"></a>00301 <span class="comment"> * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.</span>
<a name="l00302"></a>00302 <span class="comment"> * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.</span>
<a name="l00303"></a>00303 <span class="comment"> * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n} \f$.</span>
<a name="l00304"></a>00304 <span class="comment"> *</span>
<a name="l00305"></a>00305 <span class="comment"> * - O: OBS_SIZE</span>
<a name="l00306"></a>00306 <span class="comment"> * - V: VEH_SIZE</span>
<a name="l00307"></a>00307 <span class="comment"> * - F: FEAT_SIZE</span>
<a name="l00308"></a>00308 <span class="comment"> *</span>
<a name="l00309"></a>00309 <span class="comment"> * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.</span>
<a name="l00310"></a>00310 <span class="comment"> */</span>
<a name="l00311"></a>00311 <span class="keywordtype">void</span> OnInverseObservationModel(
<a name="l00312"></a>00312 <span class="keyword">const</span> KFArray_OBS & in_z,
<a name="l00313"></a>00313 KFArray_FEAT & out_yn,
<a name="l00314"></a>00314 KFMatrix_FxV & out_dyn_dxv,
<a name="l00315"></a>00315 KFMatrix_FxO & out_dyn_dhn ) <span class="keyword">const</span>;
<a name="l00316"></a>00316 <span class="comment"></span>
<a name="l00317"></a>00317 <span class="comment"> /** If applicable to the given problem, do here any special handling of adding a new landmark to the map.</span>
<a name="l00318"></a>00318 <span class="comment"> * \param in_obsIndex The index of the observation whose inverse sensor is to be computed. It corresponds to the row in in_z where the observation can be found.</span>
<a name="l00319"></a>00319 <span class="comment"> * \param in_idxNewFeat The index that this new feature will have in the state vector (0:just after the vehicle state, 1: after that,...). Save this number so data association can be done according to these indices.</span>
<a name="l00320"></a>00320 <span class="comment"> * \sa OnInverseObservationModel</span>
<a name="l00321"></a>00321 <span class="comment"> */</span>
<a name="l00322"></a>00322 <span class="keywordtype">void</span> OnNewLandmarkAddedToMap(
<a name="l00323"></a>00323 <span class="keyword">const</span> <span class="keywordtype">size_t</span> in_obsIdx,
<a name="l00324"></a>00324 <span class="keyword">const</span> <span class="keywordtype">size_t</span> in_idxNewFeat );
<a name="l00325"></a>00325
<a name="l00326"></a>00326 <span class="comment"></span>
<a name="l00327"></a>00327 <span class="comment"> /** This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it.</span>
<a name="l00328"></a>00328 <span class="comment"> */</span>
<a name="l00329"></a>00329 <span class="keywordtype">void</span> OnNormalizeStateVector();
<a name="l00330"></a>00330 <span class="comment"></span>
<a name="l00331"></a>00331 <span class="comment"> /** @}</span>
<a name="l00332"></a>00332 <span class="comment"> */</span>
<a name="l00333"></a>00333
<a name="l00334"></a>00334
<a name="l00335"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#a76474c9e8d0e66ec08c55df763394656">00335</a> <span class="keywordtype">void</span> getLandmarkIDsFromIndexInStateVector(<a class="code" href="classstd_1_1map.html" title="STL class.">std::map<unsigned int,CLandmark::TLandmarkID></a> &out_id2index)<span class="keyword"> const</span>
<a name="l00336"></a>00336 <span class="keyword"> </span>{
<a name="l00337"></a>00337 out_id2index = m_IDs.getInverseMap();
<a name="l00338"></a>00338 }
<a name="l00339"></a>00339
<a name="l00340"></a>00340 <span class="keyword">protected</span>:
<a name="l00341"></a>00341 <span class="comment"></span>
<a name="l00342"></a>00342 <span class="comment"> /** Set up by processActionObservation</span>
<a name="l00343"></a>00343 <span class="comment"> */</span>
<a name="l00344"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#aa252e8d11ce89ce7ff01696269bf0f74">00344</a> <a class="code" href="structmrpt_1_1slam_1_1_c_action_collection_ptr.html">CActionCollectionPtr</a> <a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#aa252e8d11ce89ce7ff01696269bf0f74" title="Set up by processActionObservation.">m_action</a>;
<a name="l00345"></a>00345 <span class="comment"></span>
<a name="l00346"></a>00346 <span class="comment"> /** Set up by processActionObservation</span>
<a name="l00347"></a>00347 <span class="comment"> */</span>
<a name="l00348"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#ae2df87aa3d1cc8cbf8a88fab1ea68dbd">00348</a> <a class="code" href="structmrpt_1_1slam_1_1_c_sensory_frame_ptr.html">CSensoryFramePtr</a> <a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#ae2df87aa3d1cc8cbf8a88fab1ea68dbd" title="Set up by processActionObservation.">m_SF</a>;
<a name="l00349"></a>00349 <span class="comment"></span>
<a name="l00350"></a>00350 <span class="comment"> /** The mapping between landmark IDs and indexes in the Pkk cov. matrix:</span>
<a name="l00351"></a>00351 <span class="comment"> */</span>
<a name="l00352"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#a804919da59c1ddc75e6b481a24ee6749">00352</a> mrpt<a class="code" href="classmrpt_1_1utils_1_1bimap.html">::utils::bimap<CLandmark::TLandmarkID,unsigned int></a> <a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#a804919da59c1ddc75e6b481a24ee6749" title="The mapping between landmark IDs and indexes in the Pkk cov.">m_IDs</a>;
<a name="l00353"></a>00353 <span class="comment"></span>
<a name="l00354"></a>00354 <span class="comment"> /** The sequence of all the observations and the robot path (kept for debugging, statistics,etc)</span>
<a name="l00355"></a>00355 <span class="comment"> */</span>
<a name="l00356"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#a7add6e75a3f51e219bbb3d7e0e428c63">00356</a> <a class="code" href="classmrpt_1_1slam_1_1_c_simple_map.html" title="This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...">CSimpleMap</a> <a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#a7add6e75a3f51e219bbb3d7e0e428c63" title="The sequence of all the observations and the robot path (kept for debugging, statistics,etc)">m_SFs</a>;
<a name="l00357"></a>00357
<a name="l00358"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#abc2f94171c9219ee807c5bbafc2ac611">00358</a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d_1_1_t_data_assoc_info.html" title="Information for data-association:">TDataAssocInfo</a> <a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m2_d.html#abc2f94171c9219ee807c5bbafc2ac611" title="Last data association.">m_last_data_association</a>; <span class="comment">//!< Last data association</span>
<a name="l00359"></a>00359 <span class="comment"></span>
<a name="l00360"></a>00360
<a name="l00361"></a>00361 }; <span class="comment">// end class</span>
<a name="l00362"></a>00362 } <span class="comment">// End of namespace</span>
<a name="l00363"></a>00363 } <span class="comment">// End of namespace</span>
<a name="l00364"></a>00364
<a name="l00365"></a>00365 <span class="preprocessor">#endif</span>
</pre></div></div>
</div>
<br><hr><br> <table border="0" width="100%"> <tr> <td> Page generated by <a href="http://www.doxygen.org" target="_blank">Doxygen 1.7.5</a> for MRPT 0.9.5 SVN: at Sun Sep 25 17:20:18 UTC 2011</td><td></td> <td width="100"> </td> <td width="150"> </td></tr> </table> </body></html>
