<!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>PF_implementations.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">PF_implementations.h</div> </div>
</div>
<div class="contents">
<a href="_p_f__implementations_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
<a name="l00029"></a>00029 <span class="preprocessor">#ifndef PF_implementations_H</span>
<a name="l00030"></a>00030 <span class="preprocessor"></span><span class="preprocessor">#define PF_implementations_H</span>
<a name="l00031"></a>00031 <span class="preprocessor"></span>
<a name="l00032"></a>00032 <span class="preprocessor">#include <<a class="code" href="stl__extensions_8h.html">mrpt/utils/stl_extensions.h</a>></span>
<a name="l00033"></a>00033 <span class="preprocessor">#include <<a class="code" href="_c_particle_filter_data_8h.html">mrpt/bayes/CParticleFilterData.h</a>></span>
<a name="l00034"></a>00034 <span class="preprocessor">#include <<a class="code" href="random_8h.html">mrpt/random.h</a>></span>
<a name="l00035"></a>00035 <span class="preprocessor">#include <<a class="code" href="_c_action_collection_8h.html">mrpt/slam/CActionCollection.h</a>></span>
<a name="l00036"></a>00036 <span class="preprocessor">#include <<a class="code" href="_c_action_robot_movement3_d_8h.html">mrpt/slam/CActionRobotMovement3D.h</a>></span>
<a name="l00037"></a>00037 <span class="preprocessor">#include <<a class="code" href="_c_action_robot_movement2_d_8h.html">mrpt/slam/CActionRobotMovement2D.h</a>></span>
<a name="l00038"></a>00038 <span class="preprocessor">#include <<a class="code" href="_t_k_l_d_params_8h.html">mrpt/slam/TKLDParams.h</a>></span>
<a name="l00039"></a>00039
<a name="l00040"></a>00040 <span class="preprocessor">#include <<a class="code" href="distributions_8h.html">mrpt/math/distributions.h</a>></span> <span class="comment">// chi2inv</span>
<a name="l00041"></a>00041
<a name="l00042"></a>00042 <span class="preprocessor">#include <<a class="code" href="_p_f__implementations__data_8h.html">mrpt/slam/PF_implementations_data.h</a>></span>
<a name="l00043"></a>00043
<a name="l00044"></a>00044 <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="l00045"></a>00045
<a name="l00046"></a>00046 <span class="comment"></span>
<a name="l00047"></a>00047 <span class="comment">/** \file PF_implementations.h</span>
<a name="l00048"></a>00048 <span class="comment"> * This file contains the implementations of the template members declared in mrpt::slam::PF_implementation</span>
<a name="l00049"></a>00049 <span class="comment"> */</span>
<a name="l00050"></a>00050
<a name="l00051"></a>00051 <span class="keyword">namespace </span>mrpt
<a name="l00052"></a>00052 {
<a name="l00053"></a>00053 <span class="keyword">namespace </span>slam
<a name="l00054"></a>00054 {
<a name="l00055"></a>00055 <span class="keyword">using namespace </span>std;
<a name="l00056"></a>00056 <span class="keyword">using namespace </span>mrpt::utils;
<a name="l00057"></a>00057 <span class="keyword">using namespace </span>mrpt::random;
<a name="l00058"></a>00058 <span class="keyword">using namespace </span>mrpt::poses;
<a name="l00059"></a>00059 <span class="keyword">using namespace </span>mrpt::bayes;
<a name="l00060"></a>00060 <span class="keyword">using namespace </span>mrpt::math;
<a name="l00061"></a>00061 <span class="comment"></span>
<a name="l00062"></a>00062 <span class="comment"> /** Auxiliary method called by PF implementations: return true if we have both action & observation,</span>
<a name="l00063"></a>00063 <span class="comment"> * otherwise, return false AND accumulate the odometry so when we have an observation we didn't lose a thing.</span>
<a name="l00064"></a>00064 <span class="comment"> * On return=true, the "m_movementDrawer" member is loaded and ready to draw samples of the increment of pose since last step.</span>
<a name="l00065"></a>00065 <span class="comment"> * This method is smart enough to accumulate CActionRobotMovement2D or CActionRobotMovement3D, whatever comes in.</span>
<a name="l00066"></a>00066 <span class="comment"> * \ingroup mrpt_slam_grp </span>
<a name="l00067"></a>00067 <span class="comment"> */</span>
<a name="l00068"></a>00068 <span class="keyword">template</span> <<span class="keyword">class</span> PARTICLE_TYPE,<span class="keyword">class</span> MY<span class="keywordtype">SEL</span>F>
<a name="l00069"></a>00069 <span class="keyword">template</span> <<span class="keyword">class</span> BINTYPE>
<a name="l00070"></a><a class="code" href="group__mrpt__slam__grp.html#gaf10ab5d4285a563ed99d28aed9a31c39">00070</a> <span class="keywordtype">bool</span> <a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html" title="A set of common data shared by PF implementations for both SLAM and localization.">PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_gatherActionsCheckBothActObs</a>(
<a name="l00071"></a>00071 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_c_action_collection.html" title="Declares a class for storing a collection of robot actions.">CActionCollection</a> * actions,
<a name="l00072"></a>00072 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_c_sensory_frame.html" title="Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...">CSensoryFrame</a> * sf )
<a name="l00073"></a>00073 {
<a name="l00074"></a>00074 MYSELF *me = <span class="keyword">static_cast<</span>MYSELF*<span class="keyword">></span>(<span class="keyword">this</span>);
<a name="l00075"></a>00075
<a name="l00076"></a>00076 <span class="keywordflow">if</span> (actions!=NULL) <span class="comment">// A valid action?</span>
<a name="l00077"></a>00077 {
<a name="l00078"></a>00078 {
<a name="l00079"></a>00079 <a class="code" href="structmrpt_1_1slam_1_1_c_action_robot_movement2_d_ptr.html">CActionRobotMovement2DPtr</a> robotMovement2D = actions-><a class="code" href="classmrpt_1_1slam_1_1_c_action_collection.html#a03485a37d5f8b0057ad5722b800ced25" title="Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...">getBestMovementEstimation</a>();
<a name="l00080"></a>00080 <span class="keywordflow">if</span> (robotMovement2D.present())
<a name="l00081"></a>00081 {
<a name="l00082"></a>00082 <span class="keywordflow">if</span> (m_accumRobotMovement3DIsValid) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"Mixing 2D and 3D actions is not allowed."</span>)
<a name="l00083"></a>00083
<a name="l00084"></a>00084 <span class="keywordflow">if</span> (!m_accumRobotMovement2DIsValid)
<a name="l00085"></a>00085 { <span class="comment">// First time:</span>
<a name="l00086"></a>00086 robotMovement2D->poseChange->getMean( m_accumRobotMovement2D.rawOdometryIncrementReading );
<a name="l00087"></a>00087 m_accumRobotMovement2D.motionModelConfiguration = robotMovement2D->motionModelConfiguration;
<a name="l00088"></a>00088 }
<a name="l00089"></a>00089 <span class="keywordflow">else</span> m_accumRobotMovement2D.rawOdometryIncrementReading += robotMovement2D->poseChange->getMeanVal();
<a name="l00090"></a>00090
<a name="l00091"></a>00091 m_accumRobotMovement2DIsValid = <span class="keyword">true</span>;
<a name="l00092"></a>00092 }
<a name="l00093"></a>00093 <span class="keywordflow">else</span> <span class="comment">// If there is no 2D action, look for a 3D action:</span>
<a name="l00094"></a>00094 {
<a name="l00095"></a>00095 <a class="code" href="structmrpt_1_1slam_1_1_c_action_robot_movement3_d_ptr.html">CActionRobotMovement3DPtr</a> robotMovement3D = actions-><a class="code" href="classmrpt_1_1slam_1_1_c_action_collection.html#a831ca7e47b84a4537ac8b6b2ff096040" title="Access to the i'th action of a given class, or a NULL smart pointer if there is no action of that cla...">getActionByClass</a><<a class="code" href="classmrpt_1_1slam_1_1_c_action_robot_movement3_d.html" title="Represents a probabilistic 3D (6D) movement.">CActionRobotMovement3D</a>>();
<a name="l00096"></a>00096 <span class="keywordflow">if</span> (robotMovement3D)
<a name="l00097"></a>00097 {
<a name="l00098"></a>00098 <span class="keywordflow">if</span> (m_accumRobotMovement2DIsValid) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"Mixing 2D and 3D actions is not allowed."</span>)
<a name="l00099"></a>00099
<a name="l00100"></a>00100 <span class="keywordflow">if</span> (!m_accumRobotMovement3DIsValid)
<a name="l00101"></a>00101 m_accumRobotMovement3D = robotMovement3D->poseChange;
<a name="l00102"></a>00102 <span class="keywordflow">else</span> m_accumRobotMovement3D += robotMovement3D->poseChange;
<a name="l00103"></a>00103 <span class="comment">// This "+=" takes care of all the Jacobians, etc... You MUST love C++!!! ;-)</span>
<a name="l00104"></a>00104
<a name="l00105"></a>00105 m_accumRobotMovement3DIsValid = <span class="keyword">true</span>;
<a name="l00106"></a>00106 }
<a name="l00107"></a>00107 <span class="keywordflow">else</span>
<a name="l00108"></a>00108 <span class="keywordflow">return</span> <span class="keyword">false</span>; <span class="comment">// We have no actions...</span>
<a name="l00109"></a>00109 }
<a name="l00110"></a>00110 }
<a name="l00111"></a>00111 }
<a name="l00112"></a>00112
<a name="l00113"></a>00113 <span class="keyword">const</span> <span class="keywordtype">bool</span> SFhasValidObservations = (sf==NULL) ? <span class="keyword">false</span> : PF_SLAM_implementation_doWeHaveValidObservations(me->m_particles,sf);
<a name="l00114"></a>00114
<a name="l00115"></a>00115 <span class="comment">// All the things we need?</span>
<a name="l00116"></a>00116 <span class="keywordflow">if</span> (! ((m_accumRobotMovement2DIsValid || m_accumRobotMovement3DIsValid) && SFhasValidObservations))
<a name="l00117"></a>00117 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00118"></a>00118
<a name="l00119"></a>00119 <span class="comment">// Since we're gonna return true, load the pose-drawer:</span>
<a name="l00120"></a>00120 <span class="comment">// Take the accum. actions as input:</span>
<a name="l00121"></a>00121 <span class="keywordflow">if</span> (m_accumRobotMovement3DIsValid)
<a name="l00122"></a>00122 {
<a name="l00123"></a>00123 m_movementDrawer.setPosePDF( m_accumRobotMovement3D ); <span class="comment">// <--- Set mov. drawer</span>
<a name="l00124"></a>00124 m_accumRobotMovement3DIsValid = <span class="keyword">false</span>; <span class="comment">// Reset odometry for next iteration</span>
<a name="l00125"></a>00125 }
<a name="l00126"></a>00126 <span class="keywordflow">else</span>
<a name="l00127"></a>00127 {
<a name="l00128"></a>00128 <a class="code" href="classmrpt_1_1slam_1_1_c_action_robot_movement2_d.html" title="Represents a probabilistic 2D movement of the robot mobile base.">CActionRobotMovement2D</a> theResultingRobotMov;
<a name="l00129"></a>00129 theResultingRobotMov.<a class="code" href="classmrpt_1_1slam_1_1_c_action_robot_movement2_d.html#ae311480bde721a04b002cc7b08597fbf" title="Computes the PDF of the pose increment from an odometry reading and according to the given motion mod...">computeFromOdometry</a>(
<a name="l00130"></a>00130 m_accumRobotMovement2D.rawOdometryIncrementReading,
<a name="l00131"></a>00131 m_accumRobotMovement2D.motionModelConfiguration );
<a name="l00132"></a>00132
<a name="l00133"></a>00133 m_movementDrawer.setPosePDF( theResultingRobotMov.<a class="code" href="classmrpt_1_1slam_1_1_c_action_robot_movement2_d.html#a657f5eb086d62c2cd601e5ea2ab11514" title="The 2D pose change probabilistic estimation.">poseChange</a> ); <span class="comment">// <--- Set mov. drawer</span>
<a name="l00134"></a>00134 m_accumRobotMovement2DIsValid = <span class="keyword">false</span>; <span class="comment">// Reset odometry for next iteration</span>
<a name="l00135"></a>00135 }
<a name="l00136"></a>00136 <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00137"></a>00137 } <span class="comment">// end of PF_SLAM_implementation_gatherActionsCheckBothActObs</span>
<a name="l00138"></a>00138 <span class="comment"></span>
<a name="l00139"></a>00139 <span class="comment"> /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with rejection sampling approximation),</span>
<a name="l00140"></a>00140 <span class="comment"> * common to both localization and mapping.</span>
<a name="l00141"></a>00141 <span class="comment"> *</span>
<a name="l00142"></a>00142 <span class="comment"> * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.</span>
<a name="l00143"></a>00143 <span class="comment"> *</span>
<a name="l00144"></a>00144 <span class="comment"> * This method implements optimal sampling with a rejection sampling-based approximation of the true posterior.</span>
<a name="l00145"></a>00145 <span class="comment"> * For details, see the papers:</span>
<a name="l00146"></a>00146 <span class="comment"> *</span>
<a name="l00147"></a>00147 <span class="comment"> * J.-L. Blanco, J. González, and J.-A. Fernández-Madrigal,</span>
<a name="l00148"></a>00148 <span class="comment"> * "An Optimal Filtering Algorithm for Non-Parametric Observation Models in</span>
<a name="l00149"></a>00149 <span class="comment"> * Robot Localization," in Proc. IEEE International Conference on Robotics</span>
<a name="l00150"></a>00150 <span class="comment"> * and Automation (ICRA'08), 2008, pp. 461–466.</span>
<a name="l00151"></a>00151 <span class="comment"> */</span>
<a name="l00152"></a>00152 <span class="keyword">template</span> <<span class="keyword">class</span> PARTICLE_TYPE,<span class="keyword">class</span> MY<span class="keywordtype">SEL</span>F>
<a name="l00153"></a>00153 <span class="keyword">template</span> <<span class="keyword">class</span> BINTYPE>
<a name="l00154"></a><a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html#ab73b076d0b8f477deb4b0a8bcf0fdedb">00154</a> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html" title="A set of common data shared by PF implementations for both SLAM and localization.">PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfAuxiliaryPFOptimal</a>(
<a name="l00155"></a>00155 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_c_action_collection.html" title="Declares a class for storing a collection of robot actions.">CActionCollection</a> * actions,
<a name="l00156"></a>00156 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_c_sensory_frame.html" title="Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...">CSensoryFrame</a> * sf,
<a name="l00157"></a>00157 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html" title="The configuration of a particle filter.">CParticleFilter::TParticleFilterOptions</a> &PF_options,
<a name="l00158"></a>00158 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html" title="Option set for KLD algorithm.">TKLDParams</a> &KLD_options)
<a name="l00159"></a>00159 {
<a name="l00160"></a>00160 <span class="comment">// Standard and Optimal AuxiliaryPF actually have a shared implementation body:</span>
<a name="l00161"></a>00161 PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(actions,sf,PF_options,KLD_options, <span class="keyword">true</span> <span class="comment">/*Optimal PF*/</span> );
<a name="l00162"></a>00162 }
<a name="l00163"></a>00163
<a name="l00164"></a>00164 <span class="comment"></span>
<a name="l00165"></a>00165 <span class="comment"> /** A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution, that is, a simple SIS particle filter),</span>
<a name="l00166"></a>00166 <span class="comment"> * common to both localization and mapping.</span>
<a name="l00167"></a>00167 <span class="comment"> *</span>
<a name="l00168"></a>00168 <span class="comment"> * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.</span>
<a name="l00169"></a>00169 <span class="comment"> */</span>
<a name="l00170"></a>00170 <span class="keyword">template</span> <<span class="keyword">class</span> PARTICLE_TYPE,<span class="keyword">class</span> MY<span class="keywordtype">SEL</span>F>
<a name="l00171"></a>00171 <span class="keyword">template</span> <<span class="keyword">class</span> BINTYPE>
<a name="l00172"></a><a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html#a5feefe2d7fb194f0fb74608f561e94a7">00172</a> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html" title="A set of common data shared by PF implementations for both SLAM and localization.">PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfStandardProposal</a>(
<a name="l00173"></a>00173 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_c_action_collection.html" title="Declares a class for storing a collection of robot actions.">CActionCollection</a> * actions,
<a name="l00174"></a>00174 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_c_sensory_frame.html" title="Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...">CSensoryFrame</a> * sf,
<a name="l00175"></a>00175 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html" title="The configuration of a particle filter.">CParticleFilter::TParticleFilterOptions</a> &PF_options,
<a name="l00176"></a>00176 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html" title="Option set for KLD algorithm.">TKLDParams</a> &KLD_options)
<a name="l00177"></a>00177 {
<a name="l00178"></a>00178 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00179"></a>00179 <span class="keyword">typedef</span> std<a class="code" href="classstd_1_1set.html" title="STL class.">::set<BINTYPE,typename BINTYPE::lt_operator></a> TSetStateSpaceBins;
<a name="l00180"></a>00180
<a name="l00181"></a>00181 MYSELF *me = <span class="keyword">static_cast<</span>MYSELF*<span class="keyword">></span>(<span class="keyword">this</span>);
<a name="l00182"></a>00182
<a name="l00183"></a>00183 <span class="comment">// In this method we don't need the "PF_SLAM_implementation_gatherActionsCheckBothActObs" machinery,</span>
<a name="l00184"></a>00184 <span class="comment">// since prediction & update are two independent stages well separated for this algorithm.</span>
<a name="l00185"></a>00185
<a name="l00186"></a>00186 <span class="comment">// --------------------------------------------------------------------------------------</span>
<a name="l00187"></a>00187 <span class="comment">// Prediction: Simply draw samples from the motion model</span>
<a name="l00188"></a>00188 <span class="comment">// --------------------------------------------------------------------------------------</span>
<a name="l00189"></a>00189 <span class="keywordflow">if</span> (actions)
<a name="l00190"></a>00190 {
<a name="l00191"></a>00191 <span class="comment">// Find a robot movement estimation:</span>
<a name="l00192"></a>00192 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> motionModelMeanIncr;
<a name="l00193"></a>00193 {
<a name="l00194"></a>00194 <a class="code" href="structmrpt_1_1slam_1_1_c_action_robot_movement2_d_ptr.html">CActionRobotMovement2DPtr</a> robotMovement2D = actions-><a class="code" href="classmrpt_1_1slam_1_1_c_action_collection.html#a03485a37d5f8b0057ad5722b800ced25" title="Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...">getBestMovementEstimation</a>();
<a name="l00195"></a>00195 <span class="comment">// If there is no 2D action, look for a 3D action:</span>
<a name="l00196"></a>00196 <span class="keywordflow">if</span> (robotMovement2D.present())
<a name="l00197"></a>00197 {
<a name="l00198"></a>00198 m_movementDrawer.setPosePDF( robotMovement2D->poseChange );
<a name="l00199"></a>00199 motionModelMeanIncr = robotMovement2D->poseChange->getMeanVal();
<a name="l00200"></a>00200 }
<a name="l00201"></a>00201 <span class="keywordflow">else</span>
<a name="l00202"></a>00202 {
<a name="l00203"></a>00203 <a class="code" href="structmrpt_1_1slam_1_1_c_action_robot_movement3_d_ptr.html">CActionRobotMovement3DPtr</a> robotMovement3D = actions-><a class="code" href="classmrpt_1_1slam_1_1_c_action_collection.html#a831ca7e47b84a4537ac8b6b2ff096040" title="Access to the i'th action of a given class, or a NULL smart pointer if there is no action of that cla...">getActionByClass</a><<a class="code" href="classmrpt_1_1slam_1_1_c_action_robot_movement3_d.html" title="Represents a probabilistic 3D (6D) movement.">CActionRobotMovement3D</a>>();
<a name="l00204"></a>00204 <span class="keywordflow">if</span> (robotMovement3D)
<a name="l00205"></a>00205 {
<a name="l00206"></a>00206 m_movementDrawer.setPosePDF( robotMovement3D->poseChange );
<a name="l00207"></a>00207 robotMovement3D->poseChange.getMean( motionModelMeanIncr );
<a name="l00208"></a>00208 }
<a name="l00209"></a>00209 <span class="keywordflow">else</span> { <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"Action list does not contain any CActionRobotMovement2D or CActionRobotMovement3D object!"</span>); }
<a name="l00210"></a>00210 }
<a name="l00211"></a>00211 }
<a name="l00212"></a>00212
<a name="l00213"></a>00213 <span class="comment">// Update particle poses:</span>
<a name="l00214"></a>00214 <span class="keywordflow">if</span> ( !PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#ae1373ee71fb2a0131f448042291d85f0" title="A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...">adaptiveSampleSize</a> )
<a name="l00215"></a>00215 {
<a name="l00216"></a>00216 <span class="keyword">const</span> <span class="keywordtype">size_t</span> M = me->m_particles.size();
<a name="l00217"></a>00217 <span class="comment">// -------------------------------------------------------------</span>
<a name="l00218"></a>00218 <span class="comment">// FIXED SAMPLE SIZE</span>
<a name="l00219"></a>00219 <span class="comment">// -------------------------------------------------------------</span>
<a name="l00220"></a>00220 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> incrPose;
<a name="l00221"></a>00221 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<M;i++)
<a name="l00222"></a>00222 {
<a name="l00223"></a>00223 <span class="comment">// Generate gaussian-distributed 2D-pose increments according to mean-cov:</span>
<a name="l00224"></a>00224 m_movementDrawer.drawSample( incrPose );
<a name="l00225"></a>00225 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> finalPose = <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a>(*getLastPose(i)) + incrPose;
<a name="l00226"></a>00226
<a name="l00227"></a>00227 <span class="comment">// Update the particle with the new pose: this part is caller-dependant and must be implemented there:</span>
<a name="l00228"></a>00228 PF_SLAM_implementation_custom_update_particle_with_new_pose( me->m_particles[i].d, <a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html" title="Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).">TPose3D</a>(finalPose) );
<a name="l00229"></a>00229 }
<a name="l00230"></a>00230 }
<a name="l00231"></a>00231 <span class="keywordflow">else</span>
<a name="l00232"></a>00232 {
<a name="l00233"></a>00233 <span class="comment">// -------------------------------------------------------------</span>
<a name="l00234"></a>00234 <span class="comment">// ADAPTIVE SAMPLE SIZE</span>
<a name="l00235"></a>00235 <span class="comment">// Implementation of Dieter Fox's KLD algorithm</span>
<a name="l00236"></a>00236 <span class="comment">// 31-Oct-2006 (JLBC): First version</span>
<a name="l00237"></a>00237 <span class="comment">// 19-Jan-2009 (JLBC): Rewriten within a generic template</span>
<a name="l00238"></a>00238 <span class="comment">// -------------------------------------------------------------</span>
<a name="l00239"></a>00239 TSetStateSpaceBins stateSpaceBins;
<a name="l00240"></a>00240
<a name="l00241"></a>00241 <span class="keywordtype">size_t</span> Nx = KLD_options.<a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html#a3d919d633f69c9daefd18be697d0dc71" title="Parameters for the KLD adaptive sample size algorithm (see Dieter Fox's papers), which is used only i...">KLD_minSampleSize</a>;
<a name="l00242"></a>00242 <span class="keyword">const</span> <span class="keywordtype">double</span> delta_1 = 1.0 - KLD_options.<a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html#a787e1c15d3653be9638c8e41d786a191">KLD_delta</a>;
<a name="l00243"></a>00243 <span class="keyword">const</span> <span class="keywordtype">double</span> epsilon_1 = 0.5 / KLD_options.<a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html#a92daf8859b1e75f467c23e37d280e466">KLD_epsilon</a>;
<a name="l00244"></a>00244
<a name="l00245"></a>00245 <span class="comment">// Prepare data for executing "fastDrawSample"</span>
<a name="l00246"></a>00246 me->prepareFastDrawSample(PF_options);
<a name="l00247"></a>00247
<a name="l00248"></a>00248 <span class="comment">// The new particle set:</span>
<a name="l00249"></a>00249 std<a class="code" href="classstd_1_1vector.html">::vector<TPose3D></a> newParticles;
<a name="l00250"></a>00250 <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_double</a> newParticlesWeight;
<a name="l00251"></a>00251 std<a class="code" href="classstd_1_1vector.html">::vector<size_t></a> newParticlesDerivedFromIdx;
<a name="l00252"></a>00252
<a name="l00253"></a>00253 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> increment_i;
<a name="l00254"></a>00254 <span class="keywordtype">size_t</span> N = 1;
<a name="l00255"></a>00255
<a name="l00256"></a>00256 <span class="keywordflow">do</span> <span class="comment">// THE MAIN DRAW SAMPLING LOOP</span>
<a name="l00257"></a>00257 {
<a name="l00258"></a>00258 <span class="comment">// Draw a robot movement increment:</span>
<a name="l00259"></a>00259 m_movementDrawer.drawSample( increment_i );
<a name="l00260"></a>00260
<a name="l00261"></a>00261 <span class="comment">// generate the new particle:</span>
<a name="l00262"></a>00262 <span class="keyword">const</span> <span class="keywordtype">size_t</span> drawn_idx = me->fastDrawSample(PF_options);
<a name="l00263"></a>00263 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> newPose = <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a>(*getLastPose(drawn_idx)) + increment_i;
<a name="l00264"></a>00264 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html" title="Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).">TPose3D</a> newPose_s = newPose;
<a name="l00265"></a>00265
<a name="l00266"></a>00266 <span class="comment">// Add to the new particles list:</span>
<a name="l00267"></a>00267 newParticles.push_back( newPose_s );
<a name="l00268"></a>00268 newParticlesWeight.push_back(0);
<a name="l00269"></a>00269 newParticlesDerivedFromIdx.push_back(drawn_idx);
<a name="l00270"></a>00270
<a name="l00271"></a>00271 <span class="comment">// Now, look if the particle falls in a new bin or not:</span>
<a name="l00272"></a>00272 <span class="comment">// --------------------------------------------------------</span>
<a name="l00273"></a>00273 BINTYPE p;
<a name="l00274"></a>00274 KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>(p,KLD_options, me->m_particles[drawn_idx].d, &newPose_s);
<a name="l00275"></a>00275
<a name="l00276"></a>00276 <span class="keywordflow">if</span> (stateSpaceBins.find( p )==stateSpaceBins.end())
<a name="l00277"></a>00277 {
<a name="l00278"></a>00278 <span class="comment">// It falls into a new bin:</span>
<a name="l00279"></a>00279 <span class="comment">// Add to the stateSpaceBins:</span>
<a name="l00280"></a>00280 stateSpaceBins.insert( p );
<a name="l00281"></a>00281
<a name="l00282"></a>00282 <span class="comment">// K = K + 1</span>
<a name="l00283"></a>00283 <span class="keywordtype">size_t</span> K = stateSpaceBins.size();
<a name="l00284"></a>00284 <span class="keywordflow">if</span> ( K>1) <span class="comment">//&& newParticles.size() > options.KLD_minSampleSize )</span>
<a name="l00285"></a>00285 {
<a name="l00286"></a>00286 <span class="comment">// Update the number of m_particles!!</span>
<a name="l00287"></a>00287 Nx = <a class="code" href="namespacemrpt_1_1utils.html#ab7d9cdf7d271c2f41fc1c5c9fa7d0828" title="Returns the closer integer (int) to x.">round</a>(epsilon_1 * <a class="code" href="group__stats__grp.html#ga68b8a2208349a297c19f3a45c24457b1" title="The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...">math::chi2inv</a>(delta_1,K-1));
<a name="l00288"></a>00288 <span class="comment">//printf("k=%u \tn=%u \tNx:%u\n", k, newParticles.size(), Nx);</span>
<a name="l00289"></a>00289 }
<a name="l00290"></a>00290 }
<a name="l00291"></a>00291 N = newParticles.size();
<a name="l00292"></a>00292 } <span class="keywordflow">while</span> ( N < max(Nx,(<span class="keywordtype">size_t</span>)KLD_options.<a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html#a3d919d633f69c9daefd18be697d0dc71" title="Parameters for the KLD adaptive sample size algorithm (see Dieter Fox's papers), which is used only i...">KLD_minSampleSize</a>) &&
<a name="l00293"></a>00293 N < KLD_options.<a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html#aeb1dd89a8987c1f6957653d35aa0ccae">KLD_maxSampleSize</a> );
<a name="l00294"></a>00294
<a name="l00295"></a>00295 <span class="comment">// ---------------------------------------------------------------------------------</span>
<a name="l00296"></a>00296 <span class="comment">// Substitute old by new particle set:</span>
<a name="l00297"></a>00297 <span class="comment">// Old are in "m_particles"</span>
<a name="l00298"></a>00298 <span class="comment">// New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"</span>
<a name="l00299"></a>00299 <span class="comment">// ---------------------------------------------------------------------------------</span>
<a name="l00300"></a>00300 this->PF_SLAM_implementation_replaceByNewParticleSet(
<a name="l00301"></a>00301 me->m_particles,
<a name="l00302"></a>00302 newParticles,newParticlesWeight,newParticlesDerivedFromIdx );
<a name="l00303"></a>00303
<a name="l00304"></a>00304 } <span class="comment">// end adaptive sample size</span>
<a name="l00305"></a>00305 }
<a name="l00306"></a>00306
<a name="l00307"></a>00307 <span class="keywordflow">if</span> (sf)
<a name="l00308"></a>00308 {
<a name="l00309"></a>00309 <span class="keyword">const</span> <span class="keywordtype">size_t</span> M = me->m_particles.size();
<a name="l00310"></a>00310 <span class="comment">// UPDATE STAGE</span>
<a name="l00311"></a>00311 <span class="comment">// ----------------------------------------------------------------------</span>
<a name="l00312"></a>00312 <span class="comment">// Compute all the likelihood values & update particles weight:</span>
<a name="l00313"></a>00313 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<M;i++)
<a name="l00314"></a>00314 {
<a name="l00315"></a>00315 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html" title="Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).">TPose3D</a> *partPose= getLastPose(i); <span class="comment">// Take the particle data:</span>
<a name="l00316"></a>00316 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> partPose2 = <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a>(*partPose);
<a name="l00317"></a>00317 <span class="keyword">const</span> <span class="keywordtype">double</span> obs_log_likelihood = PF_SLAM_computeObservationLikelihoodForParticle(PF_options,i,*sf,partPose2);
<a name="l00318"></a>00318 me->m_particles[i].log_w += obs_log_likelihood * PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a6ad1667bac7faa62a2376c83527c0414" title="An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...">powFactor</a>;
<a name="l00319"></a>00319 } <span class="comment">// for each particle "i"</span>
<a name="l00320"></a>00320
<a name="l00321"></a>00321 <span class="comment">// Normalization of weights is done outside of this method automatically.</span>
<a name="l00322"></a>00322 }
<a name="l00323"></a>00323
<a name="l00324"></a>00324 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00325"></a>00325 } <span class="comment">// end of PF_SLAM_implementation_pfStandardProposal</span>
<a name="l00326"></a>00326 <span class="comment"></span>
<a name="l00327"></a>00327 <span class="comment"> /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary particle filter with the standard proposal),</span>
<a name="l00328"></a>00328 <span class="comment"> * common to both localization and mapping.</span>
<a name="l00329"></a>00329 <span class="comment"> *</span>
<a name="l00330"></a>00330 <span class="comment"> * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.</span>
<a name="l00331"></a>00331 <span class="comment"> *</span>
<a name="l00332"></a>00332 <span class="comment"> * This method is described in the paper:</span>
<a name="l00333"></a>00333 <span class="comment"> * Pitt, M.K.; Shephard, N. (1999). "Filtering Via Simulation: Auxiliary Particle Filters".</span>
<a name="l00334"></a>00334 <span class="comment"> * Journal of the American Statistical Association 94 (446): 590–591. doi:10.2307/2670179.</span>
<a name="l00335"></a>00335 <span class="comment"> *</span>
<a name="l00336"></a>00336 <span class="comment"> */</span>
<a name="l00337"></a>00337 <span class="keyword">template</span> <<span class="keyword">class</span> PARTICLE_TYPE,<span class="keyword">class</span> MY<span class="keywordtype">SEL</span>F>
<a name="l00338"></a>00338 <span class="keyword">template</span> <<span class="keyword">class</span> BINTYPE>
<a name="l00339"></a><a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html#a3c73c4b465bd78e4213c25546ccd328f">00339</a> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html" title="A set of common data shared by PF implementations for both SLAM and localization.">PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfAuxiliaryPFStandard</a>(
<a name="l00340"></a>00340 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_c_action_collection.html" title="Declares a class for storing a collection of robot actions.">CActionCollection</a> * actions,
<a name="l00341"></a>00341 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_c_sensory_frame.html" title="Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...">CSensoryFrame</a> * sf,
<a name="l00342"></a>00342 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html" title="The configuration of a particle filter.">CParticleFilter::TParticleFilterOptions</a> &PF_options,
<a name="l00343"></a>00343 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html" title="Option set for KLD algorithm.">TKLDParams</a> &KLD_options)
<a name="l00344"></a>00344 {
<a name="l00345"></a>00345 <span class="comment">// Standard and Optimal AuxiliaryPF actually have a shared implementation body:</span>
<a name="l00346"></a>00346 PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(actions,sf,PF_options,KLD_options, <span class="keyword">false</span> <span class="comment">/*APF*/</span> );
<a name="l00347"></a>00347 }
<a name="l00348"></a>00348
<a name="l00349"></a>00349 <span class="comment">/*---------------------------------------------------------------</span>
<a name="l00350"></a>00350 <span class="comment"> PF_SLAM_particlesEvaluator_AuxPFOptimal</span>
<a name="l00351"></a>00351 <span class="comment"> ---------------------------------------------------------------*/</span>
<a name="l00352"></a>00352 <span class="keyword">template</span> <<span class="keyword">class</span> PARTICLE_TYPE,<span class="keyword">class</span> MY<span class="keywordtype">SEL</span>F>
<a name="l00353"></a>00353 <span class="keyword">template</span> <<span class="keyword">class</span> BINTYPE>
<a name="l00354"></a><a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html#aa6e1f16839702a7119de03deae4d7225">00354</a> <span class="keywordtype">double</span> <a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html" title="A set of common data shared by PF implementations for both SLAM and localization.">PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_particlesEvaluator_AuxPFOptimal</a>(
<a name="l00355"></a>00355 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html" title="The configuration of a particle filter.">CParticleFilter::TParticleFilterOptions</a> &PF_options,
<a name="l00356"></a>00356 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_particle_filter_capable.html" title="This virtual class defines the interface that any particles based PDF class must implement in order t...">CParticleFilterCapable</a> *obj,
<a name="l00357"></a>00357 <span class="keywordtype">size_t</span> index,
<a name="l00358"></a>00358 <span class="keyword">const</span> <span class="keywordtype">void</span> *action,
<a name="l00359"></a>00359 <span class="keyword">const</span> <span class="keywordtype">void</span> *observation )
<a name="l00360"></a>00360 {
<a name="l00361"></a>00361 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00362"></a>00362
<a name="l00363"></a>00363 <span class="comment">//const PF_implementation<PARTICLE_TYPE,MYSELF> *myObj = reinterpret_cast<const PF_implementation<PARTICLE_TYPE,MYSELF>*>( obj );</span>
<a name="l00364"></a>00364 <span class="keyword">const</span> MYSELF *me = <span class="keyword">static_cast<</span><span class="keyword">const </span>MYSELF*<span class="keyword">></span>(obj);
<a name="l00365"></a>00365
<a name="l00366"></a>00366 <span class="comment">// Compute the quantity:</span>
<a name="l00367"></a>00367 <span class="comment">// w[i]·p(zt|z^{t-1},x^{[i],t-1})</span>
<a name="l00368"></a>00368 <span class="comment">// As the Monte-Carlo approximation of the integral over all posible $x_t$.</span>
<a name="l00369"></a>00369 <span class="comment">// --------------------------------------------</span>
<a name="l00370"></a>00370 <span class="keywordtype">double</span> indivLik, maxLik= -1e300;
<a name="l00371"></a>00371 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> maxLikDraw;
<a name="l00372"></a>00372 <span class="keywordtype">size_t</span> N = PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a4054623753f30e03e99e94e85c3ff84d" title="In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...">pfAuxFilterOptimal_MaximumSearchSamples</a>;
<a name="l00373"></a>00373 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(N>1)
<a name="l00374"></a>00374
<a name="l00375"></a>00375 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> oldPose = *me->getLastPose(index);
<a name="l00376"></a>00376 <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_double</a> vectLiks(N,0); <span class="comment">// The vector with the individual log-likelihoods.</span>
<a name="l00377"></a>00377 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> drawnSample;
<a name="l00378"></a>00378 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> q=0;q<N;q++)
<a name="l00379"></a>00379 {
<a name="l00380"></a>00380 me->m_movementDrawer.drawSample(drawnSample);
<a name="l00381"></a>00381 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> x_predict = oldPose + drawnSample;
<a name="l00382"></a>00382
<a name="l00383"></a>00383 <span class="comment">// Estimate the mean...</span>
<a name="l00384"></a>00384 indivLik = me->PF_SLAM_computeObservationLikelihoodForParticle(
<a name="l00385"></a>00385 PF_options,
<a name="l00386"></a>00386 index,
<a name="l00387"></a>00387 *static_cast<const CSensoryFrame*>(observation),
<a name="l00388"></a>00388 x_predict );
<a name="l00389"></a>00389
<a name="l00390"></a>00390 <a class="code" href="mrpt__macros_8h.html#ab76a796755b4c538a2e5b4d15ef9f7e2">MRPT_CHECK_NORMAL_NUMBER</a>(indivLik);
<a name="l00391"></a>00391 vectLiks[q] = indivLik;
<a name="l00392"></a>00392 <span class="keywordflow">if</span> ( indivLik > maxLik )
<a name="l00393"></a>00393 { <span class="comment">// Keep the maximum value:</span>
<a name="l00394"></a>00394 maxLikDraw = drawnSample;
<a name="l00395"></a>00395 maxLik = indivLik;
<a name="l00396"></a>00396 }
<a name="l00397"></a>00397 }
<a name="l00398"></a>00398
<a name="l00399"></a>00399 <span class="comment">// This is done to avoid floating point overflow!!</span>
<a name="l00400"></a>00400 <span class="comment">// average_lik = \sum(e^liks) * e^maxLik / N</span>
<a name="l00401"></a>00401 <span class="comment">// log( average_lik ) = log( \sum(e^liks) ) + maxLik - log( N )</span>
<a name="l00402"></a>00402 <span class="keywordtype">double</span> avrgLogLik = <a class="code" href="group__stats__grp.html#ga6cb458d9d6c45fae59084fc57e88666b" title="A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...">math::averageLogLikelihood</a>( vectLiks );
<a name="l00403"></a>00403
<a name="l00404"></a>00404 <span class="comment">// Save into the object:</span>
<a name="l00405"></a>00405 me->m_pfAuxiliaryPFOptimal_estimatedProb[index] = avrgLogLik; <span class="comment">// log( accum / N );</span>
<a name="l00406"></a>00406 me->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
<a name="l00407"></a>00407
<a name="l00408"></a>00408 <span class="keywordflow">if</span> (PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a99406b39b2963dce127a86634f13871f" title="(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true...">pfAuxFilterOptimal_MLE</a>)
<a name="l00409"></a>00409 me->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] = maxLikDraw;
<a name="l00410"></a>00410
<a name="l00411"></a>00411 <span class="comment">// and compute the resulting probability of this particle:</span>
<a name="l00412"></a>00412 <span class="comment">// ------------------------------------------------------------</span>
<a name="l00413"></a>00413 <span class="keywordflow">return</span> me->m_particles[index].log_w + me->m_pfAuxiliaryPFOptimal_estimatedProb[index];
<a name="l00414"></a>00414
<a name="l00415"></a>00415 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00416"></a>00416 } <span class="comment">// end of PF_SLAM_particlesEvaluator_AuxPFOptimal</span>
<a name="l00417"></a>00417
<a name="l00418"></a>00418 <span class="comment"></span>
<a name="l00419"></a>00419 <span class="comment"> /** Compute w[i]·p(z_t | mu_t^i), with mu_t^i being</span>
<a name="l00420"></a>00420 <span class="comment"> * the mean of the new robot pose</span>
<a name="l00421"></a>00421 <span class="comment"> *</span>
<a name="l00422"></a>00422 <span class="comment"> * \param action MUST be a "const CPose3D*"</span>
<a name="l00423"></a>00423 <span class="comment"> * \param observation MUST be a "const CSensoryFrame*"</span>
<a name="l00424"></a>00424 <span class="comment"> */</span>
<a name="l00425"></a>00425 <span class="keyword">template</span> <<span class="keyword">class</span> PARTICLE_TYPE,<span class="keyword">class</span> MY<span class="keywordtype">SEL</span>F>
<a name="l00426"></a>00426 <span class="keyword">template</span> <<span class="keyword">class</span> BINTYPE>
<a name="l00427"></a><a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html#ae92af2f05313e99e6137a78724f198f9">00427</a> <span class="keywordtype">double</span> <a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html" title="A set of common data shared by PF implementations for both SLAM and localization.">PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_particlesEvaluator_AuxPFStandard</a>(
<a name="l00428"></a>00428 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html" title="The configuration of a particle filter.">CParticleFilter::TParticleFilterOptions</a> &PF_options,
<a name="l00429"></a>00429 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1bayes_1_1_c_particle_filter_capable.html" title="This virtual class defines the interface that any particles based PDF class must implement in order t...">CParticleFilterCapable</a> *obj,
<a name="l00430"></a>00430 <span class="keywordtype">size_t</span> index,
<a name="l00431"></a>00431 <span class="keyword">const</span> <span class="keywordtype">void</span> *action,
<a name="l00432"></a>00432 <span class="keyword">const</span> <span class="keywordtype">void</span> *observation )
<a name="l00433"></a>00433 {
<a name="l00434"></a>00434 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00435"></a>00435
<a name="l00436"></a>00436 <span class="comment">//const PF_implementation<PARTICLE_TYPE,MYSELF> *myObj = reinterpret_cast<const PF_implementation<PARTICLE_TYPE,MYSELF>*>( obj );</span>
<a name="l00437"></a>00437 <span class="keyword">const</span> MYSELF *myObj = <span class="keyword">static_cast<</span><span class="keyword">const </span>MYSELF*<span class="keyword">></span>(obj);
<a name="l00438"></a>00438
<a name="l00439"></a>00439 <span class="comment">// Take the previous particle weight:</span>
<a name="l00440"></a>00440 <span class="keyword">const</span> <span class="keywordtype">double</span> cur_logweight = myObj->m_particles[index].log_w;
<a name="l00441"></a>00441 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> oldPose = *myObj->getLastPose(index);
<a name="l00442"></a>00442
<a name="l00443"></a>00443 <span class="keywordflow">if</span> (!PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a16a5d97f9246477c32e742cc23e20a74" title="Only for PF_algorithm==pfAuxiliaryPFStandard: If false, the APF will predict the first stage weights ...">pfAuxFilterStandard_FirstStageWeightsMonteCarlo</a>)
<a name="l00444"></a>00444 {
<a name="l00445"></a>00445 <span class="comment">// Just use the mean:</span>
<a name="l00446"></a>00446 <span class="comment">// , take the mean of the posterior density:</span>
<a name="l00447"></a>00447 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> x_predict;
<a name="l00448"></a>00448 x_predict.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#a88f72fa352303454295616213fc10c2d" title="Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...">composeFrom</a>( oldPose, *static_cast<const CPose3D*>(action) );
<a name="l00449"></a>00449
<a name="l00450"></a>00450 <span class="comment">// and compute the obs. likelihood:</span>
<a name="l00451"></a>00451 <span class="comment">// --------------------------------------------</span>
<a name="l00452"></a>00452 myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
<a name="l00453"></a>00453 PF_options, index,
<a name="l00454"></a>00454 *static_cast<const CSensoryFrame*>(observation), x_predict );
<a name="l00455"></a>00455
<a name="l00456"></a>00456 <span class="comment">// Combined log_likelihood: Previous weight * obs_likelihood:</span>
<a name="l00457"></a>00457 <span class="keywordflow">return</span> cur_logweight + myObj->m_pfAuxiliaryPFStandard_estimatedProb[index];
<a name="l00458"></a>00458 }
<a name="l00459"></a>00459 <span class="keywordflow">else</span>
<a name="l00460"></a>00460 {
<a name="l00461"></a>00461 <span class="comment">// Do something similar to in Optimal sampling:</span>
<a name="l00462"></a>00462 <span class="comment">// Compute the quantity:</span>
<a name="l00463"></a>00463 <span class="comment">// w[i]·p(zt|z^{t-1},x^{[i],t-1})</span>
<a name="l00464"></a>00464 <span class="comment">// As the Monte-Carlo approximation of the integral over all posible $x_t$.</span>
<a name="l00465"></a>00465 <span class="comment">// --------------------------------------------</span>
<a name="l00466"></a>00466 <span class="keywordtype">double</span> indivLik, maxLik= -1e300;
<a name="l00467"></a>00467 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> maxLikDraw;
<a name="l00468"></a>00468 <span class="keywordtype">size_t</span> N = PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a4054623753f30e03e99e94e85c3ff84d" title="In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...">pfAuxFilterOptimal_MaximumSearchSamples</a>;
<a name="l00469"></a>00469 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(N>1)
<a name="l00470"></a>00470
<a name="l00471"></a>00471 <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_double</a> vectLiks(N,0); <span class="comment">// The vector with the individual log-likelihoods.</span>
<a name="l00472"></a>00472 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> drawnSample;
<a name="l00473"></a>00473 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> q=0;q<N;q++)
<a name="l00474"></a>00474 {
<a name="l00475"></a>00475 myObj->m_movementDrawer.drawSample(drawnSample);
<a name="l00476"></a>00476 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> x_predict = oldPose + drawnSample;
<a name="l00477"></a>00477
<a name="l00478"></a>00478 <span class="comment">// Estimate the mean...</span>
<a name="l00479"></a>00479 indivLik = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
<a name="l00480"></a>00480 PF_options,
<a name="l00481"></a>00481 index,
<a name="l00482"></a>00482 *static_cast<const CSensoryFrame*>(observation),
<a name="l00483"></a>00483 x_predict );
<a name="l00484"></a>00484
<a name="l00485"></a>00485 <a class="code" href="mrpt__macros_8h.html#ab76a796755b4c538a2e5b4d15ef9f7e2">MRPT_CHECK_NORMAL_NUMBER</a>(indivLik);
<a name="l00486"></a>00486 vectLiks[q] = indivLik;
<a name="l00487"></a>00487 <span class="keywordflow">if</span> ( indivLik > maxLik )
<a name="l00488"></a>00488 { <span class="comment">// Keep the maximum value:</span>
<a name="l00489"></a>00489 maxLikDraw = drawnSample;
<a name="l00490"></a>00490 maxLik = indivLik;
<a name="l00491"></a>00491 }
<a name="l00492"></a>00492 }
<a name="l00493"></a>00493
<a name="l00494"></a>00494 <span class="comment">// This is done to avoid floating point overflow!!</span>
<a name="l00495"></a>00495 <span class="comment">// average_lik = \sum(e^liks) * e^maxLik / N</span>
<a name="l00496"></a>00496 <span class="comment">// log( average_lik ) = log( \sum(e^liks) ) + maxLik - log( N )</span>
<a name="l00497"></a>00497 <span class="keywordtype">double</span> avrgLogLik = <a class="code" href="group__stats__grp.html#ga6cb458d9d6c45fae59084fc57e88666b" title="A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...">math::averageLogLikelihood</a>( vectLiks );
<a name="l00498"></a>00498
<a name="l00499"></a>00499 <span class="comment">// Save into the object:</span>
<a name="l00500"></a>00500 myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] = avrgLogLik; <span class="comment">// log( accum / N );</span>
<a name="l00501"></a>00501
<a name="l00502"></a>00502 myObj->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
<a name="l00503"></a>00503 <span class="keywordflow">if</span> (PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a99406b39b2963dce127a86634f13871f" title="(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true...">pfAuxFilterOptimal_MLE</a>)
<a name="l00504"></a>00504 myObj->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] = maxLikDraw;
<a name="l00505"></a>00505
<a name="l00506"></a>00506 <span class="comment">// and compute the resulting probability of this particle:</span>
<a name="l00507"></a>00507 <span class="comment">// ------------------------------------------------------------</span>
<a name="l00508"></a>00508 <span class="keywordflow">return</span> cur_logweight + myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
<a name="l00509"></a>00509 }
<a name="l00510"></a>00510 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00511"></a>00511 }
<a name="l00512"></a>00512
<a name="l00513"></a>00513 <span class="comment">// USE_OPTIMAL_SAMPLING:</span>
<a name="l00514"></a>00514 <span class="comment">// true -> PF_SLAM_implementation_pfAuxiliaryPFOptimal</span>
<a name="l00515"></a>00515 <span class="comment">// false -> PF_SLAM_implementation_pfAuxiliaryPFStandard</span>
<a name="l00516"></a>00516 <span class="keyword">template</span> <<span class="keyword">class</span> PARTICLE_TYPE,<span class="keyword">class</span> MY<span class="keywordtype">SEL</span>F>
<a name="l00517"></a>00517 <span class="keyword">template</span> <<span class="keyword">class</span> BINTYPE>
<a name="l00518"></a><a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html#a2b357386ceaa7967b124b6481edee37a">00518</a> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html" title="A set of common data shared by PF implementations for both SLAM and localization.">PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal</a>(
<a name="l00519"></a>00519 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_c_action_collection.html" title="Declares a class for storing a collection of robot actions.">CActionCollection</a> * actions,
<a name="l00520"></a>00520 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_c_sensory_frame.html" title="Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...">CSensoryFrame</a> * sf,
<a name="l00521"></a>00521 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html" title="The configuration of a particle filter.">CParticleFilter::TParticleFilterOptions</a> &PF_options,
<a name="l00522"></a>00522 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html" title="Option set for KLD algorithm.">TKLDParams</a> &KLD_options,
<a name="l00523"></a>00523 <span class="keyword">const</span> <span class="keywordtype">bool</span> USE_OPTIMAL_SAMPLING )
<a name="l00524"></a>00524 {
<a name="l00525"></a>00525 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00526"></a>00526 <span class="keyword">typedef</span> std<a class="code" href="classstd_1_1set.html" title="STL class.">::set<BINTYPE,typename BINTYPE::lt_operator></a> TSetStateSpaceBins;
<a name="l00527"></a>00527
<a name="l00528"></a>00528 MYSELF *me = <span class="keyword">static_cast<</span>MYSELF*<span class="keyword">></span>(<span class="keyword">this</span>);
<a name="l00529"></a>00529
<a name="l00530"></a>00530 <span class="keyword">const</span> <span class="keywordtype">size_t</span> M = me->m_particles.size();
<a name="l00531"></a>00531
<a name="l00532"></a>00532 <span class="comment">// ----------------------------------------------------------------------</span>
<a name="l00533"></a>00533 <span class="comment">// We can execute optimal PF only when we have both, an action, and</span>
<a name="l00534"></a>00534 <span class="comment">// a valid observation from which to compute the likelihood:</span>
<a name="l00535"></a>00535 <span class="comment">// Accumulate odometry/actions until we have a valid observation, then</span>
<a name="l00536"></a>00536 <span class="comment">// process them simultaneously.</span>
<a name="l00537"></a>00537 <span class="comment">// ----------------------------------------------------------------------</span>
<a name="l00538"></a>00538 <span class="keywordflow">if</span> (!PF_SLAM_implementation_gatherActionsCheckBothActObs<BINTYPE>(actions,sf))
<a name="l00539"></a>00539 <span class="keywordflow">return</span>; <span class="comment">// Nothing we can do here...</span>
<a name="l00540"></a>00540 <span class="comment">// OK, we have m_movementDrawer loaded and observations...let's roll!</span>
<a name="l00541"></a>00541
<a name="l00542"></a>00542
<a name="l00543"></a>00543 <span class="comment">// -------------------------------------------------------------------------------</span>
<a name="l00544"></a>00544 <span class="comment">// 0) Common part: Prepare m_particles "draw" and compute "fastDrawSample"</span>
<a name="l00545"></a>00545 <span class="comment">// -------------------------------------------------------------------------------</span>
<a name="l00546"></a>00546 <span class="comment">// We need the (aproximate) maximum likelihood value for each</span>
<a name="l00547"></a>00547 <span class="comment">// previous particle [i]:</span>
<a name="l00548"></a>00548 <span class="comment">// max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }</span>
<a name="l00549"></a>00549 <span class="comment">//</span>
<a name="l00550"></a>00550
<a name="l00551"></a>00551 m_pfAuxiliaryPFOptimal_maxLikelihood.assign(M, <a class="code" href="_c_particle_filter_capable_8h.html#acf80e35e34b1c4db82ffb680dc74227e">INVALID_LIKELIHOOD_VALUE</a>);
<a name="l00552"></a>00552 m_pfAuxiliaryPFOptimal_maxLikDrawnMovement.resize(M);
<a name="l00553"></a>00553 <span class="comment">// if (USE_OPTIMAL_SAMPLING)</span>
<a name="l00554"></a>00554 m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
<a name="l00555"></a>00555 <span class="comment">// else</span>
<a name="l00556"></a>00556 m_pfAuxiliaryPFStandard_estimatedProb.resize(M);
<a name="l00557"></a>00557
<a name="l00558"></a>00558 <span class="comment">// Pass the "mean" robot movement to the "weights" computing function:</span>
<a name="l00559"></a>00559 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> meanRobotMovement;
<a name="l00560"></a>00560 m_movementDrawer.getSamplingMean3D(meanRobotMovement);
<a name="l00561"></a>00561
<a name="l00562"></a>00562 <span class="comment">// Prepare data for executing "fastDrawSample"</span>
<a name="l00563"></a>00563 <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html" title="A set of common data shared by PF implementations for both SLAM and localization.">PF_implementation<PARTICLE_TYPE,MYSELF></a> TMyClass; <span class="comment">// Use this longer declaration to avoid errors in old GCC.</span>
<a name="l00564"></a>00564 <a class="code" href="classmrpt_1_1bayes_1_1_c_particle_filter_capable.html" title="This virtual class defines the interface that any particles based PDF class must implement in order t...">CParticleFilterCapable</a>::TParticleProbabilityEvaluator funcOpt = &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFOptimal<BINTYPE>;
<a name="l00565"></a>00565 <a class="code" href="classmrpt_1_1bayes_1_1_c_particle_filter_capable.html" title="This virtual class defines the interface that any particles based PDF class must implement in order t...">CParticleFilterCapable</a>::TParticleProbabilityEvaluator funcStd = &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFStandard<BINTYPE>;
<a name="l00566"></a>00566
<a name="l00567"></a>00567 me-><a class="code" href="classmrpt_1_1bayes_1_1_c_particle_filter_capable.html#a02d7ff724a4b73d449a0f5dc28216e63" title="Prepares data structures for calling fastDrawSample method next.">prepareFastDrawSample</a>(
<a name="l00568"></a>00568 PF_options,
<a name="l00569"></a>00569 USE_OPTIMAL_SAMPLING ? funcOpt : funcStd,
<a name="l00570"></a>00570 &meanRobotMovement,
<a name="l00571"></a>00571 sf );
<a name="l00572"></a>00572
<a name="l00573"></a>00573 <span class="comment">// For USE_OPTIMAL_SAMPLING=1, m_pfAuxiliaryPFOptimal_maxLikelihood is now computed.</span>
<a name="l00574"></a>00574
<a name="l00575"></a>00575 <span class="keywordflow">if</span> (USE_OPTIMAL_SAMPLING && PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a33e80606a59d313f3338f71367010957" title="Enable extra messages for each PF iteration (Default=false)">verbose</a>)
<a name="l00576"></a>00576 {
<a name="l00577"></a>00577 printf(<span class="stringliteral">"[prepareFastDrawSample] max (log) = %10.06f\n"</span>, <a class="code" href="namespacemrpt_1_1math.html#adeb0bd3138c2f716e4b0317cacfc46a1">math::maximum</a>(m_pfAuxiliaryPFOptimal_estimatedProb) );
<a name="l00578"></a>00578 printf(<span class="stringliteral">"[prepareFastDrawSample] max-mean (log) = %10.06f\n"</span>, -<a class="code" href="namespacemrpt_1_1math.html#a414456e3e3b75b19cfda5e0a37c46e31" title="Computes the mean value of a vector.">math::mean</a>(m_pfAuxiliaryPFOptimal_estimatedProb) + <a class="code" href="namespacemrpt_1_1math.html#adeb0bd3138c2f716e4b0317cacfc46a1">math::maximum</a>(m_pfAuxiliaryPFOptimal_estimatedProb) );
<a name="l00579"></a>00579 printf(<span class="stringliteral">"[prepareFastDrawSample] max-min (log) = %10.06f\n"</span>, -<a class="code" href="namespacemrpt_1_1math.html#a2aad5c4db5fdafc3f8c83f7b1adcbfda">math::minimum</a>(m_pfAuxiliaryPFOptimal_estimatedProb) + <a class="code" href="namespacemrpt_1_1math.html#adeb0bd3138c2f716e4b0317cacfc46a1">math::maximum</a>(m_pfAuxiliaryPFOptimal_estimatedProb) );
<a name="l00580"></a>00580 }
<a name="l00581"></a>00581
<a name="l00582"></a>00582 <span class="comment">// Now we have the vector "m_fastDrawProbability" filled out with:</span>
<a name="l00583"></a>00583 <span class="comment">// w[i]·p(zt|z^{t-1},x^{[i],t-1},X)</span>
<a name="l00584"></a>00584 <span class="comment">// where,</span>
<a name="l00585"></a>00585 <span class="comment">//</span>
<a name="l00586"></a>00586 <span class="comment">// =========== For USE_OPTIMAL_SAMPLING = true ====================</span>
<a name="l00587"></a>00587 <span class="comment">// X is the robot pose prior (as implemented in</span>
<a name="l00588"></a>00588 <span class="comment">// the aux. function "PF_SLAM_particlesEvaluator_AuxPFOptimal"),</span>
<a name="l00589"></a>00589 <span class="comment">// and also the "m_pfAuxiliaryPFOptimal_maxLikelihood" filled with the maximum lik. values.</span>
<a name="l00590"></a>00590 <span class="comment">//</span>
<a name="l00591"></a>00591 <span class="comment">// =========== For USE_OPTIMAL_SAMPLING = false ====================</span>
<a name="l00592"></a>00592 <span class="comment">// X is a single point close to the mean of the robot pose prior (as implemented in</span>
<a name="l00593"></a>00593 <span class="comment">// the aux. function "PF_SLAM_particlesEvaluator_AuxPFStandard").</span>
<a name="l00594"></a>00594 <span class="comment">//</span>
<a name="l00595"></a>00595 <a class="code" href="classstd_1_1vector.html">vector<TPose3D></a> newParticles;
<a name="l00596"></a>00596 <a class="code" href="classstd_1_1vector.html">vector<double></a> newParticlesWeight;
<a name="l00597"></a>00597 <a class="code" href="classstd_1_1vector.html">vector<size_t></a> newParticlesDerivedFromIdx;
<a name="l00598"></a>00598
<a name="l00599"></a>00599 <span class="comment">// We need the (aproximate) maximum likelihood value for each</span>
<a name="l00600"></a>00600 <span class="comment">// previous particle [i]:</span>
<a name="l00601"></a>00601 <span class="comment">//</span>
<a name="l00602"></a>00602 <span class="comment">// max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }</span>
<a name="l00603"></a>00603 <span class="comment">//</span>
<a name="l00604"></a>00604 <span class="keywordflow">if</span> (PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a99406b39b2963dce127a86634f13871f" title="(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true...">pfAuxFilterOptimal_MLE</a>)
<a name="l00605"></a>00605 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed.assign(M, <span class="keyword">false</span>);
<a name="l00606"></a>00606
<a name="l00607"></a>00607 <span class="keyword">const</span> <span class="keywordtype">double</span> maxMeanLik = <a class="code" href="eigen__plugins_8h.html#a29bcf29c0462f07b71b310f5cdf01cb5" title="[VECTORS OR MATRICES] Finds the maximum value On an empty input container">math::maximum</a>(
<a name="l00608"></a>00608 USE_OPTIMAL_SAMPLING ?
<a name="l00609"></a>00609 m_pfAuxiliaryPFOptimal_estimatedProb :
<a name="l00610"></a>00610 m_pfAuxiliaryPFStandard_estimatedProb );
<a name="l00611"></a>00611
<a name="l00612"></a>00612 <span class="keywordflow">if</span> ( !PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#ae1373ee71fb2a0131f448042291d85f0" title="A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...">adaptiveSampleSize</a> )
<a name="l00613"></a>00613 {
<a name="l00614"></a>00614 <span class="comment">// ----------------------------------------------------------------------</span>
<a name="l00615"></a>00615 <span class="comment">// 1) FIXED SAMPLE SIZE VERSION</span>
<a name="l00616"></a>00616 <span class="comment">// ----------------------------------------------------------------------</span>
<a name="l00617"></a>00617 newParticles.resize(M);
<a name="l00618"></a>00618 newParticlesWeight.resize(M);
<a name="l00619"></a>00619 newParticlesDerivedFromIdx.resize(M);
<a name="l00620"></a>00620
<a name="l00621"></a>00621 <span class="keyword">const</span> <span class="keywordtype">bool</span> doResample = me->ESS() < PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#ac905494cf568dd8291428306661acaf2" title="The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0...">BETA</a>;
<a name="l00622"></a>00622
<a name="l00623"></a>00623 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<M;i++)
<a name="l00624"></a>00624 {
<a name="l00625"></a>00625 <span class="keywordtype">size_t</span> k;
<a name="l00626"></a>00626
<a name="l00627"></a>00627 <span class="comment">// Generate a new particle:</span>
<a name="l00628"></a>00628 <span class="comment">// (a) Draw a "t-1" m_particles' index:</span>
<a name="l00629"></a>00629 <span class="comment">// ----------------------------------------------------------------</span>
<a name="l00630"></a>00630 <span class="keywordflow">if</span> (doResample)
<a name="l00631"></a>00631 k = me->fastDrawSample(PF_options); <span class="comment">// Based on weights of last step only!</span>
<a name="l00632"></a>00632 <span class="keywordflow">else</span> k = i;
<a name="l00633"></a>00633
<a name="l00634"></a>00634 <span class="comment">// Do one rejection sampling step:</span>
<a name="l00635"></a>00635 <span class="comment">// ---------------------------------------------</span>
<a name="l00636"></a>00636 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> newPose;
<a name="l00637"></a>00637 <span class="keywordtype">double</span> newParticleLogWeight;
<a name="l00638"></a>00638 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
<a name="l00639"></a>00639 USE_OPTIMAL_SAMPLING,doResample,maxMeanLik,
<a name="l00640"></a>00640 k,
<a name="l00641"></a>00641 sf,PF_options,
<a name="l00642"></a>00642 newPose, newParticleLogWeight);
<a name="l00643"></a>00643
<a name="l00644"></a>00644 <span class="comment">// Insert the new particle</span>
<a name="l00645"></a>00645 newParticles[i] = newPose;
<a name="l00646"></a>00646 newParticlesDerivedFromIdx[i] = k;
<a name="l00647"></a>00647 newParticlesWeight[i] = newParticleLogWeight;
<a name="l00648"></a>00648
<a name="l00649"></a>00649 } <span class="comment">// for i</span>
<a name="l00650"></a>00650 } <span class="comment">// end fixed sample size</span>
<a name="l00651"></a>00651 <span class="keywordflow">else</span>
<a name="l00652"></a>00652 {
<a name="l00653"></a>00653 <span class="comment">// -------------------------------------------------------------------------------------------------</span>
<a name="l00654"></a>00654 <span class="comment">// 2) ADAPTIVE SAMPLE SIZE VERSION</span>
<a name="l00655"></a>00655 <span class="comment">//</span>
<a name="l00656"></a>00656 <span class="comment">// Implementation of Dieter Fox's KLD algorithm</span>
<a name="l00657"></a>00657 <span class="comment">// JLBC (3/OCT/2006)</span>
<a name="l00658"></a>00658 <span class="comment">// -------------------------------------------------------------------------------------------------</span>
<a name="l00659"></a>00659 <span class="comment">// The new particle set:</span>
<a name="l00660"></a>00660 newParticles.clear();
<a name="l00661"></a>00661 newParticlesWeight.clear();
<a name="l00662"></a>00662 newParticlesDerivedFromIdx.clear();
<a name="l00663"></a>00663
<a name="l00664"></a>00664 <span class="comment">// ------------------------------------------------------------------------------</span>
<a name="l00665"></a>00665 <span class="comment">// 2.1) PRELIMINARY STAGE: Build a list of pairs<TPathBin,vector_uint> with the</span>
<a name="l00666"></a>00666 <span class="comment">// indexes of m_particles that fall into each multi-dimensional-path bins</span>
<a name="l00667"></a>00667 <span class="comment">// //The bins will be saved into "stateSpaceBinsLastTimestep", and the list</span>
<a name="l00668"></a>00668 <span class="comment">// //of corresponding m_particles (in the last timestep), in "stateSpaceBinsLastTimestepParticles"</span>
<a name="l00669"></a>00669 <span class="comment">// - Added JLBC (01/DEC/2006)</span>
<a name="l00670"></a>00670 <span class="comment">// ------------------------------------------------------------------------------</span>
<a name="l00671"></a>00671 TSetStateSpaceBins stateSpaceBinsLastTimestep;
<a name="l00672"></a>00672 std<a class="code" href="classstd_1_1vector.html">::vector<vector_uint></a> stateSpaceBinsLastTimestepParticles;
<a name="l00673"></a>00673 <span class="keyword">typename</span> MYSELF<a class="code" href="eigen__plugins_8h.html#a39c5d6430ea9395ae7ae729dd0c3f18c">::CParticleList::iterator</a> partIt;
<a name="l00674"></a>00674 <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> partIndex;
<a name="l00675"></a>00675
<a name="l00676"></a>00676 printf( <span class="stringliteral">"[FIXED_SAMPLING] Computing..."</span>);
<a name="l00677"></a>00677 <span class="keywordflow">for</span> (partIt = me->m_particles.begin(),partIndex=0; partIt!=me->m_particles.end(); ++partIt,++partIndex)
<a name="l00678"></a>00678 {
<a name="l00679"></a>00679 <span class="comment">// Load the bin from the path data:</span>
<a name="l00680"></a>00680 BINTYPE p;
<a name="l00681"></a>00681 KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>(p, KLD_options,partIt->d );
<a name="l00682"></a>00682
<a name="l00683"></a>00683 <span class="comment">// Is it a new bin?</span>
<a name="l00684"></a>00684 <span class="keyword">typename</span> TSetStateSpaceBins<a class="code" href="eigen__plugins_8h.html#a39c5d6430ea9395ae7ae729dd0c3f18c">::iterator</a> posFound=stateSpaceBinsLastTimestep.find(p);
<a name="l00685"></a>00685 <span class="keywordflow">if</span> ( posFound == stateSpaceBinsLastTimestep.end() )
<a name="l00686"></a>00686 { <span class="comment">// Yes, create a new pair <bin,index_list> in the list:</span>
<a name="l00687"></a>00687 stateSpaceBinsLastTimestep.insert( p );
<a name="l00688"></a>00688 stateSpaceBinsLastTimestepParticles.push_back( <a class="code" href="namespacemrpt.html#aa4dc027dbc3804c847a78892a2982047">vector_uint</a>(1,partIndex) );
<a name="l00689"></a>00689 }
<a name="l00690"></a>00690 <span class="keywordflow">else</span>
<a name="l00691"></a>00691 { <span class="comment">// No, add the particle's index to the existing entry:</span>
<a name="l00692"></a>00692 <span class="keyword">const</span> <span class="keywordtype">size_t</span> idx = <a class="code" href="group__geometry__grp.html#ga8c0a76e906f12560cfa49fcd269c8398" title="Gets the distance between two points in a 2D space.">std::distance</a>(stateSpaceBinsLastTimestep.begin(),posFound);
<a name="l00693"></a>00693 stateSpaceBinsLastTimestepParticles[idx].push_back( partIndex );
<a name="l00694"></a>00694 }
<a name="l00695"></a>00695 }
<a name="l00696"></a>00696 printf( <span class="stringliteral">"done (%u bins in t-1)\n"</span>,(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span>)stateSpaceBinsLastTimestep.size());
<a name="l00697"></a>00697
<a name="l00698"></a>00698 <span class="comment">// ------------------------------------------------------------------------------</span>
<a name="l00699"></a>00699 <span class="comment">// 2.2) THE MAIN KLD-BASED DRAW SAMPLING LOOP</span>
<a name="l00700"></a>00700 <span class="comment">// ------------------------------------------------------------------------------</span>
<a name="l00701"></a>00701 <span class="keywordtype">double</span> delta_1 = 1.0 - KLD_options.<a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html#a787e1c15d3653be9638c8e41d786a191">KLD_delta</a>;
<a name="l00702"></a>00702 <span class="keywordtype">double</span> epsilon_1 = 0.5 / KLD_options.<a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html#a92daf8859b1e75f467c23e37d280e466">KLD_epsilon</a>;
<a name="l00703"></a>00703 <span class="keywordtype">bool</span> doResample = me->ESS() < 0.5;
<a name="l00704"></a>00704 <span class="comment">//double maxLik = math::maximum(m_pfAuxiliaryPFOptimal_maxLikelihood); // For normalization purposes only</span>
<a name="l00705"></a>00705
<a name="l00706"></a>00706 <span class="comment">// The desired dynamic number of m_particles (to be modified dynamically below):</span>
<a name="l00707"></a>00707 <span class="keyword">const</span> <span class="keywordtype">size_t</span> minNumSamples_KLD = max((<span class="keywordtype">size_t</span>)KLD_options.<a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html#a3d919d633f69c9daefd18be697d0dc71" title="Parameters for the KLD adaptive sample size algorithm (see Dieter Fox's papers), which is used only i...">KLD_minSampleSize</a>, (<span class="keywordtype">size_t</span>)<a class="code" href="namespacemrpt_1_1utils.html#ab7d9cdf7d271c2f41fc1c5c9fa7d0828" title="Returns the closer integer (int) to x.">round</a>(KLD_options.<a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html#afa336d11d377a93c23130caef4d3a4b2" title="(Default: KLD_minSamplesPerBin=0) The minimum number of samples will be the maximum of KLD_minSampleS...">KLD_minSamplesPerBin</a>*stateSpaceBinsLastTimestep.size()) );
<a name="l00708"></a>00708 <span class="keywordtype">size_t</span> Nx = minNumSamples_KLD ;
<a name="l00709"></a>00709
<a name="l00710"></a>00710 <span class="keyword">const</span> <span class="keywordtype">size_t</span> Np1 = me->m_particles.size();
<a name="l00711"></a>00711 <a class="code" href="classstd_1_1vector.html">vector_size_t</a> oldPartIdxsStillNotPropragated(Np1); <span class="comment">// Use a list since we'll use "erase" a lot here.</span>
<a name="l00712"></a>00712 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<Np1;k++) oldPartIdxsStillNotPropragated[k]=k; <span class="comment">//.push_back(k);</span>
<a name="l00713"></a>00713
<a name="l00714"></a>00714 <span class="keyword">const</span> <span class="keywordtype">size_t</span> Np = stateSpaceBinsLastTimestepParticles.size();
<a name="l00715"></a>00715 <a class="code" href="classstd_1_1vector.html">vector_size_t</a> permutationPathsAuxVector(Np);
<a name="l00716"></a>00716 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<Np;k++) permutationPathsAuxVector[k]=k;
<a name="l00717"></a>00717
<a name="l00718"></a>00718 <span class="comment">// Instead of picking randomly from "permutationPathsAuxVector", we can shuffle it now just once,</span>
<a name="l00719"></a>00719 <span class="comment">// then pick in sequence from the tail and resize the container:</span>
<a name="l00720"></a>00720 std::random_shuffle(
<a name="l00721"></a>00721 permutationPathsAuxVector.begin(),
<a name="l00722"></a>00722 permutationPathsAuxVector.end(),
<a name="l00723"></a>00723 mrpt<a class="code" href="namespacemrpt_1_1random.html#a4b8c2d5323417f5760fb2ed586b07d0c" title="A random number generator for usage in STL algorithms expecting a function like this (eg...">::random::random_generator_for_STL</a> );
<a name="l00724"></a>00724
<a name="l00725"></a>00725 <span class="keywordtype">size_t</span> k = 0;
<a name="l00726"></a>00726 <span class="keywordtype">size_t</span> N = 0;
<a name="l00727"></a>00727
<a name="l00728"></a>00728 TSetStateSpaceBins stateSpaceBins;
<a name="l00729"></a>00729
<a name="l00730"></a>00730 <span class="keywordflow">do</span> <span class="comment">// "N" is the index of the current "new particle":</span>
<a name="l00731"></a>00731 {
<a name="l00732"></a>00732 <span class="comment">// Generate a new particle:</span>
<a name="l00733"></a>00733 <span class="comment">//</span>
<a name="l00734"></a>00734 <span class="comment">// (a) Propagate the last set of m_particles, and only if the</span>
<a name="l00735"></a>00735 <span class="comment">// desired number of m_particles in this step is larger,</span>
<a name="l00736"></a>00736 <span class="comment">// perform a UNIFORM sampling from the last set. In this way</span>
<a name="l00737"></a>00737 <span class="comment">// the new weights can be computed in the same way for all m_particles.</span>
<a name="l00738"></a>00738 <span class="comment">// ---------------------------------------------------------------------------</span>
<a name="l00739"></a>00739 <span class="keywordflow">if</span> (doResample)
<a name="l00740"></a>00740 {
<a name="l00741"></a>00741 k = me->fastDrawSample(PF_options); <span class="comment">// Based on weights of last step only!</span>
<a name="l00742"></a>00742 }
<a name="l00743"></a>00743 <span class="keywordflow">else</span>
<a name="l00744"></a>00744 {
<a name="l00745"></a>00745 <span class="comment">// Assure that at least one particle per "discrete-path" is taken (if the</span>
<a name="l00746"></a>00746 <span class="comment">// number of samples allows it)</span>
<a name="l00747"></a>00747 <span class="keywordflow">if</span> (permutationPathsAuxVector.size())
<a name="l00748"></a>00748 {
<a name="l00749"></a>00749 <span class="keyword">const</span> <span class="keywordtype">size_t</span> idxBinSpacePath = *permutationPathsAuxVector.rbegin();
<a name="l00750"></a>00750 permutationPathsAuxVector.resize(permutationPathsAuxVector.size()-1);
<a name="l00751"></a>00751
<a name="l00752"></a>00752 <span class="keyword">const</span> <span class="keywordtype">size_t</span> idx = <a class="code" href="namespacemrpt_1_1random.html#a4743bfa8fcb282b6f5d66395ccabaa73" title="A static instance of a CRandomGenerator class, for use in single-thread applications.">randomGenerator</a>.<a class="code" href="classmrpt_1_1random_1_1_c_random_generator.html#aede40e77053b8b4350fd786f1683e260" title="Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, in the whole range of 32-bit integers.">drawUniform32bit</a>() % stateSpaceBinsLastTimestepParticles[idxBinSpacePath].size();
<a name="l00753"></a>00753 k = stateSpaceBinsLastTimestepParticles[idxBinSpacePath][idx];
<a name="l00754"></a>00754 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(k<me->m_particles.size());
<a name="l00755"></a>00755
<a name="l00756"></a>00756 <span class="comment">// Also erase it from the other permutation vector list:</span>
<a name="l00757"></a>00757 oldPartIdxsStillNotPropragated.erase(std::find(oldPartIdxsStillNotPropragated.begin(),oldPartIdxsStillNotPropragated.end(),k));
<a name="l00758"></a>00758 }
<a name="l00759"></a>00759 <span class="keywordflow">else</span>
<a name="l00760"></a>00760 {
<a name="l00761"></a>00761 <span class="comment">// Select a particle from the previous set with a UNIFORM distribution,</span>
<a name="l00762"></a>00762 <span class="comment">// in such a way we will assign each particle the updated weight accounting</span>
<a name="l00763"></a>00763 <span class="comment">// for its last weight.</span>
<a name="l00764"></a>00764 <span class="comment">// The first "old_N" m_particles will be drawn using a uniform random selection</span>
<a name="l00765"></a>00765 <span class="comment">// without repetitions:</span>
<a name="l00766"></a>00766 <span class="comment">//</span>
<a name="l00767"></a>00767 <span class="comment">// Select a index from "oldPartIdxsStillNotPropragated" and remove it from the list:</span>
<a name="l00768"></a>00768 <span class="keywordflow">if</span> (oldPartIdxsStillNotPropragated.size())
<a name="l00769"></a>00769 {
<a name="l00770"></a>00770 <span class="keyword">const</span> <span class="keywordtype">size_t</span> idx = <a class="code" href="namespacemrpt_1_1random.html#a4743bfa8fcb282b6f5d66395ccabaa73" title="A static instance of a CRandomGenerator class, for use in single-thread applications.">randomGenerator</a>.<a class="code" href="classmrpt_1_1random_1_1_c_random_generator.html#aede40e77053b8b4350fd786f1683e260" title="Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, in the whole range of 32-bit integers.">drawUniform32bit</a>() % oldPartIdxsStillNotPropragated.size();
<a name="l00771"></a>00771 <a class="code" href="classstd_1_1vector.html">vector_size_t</a><a class="code" href="eigen__plugins_8h.html#a39c5d6430ea9395ae7ae729dd0c3f18c">::iterator</a> it = oldPartIdxsStillNotPropragated.begin() + idx; <span class="comment">//advance(it,idx);</span>
<a name="l00772"></a>00772 k = *it;
<a name="l00773"></a>00773 oldPartIdxsStillNotPropragated.erase(it);
<a name="l00774"></a>00774 }
<a name="l00775"></a>00775 <span class="keywordflow">else</span>
<a name="l00776"></a>00776 {
<a name="l00777"></a>00777 <span class="comment">// N>N_old -> Uniformly draw index:</span>
<a name="l00778"></a>00778 k = <a class="code" href="namespacemrpt_1_1random.html#a4743bfa8fcb282b6f5d66395ccabaa73" title="A static instance of a CRandomGenerator class, for use in single-thread applications.">randomGenerator</a>.<a class="code" href="classmrpt_1_1random_1_1_c_random_generator.html#aede40e77053b8b4350fd786f1683e260" title="Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, in the whole range of 32-bit integers.">drawUniform32bit</a>() % me->m_particles.size();
<a name="l00779"></a>00779 }
<a name="l00780"></a>00780 }
<a name="l00781"></a>00781 }
<a name="l00782"></a>00782
<a name="l00783"></a>00783 <span class="comment">// Do one rejection sampling step:</span>
<a name="l00784"></a>00784 <span class="comment">// ---------------------------------------------</span>
<a name="l00785"></a>00785 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> newPose;
<a name="l00786"></a>00786 <span class="keywordtype">double</span> newParticleLogWeight;
<a name="l00787"></a>00787 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
<a name="l00788"></a>00788 USE_OPTIMAL_SAMPLING,doResample,maxMeanLik,
<a name="l00789"></a>00789 k,
<a name="l00790"></a>00790 sf,PF_options,
<a name="l00791"></a>00791 newPose, newParticleLogWeight);
<a name="l00792"></a>00792
<a name="l00793"></a>00793 <span class="comment">// Insert the new particle</span>
<a name="l00794"></a>00794 newParticles.push_back( newPose );
<a name="l00795"></a>00795 newParticlesDerivedFromIdx.push_back( k );
<a name="l00796"></a>00796 newParticlesWeight.push_back(newParticleLogWeight);
<a name="l00797"></a>00797
<a name="l00798"></a>00798 <span class="comment">// ----------------------------------------------------------------</span>
<a name="l00799"></a>00799 <span class="comment">// Now, the KLD-sampling dynamic sample size stuff:</span>
<a name="l00800"></a>00800 <span class="comment">// look if the particle's PATH falls into a new bin or not:</span>
<a name="l00801"></a>00801 <span class="comment">// ----------------------------------------------------------------</span>
<a name="l00802"></a>00802 BINTYPE p;
<a name="l00803"></a>00803 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html" title="Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).">TPose3D</a> newPose_s = newPose;
<a name="l00804"></a>00804 KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>( p,KLD_options, me->m_particles[k].d, &newPose_s );
<a name="l00805"></a>00805
<a name="l00806"></a>00806 <span class="comment">// -----------------------------------------------------------------------------</span>
<a name="l00807"></a>00807 <span class="comment">// Look for the bin "p" into "stateSpaceBins": If it is not yet into the set,</span>
<a name="l00808"></a>00808 <span class="comment">// then we may increase the desired particle number:</span>
<a name="l00809"></a>00809 <span class="comment">// -----------------------------------------------------------------------------</span>
<a name="l00810"></a>00810
<a name="l00811"></a>00811 <span class="comment">// Found?</span>
<a name="l00812"></a>00812 <span class="keywordflow">if</span> ( stateSpaceBins.find(p)==stateSpaceBins.end() )
<a name="l00813"></a>00813 {
<a name="l00814"></a>00814 <span class="comment">// It falls into a new bin: add to the stateSpaceBins:</span>
<a name="l00815"></a>00815 stateSpaceBins.insert( p );
<a name="l00816"></a>00816
<a name="l00817"></a>00817 <span class="comment">// K = K + 1</span>
<a name="l00818"></a>00818 <span class="keywordtype">int</span> K = stateSpaceBins.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html#a47889c4995254b4457d9a41a501508fc">size</a>();
<a name="l00819"></a>00819 <span class="keywordflow">if</span> ( K>1 )
<a name="l00820"></a>00820 {
<a name="l00821"></a>00821 <span class="comment">// Update the number of m_particles!!</span>
<a name="l00822"></a>00822 Nx = (size_t) (epsilon_1 * <a class="code" href="group__stats__grp.html#ga68b8a2208349a297c19f3a45c24457b1" title="The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...">math::chi2inv</a>(delta_1,K-1));
<a name="l00823"></a>00823 <span class="comment">//printf("k=%u \tn=%u \tNx:%u\n", k, newParticles.size(), Nx);</span>
<a name="l00824"></a>00824 }
<a name="l00825"></a>00825 }
<a name="l00826"></a>00826
<a name="l00827"></a>00827 N = newParticles.size();
<a name="l00828"></a>00828
<a name="l00829"></a>00829 } <span class="keywordflow">while</span> (( N < KLD_options.<a class="code" href="classmrpt_1_1slam_1_1_t_k_l_d_params.html#aeb1dd89a8987c1f6957653d35aa0ccae">KLD_maxSampleSize</a> &&
<a name="l00830"></a>00830 N < max(Nx,minNumSamples_KLD)) ||
<a name="l00831"></a>00831 (permutationPathsAuxVector.size() && !doResample) );
<a name="l00832"></a>00832
<a name="l00833"></a>00833 printf(<span class="stringliteral">"\n[ADAPTIVE SAMPLE SIZE] #Bins: %u \t #Particles: %u \t Nx=%u\n"</span>, static_cast<unsigned>(stateSpaceBins.size()),static_cast<unsigned>(N), (unsigned)Nx);
<a name="l00834"></a>00834 } <span class="comment">// end adaptive sample size</span>
<a name="l00835"></a>00835
<a name="l00836"></a>00836
<a name="l00837"></a>00837 <span class="comment">// ---------------------------------------------------------------------------------</span>
<a name="l00838"></a>00838 <span class="comment">// Substitute old by new particle set:</span>
<a name="l00839"></a>00839 <span class="comment">// Old are in "m_particles"</span>
<a name="l00840"></a>00840 <span class="comment">// New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"</span>
<a name="l00841"></a>00841 <span class="comment">// ---------------------------------------------------------------------------------</span>
<a name="l00842"></a>00842 this->PF_SLAM_implementation_replaceByNewParticleSet(
<a name="l00843"></a>00843 me->m_particles,
<a name="l00844"></a>00844 newParticles,newParticlesWeight,newParticlesDerivedFromIdx );
<a name="l00845"></a>00845
<a name="l00846"></a>00846
<a name="l00847"></a>00847 <span class="comment">// In this PF_algorithm, we must do weight normalization by ourselves:</span>
<a name="l00848"></a>00848 me->normalizeWeights();
<a name="l00849"></a>00849
<a name="l00850"></a>00850 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00851"></a>00851 } <span class="comment">// end of PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal</span>
<a name="l00852"></a>00852
<a name="l00853"></a>00853
<a name="l00854"></a>00854 <span class="comment">/* ------------------------------------------------------------------------</span>
<a name="l00855"></a>00855 <span class="comment"> PF_SLAM_aux_perform_one_rejection_sampling_step</span>
<a name="l00856"></a>00856 <span class="comment"> ------------------------------------------------------------------------ */</span>
<a name="l00857"></a>00857 <span class="keyword">template</span> <<span class="keyword">class</span> PARTICLE_TYPE,<span class="keyword">class</span> MY<span class="keywordtype">SEL</span>F>
<a name="l00858"></a>00858 <span class="keyword">template</span> <<span class="keyword">class</span> BINTYPE>
<a name="l00859"></a><a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html#a366299a9fa08391084c9719dedf2af48">00859</a> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1slam_1_1_p_f__implementation.html" title="A set of common data shared by PF implementations for both SLAM and localization.">PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_aux_perform_one_rejection_sampling_step</a>(
<a name="l00860"></a>00860 <span class="keyword">const</span> <span class="keywordtype">bool</span> USE_OPTIMAL_SAMPLING,
<a name="l00861"></a>00861 <span class="keyword">const</span> <span class="keywordtype">bool</span> doResample,
<a name="l00862"></a>00862 <span class="keyword">const</span> <span class="keywordtype">double</span> maxMeanLik,
<a name="l00863"></a>00863 <span class="keywordtype">size_t</span> k, <span class="comment">// The particle from the old set "m_particles[]"</span>
<a name="l00864"></a>00864 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1slam_1_1_c_sensory_frame.html" title="Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...">CSensoryFrame</a> * sf,
<a name="l00865"></a>00865 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html" title="The configuration of a particle filter.">CParticleFilter::TParticleFilterOptions</a> &PF_options,
<a name="l00866"></a>00866 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> & out_newPose,
<a name="l00867"></a>00867 <span class="keywordtype">double</span> & out_newParticleLogWeight)
<a name="l00868"></a>00868 {
<a name="l00869"></a>00869 MYSELF *me = <span class="keyword">static_cast<</span>MYSELF*<span class="keyword">></span>(<span class="keyword">this</span>);
<a name="l00870"></a>00870
<a name="l00871"></a>00871 <span class="comment">// ADD-ON: If the 'm_pfAuxiliaryPFOptimal_estimatedProb[k]' is **extremelly** low relative to the other m_particles,</span>
<a name="l00872"></a>00872 <span class="comment">// resample only this particle with a copy of another one, uniformly:</span>
<a name="l00873"></a>00873 <span class="keywordflow">while</span> ( ( (USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb[k] : m_pfAuxiliaryPFStandard_estimatedProb[k] )
<a name="l00874"></a>00874 -maxMeanLik) < -PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a8613b2d409e3a78de42dba07f6cdcec3" title="Only for PF_algorithm=pfAuxiliaryPFOptimal: If a given particle has a max_likelihood (from the a-prio...">max_loglikelihood_dyn_range</a> )
<a name="l00875"></a>00875 {
<a name="l00876"></a>00876 <span class="comment">// Select another 'k' uniformly:</span>
<a name="l00877"></a>00877 k = <a class="code" href="namespacemrpt_1_1random.html#a4743bfa8fcb282b6f5d66395ccabaa73" title="A static instance of a CRandomGenerator class, for use in single-thread applications.">randomGenerator</a>.<a class="code" href="classmrpt_1_1random_1_1_c_random_generator.html#aede40e77053b8b4350fd786f1683e260" title="Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, in the whole range of 32-bit integers.">drawUniform32bit</a>() % me->m_particles.size();
<a name="l00878"></a>00878 <span class="keywordflow">if</span> (PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a33e80606a59d313f3338f71367010957" title="Enable extra messages for each PF iteration (Default=false)">verbose</a>) cout << <span class="stringliteral">"[PF_implementation] Warning: Discarding very unlikely particle"</span> << endl;
<a name="l00879"></a>00879 }
<a name="l00880"></a>00880
<a name="l00881"></a>00881 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> oldPose = *getLastPose(k); <span class="comment">// Get the current pose of the k'th particle</span>
<a name="l00882"></a>00882
<a name="l00883"></a>00883 <span class="comment">// (b) Rejection-sampling: Draw a new robot pose from x[k],</span>
<a name="l00884"></a>00884 <span class="comment">// and accept it with probability p(zk|x) / maxLikelihood:</span>
<a name="l00885"></a>00885 <span class="comment">// ----------------------------------------------------------------</span>
<a name="l00886"></a>00886 <span class="keywordtype">double</span> poseLogLik;
<a name="l00887"></a>00887 <span class="keywordflow">if</span> ( PF_SLAM_implementation_skipRobotMovement() )
<a name="l00888"></a>00888 {
<a name="l00889"></a>00889 <span class="comment">// The first robot pose in the SLAM execution: All m_particles start</span>
<a name="l00890"></a>00890 <span class="comment">// at the same point (this is the lowest bound of subsequent uncertainty):</span>
<a name="l00891"></a>00891 out_newPose = oldPose;
<a name="l00892"></a>00892 poseLogLik = 0;
<a name="l00893"></a>00893 }
<a name="l00894"></a>00894 <span class="keywordflow">else</span>
<a name="l00895"></a>00895 {
<a name="l00896"></a>00896 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> movementDraw;
<a name="l00897"></a>00897 <span class="keywordflow">if</span> (!USE_OPTIMAL_SAMPLING)
<a name="l00898"></a>00898 { <span class="comment">// APF:</span>
<a name="l00899"></a>00899 m_movementDrawer.drawSample( movementDraw );
<a name="l00900"></a>00900 out_newPose.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#a88f72fa352303454295616213fc10c2d" title="Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...">composeFrom</a>(oldPose, movementDraw); <span class="comment">// newPose = oldPose + movementDraw;</span>
<a name="l00901"></a>00901 <span class="comment">// Compute likelihood:</span>
<a name="l00902"></a>00902 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(PF_options, k,*sf,out_newPose);
<a name="l00903"></a>00903 }
<a name="l00904"></a>00904 <span class="keywordflow">else</span>
<a name="l00905"></a>00905 { <span class="comment">// Optimal APF with rejection sampling:</span>
<a name="l00906"></a>00906 <span class="comment">// Rejection-sampling:</span>
<a name="l00907"></a>00907 <span class="keywordtype">double</span> acceptanceProb;
<a name="l00908"></a>00908 <span class="keywordtype">int</span> timeout = 0;
<a name="l00909"></a>00909 <span class="keyword">const</span> <span class="keywordtype">int</span> maxTries=10000;
<a name="l00910"></a>00910 <span class="keywordtype">double</span> bestTryByNow_loglik= -std::numeric_limits<double>::max();
<a name="l00911"></a>00911 <a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html" title="Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).">TPose3D</a> bestTryByNow_pose;
<a name="l00912"></a>00912 <span class="keywordflow">do</span>
<a name="l00913"></a>00913 {
<a name="l00914"></a>00914 <span class="comment">// Draw new robot pose:</span>
<a name="l00915"></a>00915 <span class="keywordflow">if</span> (PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a99406b39b2963dce127a86634f13871f" title="(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true...">pfAuxFilterOptimal_MLE</a> && !m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k])
<a name="l00916"></a>00916 { <span class="comment">// No! first take advantage of a good drawn value, but only once!!</span>
<a name="l00917"></a>00917 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k] = <span class="keyword">true</span>;
<a name="l00918"></a>00918 movementDraw = <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a>( m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[k] );
<a name="l00919"></a>00919 }
<a name="l00920"></a>00920 <span class="keywordflow">else</span>
<a name="l00921"></a>00921 {
<a name="l00922"></a>00922 <span class="comment">// Draw new robot pose:</span>
<a name="l00923"></a>00923 m_movementDrawer.drawSample( movementDraw );
<a name="l00924"></a>00924 }
<a name="l00925"></a>00925
<a name="l00926"></a>00926 out_newPose.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#a88f72fa352303454295616213fc10c2d" title="Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...">composeFrom</a>(oldPose, movementDraw); <span class="comment">// out_newPose = oldPose + movementDraw;</span>
<a name="l00927"></a>00927
<a name="l00928"></a>00928 <span class="comment">// Compute acceptance probability:</span>
<a name="l00929"></a>00929 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(PF_options, k,*sf,out_newPose);
<a name="l00930"></a>00930
<a name="l00931"></a>00931 <span class="keywordflow">if</span> (poseLogLik>bestTryByNow_loglik)
<a name="l00932"></a>00932 {
<a name="l00933"></a>00933 bestTryByNow_loglik = poseLogLik;
<a name="l00934"></a>00934 bestTryByNow_pose = out_newPose;
<a name="l00935"></a>00935 }
<a name="l00936"></a>00936
<a name="l00937"></a>00937 <span class="keywordtype">double</span> ratioLikLik = std::exp( poseLogLik - m_pfAuxiliaryPFOptimal_maxLikelihood[k] );
<a name="l00938"></a>00938 acceptanceProb = std::min( 1.0, ratioLikLik );
<a name="l00939"></a>00939
<a name="l00940"></a>00940 <span class="keywordflow">if</span> ( ratioLikLik > 1)
<a name="l00941"></a>00941 {
<a name="l00942"></a>00942 m_pfAuxiliaryPFOptimal_maxLikelihood[k] = poseLogLik; <span class="comment">// :'-( !!!</span>
<a name="l00943"></a>00943 <span class="comment">//acceptanceProb = 0; // Keep searching or keep this one?</span>
<a name="l00944"></a>00944 }
<a name="l00945"></a>00945 } <span class="keywordflow">while</span> ( ++timeout<maxTries && acceptanceProb < <a class="code" href="namespacemrpt_1_1random.html#a4743bfa8fcb282b6f5d66395ccabaa73" title="A static instance of a CRandomGenerator class, for use in single-thread applications.">randomGenerator</a>.<a class="code" href="classmrpt_1_1random_1_1_c_random_generator.html#a7a4bbaa961f106cd88a872986737a2fb" title="Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.">drawUniform</a>(0.0,0.999) );
<a name="l00946"></a>00946
<a name="l00947"></a>00947 <span class="keywordflow">if</span> (timeout>=maxTries)
<a name="l00948"></a>00948 {
<a name="l00949"></a>00949 out_newPose = bestTryByNow_pose;
<a name="l00950"></a>00950 poseLogLik = bestTryByNow_loglik;
<a name="l00951"></a>00951 <span class="keywordflow">if</span> (PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a33e80606a59d313f3338f71367010957" title="Enable extra messages for each PF iteration (Default=false)">verbose</a>) cout << <span class="stringliteral">"[PF_implementation] Warning: timeout in rejection sampling."</span> << endl;
<a name="l00952"></a>00952 }
<a name="l00953"></a>00953
<a name="l00954"></a>00954 }
<a name="l00955"></a>00955
<a name="l00956"></a>00956 <span class="comment">// And its weight:</span>
<a name="l00957"></a>00957 <span class="keywordflow">if</span> (USE_OPTIMAL_SAMPLING)
<a name="l00958"></a>00958 { <span class="comment">// Optimal PF</span>
<a name="l00959"></a>00959 <span class="keywordflow">if</span> (doResample)
<a name="l00960"></a>00960 out_newParticleLogWeight = 0; <span class="comment">// By definition of our optimal PF, all samples have identical weights.</span>
<a name="l00961"></a>00961 <span class="keywordflow">else</span>
<a name="l00962"></a>00962 {
<a name="l00963"></a>00963 <span class="keyword">const</span> <span class="keywordtype">double</span> weightFact = m_pfAuxiliaryPFOptimal_estimatedProb[k] * PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a6ad1667bac7faa62a2376c83527c0414" title="An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...">powFactor</a>;
<a name="l00964"></a>00964 out_newParticleLogWeight = me->m_particles[k].log_w + weightFact;
<a name="l00965"></a>00965 }
<a name="l00966"></a>00966 }
<a name="l00967"></a>00967 <span class="keywordflow">else</span>
<a name="l00968"></a>00968 { <span class="comment">// APF:</span>
<a name="l00969"></a>00969 <span class="keyword">const</span> <span class="keywordtype">double</span> weightFact = (poseLogLik-m_pfAuxiliaryPFStandard_estimatedProb[k]) * PF_options.<a class="code" href="structmrpt_1_1bayes_1_1_c_particle_filter_1_1_t_particle_filter_options.html#a6ad1667bac7faa62a2376c83527c0414" title="An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...">powFactor</a>;
<a name="l00970"></a>00970 if (doResample)
<a name="l00971"></a>00971 out_newParticleLogWeight = weightFact;
<a name="l00972"></a>00972 <span class="keywordflow">else</span> out_newParticleLogWeight = weightFact + me->m_particles[k].log_w;
<a name="l00973"></a>00973 }
<a name="l00974"></a>00974
<a name="l00975"></a>00975 }
<a name="l00976"></a>00976 <span class="comment">// Done.</span>
<a name="l00977"></a>00977 } <span class="comment">// end PF_SLAM_aux_perform_one_rejection_sampling_step</span>
<a name="l00978"></a>00978
<a name="l00979"></a>00979
<a name="l00980"></a>00980 } <span class="comment">// end namespace</span>
<a name="l00981"></a>00981 } <span class="comment">// end namespace</span>
<a name="l00982"></a>00982
<a name="l00983"></a>00983 <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>
