<!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>The MRPT project: CRangeBearingKFSLAM.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.6.2-20100208 -->
<script type="text/javascript"><!--
var searchBox = new SearchBox("searchBox", "search",false,'Search');
--></script>
<div class="navigation" id="top">
<div class="tabs">
<ul>
<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="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">
<img id="MSearchSelect" src="search/search.png"
onmouseover="return searchBox.OnSearchSelectShow()"
onmouseout="return searchBox.OnSearchSelectHide()"
alt=""/>
<input type="text" id="MSearchField" value="Search" accesskey="S"
onfocus="searchBox.OnSearchFieldFocus(true)"
onblur="searchBox.OnSearchFieldFocus(false)"
onkeyup="searchBox.OnSearchFieldChange(event)"/>
<a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
</div>
</li>
</ul>
</div>
<div class="tabs">
<ul>
<li><a href="files.html"><span>File List</span></a></li>
<li><a href="globals.html"><span>File Members</span></a></li>
</ul>
</div>
<h1>CRangeBearingKFSLAM.h</h1><a href="_c_range_bearing_k_f_s_l_a_m_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://mrpt.sourceforge.net/ |</span>
<a name="l00005"></a>00005 <span class="comment"> | |</span>
<a name="l00006"></a>00006 <span class="comment"> | Copyright (C) 2005-2010 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 CRangeBearingKFSLAM_H</span>
<a name="l00029"></a>00029 <span class="preprocessor"></span><span class="preprocessor">#define CRangeBearingKFSLAM_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_vector_template_8h.html">mrpt/math/CVectorTemplate.h</a>></span>
<a name="l00033"></a>00033 <span class="preprocessor">#include <<a class="code" href="_c_matrix_template_numeric_8h.html">mrpt/math/CMatrixTemplateNumeric.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_point3_d_8h.html">mrpt/poses/CPoint3D.h</a>></span>
<a name="l00046"></a>00046 <span class="preprocessor">#include <<a class="code" href="_c_pose3_d_p_d_f_gaussian_8h.html">mrpt/poses/CPose3DPDFGaussian.h</a>></span>
<a name="l00047"></a>00047 <span class="preprocessor">#include <<a class="code" href="_c_pose3_d_quat_p_d_f_gaussian_8h.html">mrpt/poses/CPose3DQuatPDFGaussian.h</a>></span>
<a name="l00048"></a>00048 <span class="preprocessor">#include <<a class="code" href="_c_landmark_8h.html">mrpt/slam/CLandmark.h</a>></span>
<a name="l00049"></a>00049 <span class="preprocessor">#include <<a class="code" href="_c_simple_map_8h.html">mrpt/slam/CSimpleMap.h</a>></span>
<a name="l00050"></a>00050 <span class="preprocessor">#include <<a class="code" href="_c_incremental_map_partitioner_8h.html">mrpt/slam/CIncrementalMapPartitioner.h</a>></span>
<a name="l00051"></a>00051 <span class="preprocessor">#include <<a class="code" href="data__association_8h.html">mrpt/slam/data_association.h</a>></span>
<a name="l00052"></a>00052
<a name="l00053"></a>00053 <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="l00054"></a>00054
<a name="l00055"></a>00055 <span class="keyword">namespace </span>mrpt
<a name="l00056"></a>00056 {
<a name="l00057"></a>00057 <span class="keyword">namespace </span>slam
<a name="l00058"></a>00058 {
<a name="l00059"></a>00059 <span class="keyword">using namespace </span>mrpt::bayes;
<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, a full 6D robot pose, and 3D 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"> * The state vector comprises: 3D robot position, a quaternion for its attitude, and the 3D landmarks in the map.</span>
<a name="l00064"></a>00064 <span class="comment"> *</span>
<a name="l00065"></a>00065 <span class="comment"> * The following Wiki page describes an front-end application based on this class:</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"> * For the theory behind this implementation, see the technical report in:</span>
<a name="l00069"></a>00069 <span class="comment"> * http://www.mrpt.org/6D-SLAM</span>
<a name="l00070"></a>00070 <span class="comment"> *</span>
<a name="l00071"></a>00071 <span class="comment"> * \sa An implementation for 2D only: CRangeBearingKFSLAM2D</span>
<a name="l00072"></a>00072 <span class="comment"> */</span>
<a name="l00073"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html">00073</a> <span class="keyword">class </span><a class="code" href="slam_2include_2mrpt_2slam_2link__pragmas_8h.html#a26118d9c67fb641718c989b57c9acf64">SLAM_IMPEXP</a> <a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html" title="An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full...">CRangeBearingKFSLAM</a> :
<a name="l00074"></a>00074 <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><7 <span class="comment">/* x y z qr qx qy qz*/</span>,3 <span class="comment">/* range yaw pitch */</span>, 3 <span class="comment">/* x y z */</span>, 7 <span class="comment">/* Ax Ay Az Aqr Aqx Aqy Aqz */</span> >
<a name="l00075"></a>00075 <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="l00076"></a>00076 {
<a name="l00077"></a>00077 <span class="keyword">public</span>:<span class="comment"></span>
<a name="l00078"></a>00078 <span class="comment"> /** Constructor.</span>
<a name="l00079"></a>00079 <span class="comment"> */</span>
<a name="l00080"></a>00080 <a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html" title="An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full...">CRangeBearingKFSLAM</a>( );
<a name="l00081"></a>00081 <span class="comment"></span>
<a name="l00082"></a>00082 <span class="comment"> /** Destructor:</span>
<a name="l00083"></a>00083 <span class="comment"> */</span>
<a name="l00084"></a>00084 <span class="keyword">virtual</span> ~<a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html" title="An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full...">CRangeBearingKFSLAM</a>();
<a name="l00085"></a>00085
<a name="l00086"></a>00086 <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="l00087"></a>00087 <span class="comment"></span><span class="comment"></span>
<a name="l00088"></a>00088 <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="l00089"></a>00089 <span class="comment"> * \param action May contain odometry</span>
<a name="l00090"></a>00090 <span class="comment"> * \param SF The set of observations, must contain at least one CObservationBearingRange</span>
<a name="l00091"></a>00091 <span class="comment"> */</span>
<a name="l00092"></a>00092 <span class="keywordtype">void</span> processActionObservation(
<a name="l00093"></a>00093 CActionCollectionPtr &action,
<a name="l00094"></a>00094 CSensoryFramePtr &SF );
<a name="l00095"></a>00095 <span class="comment"></span>
<a name="l00096"></a>00096 <span class="comment"> /** Returns the complete mean and cov.</span>
<a name="l00097"></a>00097 <span class="comment"> * \param out_robotPose The mean and the 7x7 covariance matrix of the robot 6D pose</span>
<a name="l00098"></a>00098 <span class="comment"> * \param out_landmarksPositions One entry for each of the M landmark positions (3D).</span>
<a name="l00099"></a>00099 <span class="comment"> * \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.</span>
<a name="l00100"></a>00100 <span class="comment"> * \param out_fullState The complete state vector (7+3M).</span>
<a name="l00101"></a>00101 <span class="comment"> * \param out_fullCovariance The full (7+3M)x(7+3M) covariance matrix of the filter.</span>
<a name="l00102"></a>00102 <span class="comment"> * \sa getCurrentRobotPose</span>
<a name="l00103"></a>00103 <span class="comment"> */</span>
<a name="l00104"></a>00104 <span class="keywordtype">void</span> getCurrentState(
<a name="l00105"></a>00105 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose...">CPose3DQuatPDFGaussian</a> &out_robotPose,
<a name="l00106"></a>00106 std::vector<CPoint3D> &out_landmarksPositions,
<a name="l00107"></a>00107 std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
<a name="l00108"></a>00108 <a class="code" href="classmrpt_1_1math_1_1_c_vector_template.html">CVectorDouble</a> &out_fullState,
<a name="l00109"></a>00109 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">CMatrixDouble</a> &out_fullCovariance
<a name="l00110"></a>00110 ) <span class="keyword">const</span>;
<a name="l00111"></a>00111 <span class="comment"></span>
<a name="l00112"></a>00112 <span class="comment"> /** Returns the complete mean and cov.</span>
<a name="l00113"></a>00113 <span class="comment"> * \param out_robotPose The mean and the 7x7 covariance matrix of the robot 6D pose</span>
<a name="l00114"></a>00114 <span class="comment"> * \param out_landmarksPositions One entry for each of the M landmark positions (3D).</span>
<a name="l00115"></a>00115 <span class="comment"> * \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.</span>
<a name="l00116"></a>00116 <span class="comment"> * \param out_fullState The complete state vector (7+3M).</span>
<a name="l00117"></a>00117 <span class="comment"> * \param out_fullCovariance The full (7+3M)x(7+3M) covariance matrix of the filter.</span>
<a name="l00118"></a>00118 <span class="comment"> * \sa getCurrentRobotPose</span>
<a name="l00119"></a>00119 <span class="comment"> */</span>
<a name="l00120"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html#ad1fd063b3d4d844943452dec21b351bc">00120</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> getCurrentState(
<a name="l00121"></a>00121 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose...">CPose3DPDFGaussian</a> &out_robotPose,
<a name="l00122"></a>00122 std::vector<CPoint3D> &out_landmarksPositions,
<a name="l00123"></a>00123 std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
<a name="l00124"></a>00124 <a class="code" href="classmrpt_1_1math_1_1_c_vector_template.html">CVectorDouble</a> &out_fullState,
<a name="l00125"></a>00125 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">CMatrixDouble</a> &out_fullCovariance
<a name="l00126"></a>00126 )<span class="keyword"> const</span>
<a name="l00127"></a>00127 <span class="keyword"> </span>{
<a name="l00128"></a>00128 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose...">CPose3DQuatPDFGaussian</a> q(<a class="code" href="_c_quaternion_8h.html#a7e2cb7c410e8529db2fba77ecc5f4c4f">UNINITIALIZED_QUATERNION</a>);
<a name="l00129"></a>00129 this->getCurrentState(q,out_landmarksPositions,out_landmarkIDs,out_fullState,out_fullCovariance);
<a name="l00130"></a>00130 out_robotPose = <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose...">CPose3DPDFGaussian</a>(q);
<a name="l00131"></a>00131 }
<a name="l00132"></a>00132 <span class="comment"></span>
<a name="l00133"></a>00133 <span class="comment"> /** Returns the mean & the 7x7 covariance matrix of the robot 6D pose (with rotation as a quaternion).</span>
<a name="l00134"></a>00134 <span class="comment"> * \sa getCurrentState, getCurrentRobotPoseMean</span>
<a name="l00135"></a>00135 <span class="comment"> */</span>
<a name="l00136"></a>00136 <span class="keywordtype">void</span> getCurrentRobotPose( <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose...">CPose3DQuatPDFGaussian</a> &out_robotPose ) <span class="keyword">const</span>;
<a name="l00137"></a>00137 <span class="comment"></span>
<a name="l00138"></a>00138 <span class="comment"> /** Get the current robot pose mean, as a 3D+quaternion pose.</span>
<a name="l00139"></a>00139 <span class="comment"> * \sa getCurrentRobotPose</span>
<a name="l00140"></a>00140 <span class="comment"> */</span>
<a name="l00141"></a>00141 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html" title="A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx...">mrpt::poses::CPose3DQuat</a> getCurrentRobotPoseMean() <span class="keyword">const</span>;
<a name="l00142"></a>00142 <span class="comment"></span>
<a name="l00143"></a>00143 <span class="comment"> /** Returns the mean & the 6x6 covariance matrix of the robot 6D pose (with rotation as 3 angles).</span>
<a name="l00144"></a>00144 <span class="comment"> * \sa getCurrentState</span>
<a name="l00145"></a>00145 <span class="comment"> */</span>
<a name="l00146"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html#a99a9e916ddc3b00015732bd6f971750e">00146</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> getCurrentRobotPose( <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose...">CPose3DPDFGaussian</a> &out_robotPose )<span class="keyword"> const </span>
<a name="l00147"></a>00147 <span class="keyword"> </span>{
<a name="l00148"></a>00148 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose...">CPose3DQuatPDFGaussian</a> q(<a class="code" href="_c_quaternion_8h.html#a7e2cb7c410e8529db2fba77ecc5f4c4f">UNINITIALIZED_QUATERNION</a>);
<a name="l00149"></a>00149 this->getCurrentRobotPose(q);
<a name="l00150"></a>00150 out_robotPose = <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose...">CPose3DPDFGaussian</a>(q);
<a name="l00151"></a>00151 }
<a name="l00152"></a>00152 <span class="comment"></span>
<a name="l00153"></a>00153 <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="l00154"></a>00154 <span class="comment"> * \param out_objects</span>
<a name="l00155"></a>00155 <span class="comment"> */</span>
<a name="l00156"></a>00156 <span class="keywordtype">void</span> getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) <span class="keyword">const</span>;
<a name="l00157"></a>00157 <span class="comment"></span>
<a name="l00158"></a>00158 <span class="comment"> /** Load options from a ini-like file/text</span>
<a name="l00159"></a>00159 <span class="comment"> */</span>
<a name="l00160"></a>00160 <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...">mrpt::utils::CConfigFileBase</a> &ini );
<a name="l00161"></a>00161 <span class="comment"></span>
<a name="l00162"></a>00162 <span class="comment"> /** The options for the algorithm</span>
<a name="l00163"></a>00163 <span class="comment"> */</span>
<a name="l00164"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html">00164</a> <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_m_1_1_t_options.html" title="The options for the algorithm.">TOptions</a> : utils::<a class="code" href="classmrpt_1_1utils_1_1_c_loadable_options.html" title="This is a virtual base class for sets of options than can be loaded from and/or saved...">CLoadableOptions</a>
<a name="l00165"></a>00165 {<span class="comment"></span>
<a name="l00166"></a>00166 <span class="comment"> /** Default values</span>
<a name="l00167"></a>00167 <span class="comment"> */</span>
<a name="l00168"></a>00168 <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html" title="The options for the algorithm.">TOptions</a>();
<a name="l00169"></a>00169 <span class="comment"></span>
<a name="l00170"></a>00170 <span class="comment"> /** Load from a config file/text</span>
<a name="l00171"></a>00171 <span class="comment"> */</span>
<a name="l00172"></a>00172 <span class="keywordtype">void</span> loadFromConfigFile(
<a name="l00173"></a>00173 <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...">mrpt::utils::CConfigFileBase</a> &source,
<a name="l00174"></a>00174 <span class="keyword">const</span> std::string &section);
<a name="l00175"></a>00175 <span class="comment"></span>
<a name="l00176"></a>00176 <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="l00177"></a>00177 <span class="comment"> */</span>
<a name="l00178"></a>00178 <span class="keywordtype">void</span> dumpToTextStream(<a class="code" href="classmrpt_1_1utils_1_1_c_stream.html" title="This base class is used to provide a unified interface to files,memory buffers,....">CStream</a> &out) <span class="keyword">const</span>;
<a name="l00179"></a>00179 <span class="comment"></span>
<a name="l00180"></a>00180 <span class="comment"> /** A 7-length vector with the std. deviation of the transition model in (x,y,z, qr,qx,qy,qz) used only when there is no odometry (if there is odo, its uncertainty values will be used instead); x y z: In meters.</span>
<a name="l00181"></a>00181 <span class="comment"> */</span>
<a name="l00182"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html#a11a804112b478e3dd9f30f6c775a9864">00182</a> <a class="code" href="structmrpt_1_1mrpt__base__vector.html">vector_float</a> stds_Q_no_odo;
<a name="l00183"></a>00183 <span class="comment"></span>
<a name="l00184"></a>00184 <span class="comment"> /** The std. deviation of the sensor (for the matrix R in the kalman filters), in meters and radians.</span>
<a name="l00185"></a>00185 <span class="comment"> */</span>
<a name="l00186"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html#aa9a5b9818f6b505b8930971e76f5868f">00186</a> <span class="keywordtype">float</span> std_sensor_range, std_sensor_yaw, std_sensor_pitch;
<a name="l00187"></a>00187 <span class="comment"></span>
<a name="l00188"></a>00188 <span class="comment"> /** Additional std. dev. to sum to the motion model in the z axis (useful when there is only 2D odometry and we want to put things hard to the algorithm) (default=0)</span>
<a name="l00189"></a>00189 <span class="comment"> */</span>
<a name="l00190"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html#afcdb40ab9d24576aa4e9e40d1dbf7d17">00190</a> <span class="keywordtype">float</span> std_odo_z_additional;
<a name="l00191"></a>00191 <span class="comment"></span>
<a name="l00192"></a>00192 <span class="comment"> /** If set to true (default=false), map will be partitioned using the method stated by partitioningMethod</span>
<a name="l00193"></a>00193 <span class="comment"> */</span>
<a name="l00194"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html#a4bb8fada5758bbef06d427843fa6a155">00194</a> <span class="keywordtype">bool</span> doPartitioningExperiment;
<a name="l00195"></a>00195 <span class="comment"></span>
<a name="l00196"></a>00196 <span class="comment"> /** Default = 3</span>
<a name="l00197"></a>00197 <span class="comment"> */</span>
<a name="l00198"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html#a4f39f5eddb65c90f7ce95bfd2790a882">00198</a> <span class="keywordtype">float</span> quantiles_3D_representation;
<a name="l00199"></a>00199 <span class="comment"></span>
<a name="l00200"></a>00200 <span class="comment"> /** Applicable only if "doPartitioningExperiment=true".</span>
<a name="l00201"></a>00201 <span class="comment"> * 0: Automatically detect partition through graph-cut.</span>
<a name="l00202"></a>00202 <span class="comment"> * N>=1: Cut every "N" observations.</span>
<a name="l00203"></a>00203 <span class="comment"> */</span>
<a name="l00204"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html#a0c9342db339f3395f5606527ca025879">00204</a> <span class="keywordtype">int</span> partitioningMethod;
<a name="l00205"></a>00205
<a name="l00206"></a>00206 <span class="comment">// Data association:</span>
<a name="l00207"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html#acca007e5863d810074b6a055d345b0d5">00207</a> <a class="code" href="namespacemrpt_1_1slam.html#af66b40e2929429a8b181039f447caf84" title="Different algorithms for data association, used in mrpt::slam::data_association.">TDataAssociationMethod</a> data_assoc_method;
<a name="l00208"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html#a57802ad443ca64aa91b3003f2bb58fec">00208</a> <a class="code" href="namespacemrpt_1_1slam.html#aba3986ed0165435285d91f15552abbc5" title="Different metrics for data association, used in mrpt::slam::data_association.">TDataAssociationMetric</a> data_assoc_metric;
<a name="l00209"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html#aeedc7388b405c212303d5a4bb76a8df6">00209</a> <span class="keywordtype">double</span> data_assoc_IC_chi2_thres; <span class="comment">//!< Threshold in [0,1] for the chi2square test for individual compatibility between predictions and observations (default: 0.99)</span>
<a name="l00210"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html#a287220b7ce72f31acc7bd238e00df3dc">00210</a> <span class="comment"></span> <a class="code" href="namespacemrpt_1_1slam.html#aba3986ed0165435285d91f15552abbc5" title="Different metrics for data association, used in mrpt::slam::data_association.">TDataAssociationMetric</a> data_assoc_IC_metric; <span class="comment">//!< Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.</span>
<a name="l00211"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html#a983894465205eee662e00bec1bc4d972">00211</a> <span class="comment"></span> <span class="keywordtype">double</span> data_assoc_IC_ml_threshold;<span class="comment">//!< Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)</span>
<a name="l00212"></a>00212 <span class="comment"></span>
<a name="l00213"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html#a165ea9065efa38e0f11338f24fa0413b">00213</a> <span class="keywordtype">bool</span> create_simplemap; <span class="comment">//!< Whether to fill m_SFs (default=false)</span>
<a name="l00214"></a>00214 <span class="comment"></span>
<a name="l00215"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_options.html#a15982d08d9cb9c11e605166f59c52a5f">00215</a> <span class="keywordtype">bool</span> force_ignore_odometry; <span class="comment">//!< Whether to ignore the input odometry and behave as if there was no odometry at all (default: false)</span>
<a name="l00216"></a>00216 <span class="comment"></span> } options;
<a name="l00217"></a>00217 <span class="comment"></span>
<a name="l00218"></a>00218 <span class="comment"> /** Information for data-association:</span>
<a name="l00219"></a>00219 <span class="comment"> * \sa getLastDataAssociation</span>
<a name="l00220"></a>00220 <span class="comment"> */</span>
<a name="l00221"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_data_assoc_info.html">00221</a> <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_m_1_1_t_data_assoc_info.html" title="Information for data-association:.">TDataAssocInfo</a>
<a name="l00222"></a>00222 {
<a name="l00223"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_data_assoc_info.html#a6b5ae66c754bc7036186f7aa9faa00cd">00223</a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_data_assoc_info.html" title="Information for data-association:.">TDataAssocInfo</a>() :
<a name="l00224"></a>00224 Y_pred_means(0,0),
<a name="l00225"></a>00225 Y_pred_covs(0,0)
<a name="l00226"></a>00226 {
<a name="l00227"></a>00227 }
<a name="l00228"></a>00228
<a name="l00229"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_data_assoc_info.html#a0f4c4f27edd09981f33e231306921f6d">00229</a> <span class="keywordtype">void</span> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_data_assoc_info.html#a0f4c4f27edd09981f33e231306921f6d">clear</a>() {
<a name="l00230"></a>00230 results.clear();
<a name="l00231"></a>00231 predictions_IDs.clear();
<a name="l00232"></a>00232 newly_inserted_landmarks.clear();
<a name="l00233"></a>00233 }
<a name="l00234"></a>00234
<a name="l00235"></a>00235 <span class="comment">// Predictions from the map:</span>
<a name="l00236"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_data_assoc_info.html#ad81d4608642841c99131d5543806e2b8">00236</a> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">CMatrixTemplateNumeric<kftype></a> Y_pred_means,Y_pred_covs;
<a name="l00237"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_data_assoc_info.html#ac748170a09b8daee2264cc0409ca654a">00237</a> <a class="code" href="structmrpt_1_1mrpt__base__vector.html">mrpt::vector_size_t</a> predictions_IDs;
<a name="l00238"></a>00238 <span class="comment"></span>
<a name="l00239"></a>00239 <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="l00240"></a>00240 <span class="comment"> Only used for stats and so. */</span>
<a name="l00241"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_data_assoc_info.html#a3379c141330fa2b0131386f29cac6d39">00241</a> std::map<size_t,size_t> newly_inserted_landmarks;
<a name="l00242"></a>00242
<a name="l00243"></a>00243 <span class="comment">// DA results:</span>
<a name="l00244"></a><a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_data_assoc_info.html#a1a020f3cffc5dc7244835390669ed8f5">00244</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> results;
<a name="l00245"></a>00245 };
<a name="l00246"></a>00246 <span class="comment"></span>
<a name="l00247"></a>00247 <span class="comment"> /** Returns a read-only reference to the information on the last data-association */</span>
<a name="l00248"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html#a33ef299adf1aa1774b4be0c3a0eaa9a4">00248</a> <span class="keyword">const</span> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_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_m.html#a33ef299adf1aa1774b4be0c3a0eaa9a4" title="Returns a read-only reference to the information on the last data-association.">getLastDataAssociation</a>()<span class="keyword"> const </span>{
<a name="l00249"></a>00249 <span class="keywordflow">return</span> m_last_data_association;
<a name="l00250"></a>00250 }
<a name="l00251"></a>00251
<a name="l00252"></a>00252 <span class="comment"></span>
<a name="l00253"></a>00253 <span class="comment"> /** Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!)</span>
<a name="l00254"></a>00254 <span class="comment"> * Only if options.doPartitioningExperiment = true</span>
<a name="l00255"></a>00255 <span class="comment"> * \sa getLastPartitionLandmarks</span>
<a name="l00256"></a>00256 <span class="comment"> */</span>
<a name="l00257"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html#aa27ce863f1d277c07acddde20a664e52">00257</a> <span class="keywordtype">void</span> getLastPartition( std::vector<vector_uint> &parts )
<a name="l00258"></a>00258 {
<a name="l00259"></a>00259 parts = m_lastPartitionSet;
<a name="l00260"></a>00260 }
<a name="l00261"></a>00261 <span class="comment"></span>
<a name="l00262"></a>00262 <span class="comment"> /** Return the partitioning of the landmarks in clusters accoring to the last partition.</span>
<a name="l00263"></a>00263 <span class="comment"> * Note that the same landmark may appear in different clusters (the partition is not in the space of landmarks)</span>
<a name="l00264"></a>00264 <span class="comment"> * Only if options.doPartitioningExperiment = true</span>
<a name="l00265"></a>00265 <span class="comment"> * \param landmarksMembership The i'th element of this vector is the set of clusters to which the i'th landmark in the map belongs to (landmark index != landmark ID !!).</span>
<a name="l00266"></a>00266 <span class="comment"> * \sa getLastPartition</span>
<a name="l00267"></a>00267 <span class="comment"> */</span>
<a name="l00268"></a>00268 <span class="keywordtype">void</span> getLastPartitionLandmarks( std::vector<vector_uint> &landmarksMembership ) <span class="keyword">const</span>;
<a name="l00269"></a>00269 <span class="comment"></span>
<a name="l00270"></a>00270 <span class="comment"> /** For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size submaps (size K) were have been used.</span>
<a name="l00271"></a>00271 <span class="comment"> */</span>
<a name="l00272"></a>00272 <span class="keywordtype">void</span> getLastPartitionLandmarksAsIfFixedSubmaps( <span class="keywordtype">size_t</span> K, std::vector<vector_uint> &landmarksMembership );
<a name="l00273"></a>00273
<a name="l00274"></a>00274 <span class="comment"></span>
<a name="l00275"></a>00275 <span class="comment"> /** Computes the ratio of the missing information matrix elements which are ignored under a certain partitioning of the landmarks.</span>
<a name="l00276"></a>00276 <span class="comment"> * \sa getLastPartitionLandmarks, getLastPartitionLandmarksAsIfFixedSubmaps</span>
<a name="l00277"></a>00277 <span class="comment"> */</span>
<a name="l00278"></a>00278 <span class="keywordtype">double</span> computeOffDiagonalBlocksApproximationError( <span class="keyword">const</span> std::vector<vector_uint> &landmarksMembership ) <span class="keyword">const</span>;
<a name="l00279"></a>00279
<a name="l00280"></a>00280 <span class="comment"></span>
<a name="l00281"></a>00281 <span class="comment"> /** The partitioning of the entire map is recomputed again.</span>
<a name="l00282"></a>00282 <span class="comment"> * Only when options.doPartitioningExperiment = true.</span>
<a name="l00283"></a>00283 <span class="comment"> * This can be used after changing the parameters of the partitioning method.</span>
<a name="l00284"></a>00284 <span class="comment"> * After this method, you can call getLastPartitionLandmarks.</span>
<a name="l00285"></a>00285 <span class="comment"> * \sa getLastPartitionLandmarks</span>
<a name="l00286"></a>00286 <span class="comment"> */</span>
<a name="l00287"></a>00287 <span class="keywordtype">void</span> reconsiderPartitionsNow();
<a name="l00288"></a>00288
<a name="l00289"></a>00289 <span class="comment"></span>
<a name="l00290"></a>00290 <span class="comment"> /** Provides access to the parameters of the map partitioning algorithm.</span>
<a name="l00291"></a>00291 <span class="comment"> */</span>
<a name="l00292"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html#afbaf941fcb4fa7acd32092835d400f41">00292</a> <a class="code" href="structmrpt_1_1slam_1_1_c_incremental_map_partitioner_1_1_t_options.html" title="Configuration of the algorithm:.">CIncrementalMapPartitioner::TOptions</a> * mapPartitionOptions()
<a name="l00293"></a>00293 {
<a name="l00294"></a>00294 <span class="keywordflow">return</span> &mapPartitioner.options;
<a name="l00295"></a>00295 }
<a name="l00296"></a>00296 <span class="comment"></span>
<a name="l00297"></a>00297 <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="l00298"></a>00298 <span class="comment"> */</span>
<a name="l00299"></a>00299 <span class="keywordtype">void</span> saveMapAndPath2DRepresentationAsMATLABFile(
<a name="l00300"></a>00300 <span class="keyword">const</span> std::string &fil,
<a name="l00301"></a>00301 <span class="keywordtype">float</span> stdCount=3.0f,
<a name="l00302"></a>00302 <span class="keyword">const</span> std::string &styleLandmarks = std::string(<span class="stringliteral">"b"</span>),
<a name="l00303"></a>00303 <span class="keyword">const</span> std::string &stylePath = std::string(<span class="stringliteral">"r"</span>),
<a name="l00304"></a>00304 <span class="keyword">const</span> std::string &styleRobot = std::string(<span class="stringliteral">"r"</span>) ) <span class="keyword">const</span>;
<a name="l00305"></a>00305
<a name="l00306"></a>00306
<a name="l00307"></a>00307
<a name="l00308"></a>00308 <span class="keyword">protected</span>:
<a name="l00309"></a>00309 <span class="comment"></span>
<a name="l00310"></a>00310 <span class="comment"> /** @name Virtual methods for Kalman Filter implementation</span>
<a name="l00311"></a>00311 <span class="comment"> @{</span>
<a name="l00312"></a>00312 <span class="comment"> */</span>
<a name="l00313"></a>00313 <span class="comment"></span>
<a name="l00314"></a>00314 <span class="comment"> /** Must return the action vector u.</span>
<a name="l00315"></a>00315 <span class="comment"> * \param out_u The action vector which will be passed to OnTransitionModel</span>
<a name="l00316"></a>00316 <span class="comment"> */</span>
<a name="l00317"></a>00317 <span class="keywordtype">void</span> OnGetAction( KFArray_ACT &out_u ) <span class="keyword">const</span>;
<a name="l00318"></a>00318 <span class="comment"></span>
<a name="l00319"></a>00319 <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="l00320"></a>00320 <span class="comment"> * \param in_u The vector returned by OnGetAction.</span>
<a name="l00321"></a>00321 <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="l00322"></a>00322 <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="l00323"></a>00323 <span class="comment"> */</span>
<a name="l00324"></a>00324 <span class="keywordtype">void</span> OnTransitionModel(
<a name="l00325"></a>00325 <span class="keyword">const</span> KFArray_ACT &in_u,
<a name="l00326"></a>00326 KFArray_VEH &inout_x,
<a name="l00327"></a>00327 <span class="keywordtype">bool</span> &out_skipPrediction
<a name="l00328"></a>00328 ) <span class="keyword">const</span>;
<a name="l00329"></a>00329 <span class="comment"></span>
<a name="l00330"></a>00330 <span class="comment"> /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$</span>
<a name="l00331"></a>00331 <span class="comment"> * \param out_F Must return the Jacobian.</span>
<a name="l00332"></a>00332 <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="l00333"></a>00333 <span class="comment"> */</span>
<a name="l00334"></a>00334 <span class="keywordtype">void</span> OnTransitionJacobian( KFMatrix_VxV &out_F ) <span class="keyword">const</span>;
<a name="l00335"></a>00335 <span class="comment"></span>
<a name="l00336"></a>00336 <span class="comment"> /** Implements the transition noise covariance \f$ Q_k \f$</span>
<a name="l00337"></a>00337 <span class="comment"> * \param out_Q Must return the covariance matrix.</span>
<a name="l00338"></a>00338 <span class="comment"> * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian</span>
<a name="l00339"></a>00339 <span class="comment"> */</span>
<a name="l00340"></a>00340 <span class="keywordtype">void</span> OnTransitionNoise( KFMatrix_VxV &out_Q ) <span class="keyword">const</span>;
<a name="l00341"></a>00341 <span class="comment"></span>
<a name="l00342"></a>00342 <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="l00343"></a>00343 <span class="comment"> *</span>
<a name="l00344"></a>00344 <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="l00345"></a>00345 <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="l00346"></a>00346 <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="l00347"></a>00347 <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="l00348"></a>00348 <span class="comment"> *</span>
<a name="l00349"></a>00349 <span class="comment"> * This method will be called just once for each complete KF iteration.</span>
<a name="l00350"></a>00350 <span class="comment"> * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.</span>
<a name="l00351"></a>00351 <span class="comment"> */</span>
<a name="l00352"></a>00352 <span class="keywordtype">void</span> OnGetObservationsAndDataAssociation(
<a name="l00353"></a>00353 std::vector<KFArray_OBS> &out_z,
<a name="l00354"></a>00354 <a class="code" href="structmrpt_1_1mrpt__base__vector.html">vector_int</a> &out_data_association,
<a name="l00355"></a>00355 <span class="keyword">const</span> vector<KFArray_OBS> &in_all_predictions,
<a name="l00356"></a>00356 <span class="keyword">const</span> KFMatrix &in_S,
<a name="l00357"></a>00357 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1mrpt__base__vector.html">vector_size_t</a> &in_lm_indices_in_S,
<a name="l00358"></a>00358 <span class="keyword">const</span> KFMatrix_OxO &in_R
<a name="l00359"></a>00359 );
<a name="l00360"></a>00360
<a name="l00361"></a>00361 <span class="keywordtype">void</span> OnObservationModel(
<a name="l00362"></a>00362 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1mrpt__base__vector.html">vector_size_t</a> &idx_landmarks_to_predict,
<a name="l00363"></a>00363 std::vector<KFArray_OBS> &out_predictions
<a name="l00364"></a>00364 ) <span class="keyword">const</span>;
<a name="l00365"></a>00365 <span class="comment"></span>
<a name="l00366"></a>00366 <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="l00367"></a>00367 <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="l00368"></a>00368 <span class="comment"> * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.</span>
<a name="l00369"></a>00369 <span class="comment"> * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$.</span>
<a name="l00370"></a>00370 <span class="comment"> */</span>
<a name="l00371"></a>00371 <span class="keywordtype">void</span> OnObservationJacobians(
<a name="l00372"></a>00372 <span class="keyword">const</span> <span class="keywordtype">size_t</span> &idx_landmark_to_predict,
<a name="l00373"></a>00373 KFMatrix_OxV &Hx,
<a name="l00374"></a>00374 KFMatrix_OxF &Hy
<a name="l00375"></a>00375 ) <span class="keyword">const</span>;
<a name="l00376"></a>00376 <span class="comment"></span>
<a name="l00377"></a>00377 <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="l00378"></a>00378 <span class="comment"> */</span>
<a name="l00379"></a>00379 <span class="keywordtype">void</span> OnSubstractObservationVectors(KFArray_OBS &A, <span class="keyword">const</span> KFArray_OBS &B) <span class="keyword">const</span>;
<a name="l00380"></a>00380 <span class="comment"></span>
<a name="l00381"></a>00381 <span class="comment"> /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.</span>
<a name="l00382"></a>00382 <span class="comment"> * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.</span>
<a name="l00383"></a>00383 <span class="comment"> */</span>
<a name="l00384"></a>00384 <span class="keywordtype">void</span> OnGetObservationNoise(KFMatrix_OxO &out_R) <span class="keyword">const</span>;
<a name="l00385"></a>00385 <span class="comment"></span>
<a name="l00386"></a>00386 <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="l00387"></a>00387 <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="l00388"></a>00388 <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="l00389"></a>00389 <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="l00390"></a>00390 <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="l00391"></a>00391 <span class="comment"> * \sa OnGetObservations, OnDataAssociation</span>
<a name="l00392"></a>00392 <span class="comment"> */</span>
<a name="l00393"></a>00393 <span class="keywordtype">void</span> OnPreComputingPredictions(
<a name="l00394"></a>00394 <span class="keyword">const</span> vector<KFArray_OBS> &in_all_prediction_means,
<a name="l00395"></a>00395 <a class="code" href="structmrpt_1_1mrpt__base__vector.html">vector_size_t</a> &out_LM_indices_to_predict ) <span class="keyword">const</span>;
<a name="l00396"></a>00396 <span class="comment"></span>
<a name="l00397"></a>00397 <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="l00398"></a>00398 <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="l00399"></a>00399 <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="l00400"></a>00400 <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="l00401"></a>00401 <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="l00402"></a>00402 <span class="comment"> *</span>
<a name="l00403"></a>00403 <span class="comment"> * - O: OBS_SIZE</span>
<a name="l00404"></a>00404 <span class="comment"> * - V: VEH_SIZE</span>
<a name="l00405"></a>00405 <span class="comment"> * - F: FEAT_SIZE</span>
<a name="l00406"></a>00406 <span class="comment"> *</span>
<a name="l00407"></a>00407 <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="l00408"></a>00408 <span class="comment"> */</span>
<a name="l00409"></a>00409 <span class="keywordtype">void</span> OnInverseObservationModel(
<a name="l00410"></a>00410 <span class="keyword">const</span> KFArray_OBS & in_z,
<a name="l00411"></a>00411 KFArray_FEAT & out_yn,
<a name="l00412"></a>00412 KFMatrix_FxV & out_dyn_dxv,
<a name="l00413"></a>00413 KFMatrix_FxO & out_dyn_dhn ) <span class="keyword">const</span>;
<a name="l00414"></a>00414 <span class="comment"></span>
<a name="l00415"></a>00415 <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="l00416"></a>00416 <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="l00417"></a>00417 <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="l00418"></a>00418 <span class="comment"> * \sa OnInverseObservationModel</span>
<a name="l00419"></a>00419 <span class="comment"> */</span>
<a name="l00420"></a>00420 <span class="keywordtype">void</span> OnNewLandmarkAddedToMap(
<a name="l00421"></a>00421 <span class="keyword">const</span> <span class="keywordtype">size_t</span> in_obsIdx,
<a name="l00422"></a>00422 <span class="keyword">const</span> <span class="keywordtype">size_t</span> in_idxNewFeat );
<a name="l00423"></a>00423
<a name="l00424"></a>00424 <span class="comment"></span>
<a name="l00425"></a>00425 <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="l00426"></a>00426 <span class="comment"> */</span>
<a name="l00427"></a>00427 <span class="keywordtype">void</span> OnNormalizeStateVector();
<a name="l00428"></a>00428 <span class="comment"></span>
<a name="l00429"></a>00429 <span class="comment"> /** @}</span>
<a name="l00430"></a>00430 <span class="comment"> */</span>
<a name="l00431"></a>00431 <span class="comment"></span>
<a name="l00432"></a>00432 <span class="comment"> /** Set up by processActionObservation</span>
<a name="l00433"></a>00433 <span class="comment"> */</span>
<a name="l00434"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html#a325aa177708f2e1203f9a8b1ad060b3e">00434</a> CActionCollectionPtr m_action;
<a name="l00435"></a>00435 <span class="comment"></span>
<a name="l00436"></a>00436 <span class="comment"> /** Set up by processActionObservation</span>
<a name="l00437"></a>00437 <span class="comment"> */</span>
<a name="l00438"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html#a9ceb909016d80c7c1f8ea9080eef89cb">00438</a> CSensoryFramePtr m_SF;
<a name="l00439"></a>00439 <span class="comment"></span>
<a name="l00440"></a>00440 <span class="comment"> /** The mapping between landmark IDs and indexes in the Pkk cov. matrix:</span>
<a name="l00441"></a>00441 <span class="comment"> */</span>
<a name="l00442"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html#a63ce956a06ecc55a8e8c7de2b59244d8">00442</a> <a class="code" href="classmrpt_1_1utils_1_1bimap.html">mrpt::utils::bimap<CLandmark::TLandmarkID,unsigned int></a> m_IDs;
<a name="l00443"></a>00443
<a name="l00444"></a>00444 <span class="comment"></span>
<a name="l00445"></a>00445 <span class="comment"> /** Used for map partitioning experiments</span>
<a name="l00446"></a>00446 <span class="comment"> */</span>
<a name="l00447"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html#a97aa37a9b59183096fe486544b1b4d87">00447</a> <a class="code" href="classmrpt_1_1slam_1_1_c_incremental_map_partitioner.html" title="This class can be used to make partitions on a map/graph build from observations...">CIncrementalMapPartitioner</a> mapPartitioner;
<a name="l00448"></a>00448 <span class="comment"></span>
<a name="l00449"></a>00449 <span class="comment"> /** The sequence of all the observations and the robot path (kept for debugging, statistics,etc)</span>
<a name="l00450"></a>00450 <span class="comment"> */</span>
<a name="l00451"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html#a7bce6dc04978cb42d426c00c476b9e77">00451</a> <a class="code" href="classmrpt_1_1slam_1_1_c_simple_map.html" title="This class stores a sequence of &lt;Probabilistic Pose,SensoryFrame&gt; pairs, thus...">CSimpleMap</a> m_SFs;
<a name="l00452"></a>00452
<a name="l00453"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html#a099e1451f9addc6543c737f749deb60a">00453</a> std::vector<vector_uint> m_lastPartitionSet;
<a name="l00454"></a>00454
<a name="l00455"></a><a class="code" href="classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html#a129a478e617bb6f5be7ded15fee98a7b">00455</a> <a class="code" href="structmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m_1_1_t_data_assoc_info.html" title="Information for data-association:.">TDataAssocInfo</a> m_last_data_association; <span class="comment">//!< Last data association</span>
<a name="l00456"></a>00456 <span class="comment"></span><span class="comment"></span>
<a name="l00457"></a>00457 <span class="comment"> /** Return the last odometry, as a pose increment. */</span>
<a name="l00458"></a>00458 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html" title="A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx...">mrpt::poses::CPose3DQuat</a> getIncrementFromOdometry() <span class="keyword">const</span>;
<a name="l00459"></a>00459
<a name="l00460"></a>00460
<a name="l00461"></a>00461 }; <span class="comment">// end class</span>
<a name="l00462"></a>00462 } <span class="comment">// End of namespace</span>
<a name="l00463"></a>00463 } <span class="comment">// End of namespace</span>
<a name="l00464"></a>00464
<a name="l00465"></a>00465 <span class="preprocessor">#endif</span>
</pre></div></div>
<!--- window showing the filter options -->
<div id="MSearchSelectWindow"
onmouseover="return searchBox.OnSearchSelectShow()"
onmouseout="return searchBox.OnSearchSelectHide()"
onkeydown="return searchBox.OnSearchSelectKey(event)">
<a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(0)"><span class="SelectionMark"> </span>All</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(1)"><span class="SelectionMark"> </span>Classes</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(2)"><span class="SelectionMark"> </span>Namespaces</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(3)"><span class="SelectionMark"> </span>Files</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(4)"><span class="SelectionMark"> </span>Functions</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(5)"><span class="SelectionMark"> </span>Variables</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(6)"><span class="SelectionMark"> </span>Typedefs</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(7)"><span class="SelectionMark"> </span>Enumerations</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(8)"><span class="SelectionMark"> </span>Enumerator</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(9)"><span class="SelectionMark"> </span>Friends</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(10)"><span class="SelectionMark"> </span>Defines</a></div>
<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="" frameborder="0"
name="MSearchResults" id="MSearchResults">
</iframe>
</div>
<br><hr><br> <table border="0" width="100%"> <tr> <td> Page generated by <a href="http://www.doxygen.org" target="_blank">Doxygen 1.6.2-20100208</a> for MRPT 0.9.0 SVN: at Wed Jul 14 12:48:09 UTC 2010</td><td></td> <td width="100"> </td> <td width="150"> </td></tr> </table> </body></html>
