<!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>transform_gaussian.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">transform_gaussian.h</div> </div>
</div>
<div class="contents">
<a href="transform__gaussian_8h.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/* +---------------------------------------------------------------------------+</span>
<a name="l00002"></a>00002 <span class="comment"> | The Mobile Robot Programming Toolkit (MRPT) C++ library |</span>
<a name="l00003"></a>00003 <span class="comment"> | |</span>
<a name="l00004"></a>00004 <span class="comment"> | http://www.mrpt.org/ |</span>
<a name="l00005"></a>00005 <span class="comment"> | |</span>
<a name="l00006"></a>00006 <span class="comment"> | Copyright (C) 2005-2011 University of Malaga |</span>
<a name="l00007"></a>00007 <span class="comment"> | |</span>
<a name="l00008"></a>00008 <span class="comment"> | This software was written by the Machine Perception and Intelligent |</span>
<a name="l00009"></a>00009 <span class="comment"> | Robotics Lab, University of Malaga (Spain). |</span>
<a name="l00010"></a>00010 <span class="comment"> | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |</span>
<a name="l00011"></a>00011 <span class="comment"> | |</span>
<a name="l00012"></a>00012 <span class="comment"> | This file is part of the MRPT project. |</span>
<a name="l00013"></a>00013 <span class="comment"> | |</span>
<a name="l00014"></a>00014 <span class="comment"> | MRPT is free software: you can redistribute it and/or modify |</span>
<a name="l00015"></a>00015 <span class="comment"> | it under the terms of the GNU General Public License as published by |</span>
<a name="l00016"></a>00016 <span class="comment"> | the Free Software Foundation, either version 3 of the License, or |</span>
<a name="l00017"></a>00017 <span class="comment"> | (at your option) any later version. |</span>
<a name="l00018"></a>00018 <span class="comment"> | |</span>
<a name="l00019"></a>00019 <span class="comment"> | MRPT is distributed in the hope that it will be useful, |</span>
<a name="l00020"></a>00020 <span class="comment"> | but WITHOUT ANY WARRANTY; without even the implied warranty of |</span>
<a name="l00021"></a>00021 <span class="comment"> | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |</span>
<a name="l00022"></a>00022 <span class="comment"> | GNU General Public License for more details. |</span>
<a name="l00023"></a>00023 <span class="comment"> | |</span>
<a name="l00024"></a>00024 <span class="comment"> | You should have received a copy of the GNU General Public License |</span>
<a name="l00025"></a>00025 <span class="comment"> | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |</span>
<a name="l00026"></a>00026 <span class="comment"> | |</span>
<a name="l00027"></a>00027 <span class="comment"> +---------------------------------------------------------------------------+ */</span>
<a name="l00028"></a>00028 <span class="preprocessor">#ifndef transform_gaussian_H</span>
<a name="l00029"></a>00029 <span class="preprocessor"></span><span class="preprocessor">#define transform_gaussian_H</span>
<a name="l00030"></a>00030 <span class="preprocessor"></span>
<a name="l00031"></a>00031 <span class="preprocessor">#include <<a class="code" href="utils__defs_8h.html">mrpt/utils/utils_defs.h</a>></span>
<a name="l00032"></a>00032 <span class="preprocessor">#include <<a class="code" href="math__frwds_8h.html">mrpt/math/math_frwds.h</a>></span>
<a name="l00033"></a>00033 <span class="preprocessor">#include <<a class="code" href="_c_matrix_template_numeric_8h.html">mrpt/math/CMatrixTemplateNumeric.h</a>></span>
<a name="l00034"></a>00034 <span class="preprocessor">#include <<a class="code" href="_c_matrix_fixed_numeric_8h.html">mrpt/math/CMatrixFixedNumeric.h</a>></span>
<a name="l00035"></a>00035 <span class="preprocessor">#include <<a class="code" href="ops__matrices_8h.html" title="This file implements miscelaneous matrix and matrix/vector operations, plus internal functions in mrp...">mrpt/math/ops_matrices.h</a>></span>
<a name="l00036"></a>00036 <span class="preprocessor">#include <<a class="code" href="jacobians_8h.html">mrpt/math/jacobians.h</a>></span>
<a name="l00037"></a>00037 <span class="preprocessor">#include <<a class="code" href="random_8h.html">mrpt/random.h</a>></span>
<a name="l00038"></a>00038
<a name="l00039"></a>00039 <span class="keyword">namespace </span>mrpt
<a name="l00040"></a>00040 {
<a name="l00041"></a>00041 <span class="keyword">namespace </span>math
<a name="l00042"></a>00042 {<span class="comment"></span>
<a name="l00043"></a>00043 <span class="comment"> /** @addtogroup gausspdf_transform_grp Gaussian PDF transformation functions</span>
<a name="l00044"></a>00044 <span class="comment"> * \ingroup mrpt_base_grp</span>
<a name="l00045"></a>00045 <span class="comment"> * @{ */</span>
<a name="l00046"></a>00046 <span class="comment"></span>
<a name="l00047"></a>00047 <span class="comment"> /** Scaled unscented transformation (SUT) for estimating the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.</span>
<a name="l00048"></a>00048 <span class="comment"> * The user must supply the function in "functor" which takes points in the X space and returns the mapped point in Y, optionally using an extra, constant parameter ("fixed_param") which remains constant.</span>
<a name="l00049"></a>00049 <span class="comment"> *</span>
<a name="l00050"></a>00050 <span class="comment"> * The parameters alpha, K and beta are the common names of the SUT method, and the default values are those recommended in most papers.</span>
<a name="l00051"></a>00051 <span class="comment"> *</span>
<a name="l00052"></a>00052 <span class="comment"> * \param elem_do_wrap2pi If !=NULL; it must point to an array of "bool" of size()==dimension of each element, stating if it's needed to do a wrap to [-pi,pi] to each dimension.</span>
<a name="l00053"></a>00053 <span class="comment"> * \sa The example in MRPT/samples/unscented_transform_test</span>
<a name="l00054"></a>00054 <span class="comment"> * \sa transform_gaussian_montecarlo, transform_gaussian_linear</span>
<a name="l00055"></a>00055 <span class="comment"> */</span>
<a name="l00056"></a>00056 <span class="keyword">template</span> <<span class="keyword">class</span> VECTORLIKE1,<span class="keyword">class</span> MATLIKE1, <span class="keyword">class</span> USERPARAM,<span class="keyword">class</span> VECTORLIKE2,<span class="keyword">class</span> VECTORLIKE3,<span class="keyword">class</span> MATLIKE2>
<a name="l00057"></a><a class="code" href="group__gausspdf__transform__grp.html#gaabbaca6808e3d9be3fd37f82f98049f0">00057</a> <span class="keywordtype">void</span> <a class="code" href="group__gausspdf__transform__grp.html#gaabbaca6808e3d9be3fd37f82f98049f0" title="Scaled unscented transformation (SUT) for estimating the Gaussian distribution of a variable Y=f(X) f...">transform_gaussian_unscented</a>(
<a name="l00058"></a>00058 <span class="keyword">const</span> VECTORLIKE1 &x_mean,
<a name="l00059"></a>00059 <span class="keyword">const</span> MATLIKE1 &x_cov,
<a name="l00060"></a>00060 <span class="keywordtype">void</span> (*functor)(<span class="keyword">const</span> VECTORLIKE1 &x,<span class="keyword">const</span> USERPARAM &fixed_param, VECTORLIKE3 &<a class="code" href="namespace_eigen_1_1internal.html#a3d7a581aeb951248dc6fe114e9e05f07">y</a>),
<a name="l00061"></a>00061 <span class="keyword">const</span> USERPARAM &fixed_param,
<a name="l00062"></a>00062 VECTORLIKE2 &y_mean,
<a name="l00063"></a>00063 MATLIKE2 &y_cov,
<a name="l00064"></a>00064 <span class="keyword">const</span> <span class="keywordtype">bool</span> *elem_do_wrap2pi = NULL,
<a name="l00065"></a>00065 <span class="keyword">const</span> <span class="keywordtype">double</span> alpha = 1e-3,
<a name="l00066"></a>00066 <span class="keyword">const</span> <span class="keywordtype">double</span> K = 0,
<a name="l00067"></a>00067 <span class="keyword">const</span> <span class="keywordtype">double</span> beta = 2.0
<a name="l00068"></a>00068 )
<a name="l00069"></a>00069 {
<a name="l00070"></a>00070 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00071"></a>00071 <span class="keyword">const</span> <span class="keywordtype">size_t</span> Nx = x_mean.size(); <span class="comment">// Dimensionality of inputs X</span>
<a name="l00072"></a>00072 <span class="keyword">const</span> <span class="keywordtype">double</span> lambda = alpha*alpha*(Nx+K)-Nx;
<a name="l00073"></a>00073 <span class="keyword">const</span> <span class="keywordtype">double</span> c = Nx+lambda;
<a name="l00074"></a>00074
<a name="l00075"></a>00075 <span class="comment">// Generate weights:</span>
<a name="l00076"></a>00076 <span class="keyword">const</span> <span class="keywordtype">double</span> Wi = 0.5/c;
<a name="l00077"></a>00077 <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> W_mean(1+2*Nx,Wi),W_cov(1+2*Nx,Wi);
<a name="l00078"></a>00078 W_mean[0] = lambda/c;
<a name="l00079"></a>00079 W_cov[0] = W_mean[0]+(1-alpha*alpha+beta);
<a name="l00080"></a>00080
<a name="l00081"></a>00081 <span class="comment">// Generate X_i samples:</span>
<a name="l00082"></a>00082 MATLIKE1 L;
<a name="l00083"></a>00083 <span class="keyword">const</span> <span class="keywordtype">bool</span> valid = x_cov.chol(L);
<a name="l00084"></a>00084 <span class="keywordflow">if</span> (!valid) <span class="keywordflow">throw</span> <a class="code" href="classstd_1_1runtime__error.html" title="STL class.">std::runtime_error</a>(<span class="stringliteral">"transform_gaussian_unscented: Singular covariance matrix in Cholesky."</span>);
<a name="l00085"></a>00085 L*= sqrt(c);
<a name="l00086"></a>00086
<a name="l00087"></a>00087 <span class="comment">// Propagate the samples X_i -> Y_i:</span>
<a name="l00088"></a>00088 <span class="comment">// We don't need to store the X sigma points: just use one vector to compute all the Y sigma points:</span>
<a name="l00089"></a>00089 <span class="keyword">typename</span> mrpt<a class="code" href="classstd_1_1vector.html" title="STL class.">::aligned_containers<VECTORLIKE3>::vector_t</a> Y(1+2*Nx); <span class="comment">// 2Nx+1 sigma points</span>
<a name="l00090"></a>00090 VECTORLIKE1 X = x_mean;
<a name="l00091"></a>00091 functor(X,fixed_param,Y[0]);
<a name="l00092"></a>00092 VECTORLIKE1 delta; <span class="comment">// i'th row of L:</span>
<a name="l00093"></a>00093 delta.resize(Nx);
<a name="l00094"></a>00094 <span class="keywordtype">size_t</span> row=1;
<a name="l00095"></a>00095 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<Nx;i++)
<a name="l00096"></a>00096 {
<a name="l00097"></a>00097 L.extractRowAsCol(i,delta);
<a name="l00098"></a>00098 X=x_mean;X-=delta;
<a name="l00099"></a>00099 functor(X,fixed_param,Y[row++]);
<a name="l00100"></a>00100 X=x_mean;X+=delta;
<a name="l00101"></a>00101 functor(X,fixed_param,Y[row++]);
<a name="l00102"></a>00102 }
<a name="l00103"></a>00103
<a name="l00104"></a>00104 <span class="comment">// Estimate weighted cov & mean:</span>
<a name="l00105"></a>00105 <a class="code" href="group__stats__grp.html#gaf0a0f292b7248680014f55effd35873f" title="Computes covariances and mean of any vector of containers, given optional weights for the different s...">mrpt::math::covariancesAndMeanWeighted</a>(Y, y_cov,y_mean,&W_mean,&W_cov,elem_do_wrap2pi);
<a name="l00106"></a>00106 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00107"></a>00107 }
<a name="l00108"></a>00108 <span class="comment"></span>
<a name="l00109"></a>00109 <span class="comment"> /** Simple Montecarlo-base estimation of the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.</span>
<a name="l00110"></a>00110 <span class="comment"> * The user must supply the function in "functor" which takes points in the X space and returns the mapped point in Y, optionally using an extra, constant parameter ("fixed_param") which remains constant.</span>
<a name="l00111"></a>00111 <span class="comment"> * \param out_samples_y If !=NULL, this vector will contain, upon return, the sequence of random samples generated and propagated through the functor().</span>
<a name="l00112"></a>00112 <span class="comment"> * \sa The example in MRPT/samples/unscented_transform_test</span>
<a name="l00113"></a>00113 <span class="comment"> * \sa transform_gaussian_unscented, transform_gaussian_linear</span>
<a name="l00114"></a>00114 <span class="comment"> */</span>
<a name="l00115"></a>00115 <span class="keyword">template</span> <<span class="keyword">class</span> VECTORLIKE1,<span class="keyword">class</span> MATLIKE1, <span class="keyword">class</span> USERPARAM,<span class="keyword">class</span> VECTORLIKE2,<span class="keyword">class</span> VECTORLIKE3,<span class="keyword">class</span> MATLIKE2>
<a name="l00116"></a><a class="code" href="group__gausspdf__transform__grp.html#ga690d0b41f293578fcfd067e9177fc863">00116</a> <span class="keywordtype">void</span> <a class="code" href="group__gausspdf__transform__grp.html#ga690d0b41f293578fcfd067e9177fc863" title="Simple Montecarlo-base estimation of the Gaussian distribution of a variable Y=f(X) for an arbitrary ...">transform_gaussian_montecarlo</a>(
<a name="l00117"></a>00117 <span class="keyword">const</span> VECTORLIKE1 &x_mean,
<a name="l00118"></a>00118 <span class="keyword">const</span> MATLIKE1 &x_cov,
<a name="l00119"></a>00119 <span class="keywordtype">void</span> (*functor)(<span class="keyword">const</span> VECTORLIKE1 &x,<span class="keyword">const</span> USERPARAM &fixed_param,VECTORLIKE3 &<a class="code" href="namespace_eigen_1_1internal.html#a3d7a581aeb951248dc6fe114e9e05f07">y</a>),
<a name="l00120"></a>00120 <span class="keyword">const</span> USERPARAM &fixed_param,
<a name="l00121"></a>00121 VECTORLIKE2 &y_mean,
<a name="l00122"></a>00122 MATLIKE2 &y_cov,
<a name="l00123"></a>00123 <span class="keyword">const</span> <span class="keywordtype">size_t</span> num_samples = 1000,
<a name="l00124"></a>00124 <span class="keyword">typename</span> <a class="code" href="classstd_1_1vector.html" title="STL class.">mrpt::aligned_containers<VECTORLIKE3>::vector_t</a> *out_samples_y = NULL
<a name="l00125"></a>00125 )
<a name="l00126"></a>00126 {
<a name="l00127"></a>00127 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00128"></a>00128 <span class="keyword">typename</span> mrpt<a class="code" href="classstd_1_1vector.html" title="STL class.">::aligned_containers<VECTORLIKE1>::vector_t</a> samples_x;
<a name="l00129"></a>00129 mrpt<a class="code" href="namespacemrpt_1_1random.html#a4743bfa8fcb282b6f5d66395ccabaa73" title="A static instance of a CRandomGenerator class, for use in single-thread applications.">::random::randomGenerator</a>.<a class="code" href="classmrpt_1_1random_1_1_c_random_generator.html#a02ebde6aa19bc11b17960e88e2a9e58b" title="Generate a given number of multidimensional random samples according to a given covariance matrix...">drawGaussianMultivariateMany</a>(samples_x,num_samples,x_cov,&x_mean);
<a name="l00130"></a>00130 <span class="keyword">typename</span> mrpt<a class="code" href="classstd_1_1vector.html" title="STL class.">::aligned_containers<VECTORLIKE3>::vector_t</a> samples_y(num_samples);
<a name="l00131"></a>00131 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<num_samples;i++)
<a name="l00132"></a>00132 functor(samples_x[i],fixed_param,samples_y[i]);
<a name="l00133"></a>00133 <a class="code" href="group__stats__grp.html#gaa1cf7357c4043fb790efe19d3f6c2b7d" title="Computes covariances and mean of any vector of containers.">mrpt::math::covariancesAndMean</a>(samples_y,y_cov,y_mean);
<a name="l00134"></a>00134 <span class="keywordflow">if</span> (out_samples_y) { out_samples_y->clear(); samples_y.swap(*out_samples_y); }
<a name="l00135"></a>00135 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00136"></a>00136 }
<a name="l00137"></a>00137 <span class="comment"></span>
<a name="l00138"></a>00138 <span class="comment"> /** First order uncertainty propagation estimator of the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.</span>
<a name="l00139"></a>00139 <span class="comment"> * The user must supply the function in "functor" which takes points in the X space and returns the mapped point in Y, optionally using an extra, constant parameter ("fixed_param") which remains constant.</span>
<a name="l00140"></a>00140 <span class="comment"> * The Jacobians are estimated numerically using the vector of small increments "x_increments".</span>
<a name="l00141"></a>00141 <span class="comment"> * \sa The example in MRPT/samples/unscented_transform_test</span>
<a name="l00142"></a>00142 <span class="comment"> * \sa transform_gaussian_unscented, transform_gaussian_montecarlo</span>
<a name="l00143"></a>00143 <span class="comment"> */</span>
<a name="l00144"></a>00144 <span class="keyword">template</span> <<span class="keyword">class</span> VECTORLIKE1,<span class="keyword">class</span> MATLIKE1, <span class="keyword">class</span> USERPARAM,<span class="keyword">class</span> VECTORLIKE2,<span class="keyword">class</span> VECTORLIKE3,<span class="keyword">class</span> MATLIKE2>
<a name="l00145"></a><a class="code" href="group__gausspdf__transform__grp.html#gac9caace672d90279c312d43877d8480b">00145</a> <span class="keywordtype">void</span> <a class="code" href="group__gausspdf__transform__grp.html#gac9caace672d90279c312d43877d8480b" title="First order uncertainty propagation estimator of the Gaussian distribution of a variable Y=f(X) for a...">transform_gaussian_linear</a>(
<a name="l00146"></a>00146 <span class="keyword">const</span> VECTORLIKE1 &x_mean,
<a name="l00147"></a>00147 <span class="keyword">const</span> MATLIKE1 &x_cov,
<a name="l00148"></a>00148 <span class="keywordtype">void</span> (*functor)(<span class="keyword">const</span> VECTORLIKE1 &x,<span class="keyword">const</span> USERPARAM &fixed_param,VECTORLIKE3 &<a class="code" href="namespace_eigen_1_1internal.html#a3d7a581aeb951248dc6fe114e9e05f07">y</a>),
<a name="l00149"></a>00149 <span class="keyword">const</span> USERPARAM &fixed_param,
<a name="l00150"></a>00150 VECTORLIKE2 &y_mean,
<a name="l00151"></a>00151 MATLIKE2 &y_cov,
<a name="l00152"></a>00152 <span class="keyword">const</span> VECTORLIKE1 &x_increments
<a name="l00153"></a>00153 )
<a name="l00154"></a>00154 {
<a name="l00155"></a>00155 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00156"></a>00156 <span class="comment">// Mean: simple propagation:</span>
<a name="l00157"></a>00157 functor(x_mean,fixed_param,y_mean);
<a name="l00158"></a>00158 <span class="comment">// Cov: COV = H C Ht</span>
<a name="l00159"></a>00159 Eigen::Matrix<double,VECTORLIKE3::RowsAtCompileTime,VECTORLIKE1::RowsAtCompileTime> H;
<a name="l00160"></a>00160 <a class="code" href="namespacemrpt_1_1math_1_1jacobians.html#a6bfc725b0889aa6b7eee78bb91f19441" title="Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::m...">mrpt::math::jacobians::jacob_numeric_estimate</a>(x_mean,functor,x_increments,fixed_param,H);
<a name="l00161"></a>00161 H.multiply_HCHt(x_cov, y_cov);
<a name="l00162"></a>00162 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00163"></a>00163 }
<a name="l00164"></a>00164 <span class="comment"></span>
<a name="l00165"></a>00165 <span class="comment"> /** @} */</span>
<a name="l00166"></a>00166
<a name="l00167"></a>00167 } <span class="comment">// End of MATH namespace</span>
<a name="l00168"></a>00168
<a name="l00169"></a>00169 } <span class="comment">// End of namespace</span>
<a name="l00170"></a>00170
<a name="l00171"></a>00171
<a name="l00172"></a>00172 <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>
