<!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>utils.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">utils.h</div> </div>
</div>
<div class="contents">
<a href="base_2include_2mrpt_2math_2utils_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 MRPT_MATH_H</span>
<a name="l00029"></a>00029 <span class="preprocessor"></span><span class="preprocessor">#define MRPT_MATH_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="_c_matrix_template_numeric_8h.html">mrpt/math/CMatrixTemplateNumeric.h</a>></span>
<a name="l00033"></a>00033 <span class="preprocessor">#include <<a class="code" href="_c_matrix_fixed_numeric_8h.html">mrpt/math/CMatrixFixedNumeric.h</a>></span>
<a name="l00034"></a>00034 <span class="preprocessor">#include <<a class="code" href="_c_histogram_8h.html">mrpt/math/CHistogram.h</a>></span>
<a name="l00035"></a>00035
<a name="l00036"></a>00036 <span class="preprocessor">#include <<a class="code" href="ops__vectors_8h.html">mrpt/math/ops_vectors.h</a>></span>
<a name="l00037"></a>00037 <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="l00038"></a>00038
<a name="l00039"></a>00039 <span class="preprocessor">#include <numeric></span>
<a name="l00040"></a>00040 <span class="preprocessor">#include <cmath></span>
<a name="l00041"></a>00041
<a name="l00042"></a>00042 <span class="comment">/*---------------------------------------------------------------</span>
<a name="l00043"></a>00043 <span class="comment"> Namespace</span>
<a name="l00044"></a>00044 <span class="comment"> ---------------------------------------------------------------*/</span>
<a name="l00045"></a>00045 <span class="keyword">namespace </span>mrpt
<a name="l00046"></a>00046 {<span class="comment"></span>
<a name="l00047"></a>00047 <span class="comment"> /** This base provides a set of functions for maths stuff. \ingroup mrpt_base_grp</span>
<a name="l00048"></a>00048 <span class="comment"> */</span>
<a name="l00049"></a>00049 <span class="keyword">namespace </span>math
<a name="l00050"></a>00050 {
<a name="l00051"></a>00051 <span class="keyword">using namespace </span>mrpt::utils;
<a name="l00052"></a>00052 <span class="comment"></span>
<a name="l00053"></a>00053 <span class="comment"> /** \addtogroup container_ops_grp</span>
<a name="l00054"></a>00054 <span class="comment"> * @{ */</span>
<a name="l00055"></a>00055 <span class="comment"></span>
<a name="l00056"></a>00056 <span class="comment"> /** Loads one row of a text file as a numerical std::vector.</span>
<a name="l00057"></a>00057 <span class="comment"> * \return false on EOF or invalid format.</span>
<a name="l00058"></a>00058 <span class="comment"> * The body of the function is implemented in MATH.cpp</span>
<a name="l00059"></a>00059 <span class="comment"> */</span>
<a name="l00060"></a>00060 <span class="keywordtype">bool</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__container__ops__grp.html#ga40e8e47dea9f504a28d2a70ea8ddb158" title="Loads one row of a text file as a numerical std::vector.">loadVector</a>( utils::CFileStream &f, <a class="code" href="classstd_1_1vector.html">std::vector<int></a> &d);
<a name="l00061"></a>00061 <span class="comment"></span>
<a name="l00062"></a>00062 <span class="comment"> /** Loads one row of a text file as a numerical std::vector.</span>
<a name="l00063"></a>00063 <span class="comment"> * \return false on EOF or invalid format.</span>
<a name="l00064"></a>00064 <span class="comment"> * The body of the function is implemented in MATH.cpp</span>
<a name="l00065"></a>00065 <span class="comment"> */</span>
<a name="l00066"></a>00066 <span class="keywordtype">bool</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__container__ops__grp.html#ga40e8e47dea9f504a28d2a70ea8ddb158" title="Loads one row of a text file as a numerical std::vector.">loadVector</a>( utils::CFileStream &f, <a class="code" href="classstd_1_1vector.html">std::vector<double></a> &d);
<a name="l00067"></a>00067
<a name="l00068"></a>00068 <span class="comment"></span>
<a name="l00069"></a>00069 <span class="comment"> /** Returns true if the number is NaN. */</span>
<a name="l00070"></a>00070 <span class="keywordtype">bool</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__container__ops__grp.html#ga0789f5c3dde7a158cb9780f6061ddcc6" title="Returns true if the number is NaN.">isNaN</a>(<span class="keywordtype">float</span> f) <a class="code" href="mrpt__macros_8h.html#afed971bfd24ff010f488cce2aa424de4" title="Used after member declarations.">MRPT_NO_THROWS</a>;
<a name="l00071"></a>00071 <span class="comment"></span>
<a name="l00072"></a>00072 <span class="comment"> /** Returns true if the number is NaN. */</span>
<a name="l00073"></a>00073 <span class="keywordtype">bool</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__container__ops__grp.html#ga0789f5c3dde7a158cb9780f6061ddcc6" title="Returns true if the number is NaN.">isNaN</a>(<span class="keywordtype">double</span> f) <a class="code" href="mrpt__macros_8h.html#afed971bfd24ff010f488cce2aa424de4" title="Used after member declarations.">MRPT_NO_THROWS</a>;
<a name="l00074"></a>00074 <span class="comment"></span>
<a name="l00075"></a>00075 <span class="comment"> /** Returns true if the number is non infinity. */</span>
<a name="l00076"></a>00076 <span class="keywordtype">bool</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__container__ops__grp.html#gaf4a419952a205931911e0a5627a192fe" title="Returns true if the number is non infinity.">isFinite</a>(<span class="keywordtype">float</span> f) <a class="code" href="mrpt__macros_8h.html#afed971bfd24ff010f488cce2aa424de4" title="Used after member declarations.">MRPT_NO_THROWS</a>;
<a name="l00077"></a>00077 <span class="comment"></span>
<a name="l00078"></a>00078 <span class="comment"> /** Returns true if the number is non infinity. */</span>
<a name="l00079"></a>00079 <span class="keywordtype">bool</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__container__ops__grp.html#gaf4a419952a205931911e0a5627a192fe" title="Returns true if the number is non infinity.">isFinite</a>(<span class="keywordtype">double</span> f) <a class="code" href="mrpt__macros_8h.html#afed971bfd24ff010f488cce2aa424de4" title="Used after member declarations.">MRPT_NO_THROWS</a>;
<a name="l00080"></a>00080
<a name="l00081"></a>00081 <span class="keywordtype">void</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__container__ops__grp.html#gafa11ba6f88e921e59b8971131fb1ec06">medianFilter</a>( <span class="keyword">const</span> <a class="code" href="classstd_1_1vector.html">std::vector<double></a> &inV, <a class="code" href="classstd_1_1vector.html">std::vector<double></a> &outV, <span class="keyword">const</span> <span class="keywordtype">int</span> &winSize, <span class="keyword">const</span> <span class="keywordtype">int</span> &numberOfSigmas = 2 );
<a name="l00082"></a>00082
<a name="l00083"></a>00083 <span class="preprocessor">#ifdef HAVE_LONG_DOUBLE</span>
<a name="l00084"></a>00084 <span class="preprocessor"></span><span class="comment"> /** Returns true if the number is NaN. */</span>
<a name="l00085"></a>00085 <span class="keywordtype">bool</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__container__ops__grp.html#ga0789f5c3dde7a158cb9780f6061ddcc6" title="Returns true if the number is NaN.">isNaN</a>(<span class="keywordtype">long</span> <span class="keywordtype">double</span> f) <a class="code" href="mrpt__macros_8h.html#afed971bfd24ff010f488cce2aa424de4" title="Used after member declarations.">MRPT_NO_THROWS</a>;
<a name="l00086"></a>00086 <span class="comment"></span>
<a name="l00087"></a>00087 <span class="comment"> /** Returns true if the number is non infinity. */</span>
<a name="l00088"></a>00088 <span class="keywordtype">bool</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__container__ops__grp.html#gaf4a419952a205931911e0a5627a192fe" title="Returns true if the number is non infinity.">isFinite</a>(<span class="keywordtype">long</span> <span class="keywordtype">double</span> f) <a class="code" href="mrpt__macros_8h.html#afed971bfd24ff010f488cce2aa424de4" title="Used after member declarations.">MRPT_NO_THROWS</a>;
<a name="l00089"></a>00089 <span class="preprocessor">#endif</span>
<a name="l00090"></a>00090 <span class="preprocessor"></span><span class="comment"></span>
<a name="l00091"></a>00091 <span class="comment"> /** Generates an equidistant sequence of numbers given the first one, the last one and the desired number of points.</span>
<a name="l00092"></a>00092 <span class="comment"> \sa sequence */</span>
<a name="l00093"></a>00093 <span class="keyword">template</span><<span class="keyword">typename</span> T,<span class="keyword">typename</span> VECTOR>
<a name="l00094"></a><a class="code" href="group__container__ops__grp.html#gac77eb1a332f0b7cd60ecf5918d2f4a9a">00094</a> <span class="keywordtype">void</span> <a class="code" href="group__container__ops__grp.html#gac77eb1a332f0b7cd60ecf5918d2f4a9a" title="Generates an equidistant sequence of numbers given the first one, the last one and the desired number...">linspace</a>(T first,T last, <span class="keywordtype">size_t</span> count, VECTOR &out_vector)
<a name="l00095"></a>00095 {
<a name="l00096"></a>00096 <span class="keywordflow">if</span> (count<2)
<a name="l00097"></a>00097 {
<a name="l00098"></a>00098 out_vector.assign(count,last);
<a name="l00099"></a>00099 <span class="keywordflow">return</span>;
<a name="l00100"></a>00100 }
<a name="l00101"></a>00101 <span class="keywordflow">else</span>
<a name="l00102"></a>00102 {
<a name="l00103"></a>00103 out_vector.resize(count);
<a name="l00104"></a>00104 <span class="keyword">const</span> T incr = (last-first)/T(count-1);
<a name="l00105"></a>00105 T c = first;
<a name="l00106"></a>00106 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<count;i++,c+=incr)
<a name="l00107"></a>00107 out_vector[i] = static_cast<typename VECTOR::value_type>(c);
<a name="l00108"></a>00108 }
<a name="l00109"></a>00109 }
<a name="l00110"></a>00110 <span class="comment"></span>
<a name="l00111"></a>00111 <span class="comment"> /** Generates an equidistant sequence of numbers given the first one, the last one and the desired number of points.</span>
<a name="l00112"></a>00112 <span class="comment"> \sa sequence */</span>
<a name="l00113"></a>00113 <span class="keyword">template</span><<span class="keyword">class</span> T>
<a name="l00114"></a><a class="code" href="group__container__ops__grp.html#ga2971708259446f11d677f849d0b8a07d">00114</a> <span class="keyword">inline</span> Eigen::Matrix<T,Eigen::Dynamic,1> <a class="code" href="group__container__ops__grp.html#gac77eb1a332f0b7cd60ecf5918d2f4a9a" title="Generates an equidistant sequence of numbers given the first one, the last one and the desired number...">linspace</a>(T first,T last, <span class="keywordtype">size_t</span> count)
<a name="l00115"></a>00115 {
<a name="l00116"></a>00116 Eigen::Matrix<T,Eigen::Dynamic,1> ret;
<a name="l00117"></a>00117 <a class="code" href="group__container__ops__grp.html#gac77eb1a332f0b7cd60ecf5918d2f4a9a" title="Generates an equidistant sequence of numbers given the first one, the last one and the desired number...">mrpt::math::linspace</a>(first,last,count,ret);
<a name="l00118"></a>00118 <span class="keywordflow">return</span> ret;
<a name="l00119"></a>00119 }
<a name="l00120"></a>00120 <span class="comment"></span>
<a name="l00121"></a>00121 <span class="comment"> /** Generates a sequence of values [first,first+STEP,first+2*STEP,...] \sa linspace, sequenceStdVec */</span>
<a name="l00122"></a>00122 <span class="keyword">template</span><<span class="keyword">class</span> T,T STEP>
<a name="l00123"></a><a class="code" href="group__container__ops__grp.html#ga9b7b817146f7f39b83e3b3fbe2225ca5">00123</a> <span class="keyword">inline</span> Eigen::Matrix<T,Eigen::Dynamic,1> <a class="code" href="group__container__ops__grp.html#ga9b7b817146f7f39b83e3b3fbe2225ca5" title="Generates a sequence of values [first,first+STEP,first+2*STEP,...].">sequence</a>(T first,<span class="keywordtype">size_t</span> length)
<a name="l00124"></a>00124 {
<a name="l00125"></a>00125 Eigen::Matrix<T,Eigen::Dynamic,1> ret(length);
<a name="l00126"></a>00126 <span class="keywordflow">if</span> (!length) <span class="keywordflow">return</span> ret;
<a name="l00127"></a>00127 <span class="keywordtype">size_t</span> i=0;
<a name="l00128"></a>00128 <span class="keywordflow">while</span> (length--) { ret[i++]=first; first+=STEP; }
<a name="l00129"></a>00129 <span class="keywordflow">return</span> ret;
<a name="l00130"></a>00130 }
<a name="l00131"></a>00131 <span class="comment"></span>
<a name="l00132"></a>00132 <span class="comment"> /** Generates a sequence of values [first,first+STEP,first+2*STEP,...] \sa linspace, sequence */</span>
<a name="l00133"></a>00133 <span class="keyword">template</span><<span class="keyword">class</span> T,T STEP>
<a name="l00134"></a><a class="code" href="group__container__ops__grp.html#ga897c75f8fa614c54da6c0500b869f2a1">00134</a> <span class="keyword">inline</span> std<a class="code" href="classstd_1_1vector.html">::vector<T></a> <a class="code" href="group__container__ops__grp.html#ga897c75f8fa614c54da6c0500b869f2a1" title="Generates a sequence of values [first,first+STEP,first+2*STEP,...].">sequenceStdVec</a>(T first,<span class="keywordtype">size_t</span> length)
<a name="l00135"></a>00135 {
<a name="l00136"></a>00136 std<a class="code" href="classstd_1_1vector.html">::vector<T></a> ret(length);
<a name="l00137"></a>00137 <span class="keywordflow">if</span> (!length) <span class="keywordflow">return</span> ret;
<a name="l00138"></a>00138 <span class="keywordtype">size_t</span> i=0;
<a name="l00139"></a>00139 <span class="keywordflow">while</span> (length--) { ret[i++]=first; first+=STEP; }
<a name="l00140"></a>00140 <span class="keywordflow">return</span> ret;
<a name="l00141"></a>00141 }
<a name="l00142"></a>00142 <span class="comment"></span>
<a name="l00143"></a>00143 <span class="comment"> /** Generates a vector of all ones of the given length. */</span>
<a name="l00144"></a><a class="code" href="group__container__ops__grp.html#gaee0433b2568e6a7847b0841d08722a51">00144</a> <span class="keyword">template</span><<span class="keyword">class</span> T> <span class="keyword">inline</span> Eigen::Matrix<T,Eigen::Dynamic,1> <a class="code" href="group__container__ops__grp.html#gaee0433b2568e6a7847b0841d08722a51" title="Generates a vector of all ones of the given length.">ones</a>(<span class="keywordtype">size_t</span> count)
<a name="l00145"></a>00145 {
<a name="l00146"></a>00146 Eigen::Matrix<T,Eigen::Dynamic,1> v(count);
<a name="l00147"></a>00147 v.setOnes();
<a name="l00148"></a>00148 <span class="keywordflow">return</span> v;
<a name="l00149"></a>00149 }
<a name="l00150"></a>00150 <span class="comment"></span>
<a name="l00151"></a>00151 <span class="comment"> /** Generates a vector of all zeros of the given length. */</span>
<a name="l00152"></a><a class="code" href="group__container__ops__grp.html#ga6a0207f476314788b520568a0a16afc1">00152</a> <span class="keyword">template</span><<span class="keyword">class</span> T> <span class="keyword">inline</span> Eigen::Matrix<T,Eigen::Dynamic,1> <a class="code" href="group__container__ops__grp.html#ga6a0207f476314788b520568a0a16afc1" title="Generates a vector of all zeros of the given length.">zeros</a>(<span class="keywordtype">size_t</span> count)
<a name="l00153"></a>00153 {
<a name="l00154"></a>00154 Eigen::Matrix<T,Eigen::Dynamic,1> v(count);
<a name="l00155"></a>00155 v.setZero();
<a name="l00156"></a>00156 <span class="keywordflow">return</span> v;
<a name="l00157"></a>00157 }
<a name="l00158"></a>00158
<a name="l00159"></a>00159 <span class="comment"></span>
<a name="l00160"></a>00160 <span class="comment"> /** Modifies the given angle to translate it into the [0,2pi[ range.</span>
<a name="l00161"></a>00161 <span class="comment"> * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.</span>
<a name="l00162"></a>00162 <span class="comment"> * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence</span>
<a name="l00163"></a>00163 <span class="comment"> */</span>
<a name="l00164"></a>00164 <span class="keyword">template</span> <<span class="keyword">class</span> T>
<a name="l00165"></a><a class="code" href="group__container__ops__grp.html#gaf3dcff278e3ccf372395351cfbb4d3dd">00165</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="group__container__ops__grp.html#gaf3dcff278e3ccf372395351cfbb4d3dd" title="Modifies the given angle to translate it into the [0,2pi[ range.">wrapTo2PiInPlace</a>(T &a)
<a name="l00166"></a>00166 {
<a name="l00167"></a>00167 <span class="keywordtype">bool</span> was_neg = a<0;
<a name="l00168"></a>00168 a = fmod(a, static_cast<T>(<a class="code" href="mrpt__macros_8h.html#a4aa2c43b09c1300c334821f5507d6f71">M_2PI</a>) );
<a name="l00169"></a>00169 <span class="keywordflow">if</span> (was_neg) a+=<span class="keyword">static_cast<</span>T<span class="keyword">></span>(<a class="code" href="mrpt__macros_8h.html#a4aa2c43b09c1300c334821f5507d6f71">M_2PI</a>);
<a name="l00170"></a>00170 }
<a name="l00171"></a>00171 <span class="comment"></span>
<a name="l00172"></a>00172 <span class="comment"> /** Modifies the given angle to translate it into the [0,2pi[ range.</span>
<a name="l00173"></a>00173 <span class="comment"> * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.</span>
<a name="l00174"></a>00174 <span class="comment"> * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence</span>
<a name="l00175"></a>00175 <span class="comment"> */</span>
<a name="l00176"></a>00176 <span class="keyword">template</span> <<span class="keyword">class</span> T>
<a name="l00177"></a><a class="code" href="group__container__ops__grp.html#ga81ffbc190c3274c294a71aad568e98e3">00177</a> <span class="keyword">inline</span> T <a class="code" href="group__container__ops__grp.html#ga81ffbc190c3274c294a71aad568e98e3" title="Modifies the given angle to translate it into the [0,2pi[ range.">wrapTo2Pi</a>(T a)
<a name="l00178"></a>00178 {
<a name="l00179"></a>00179 <a class="code" href="group__container__ops__grp.html#gaf3dcff278e3ccf372395351cfbb4d3dd" title="Modifies the given angle to translate it into the [0,2pi[ range.">wrapTo2PiInPlace</a>(a);
<a name="l00180"></a>00180 <span class="keywordflow">return</span> a;
<a name="l00181"></a>00181 }
<a name="l00182"></a>00182 <span class="comment"></span>
<a name="l00183"></a>00183 <span class="comment"> /** Modifies the given angle to translate it into the ]-pi,pi] range.</span>
<a name="l00184"></a>00184 <span class="comment"> * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.</span>
<a name="l00185"></a>00185 <span class="comment"> * \sa wrapTo2Pi, wrapToPiInPlace, unwrap2PiSequence</span>
<a name="l00186"></a>00186 <span class="comment"> */</span>
<a name="l00187"></a>00187 <span class="keyword">template</span> <<span class="keyword">class</span> T>
<a name="l00188"></a><a class="code" href="group__container__ops__grp.html#gaa2ddb99c5a5532075417f855b1c132fd">00188</a> <span class="keyword">inline</span> T <a class="code" href="group__container__ops__grp.html#gaa2ddb99c5a5532075417f855b1c132fd" title="Modifies the given angle to translate it into the ]-pi,pi] range.">wrapToPi</a>(T a)
<a name="l00189"></a>00189 {
<a name="l00190"></a>00190 <span class="keywordflow">return</span> <a class="code" href="group__container__ops__grp.html#ga81ffbc190c3274c294a71aad568e98e3" title="Modifies the given angle to translate it into the [0,2pi[ range.">wrapTo2Pi</a>( a + static_cast<T>(<a class="code" href="mrpt__macros_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>) )-<span class="keyword">static_cast<</span>T<span class="keyword">></span>(<a class="code" href="mrpt__macros_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>);
<a name="l00191"></a>00191 }
<a name="l00192"></a>00192 <span class="comment"></span>
<a name="l00193"></a>00193 <span class="comment"> /** Modifies the given angle to translate it into the ]-pi,pi] range.</span>
<a name="l00194"></a>00194 <span class="comment"> * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.</span>
<a name="l00195"></a>00195 <span class="comment"> * \sa wrapToPi,wrapTo2Pi, unwrap2PiSequence</span>
<a name="l00196"></a>00196 <span class="comment"> */</span>
<a name="l00197"></a>00197 <span class="keyword">template</span> <<span class="keyword">class</span> T>
<a name="l00198"></a><a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42">00198</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42" title="Modifies the given angle to translate it into the ]-pi,pi] range.">wrapToPiInPlace</a>(T &a)
<a name="l00199"></a>00199 {
<a name="l00200"></a>00200 a = <a class="code" href="group__container__ops__grp.html#gaa2ddb99c5a5532075417f855b1c132fd" title="Modifies the given angle to translate it into the ]-pi,pi] range.">wrapToPi</a>(a);
<a name="l00201"></a>00201 }
<a name="l00202"></a>00202
<a name="l00203"></a>00203 <span class="comment"></span>
<a name="l00204"></a>00204 <span class="comment"> /** Normalize a vector, such as its norm is the unity.</span>
<a name="l00205"></a>00205 <span class="comment"> * If the vector has a null norm, the output is a null vector.</span>
<a name="l00206"></a>00206 <span class="comment"> */</span>
<a name="l00207"></a>00207 <span class="keyword">template</span><<span class="keyword">class</span> VEC1,<span class="keyword">class</span> VEC2>
<a name="l00208"></a><a class="code" href="group__container__ops__grp.html#gab64bd509dec6f39f1db1086bd48b5f45">00208</a> <span class="keywordtype">void</span> <a class="code" href="group__container__ops__grp.html#gab64bd509dec6f39f1db1086bd48b5f45" title="Normalize a vector, such as its norm is the unity.">normalize</a>(<span class="keyword">const</span> VEC1 &v, VEC2 &out_v)
<a name="l00209"></a>00209 {
<a name="l00210"></a>00210 <span class="keyword">typename</span> VEC1<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a> total=0;
<a name="l00211"></a>00211 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = v.size();
<a name="l00212"></a>00212 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<N;i++)
<a name="l00213"></a>00213 total += <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(v[i]);
<a name="l00214"></a>00214 total = std::sqrt(total);
<a name="l00215"></a>00215 <span class="keywordflow">if</span> (total)
<a name="l00216"></a>00216 {
<a name="l00217"></a>00217 out_v = v * (1.0/total);
<a name="l00218"></a>00218 }
<a name="l00219"></a>00219 <span class="keywordflow">else</span> out_v.assign(v.size(),0);
<a name="l00220"></a>00220 }
<a name="l00221"></a>00221 <span class="comment"></span>
<a name="l00222"></a>00222 <span class="comment"> /** Computes covariances and mean of any vector of containers, given optional weights for the different samples.</span>
<a name="l00223"></a>00223 <span class="comment"> * \param elements Any kind of vector of vectors/arrays, eg. std::vector<vector_double>, with all the input samples, each sample in a "row".</span>
<a name="l00224"></a>00224 <span class="comment"> * \param covariances Output estimated covariance; it can be a fixed/dynamic matrix or a matrixview.</span>
<a name="l00225"></a>00225 <span class="comment"> * \param means Output estimated mean; it can be vector_double/CArrayDouble, etc...</span>
<a name="l00226"></a>00226 <span class="comment"> * \param weights_mean If !=NULL, it must point to a vector of size()==number of elements, with normalized weights to take into account for the mean.</span>
<a name="l00227"></a>00227 <span class="comment"> * \param weights_cov If !=NULL, it must point to a vector of size()==number of elements, with normalized weights to take into account for the covariance.</span>
<a name="l00228"></a>00228 <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="l00229"></a>00229 <span class="comment"> * \sa This method is used in mrpt::math::unscented_transform_gaussian</span>
<a name="l00230"></a>00230 <span class="comment"> * \ingroup stats_grp</span>
<a name="l00231"></a>00231 <span class="comment"> */</span>
<a name="l00232"></a>00232 <span class="keyword">template</span><<span class="keyword">class</span> VECTOR_OF_VECTORS, <span class="keyword">class</span> MATRIXLIKE,<span class="keyword">class</span> VECTORLIKE,<span class="keyword">class</span> VECTORLIKE2,<span class="keyword">class</span> VECTORLIKE3>
<a name="l00233"></a><a class="code" href="group__stats__grp.html#gaf0a0f292b7248680014f55effd35873f">00233</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <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...">covariancesAndMeanWeighted</a>( <span class="comment">// Done inline to speed-up the special case expanded in covariancesAndMean() below.</span>
<a name="l00234"></a>00234 <span class="keyword">const</span> VECTOR_OF_VECTORS &elements,
<a name="l00235"></a>00235 MATRIXLIKE &covariances,
<a name="l00236"></a>00236 VECTORLIKE &means,
<a name="l00237"></a>00237 <span class="keyword">const</span> VECTORLIKE2 *weights_mean,
<a name="l00238"></a>00238 <span class="keyword">const</span> VECTORLIKE3 *weights_cov,
<a name="l00239"></a>00239 <span class="keyword">const</span> <span class="keywordtype">bool</span> *elem_do_wrap2pi = NULL
<a name="l00240"></a>00240 )
<a name="l00241"></a>00241 {
<a name="l00242"></a>00242 <a class="code" href="mrpt__macros_8h.html#ad30ea0382c594c0e2efe88212e9352b0">ASSERTMSG_</a>(elements.size()!=0,<span class="stringliteral">"No samples provided, so there is no way to deduce the output size."</span>)
<a name="l00243"></a>00243 <span class="keyword">typedef</span> <span class="keyword">typename</span> <a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">VECTORLIKE::value_type</a> T;
<a name="l00244"></a>00244 <span class="keyword">const</span> <span class="keywordtype">size_t</span> DIM = elements[0].size();
<a name="l00245"></a>00245 means.resize(DIM);
<a name="l00246"></a>00246 covariances.setSize(DIM,DIM);
<a name="l00247"></a>00247 <span class="keyword">const</span> <span class="keywordtype">size_t</span> nElms=elements.size();
<a name="l00248"></a>00248 <span class="keyword">const</span> T NORM=1.0/nElms;
<a name="l00249"></a>00249 <span class="keywordflow">if</span> (weights_mean) { <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(<span class="keywordtype">size_t</span>(weights_mean->size())==<span class="keywordtype">size_t</span>(nElms)) }
<a name="l00250"></a>00250 <span class="comment">// The mean goes first:</span>
<a name="l00251"></a>00251 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<DIM;i++)
<a name="l00252"></a>00252 {
<a name="l00253"></a>00253 T accum = 0;
<a name="l00254"></a>00254 <span class="keywordflow">if</span> (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
<a name="l00255"></a>00255 { <span class="comment">// i'th dimension is a "normal", real number:</span>
<a name="l00256"></a>00256 <span class="keywordflow">if</span> (weights_mean)
<a name="l00257"></a>00257 {
<a name="l00258"></a>00258 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0;j<nElms;j++)
<a name="l00259"></a>00259 accum+= (*weights_mean)[j] * elements[j][i];
<a name="l00260"></a>00260 }
<a name="l00261"></a>00261 <span class="keywordflow">else</span>
<a name="l00262"></a>00262 {
<a name="l00263"></a>00263 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0;j<nElms;j++) accum+=elements[j][i];
<a name="l00264"></a>00264 accum*=NORM;
<a name="l00265"></a>00265 }
<a name="l00266"></a>00266 }
<a name="l00267"></a>00267 <span class="keywordflow">else</span>
<a name="l00268"></a>00268 { <span class="comment">// i'th dimension is a circle in [-pi,pi]: we need a little trick here:</span>
<a name="l00269"></a>00269 <span class="keywordtype">double</span> accum_L=0,accum_R=0;
<a name="l00270"></a>00270 <span class="keywordtype">double</span> Waccum_L=0,Waccum_R=0;
<a name="l00271"></a>00271 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0;j<nElms;j++)
<a name="l00272"></a>00272 {
<a name="l00273"></a>00273 <span class="keywordtype">double</span> ang = elements[j][i];
<a name="l00274"></a>00274 <span class="keyword">const</span> <span class="keywordtype">double</span> w = weights_mean!=NULL ? (*weights_mean)[j] : NORM;
<a name="l00275"></a>00275 <span class="keywordflow">if</span> (fabs( ang )>0.5*<a class="code" href="mrpt__macros_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>)
<a name="l00276"></a>00276 { <span class="comment">// LEFT HALF: 0,2pi</span>
<a name="l00277"></a>00277 <span class="keywordflow">if</span> (ang<0) ang = (<a class="code" href="mrpt__macros_8h.html#a4aa2c43b09c1300c334821f5507d6f71">M_2PI</a> + ang);
<a name="l00278"></a>00278 accum_L += ang * w;
<a name="l00279"></a>00279 Waccum_L += w;
<a name="l00280"></a>00280 }
<a name="l00281"></a>00281 <span class="keywordflow">else</span>
<a name="l00282"></a>00282 { <span class="comment">// RIGHT HALF: -pi,pi</span>
<a name="l00283"></a>00283 accum_R += ang * w;
<a name="l00284"></a>00284 Waccum_R += w;
<a name="l00285"></a>00285 }
<a name="l00286"></a>00286 }
<a name="l00287"></a>00287 <span class="keywordflow">if</span> (Waccum_L>0) accum_L /= Waccum_L; <span class="comment">// [0,2pi]</span>
<a name="l00288"></a>00288 <span class="keywordflow">if</span> (Waccum_R>0) accum_R /= Waccum_R; <span class="comment">// [-pi,pi]</span>
<a name="l00289"></a>00289 <span class="keywordflow">if</span> (accum_L><a class="code" href="mrpt__macros_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>) accum_L -= <a class="code" href="mrpt__macros_8h.html#a4aa2c43b09c1300c334821f5507d6f71">M_2PI</a>; <span class="comment">// Left side to [-pi,pi] again:</span>
<a name="l00290"></a>00290 accum = (accum_L* Waccum_L + accum_R * Waccum_R ); <span class="comment">// The overall result:</span>
<a name="l00291"></a>00291 }
<a name="l00292"></a>00292 means[i]=accum;
<a name="l00293"></a>00293 }
<a name="l00294"></a>00294 <span class="comment">// Now the covariance:</span>
<a name="l00295"></a>00295 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<DIM;i++)
<a name="l00296"></a>00296 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0;j<=i;j++) <span class="comment">// Only 1/2 of the matrix</span>
<a name="l00297"></a>00297 {
<a name="l00298"></a>00298 <span class="keyword">typename</span> MATRIXLIKE<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a> elem=0;
<a name="l00299"></a>00299 <span class="keywordflow">if</span> (weights_cov)
<a name="l00300"></a>00300 {
<a name="l00301"></a>00301 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(<span class="keywordtype">size_t</span>(weights_cov->size())==<span class="keywordtype">size_t</span>(nElms))
<a name="l00302"></a>00302 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<nElms;k++)
<a name="l00303"></a>00303 {
<a name="l00304"></a>00304 <span class="keyword">const</span> T Ai = (elements[k][i]-means[i]);
<a name="l00305"></a>00305 <span class="keyword">const</span> T Aj = (elements[k][j]-means[j]);
<a name="l00306"></a>00306 <span class="keywordflow">if</span> (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
<a name="l00307"></a>00307 elem+= (*weights_cov)[k] * Ai * Aj;
<a name="l00308"></a>00308 <span class="keywordflow">else</span> elem+= (*weights_cov)[k] * <a class="code" href="group__container__ops__grp.html#gaa2ddb99c5a5532075417f855b1c132fd" title="Modifies the given angle to translate it into the ]-pi,pi] range.">mrpt::math::wrapToPi</a>(Ai) * <a class="code" href="group__container__ops__grp.html#gaa2ddb99c5a5532075417f855b1c132fd" title="Modifies the given angle to translate it into the ]-pi,pi] range.">mrpt::math::wrapToPi</a>(Aj);
<a name="l00309"></a>00309 }
<a name="l00310"></a>00310 }
<a name="l00311"></a>00311 <span class="keywordflow">else</span>
<a name="l00312"></a>00312 {
<a name="l00313"></a>00313 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<nElms;k++)
<a name="l00314"></a>00314 {
<a name="l00315"></a>00315 <span class="keyword">const</span> T Ai = (elements[k][i]-means[i]);
<a name="l00316"></a>00316 <span class="keyword">const</span> T Aj = (elements[k][j]-means[j]);
<a name="l00317"></a>00317 <span class="keywordflow">if</span> (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
<a name="l00318"></a>00318 elem+= Ai * Aj;
<a name="l00319"></a>00319 <span class="keywordflow">else</span> elem+= <a class="code" href="group__container__ops__grp.html#gaa2ddb99c5a5532075417f855b1c132fd" title="Modifies the given angle to translate it into the ]-pi,pi] range.">mrpt::math::wrapToPi</a>(Ai) * <a class="code" href="group__container__ops__grp.html#gaa2ddb99c5a5532075417f855b1c132fd" title="Modifies the given angle to translate it into the ]-pi,pi] range.">mrpt::math::wrapToPi</a>(Aj);
<a name="l00320"></a>00320 }
<a name="l00321"></a>00321 elem*=NORM;
<a name="l00322"></a>00322 }
<a name="l00323"></a>00323 covariances.get_unsafe(i,j) = elem;
<a name="l00324"></a>00324 <span class="keywordflow">if</span> (i!=j) covariances.get_unsafe(j,i)=elem;
<a name="l00325"></a>00325 }
<a name="l00326"></a>00326 }
<a name="l00327"></a>00327 <span class="comment"></span>
<a name="l00328"></a>00328 <span class="comment"> /** Computes covariances and mean of any vector of containers.</span>
<a name="l00329"></a>00329 <span class="comment"> * \param elements Any kind of vector of vectors/arrays, eg. std::vector<vector_double>, with all the input samples, each sample in a "row".</span>
<a name="l00330"></a>00330 <span class="comment"> * \param covariances Output estimated covariance; it can be a fixed/dynamic matrix or a matrixview.</span>
<a name="l00331"></a>00331 <span class="comment"> * \param means Output estimated mean; it can be vector_double/CArrayDouble, etc...</span>
<a name="l00332"></a>00332 <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="l00333"></a>00333 <span class="comment"> * \ingroup stats_grp</span>
<a name="l00334"></a>00334 <span class="comment"> */</span>
<a name="l00335"></a>00335 <span class="keyword">template</span><<span class="keyword">class</span> VECTOR_OF_VECTORS, <span class="keyword">class</span> MATRIXLIKE,<span class="keyword">class</span> VECTORLIKE>
<a name="l00336"></a><a class="code" href="group__stats__grp.html#gaa1cf7357c4043fb790efe19d3f6c2b7d">00336</a> <span class="keywordtype">void</span> <a class="code" href="group__stats__grp.html#gaa1cf7357c4043fb790efe19d3f6c2b7d" title="Computes covariances and mean of any vector of containers.">covariancesAndMean</a>(<span class="keyword">const</span> VECTOR_OF_VECTORS &elements,MATRIXLIKE &covariances,VECTORLIKE &means, <span class="keyword">const</span> <span class="keywordtype">bool</span> *elem_do_wrap2pi = NULL)
<a name="l00337"></a>00337 { <span class="comment">// The function below is inline-expanded here:</span>
<a name="l00338"></a>00338 covariancesAndMeanWeighted<VECTOR_OF_VECTORS,MATRIXLIKE,VECTORLIKE,vector_double,vector_double>(elements,covariances,means,NULL,NULL,elem_do_wrap2pi);
<a name="l00339"></a>00339 }
<a name="l00340"></a>00340
<a name="l00341"></a>00341 <span class="comment"></span>
<a name="l00342"></a>00342 <span class="comment"> /** Computes the weighted histogram for a vector of values and their corresponding weights.</span>
<a name="l00343"></a>00343 <span class="comment"> * \param values [IN] The N values</span>
<a name="l00344"></a>00344 <span class="comment"> * \param weights [IN] The weights for the corresponding N values (don't need to be normalized)</span>
<a name="l00345"></a>00345 <span class="comment"> * \param binWidth [IN] The desired width of the bins</span>
<a name="l00346"></a>00346 <span class="comment"> * \param out_binCenters [OUT] The centers of the M bins generated to cover from the minimum to the maximum value of "values" with the given "binWidth"</span>
<a name="l00347"></a>00347 <span class="comment"> * \param out_binValues [OUT] The ratio of values at each given bin, such as the whole vector sums up the unity.</span>
<a name="l00348"></a>00348 <span class="comment"> * \sa weightedHistogramLog</span>
<a name="l00349"></a>00349 <span class="comment"> */</span>
<a name="l00350"></a>00350 <span class="keyword">template</span><<span class="keyword">class</span> VECTORLIKE1,<span class="keyword">class</span> VECTORLIKE2>
<a name="l00351"></a><a class="code" href="group__container__ops__grp.html#ga4ca4d8616fc5c34fbaddd4d479060b91">00351</a> <span class="keywordtype">void</span> <a class="code" href="group__container__ops__grp.html#ga4ca4d8616fc5c34fbaddd4d479060b91" title="Computes the weighted histogram for a vector of values and their corresponding weights.">weightedHistogram</a>(
<a name="l00352"></a>00352 <span class="keyword">const</span> VECTORLIKE1 &values,
<a name="l00353"></a>00353 <span class="keyword">const</span> VECTORLIKE1 &weights,
<a name="l00354"></a>00354 <span class="keywordtype">float</span> binWidth,
<a name="l00355"></a>00355 VECTORLIKE2 &out_binCenters,
<a name="l00356"></a>00356 VECTORLIKE2 &out_binValues )
<a name="l00357"></a>00357 {
<a name="l00358"></a>00358 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00359"></a>00359 <span class="keyword">typedef</span> <span class="keyword">typename</span> VECTORLIKE1<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a> TNum;
<a name="l00360"></a>00360
<a name="l00361"></a>00361 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( values.size() == weights.size() );
<a name="l00362"></a>00362 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( binWidth > 0 );
<a name="l00363"></a>00363 TNum minBin = <a class="code" href="namespacemrpt_1_1math.html#a2aad5c4db5fdafc3f8c83f7b1adcbfda">minimum</a>( values );
<a name="l00364"></a>00364 <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> nBins = <span class="keyword">static_cast<</span><span class="keywordtype">unsigned</span><span class="keyword">></span>(ceil((<a class="code" href="namespacemrpt_1_1math.html#adeb0bd3138c2f716e4b0317cacfc46a1">maximum</a>( values )-minBin) / binWidth));
<a name="l00365"></a>00365
<a name="l00366"></a>00366 <span class="comment">// Generate bin center and border values:</span>
<a name="l00367"></a>00367 out_binCenters.resize(nBins);
<a name="l00368"></a>00368 out_binValues.clear(); out_binValues.resize(nBins,0);
<a name="l00369"></a>00369 TNum halfBin = TNum(0.5)*binWidth;;
<a name="l00370"></a>00370 VECTORLIKE2 binBorders(nBins+1,minBin-halfBin);
<a name="l00371"></a>00371 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i=0;i<nBins;i++)
<a name="l00372"></a>00372 {
<a name="l00373"></a>00373 binBorders[i+1] = binBorders[i]+binWidth;
<a name="l00374"></a>00374 out_binCenters[i] = binBorders[i]+halfBin;
<a name="l00375"></a>00375 }
<a name="l00376"></a>00376
<a name="l00377"></a>00377 <span class="comment">// Compute the histogram:</span>
<a name="l00378"></a>00378 TNum totalSum = 0;
<a name="l00379"></a>00379 <span class="keyword">typename</span> VECTORLIKE1<a class="code" href="eigen__plugins_8h.html#a8dbda719917732693c56cee228465ed9">::const_iterator</a> itVal, itW;
<a name="l00380"></a>00380 <span class="keywordflow">for</span> (itVal = values.begin(), itW = weights.begin(); itVal!=values.end(); ++itVal, ++itW )
<a name="l00381"></a>00381 {
<a name="l00382"></a>00382 <span class="keywordtype">int</span> idx = <a class="code" href="namespacemrpt_1_1utils.html#ab7d9cdf7d271c2f41fc1c5c9fa7d0828" title="Returns the closer integer (int) to x.">round</a>(((*itVal)-minBin)/binWidth);
<a name="l00383"></a>00383 <span class="keywordflow">if</span> (idx>=<span class="keywordtype">int</span>(nBins)) idx=nBins-1;
<a name="l00384"></a>00384 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(idx>=0);
<a name="l00385"></a>00385 out_binValues[idx] += *itW;
<a name="l00386"></a>00386 totalSum+= *itW;
<a name="l00387"></a>00387 }
<a name="l00388"></a>00388
<a name="l00389"></a>00389 <span class="keywordflow">if</span> (totalSum)
<a name="l00390"></a>00390 out_binValues /= totalSum;
<a name="l00391"></a>00391
<a name="l00392"></a>00392
<a name="l00393"></a>00393 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00394"></a>00394 }
<a name="l00395"></a>00395 <span class="comment"></span>
<a name="l00396"></a>00396 <span class="comment"> /** Computes the weighted histogram for a vector of values and their corresponding log-weights.</span>
<a name="l00397"></a>00397 <span class="comment"> * \param values [IN] The N values</span>
<a name="l00398"></a>00398 <span class="comment"> * \param weights [IN] The log-weights for the corresponding N values (don't need to be normalized)</span>
<a name="l00399"></a>00399 <span class="comment"> * \param binWidth [IN] The desired width of the bins</span>
<a name="l00400"></a>00400 <span class="comment"> * \param out_binCenters [OUT] The centers of the M bins generated to cover from the minimum to the maximum value of "values" with the given "binWidth"</span>
<a name="l00401"></a>00401 <span class="comment"> * \param out_binValues [OUT] The ratio of values at each given bin, such as the whole vector sums up the unity.</span>
<a name="l00402"></a>00402 <span class="comment"> * \sa weightedHistogram</span>
<a name="l00403"></a>00403 <span class="comment"> */</span>
<a name="l00404"></a>00404 <span class="keyword">template</span><<span class="keyword">class</span> VECTORLIKE1,<span class="keyword">class</span> VECTORLIKE2>
<a name="l00405"></a><a class="code" href="group__container__ops__grp.html#gabc0b6008e33cc8852a18b746d2fa84d4">00405</a> <span class="keywordtype">void</span> <a class="code" href="group__container__ops__grp.html#gabc0b6008e33cc8852a18b746d2fa84d4" title="Computes the weighted histogram for a vector of values and their corresponding log-weights.">weightedHistogramLog</a>(
<a name="l00406"></a>00406 <span class="keyword">const</span> VECTORLIKE1 &values,
<a name="l00407"></a>00407 <span class="keyword">const</span> VECTORLIKE1 &log_weights,
<a name="l00408"></a>00408 <span class="keywordtype">float</span> binWidth,
<a name="l00409"></a>00409 VECTORLIKE2 &out_binCenters,
<a name="l00410"></a>00410 VECTORLIKE2 &out_binValues )
<a name="l00411"></a>00411 {
<a name="l00412"></a>00412 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00413"></a>00413 <span class="keyword">typedef</span> <span class="keyword">typename</span> VECTORLIKE1<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a> TNum;
<a name="l00414"></a>00414
<a name="l00415"></a>00415 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( values.size() == log_weights.size() );
<a name="l00416"></a>00416 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( binWidth > 0 );
<a name="l00417"></a>00417 TNum minBin = <a class="code" href="namespacemrpt_1_1math.html#a2aad5c4db5fdafc3f8c83f7b1adcbfda">minimum</a>( values );
<a name="l00418"></a>00418 <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> nBins = <span class="keyword">static_cast<</span><span class="keywordtype">unsigned</span><span class="keyword">></span>(ceil((<a class="code" href="namespacemrpt_1_1math.html#adeb0bd3138c2f716e4b0317cacfc46a1">maximum</a>( values )-minBin) / binWidth));
<a name="l00419"></a>00419
<a name="l00420"></a>00420 <span class="comment">// Generate bin center and border values:</span>
<a name="l00421"></a>00421 out_binCenters.resize(nBins);
<a name="l00422"></a>00422 out_binValues.clear(); out_binValues.resize(nBins,0);
<a name="l00423"></a>00423 TNum halfBin = TNum(0.5)*binWidth;;
<a name="l00424"></a>00424 VECTORLIKE2 binBorders(nBins+1,minBin-halfBin);
<a name="l00425"></a>00425 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i=0;i<nBins;i++)
<a name="l00426"></a>00426 {
<a name="l00427"></a>00427 binBorders[i+1] = binBorders[i]+binWidth;
<a name="l00428"></a>00428 out_binCenters[i] = binBorders[i]+halfBin;
<a name="l00429"></a>00429 }
<a name="l00430"></a>00430
<a name="l00431"></a>00431 <span class="comment">// Compute the histogram:</span>
<a name="l00432"></a>00432 <span class="keyword">const</span> TNum max_log_weight = <a class="code" href="namespacemrpt_1_1math.html#adeb0bd3138c2f716e4b0317cacfc46a1">maximum</a>(log_weights);
<a name="l00433"></a>00433 TNum totalSum = 0;
<a name="l00434"></a>00434 <span class="keyword">typename</span> VECTORLIKE1<a class="code" href="eigen__plugins_8h.html#a8dbda719917732693c56cee228465ed9">::const_iterator</a> itVal, itW;
<a name="l00435"></a>00435 <span class="keywordflow">for</span> (itVal = values.begin(), itW = log_weights.begin(); itVal!=values.end(); ++itVal, ++itW )
<a name="l00436"></a>00436 {
<a name="l00437"></a>00437 <span class="keywordtype">int</span> idx = <a class="code" href="namespacemrpt_1_1utils.html#ab7d9cdf7d271c2f41fc1c5c9fa7d0828" title="Returns the closer integer (int) to x.">round</a>(((*itVal)-minBin)/binWidth);
<a name="l00438"></a>00438 <span class="keywordflow">if</span> (idx>=<span class="keywordtype">int</span>(nBins)) idx=nBins-1;
<a name="l00439"></a>00439 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(idx>=0);
<a name="l00440"></a>00440 <span class="keyword">const</span> TNum w = exp(*itW-max_log_weight);
<a name="l00441"></a>00441 out_binValues[idx] += w;
<a name="l00442"></a>00442 totalSum+= w;
<a name="l00443"></a>00443 }
<a name="l00444"></a>00444
<a name="l00445"></a>00445 <span class="keywordflow">if</span> (totalSum)
<a name="l00446"></a>00446 out_binValues /= totalSum;
<a name="l00447"></a>00447
<a name="l00448"></a>00448 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00449"></a>00449 }
<a name="l00450"></a>00450
<a name="l00451"></a>00451 <span class="comment"></span>
<a name="l00452"></a>00452 <span class="comment"> /** Extract a column from a vector of vectors, and store it in another vector.</span>
<a name="l00453"></a>00453 <span class="comment"> * - Input data can be: std::vector<vector_double>, std::deque<std::list<double> >, std::list<CArrayDouble<5> >, etc. etc.</span>
<a name="l00454"></a>00454 <span class="comment"> * - Output is the sequence: data[0][idx],data[1][idx],data[2][idx], etc..</span>
<a name="l00455"></a>00455 <span class="comment"> *</span>
<a name="l00456"></a>00456 <span class="comment"> * For the sake of generality, this function does NOT check the limits in the number of column, unless it's implemented in the [] operator of each of the "rows".</span>
<a name="l00457"></a>00457 <span class="comment"> */</span>
<a name="l00458"></a>00458 <span class="keyword">template</span> <<span class="keyword">class</span> VECTOR_OF_VECTORS, <span class="keyword">class</span> VECTORLIKE>
<a name="l00459"></a><a class="code" href="group__container__ops__grp.html#ga7e35ac8264c0ad08d184f5e50ceca5d6">00459</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="group__container__ops__grp.html#ga7e35ac8264c0ad08d184f5e50ceca5d6" title="Extract a column from a vector of vectors, and store it in another vector.">extractColumnFromVectorOfVectors</a>(<span class="keyword">const</span> <span class="keywordtype">size_t</span> colIndex, <span class="keyword">const</span> VECTOR_OF_VECTORS &data, VECTORLIKE &out_column)
<a name="l00460"></a>00460 {
<a name="l00461"></a>00461 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = data.size();
<a name="l00462"></a>00462 out_column.resize(N);
<a name="l00463"></a>00463 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<N;i++)
<a name="l00464"></a>00464 out_column[i]=data[i][colIndex];
<a name="l00465"></a>00465 }
<a name="l00466"></a>00466 <span class="comment"></span>
<a name="l00467"></a>00467 <span class="comment"> /** Computes the factorial of an integer number and returns it as a 64-bit integer number.</span>
<a name="l00468"></a>00468 <span class="comment"> */</span>
<a name="l00469"></a>00469 uint64_t <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__container__ops__grp.html#gabde7e0c25e9bb6a0a962f8051fe8c19d" title="Computes the factorial of an integer number and returns it as a 64-bit integer number.">factorial64</a>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> n);
<a name="l00470"></a>00470 <span class="comment"></span>
<a name="l00471"></a>00471 <span class="comment"> /** Computes the factorial of an integer number and returns it as a double value (internally it uses logarithms for avoiding overflow).</span>
<a name="l00472"></a>00472 <span class="comment"> */</span>
<a name="l00473"></a>00473 <span class="keywordtype">double</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__container__ops__grp.html#ga2fbe27097242531b344c8d5b9fe148c9" title="Computes the factorial of an integer number and returns it as a double value (internally it uses loga...">factorial</a>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> n);
<a name="l00474"></a>00474 <span class="comment"></span>
<a name="l00475"></a>00475 <span class="comment"> /** Round up to the nearest power of two of a given number</span>
<a name="l00476"></a>00476 <span class="comment"> */</span>
<a name="l00477"></a>00477 <span class="keyword">template</span> <<span class="keyword">class</span> T>
<a name="l00478"></a><a class="code" href="group__container__ops__grp.html#gae238d11a8827f1a9d10792fb8d26b0a1">00478</a> T <a class="code" href="group__container__ops__grp.html#gae238d11a8827f1a9d10792fb8d26b0a1" title="Round up to the nearest power of two of a given number.">round2up</a>(T val)
<a name="l00479"></a>00479 {
<a name="l00480"></a>00480 T n = 1;
<a name="l00481"></a>00481 <span class="keywordflow">while</span> (n < val)
<a name="l00482"></a>00482 {
<a name="l00483"></a>00483 n <<= 1;
<a name="l00484"></a>00484 <span class="keywordflow">if</span> (n<=1)
<a name="l00485"></a>00485 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"Overflow!"</span>);
<a name="l00486"></a>00486 }
<a name="l00487"></a>00487 <span class="keywordflow">return</span> n;
<a name="l00488"></a>00488 }
<a name="l00489"></a>00489 <span class="comment"></span>
<a name="l00490"></a>00490 <span class="comment"> /** Round a decimal number up to the given 10'th power (eg, to 1000,100,10, and also fractions)</span>
<a name="l00491"></a>00491 <span class="comment"> * power10 means round up to: 1 -> 10, 2 -> 100, 3 -> 1000, ... -1 -> 0.1, -2 -> 0.01, ...</span>
<a name="l00492"></a>00492 <span class="comment"> */</span>
<a name="l00493"></a>00493 <span class="keyword">template</span> <<span class="keyword">class</span> T>
<a name="l00494"></a><a class="code" href="group__container__ops__grp.html#ga2b186cb9b674da4aa7b697fe8cd57567">00494</a> T <a class="code" href="group__container__ops__grp.html#ga2b186cb9b674da4aa7b697fe8cd57567" title="Round a decimal number up to the given 10'th power (eg, to 1000,100,10, and also fractions) power10 m...">round_10power</a>(T val, <span class="keywordtype">int</span> power10)
<a name="l00495"></a>00495 {
<a name="l00496"></a>00496 <span class="keywordtype">long</span> <span class="keywordtype">double</span> F = ::pow((<span class="keywordtype">long</span> <span class="keywordtype">double</span>)10.0,-(<span class="keywordtype">long</span> <span class="keywordtype">double</span>)power10);
<a name="l00497"></a>00497 <span class="keywordtype">long</span> <span class="keywordtype">int</span> <a class="code" href="eigen__plugins_8h.html#a7b88b312dc3827120dbfc60da344625d" title="Transpose.">t</a> = <a class="code" href="namespacemrpt_1_1utils.html#a2e46eb8436c85065115cdbdd735d1493" title="Returns the closer integer (long) to x.">round_long</a>( val * F );
<a name="l00498"></a>00498 <span class="keywordflow">return</span> T(t/F);
<a name="l00499"></a>00499 }
<a name="l00500"></a>00500 <span class="comment"></span>
<a name="l00501"></a>00501 <span class="comment"> /** Calculate the correlation between two matrices</span>
<a name="l00502"></a>00502 <span class="comment"> * (by AJOGD @ JAN-2007)</span>
<a name="l00503"></a>00503 <span class="comment"> */</span>
<a name="l00504"></a>00504 <span class="keyword">template</span><<span class="keyword">class</span> T>
<a name="l00505"></a><a class="code" href="group__container__ops__grp.html#ga914e0802031e773745a2c99c3aebdec1">00505</a> <span class="keywordtype">double</span> <a class="code" href="group__container__ops__grp.html#ga914e0802031e773745a2c99c3aebdec1" title="Calculate the correlation between two matrices (by AJOGD @ JAN-2007)">correlate_matrix</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<T></a> &a1, <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<T></a> &a2)
<a name="l00506"></a>00506 {
<a name="l00507"></a>00507 <span class="keywordflow">if</span> ((a1.getColCount()!=a2.getColCount())|(a1.getRowCount()!=a2.getRowCount()))
<a name="l00508"></a>00508 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"Correlation Error!, images with no same size"</span>);
<a name="l00509"></a>00509
<a name="l00510"></a>00510 <span class="keywordtype">int</span> i,j;
<a name="l00511"></a>00511 T x1,x2;
<a name="l00512"></a>00512 T syy=0, sxy=0, sxx=0, m1=0, m2=0 ,n=a1.getRowCount()*a2.getColCount();
<a name="l00513"></a>00513
<a name="l00514"></a>00514 <span class="comment">//find the means</span>
<a name="l00515"></a>00515 <span class="keywordflow">for</span> (i=0;i<a1.getRowCount();i++)
<a name="l00516"></a>00516 {
<a name="l00517"></a>00517 <span class="keywordflow">for</span> (j=0;j<a1.getColCount();j++)
<a name="l00518"></a>00518 {
<a name="l00519"></a>00519 m1 += a1(i,j);
<a name="l00520"></a>00520 m2 += a2(i,j);
<a name="l00521"></a>00521 }
<a name="l00522"></a>00522 }
<a name="l00523"></a>00523 m1 /= n;
<a name="l00524"></a>00524 m2 /= n;
<a name="l00525"></a>00525
<a name="l00526"></a>00526 <span class="keywordflow">for</span> (i=0;i<a1.getRowCount();i++)
<a name="l00527"></a>00527 {
<a name="l00528"></a>00528 <span class="keywordflow">for</span> (j=0;j<a1.getColCount();j++)
<a name="l00529"></a>00529 {
<a name="l00530"></a>00530 x1 = a1(i,j) - m1;
<a name="l00531"></a>00531 x2 = a2(i,j) - m2;
<a name="l00532"></a>00532 sxx += x1*x1;
<a name="l00533"></a>00533 syy += x2*x2;
<a name="l00534"></a>00534 sxy += x1*x2;
<a name="l00535"></a>00535 }
<a name="l00536"></a>00536 }
<a name="l00537"></a>00537
<a name="l00538"></a>00538 <span class="keywordflow">return</span> sxy / sqrt(sxx * syy);
<a name="l00539"></a>00539 }
<a name="l00540"></a>00540 <span class="comment"></span>
<a name="l00541"></a>00541 <span class="comment"> /** A numerically-stable method to compute average likelihood values with strongly different ranges (unweighted likelihoods: compute the arithmetic mean).</span>
<a name="l00542"></a>00542 <span class="comment"> * This method implements this equation:</span>
<a name="l00543"></a>00543 <span class="comment"> *</span>
<a name="l00544"></a>00544 <span class="comment"> * \f[ return = - \log N + \log \sum_{i=1}^N e^{ll_i-ll_{max}} + ll_{max} \f]</span>
<a name="l00545"></a>00545 <span class="comment"> *</span>
<a name="l00546"></a>00546 <span class="comment"> * See also the <a href="http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial page</a>.</span>
<a name="l00547"></a>00547 <span class="comment"> * \ingroup stats_grp</span>
<a name="l00548"></a>00548 <span class="comment"> */</span>
<a name="l00549"></a>00549 <span class="keywordtype">double</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__stats__grp.html#ga6cb458d9d6c45fae59084fc57e88666b" title="A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...">averageLogLikelihood</a>( <span class="keyword">const</span> <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> &logLikelihoods );
<a name="l00550"></a>00550 <span class="comment"></span>
<a name="l00551"></a>00551 <span class="comment"> /** Computes the average of a sequence of angles in radians taking into account the correct wrapping in the range \f$ ]-\pi,\pi [ \f$, for example, the mean of (2,-2) is \f$ \pi \f$, not 0.</span>
<a name="l00552"></a>00552 <span class="comment"> * \ingroup stats_grp</span>
<a name="l00553"></a>00553 <span class="comment"> */</span>
<a name="l00554"></a>00554 <span class="keywordtype">double</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__stats__grp.html#ga3bd9ff688b42b58bd8c2f5d6f420868f" title="Computes the average of a sequence of angles in radians taking into account the correct wrapping in t...">averageWrap2Pi</a>(<span class="keyword">const</span> <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> &angles );
<a name="l00555"></a>00555 <span class="comment"></span>
<a name="l00556"></a>00556 <span class="comment"> /** A numerically-stable method to average likelihood values with strongly different ranges (weighted likelihoods).</span>
<a name="l00557"></a>00557 <span class="comment"> * This method implements this equation:</span>
<a name="l00558"></a>00558 <span class="comment"> *</span>
<a name="l00559"></a>00559 <span class="comment"> * \f[ return = \log \left( \frac{1}{\sum_i e^{lw_i}} \sum_i e^{lw_i} e^{ll_i} \right) \f]</span>
<a name="l00560"></a>00560 <span class="comment"> *</span>
<a name="l00561"></a>00561 <span class="comment"> * See also the <a href="http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial page</a>.</span>
<a name="l00562"></a>00562 <span class="comment"> * \ingroup stats_grp</span>
<a name="l00563"></a>00563 <span class="comment"> */</span>
<a name="l00564"></a>00564 <span class="keywordtype">double</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__stats__grp.html#ga6cb458d9d6c45fae59084fc57e88666b" title="A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...">averageLogLikelihood</a>(
<a name="l00565"></a>00565 <span class="keyword">const</span> <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> &logWeights,
<a name="l00566"></a>00566 <span class="keyword">const</span> <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> &logLikelihoods );
<a name="l00567"></a>00567 <span class="comment"></span>
<a name="l00568"></a>00568 <span class="comment"> /** Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2D Gaussian ('float' version)..</span>
<a name="l00569"></a>00569 <span class="comment"> * \param cov22 The 2x2 covariance matrix</span>
<a name="l00570"></a>00570 <span class="comment"> * \param mean The 2-length vector with the mean</span>
<a name="l00571"></a>00571 <span class="comment"> * \param stdCount How many "quantiles" to get into the area of the ellipse: 2: 95%, 3:99.97%,...</span>
<a name="l00572"></a>00572 <span class="comment"> * \param style A matlab style string, for colors, line styles,...</span>
<a name="l00573"></a>00573 <span class="comment"> * \param nEllipsePoints The number of points in the ellipse to generate</span>
<a name="l00574"></a>00574 <span class="comment"> * \ingroup stats_grp</span>
<a name="l00575"></a>00575 <span class="comment"> */</span>
<a name="l00576"></a>00576 std::string <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__stats__grp.html#ga9cbd4ab1844fcd749cfafff8a5f592a1" title="Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...">MATLAB_plotCovariance2D</a>(
<a name="l00577"></a>00577 <span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1math.html#a46578d070e41e17dead613002e755aa3" title="Declares a matrix of float numbers (non serializable).">CMatrixFloat</a> &cov22,
<a name="l00578"></a>00578 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1dynamicsize__vector.html" title="The base class of MRPT vectors, actually, Eigen column matrices of dynamic size with specialized cons...">vector_float</a> &<a class="code" href="namespacemrpt_1_1math.html#a414456e3e3b75b19cfda5e0a37c46e31" title="Computes the mean value of a vector.">mean</a>,
<a name="l00579"></a>00579 <span class="keyword">const</span> <span class="keywordtype">float</span> &stdCount,
<a name="l00580"></a>00580 <span class="keyword">const</span> <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a> &style = <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a>(<span class="stringliteral">"b"</span>),
<a name="l00581"></a>00581 <span class="keyword">const</span> <span class="keywordtype">size_t</span> &nEllipsePoints = 30 );
<a name="l00582"></a>00582 <span class="comment"></span>
<a name="l00583"></a>00583 <span class="comment"> /** Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2D Gaussian ('double' version).</span>
<a name="l00584"></a>00584 <span class="comment"> * \param cov22 The 2x2 covariance matrix</span>
<a name="l00585"></a>00585 <span class="comment"> * \param mean The 2-length vector with the mean</span>
<a name="l00586"></a>00586 <span class="comment"> * \param stdCount How many "quantiles" to get into the area of the ellipse: 2: 95%, 3:99.97%,...</span>
<a name="l00587"></a>00587 <span class="comment"> * \param style A matlab style string, for colors, line styles,...</span>
<a name="l00588"></a>00588 <span class="comment"> * \param nEllipsePoints The number of points in the ellipse to generate</span>
<a name="l00589"></a>00589 <span class="comment"> * \ingroup stats_grp</span>
<a name="l00590"></a>00590 <span class="comment"> */</span>
<a name="l00591"></a>00591 std::string <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__stats__grp.html#ga9cbd4ab1844fcd749cfafff8a5f592a1" title="Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...">MATLAB_plotCovariance2D</a>(
<a name="l00592"></a>00592 <span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1math.html#a3814c2b868f059d6a7ab0d8ecd2311d6" title="Declares a matrix of double numbers (non serializable).">CMatrixDouble</a> &cov22,
<a name="l00593"></a>00593 <span class="keyword">const</span> <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> &mean,
<a name="l00594"></a>00594 <span class="keyword">const</span> <span class="keywordtype">float</span> &stdCount,
<a name="l00595"></a>00595 <span class="keyword">const</span> <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a> &style = <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a>(<span class="stringliteral">"b"</span>),
<a name="l00596"></a>00596 <span class="keyword">const</span> <span class="keywordtype">size_t</span> &nEllipsePoints = 30 );
<a name="l00597"></a>00597
<a name="l00598"></a>00598 <span class="comment"></span>
<a name="l00599"></a>00599 <span class="comment"> /** Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part and solving the translation with dot products.</span>
<a name="l00600"></a>00600 <span class="comment"> * This is a generic template which works with:</span>
<a name="l00601"></a>00601 <span class="comment"> * MATRIXLIKE: CMatrixTemplateNumeric, CMatrixFixedNumeric</span>
<a name="l00602"></a>00602 <span class="comment"> */</span>
<a name="l00603"></a>00603 <span class="keyword">template</span> <<span class="keyword">class</span> MATRIXLIKE1,<span class="keyword">class</span> MATRIXLIKE2>
<a name="l00604"></a><a class="code" href="group__container__ops__grp.html#ga65a4cb93289c9373a8830102a2296e2d">00604</a> <span class="keywordtype">void</span> <a class="code" href="group__container__ops__grp.html#ga65a4cb93289c9373a8830102a2296e2d" title="Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part...">homogeneousMatrixInverse</a>(<span class="keyword">const</span> MATRIXLIKE1 &M, MATRIXLIKE2 &out_inverse_M)
<a name="l00605"></a>00605 {
<a name="l00606"></a>00606 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00607"></a>00607 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( M.isSquare() && <a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">size</a>(M,1)==4);
<a name="l00608"></a>00608
<a name="l00609"></a>00609 <span class="comment">/* Instead of performing a generic 4x4 matrix inversion, we only need to</span>
<a name="l00610"></a>00610 <span class="comment"> transpose the rotation part, then replace the translation part by</span>
<a name="l00611"></a>00611 <span class="comment"> three dot products. See, for example:</span>
<a name="l00612"></a>00612 <span class="comment"> https://graphics.stanford.edu/courses/cs248-98-fall/Final/q4.html</span>
<a name="l00613"></a>00613 <span class="comment"></span>
<a name="l00614"></a>00614 <span class="comment"> [ux vx wx tx] -1 [ux uy uz -dot(u,t)]</span>
<a name="l00615"></a>00615 <span class="comment"> [uy vy wy ty] [vx vy vz -dot(v,t)]</span>
<a name="l00616"></a>00616 <span class="comment"> [uz vz wz tz] = [wx wy wz -dot(w,t)]</span>
<a name="l00617"></a>00617 <span class="comment"> [ 0 0 0 1] [ 0 0 0 1 ]</span>
<a name="l00618"></a>00618 <span class="comment"> */</span>
<a name="l00619"></a>00619
<a name="l00620"></a>00620 out_inverse_M.setSize(4,4);
<a name="l00621"></a>00621
<a name="l00622"></a>00622 <span class="comment">// 3x3 rotation part:</span>
<a name="l00623"></a>00623 out_inverse_M.set_unsafe(0,0, M.get_unsafe(0,0));
<a name="l00624"></a>00624 out_inverse_M.set_unsafe(0,1, M.get_unsafe(1,0));
<a name="l00625"></a>00625 out_inverse_M.set_unsafe(0,2, M.get_unsafe(2,0));
<a name="l00626"></a>00626
<a name="l00627"></a>00627 out_inverse_M.set_unsafe(1,0, M.get_unsafe(0,1));
<a name="l00628"></a>00628 out_inverse_M.set_unsafe(1,1, M.get_unsafe(1,1));
<a name="l00629"></a>00629 out_inverse_M.set_unsafe(1,2, M.get_unsafe(2,1));
<a name="l00630"></a>00630
<a name="l00631"></a>00631 out_inverse_M.set_unsafe(2,0, M.get_unsafe(0,2));
<a name="l00632"></a>00632 out_inverse_M.set_unsafe(2,1, M.get_unsafe(1,2));
<a name="l00633"></a>00633 out_inverse_M.set_unsafe(2,2, M.get_unsafe(2,2));
<a name="l00634"></a>00634
<a name="l00635"></a>00635 <span class="keyword">const</span> <span class="keywordtype">double</span> tx = -M.get_unsafe(0,3);
<a name="l00636"></a>00636 <span class="keyword">const</span> <span class="keywordtype">double</span> ty = -M.get_unsafe(1,3);
<a name="l00637"></a>00637 <span class="keyword">const</span> <span class="keywordtype">double</span> tz = -M.get_unsafe(2,3);
<a name="l00638"></a>00638
<a name="l00639"></a>00639 <span class="keyword">const</span> <span class="keywordtype">double</span> tx_ = tx*M.get_unsafe(0,0)+ty*M.get_unsafe(1,0)+tz*M.get_unsafe(2,0);
<a name="l00640"></a>00640 <span class="keyword">const</span> <span class="keywordtype">double</span> ty_ = tx*M.get_unsafe(0,1)+ty*M.get_unsafe(1,1)+tz*M.get_unsafe(2,1);
<a name="l00641"></a>00641 <span class="keyword">const</span> <span class="keywordtype">double</span> tz_ = tx*M.get_unsafe(0,2)+ty*M.get_unsafe(1,2)+tz*M.get_unsafe(2,2);
<a name="l00642"></a>00642
<a name="l00643"></a>00643 out_inverse_M.set_unsafe(0,3, tx_ );
<a name="l00644"></a>00644 out_inverse_M.set_unsafe(1,3, ty_ );
<a name="l00645"></a>00645 out_inverse_M.set_unsafe(2,3, tz_ );
<a name="l00646"></a>00646
<a name="l00647"></a>00647 out_inverse_M.set_unsafe(3,0, 0);
<a name="l00648"></a>00648 out_inverse_M.set_unsafe(3,1, 0);
<a name="l00649"></a>00649 out_inverse_M.set_unsafe(3,2, 0);
<a name="l00650"></a>00650 out_inverse_M.set_unsafe(3,3, 1);
<a name="l00651"></a>00651
<a name="l00652"></a>00652 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00653"></a>00653 }<span class="comment"></span>
<a name="l00654"></a>00654 <span class="comment"> //! \overload</span>
<a name="l00655"></a>00655 <span class="comment"></span> <span class="keyword">template</span> <<span class="keyword">class</span> IN_ROTMATRIX,<span class="keyword">class</span> IN_XYZ, <span class="keyword">class</span> OUT_ROTMATRIX, <span class="keyword">class</span> OUT_XYZ>
<a name="l00656"></a><a class="code" href="group__container__ops__grp.html#ga44be80165962896f21d32ee9a2098ff0">00656</a> <span class="keywordtype">void</span> <a class="code" href="group__container__ops__grp.html#ga65a4cb93289c9373a8830102a2296e2d" title="Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part...">homogeneousMatrixInverse</a>(
<a name="l00657"></a>00657 <span class="keyword">const</span> IN_ROTMATRIX & in_R,
<a name="l00658"></a>00658 <span class="keyword">const</span> IN_XYZ & in_xyz,
<a name="l00659"></a>00659 OUT_ROTMATRIX & out_R,
<a name="l00660"></a>00660 OUT_XYZ & out_xyz
<a name="l00661"></a>00661 )
<a name="l00662"></a>00662 {
<a name="l00663"></a>00663 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00664"></a>00664 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( in_R.isSquare() && <a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">size</a>(in_R,1)==3 && in_xyz.size()==3)
<a name="l00665"></a>00665 out_R.setSize(3,3);
<a name="l00666"></a>00666 out_xyz.resize(3);
<a name="l00667"></a>00667
<a name="l00668"></a>00668 <span class="comment">// translation part:</span>
<a name="l00669"></a>00669 <span class="keyword">const</span> <span class="keywordtype">double</span> tx = -in_xyz[0];
<a name="l00670"></a>00670 <span class="keyword">const</span> <span class="keywordtype">double</span> ty = -in_xyz[1];
<a name="l00671"></a>00671 <span class="keyword">const</span> <span class="keywordtype">double</span> tz = -in_xyz[2];
<a name="l00672"></a>00672
<a name="l00673"></a>00673 out_xyz[0] = tx*in_R.get_unsafe(0,0)+ty*in_R.get_unsafe(1,0)+tz*in_R.get_unsafe(2,0);
<a name="l00674"></a>00674 out_xyz[1] = tx*in_R.get_unsafe(0,1)+ty*in_R.get_unsafe(1,1)+tz*in_R.get_unsafe(2,1);
<a name="l00675"></a>00675 out_xyz[2] = tx*in_R.get_unsafe(0,2)+ty*in_R.get_unsafe(1,2)+tz*in_R.get_unsafe(2,2);
<a name="l00676"></a>00676
<a name="l00677"></a>00677 <span class="comment">// 3x3 rotation part: transpose</span>
<a name="l00678"></a>00678 out_R = in_R.adjoint();
<a name="l00679"></a>00679
<a name="l00680"></a>00680 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00681"></a>00681 }<span class="comment"></span>
<a name="l00682"></a>00682 <span class="comment"> //! \overload</span>
<a name="l00683"></a>00683 <span class="comment"></span> <span class="keyword">template</span> <<span class="keyword">class</span> MATRIXLIKE>
<a name="l00684"></a><a class="code" href="group__container__ops__grp.html#gab0feb72f7a668233a13350ecf4a942a3">00684</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="group__container__ops__grp.html#ga65a4cb93289c9373a8830102a2296e2d" title="Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part...">homogeneousMatrixInverse</a>(MATRIXLIKE &M)
<a name="l00685"></a>00685 {
<a name="l00686"></a>00686 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>( M.isSquare() && <a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">size</a>(M,1)==4);
<a name="l00687"></a>00687 <span class="comment">// translation:</span>
<a name="l00688"></a>00688 <span class="keyword">const</span> <span class="keywordtype">double</span> tx = -M(0,3);
<a name="l00689"></a>00689 <span class="keyword">const</span> <span class="keywordtype">double</span> ty = -M(1,3);
<a name="l00690"></a>00690 <span class="keyword">const</span> <span class="keywordtype">double</span> tz = -M(2,3);
<a name="l00691"></a>00691 M(0,3) = tx*M(0,0)+ty*M(1,0)+tz*M(2,0);
<a name="l00692"></a>00692 M(1,3) = tx*M(0,1)+ty*M(1,1)+tz*M(2,1);
<a name="l00693"></a>00693 M(2,3) = tx*M(0,2)+ty*M(1,2)+tz*M(2,2);
<a name="l00694"></a>00694 <span class="comment">// 3x3 rotation part:</span>
<a name="l00695"></a>00695 std::swap( M(1,0),M(0,1) );
<a name="l00696"></a>00696 std::swap( M(2,0),M(0,2) );
<a name="l00697"></a>00697 std::swap( M(1,2),M(2,1) );
<a name="l00698"></a>00698 }
<a name="l00699"></a>00699
<a name="l00700"></a>00700 <span class="comment"></span>
<a name="l00701"></a>00701 <span class="comment"> /** Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of a given size in each input dimension.</span>
<a name="l00702"></a>00702 <span class="comment"> * The template argument USERPARAM is for the data can be passed to the functor.</span>
<a name="l00703"></a>00703 <span class="comment"> * If it is not required, set to "int" or any other basic type.</span>
<a name="l00704"></a>00704 <span class="comment"> *</span>
<a name="l00705"></a>00705 <span class="comment"> * This is a generic template which works with:</span>
<a name="l00706"></a>00706 <span class="comment"> * VECTORLIKE: vector_float, vector_double, CArrayNumeric<>, double [N], ...</span>
<a name="l00707"></a>00707 <span class="comment"> * MATRIXLIKE: CMatrixTemplateNumeric, CMatrixFixedNumeric</span>
<a name="l00708"></a>00708 <span class="comment"> */</span>
<a name="l00709"></a>00709 <span class="keyword">template</span> <<span class="keyword">class</span> VECTORLIKE,<span class="keyword">class</span> VECTORLIKE2, <span class="keyword">class</span> VECTORLIKE3, <span class="keyword">class</span> MATRIXLIKE, <span class="keyword">class</span> USERPARAM >
<a name="l00710"></a><a class="code" href="group__container__ops__grp.html#ga13e2e339d944b37f2386fc4e0bb56935">00710</a> <span class="keywordtype">void</span> <a class="code" href="group__container__ops__grp.html#ga13e2e339d944b37f2386fc4e0bb56935" title="Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...">estimateJacobian</a>(
<a name="l00711"></a>00711 <span class="keyword">const</span> VECTORLIKE &x,
<a name="l00712"></a>00712 <span class="keywordtype">void</span> (*functor) (<span class="keyword">const</span> VECTORLIKE &x,<span class="keyword">const</span> USERPARAM &<a class="code" href="namespace_eigen_1_1internal.html#a3d7a581aeb951248dc6fe114e9e05f07">y</a>, VECTORLIKE3 &out),
<a name="l00713"></a>00713 <span class="keyword">const</span> VECTORLIKE2 &increments,
<a name="l00714"></a>00714 <span class="keyword">const</span> USERPARAM &userParam,
<a name="l00715"></a>00715 MATRIXLIKE &out_Jacobian )
<a name="l00716"></a>00716 {
<a name="l00717"></a>00717 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00718"></a>00718 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(x.size()>0 && increments.size() == x.size());
<a name="l00719"></a>00719
<a name="l00720"></a>00720 <span class="keywordtype">size_t</span> m = 0; <span class="comment">// will determine automatically on the first call to "f":</span>
<a name="l00721"></a>00721 <span class="keyword">const</span> <span class="keywordtype">size_t</span> n = x.size();
<a name="l00722"></a>00722
<a name="l00723"></a>00723 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0;j<n;j++) { <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( increments[j]>0 ) } <span class="comment">// Who knows...</span>
<a name="l00724"></a>00724
<a name="l00725"></a>00725 VECTORLIKE3 f_minus, f_plus;
<a name="l00726"></a>00726 VECTORLIKE x_mod(x);
<a name="l00727"></a>00727
<a name="l00728"></a>00728 <span class="comment">// Evaluate the function "i" with increments in the "j" input x variable:</span>
<a name="l00729"></a>00729 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0;j<n;j++)
<a name="l00730"></a>00730 {
<a name="l00731"></a>00731 <span class="comment">// Create the modified "x" vector:</span>
<a name="l00732"></a>00732 x_mod[j]=x[j]+increments[j];
<a name="l00733"></a>00733 functor(x_mod,userParam, f_plus);
<a name="l00734"></a>00734
<a name="l00735"></a>00735 x_mod[j]=x[j]-increments[j];
<a name="l00736"></a>00736 functor(x_mod,userParam, f_minus);
<a name="l00737"></a>00737
<a name="l00738"></a>00738 x_mod[j]=x[j]; <span class="comment">// Leave as original</span>
<a name="l00739"></a>00739 <span class="keyword">const</span> <span class="keywordtype">double</span> Ax_2_inv = 0.5/increments[j];
<a name="l00740"></a>00740
<a name="l00741"></a>00741 <span class="comment">// The first time?</span>
<a name="l00742"></a>00742 <span class="keywordflow">if</span> (j==0)
<a name="l00743"></a>00743 {
<a name="l00744"></a>00744 m = f_plus.size();
<a name="l00745"></a>00745 out_Jacobian.setSize(m,n);
<a name="l00746"></a>00746 }
<a name="l00747"></a>00747
<a name="l00748"></a>00748 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<m;i++)
<a name="l00749"></a>00749 out_Jacobian.get_unsafe(i,j) = Ax_2_inv* (f_plus[i]-f_minus[i]);
<a name="l00750"></a>00750
<a name="l00751"></a>00751 } <span class="comment">// end for j</span>
<a name="l00752"></a>00752
<a name="l00753"></a>00753 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00754"></a>00754 }
<a name="l00755"></a>00755 <span class="comment"></span>
<a name="l00756"></a>00756 <span class="comment"> /** Assignment operator for initializing a std::vector from a C array (The vector will be automatically set to the correct size).</span>
<a name="l00757"></a>00757 <span class="comment"> * \code</span>
<a name="l00758"></a>00758 <span class="comment"> * vector_double v;</span>
<a name="l00759"></a>00759 <span class="comment"> * const double numbers[] = { 1,2,3,5,6,7,8,9,10 };</span>
<a name="l00760"></a>00760 <span class="comment"> * loadVector( v, numbers );</span>
<a name="l00761"></a>00761 <span class="comment"> * \endcode</span>
<a name="l00762"></a>00762 <span class="comment"> * \note This operator performs the appropiate type castings, if required.</span>
<a name="l00763"></a>00763 <span class="comment"> */</span>
<a name="l00764"></a>00764 <span class="keyword">template</span> <<span class="keyword">typename</span> T, <span class="keyword">typename</span> At, <span class="keywordtype">size_t</span> N>
<a name="l00765"></a><a class="code" href="group__container__ops__grp.html#ga3f52023c60b58a0d49abca36d14d880e">00765</a> std<a class="code" href="classstd_1_1vector.html">::vector<T></a>& <a class="code" href="group__container__ops__grp.html#ga40e8e47dea9f504a28d2a70ea8ddb158" title="Loads one row of a text file as a numerical std::vector.">loadVector</a>( <a class="code" href="classstd_1_1vector.html">std::vector<T></a> &v, At (&theArray)[N] )
<a name="l00766"></a>00766 {
<a name="l00767"></a>00767 <a class="code" href="mrpt__macros_8h.html#a375dbd0ce9cb69a56d76d0fa21536bae">MRPT_COMPILE_TIME_ASSERT</a>(N!=0)
<a name="l00768"></a>00768 v.resize(N);
<a name="l00769"></a>00769 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0; i < N; i++)
<a name="l00770"></a>00770 v[i] = static_cast<T>(theArray[i]);
<a name="l00771"></a>00771 <span class="keywordflow">return</span> v;
<a name="l00772"></a>00772 }<span class="comment"></span>
<a name="l00773"></a>00773 <span class="comment"> //! \overload</span>
<a name="l00774"></a>00774 <span class="comment"></span> <span class="keyword">template</span> <<span class="keyword">typename</span> Derived, <span class="keyword">typename</span> At, <span class="keywordtype">size_t</span> N>
<a name="l00775"></a><a class="code" href="group__container__ops__grp.html#ga37ffa47225a54cc4effc4b666a99bf8c">00775</a> Eigen::EigenBase<Derived>& <a class="code" href="group__container__ops__grp.html#ga40e8e47dea9f504a28d2a70ea8ddb158" title="Loads one row of a text file as a numerical std::vector.">loadVector</a>( <a class="code" href="struct_eigen_1_1_eigen_base.html">Eigen::EigenBase<Derived></a> &v, At (&theArray)[N] )
<a name="l00776"></a>00776 {
<a name="l00777"></a>00777 <a class="code" href="mrpt__macros_8h.html#a375dbd0ce9cb69a56d76d0fa21536bae">MRPT_COMPILE_TIME_ASSERT</a>(N!=0)
<a name="l00778"></a>00778 v.<a class="code" href="struct_eigen_1_1_eigen_base.html#a35a4d307c7899aa1085b514457a115f0">derived</a>().resize(N);
<a name="l00779"></a>00779 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0; i < N; i++)
<a name="l00780"></a>00780 (v.<a class="code" href="struct_eigen_1_1_eigen_base.html#a35a4d307c7899aa1085b514457a115f0">derived</a>())[i] = static_cast<typename Derived::Scalar>(theArray[i]);
<a name="l00781"></a>00781 <span class="keywordflow">return</span> v;
<a name="l00782"></a>00782 }
<a name="l00783"></a>00783 <span class="comment"></span>
<a name="l00784"></a>00784 <span class="comment"> /** Modify a sequence of angle values such as no consecutive values have a jump larger than PI in absolute value.</span>
<a name="l00785"></a>00785 <span class="comment"> * \sa wrapToPi</span>
<a name="l00786"></a>00786 <span class="comment"> */</span>
<a name="l00787"></a>00787 <span class="keywordtype">void</span> <a class="code" href="group__container__ops__grp.html#ga6b94c41c7406e0d606111a7b771b72e0" title="Modify a sequence of angle values such as no consecutive values have a jump larger than PI in absolut...">unwrap2PiSequence</a>(<a class="code" href="structmrpt_1_1dynamicsize__vector.html" title="The base class of MRPT vectors, actually, Eigen column matrices of dynamic size with specialized cons...">vector_double</a> &x);
<a name="l00788"></a>00788 <span class="comment"></span>
<a name="l00789"></a>00789 <span class="comment"> /** A versatile template to build vectors on-the-fly in a style close to MATLAB's v=[a b c d ...]</span>
<a name="l00790"></a>00790 <span class="comment"> * The first argument of the template is the vector length, and the second the type of the numbers.</span>
<a name="l00791"></a>00791 <span class="comment"> * Some examples:</span>
<a name="l00792"></a>00792 <span class="comment"> *</span>
<a name="l00793"></a>00793 <span class="comment"> * \code</span>
<a name="l00794"></a>00794 <span class="comment"> * vector_double = make_vector<4,double>(1.0,3.0,4.0,5.0);</span>
<a name="l00795"></a>00795 <span class="comment"> * vector_float = make_vector<2,float>(-8.12, 3e4);</span>
<a name="l00796"></a>00796 <span class="comment"> * \endcode</span>
<a name="l00797"></a>00797 <span class="comment"> */</span>
<a name="l00798"></a>00798 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> N, <span class="keyword">typename</span> T>
<a name="l00799"></a><a class="code" href="group__container__ops__grp.html#gae0851fd327043ae2a7054c33a3bbfcde">00799</a> std<a class="code" href="classstd_1_1vector.html">::vector<T></a> <a class="code" href="group__container__ops__grp.html#gae0851fd327043ae2a7054c33a3bbfcde" title="A versatile template to build vectors on-the-fly in a style close to MATLAB's v=[a b c d ...">make_vector</a>(<span class="keyword">const</span> T val1, ...)
<a name="l00800"></a>00800 {
<a name="l00801"></a>00801 <a class="code" href="mrpt__macros_8h.html#a375dbd0ce9cb69a56d76d0fa21536bae">MRPT_COMPILE_TIME_ASSERT</a>( N>0 )
<a name="l00802"></a>00802 std<a class="code" href="classstd_1_1vector.html">::vector<T></a> ret;
<a name="l00803"></a>00803 ret.reserve(N);
<a name="l00804"></a>00804
<a name="l00805"></a>00805 ret.push_back(val1);
<a name="l00806"></a>00806
<a name="l00807"></a>00807 va_list args;
<a name="l00808"></a>00808 va_start(args,val1);
<a name="l00809"></a>00809 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<N-1;i++)
<a name="l00810"></a>00810 ret.push_back( va_arg(args,T) );
<a name="l00811"></a>00811
<a name="l00812"></a>00812 va_end(args);
<a name="l00813"></a>00813 <span class="keywordflow">return</span> ret;
<a name="l00814"></a>00814 }
<a name="l00815"></a>00815 <span class="comment"></span>
<a name="l00816"></a>00816 <span class="comment"> /** @} */</span> <span class="comment">// end of grouping container_ops_grp</span>
<a name="l00817"></a>00817 <span class="comment"></span>
<a name="l00818"></a>00818 <span class="comment"> /** \addtogroup stats_grp</span>
<a name="l00819"></a>00819 <span class="comment"> * @{</span>
<a name="l00820"></a>00820 <span class="comment"> */</span>
<a name="l00821"></a>00821 <span class="comment"></span>
<a name="l00822"></a>00822 <span class="comment"> /** @name Probability density distributions (pdf) distance metrics</span>
<a name="l00823"></a>00823 <span class="comment"> @{ */</span>
<a name="l00824"></a>00824 <span class="comment"></span>
<a name="l00825"></a>00825 <span class="comment"> /** Computes the squared mahalanobis distance of a vector X given the mean MU and the covariance *inverse* COV_inv</span>
<a name="l00826"></a>00826 <span class="comment"> * \f[ d^2 = (X-MU)^\top \Sigma^{-1} (X-MU) \f]</span>
<a name="l00827"></a>00827 <span class="comment"> */</span>
<a name="l00828"></a>00828 <span class="keyword">template</span><<span class="keyword">class</span> VECTORLIKE1,<span class="keyword">class</span> VECTORLIKE2,<span class="keyword">class</span> MAT>
<a name="l00829"></a><a class="code" href="group__stats__grp.html#ga83890daff9ce929f7acd216a5a248842">00829</a> <span class="keyword">typename</span> VECTORLIKE1<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a> <a class="code" href="group__stats__grp.html#ga83890daff9ce929f7acd216a5a248842" title="Computes the squared mahalanobis distance of a vector X given the mean MU and the covariance *inverse...">mahalanobisDistance2</a>(
<a name="l00830"></a>00830 <span class="keyword">const</span> VECTORLIKE1 &X,
<a name="l00831"></a>00831 <span class="keyword">const</span> VECTORLIKE2 &MU,
<a name="l00832"></a>00832 <span class="keyword">const</span> MAT &COV )
<a name="l00833"></a>00833 {
<a name="l00834"></a>00834 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00835"></a>00835 <span class="preprocessor"> #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)</span>
<a name="l00836"></a>00836 <span class="preprocessor"></span> <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( !X.empty() );
<a name="l00837"></a>00837 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( X.size()==MU.size() );
<a name="l00838"></a>00838 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( X.size()==<a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">size</a>(COV,1) && COV.isSquare() );
<a name="l00839"></a>00839 <span class="preprocessor"> #endif</span>
<a name="l00840"></a>00840 <span class="preprocessor"></span> <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = X.size();
<a name="l00841"></a>00841 mrpt<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...">::dynamicsize_vector<typename VECTORLIKE1::value_type></a> X_MU(N);
<a name="l00842"></a>00842 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<N;i++) X_MU[i]=X[i]-MU[i];
<a name="l00843"></a>00843 <span class="keywordflow">return</span> <a class="code" href="namespacemrpt_1_1math.html#aa8357cce481e22376ecadae393167977" title="r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)">multiply_HCHt_scalar</a>(X_MU, COV.inv() );
<a name="l00844"></a>00844 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00845"></a>00845 }
<a name="l00846"></a>00846
<a name="l00847"></a>00847 <span class="comment"></span>
<a name="l00848"></a>00848 <span class="comment"> /** Computes the mahalanobis distance of a vector X given the mean MU and the covariance *inverse* COV_inv</span>
<a name="l00849"></a>00849 <span class="comment"> * \f[ d = \sqrt{ (X-MU)^\top \Sigma^{-1} (X-MU) } \f]</span>
<a name="l00850"></a>00850 <span class="comment"> */</span>
<a name="l00851"></a>00851 <span class="keyword">template</span><<span class="keyword">class</span> VECTORLIKE1,<span class="keyword">class</span> VECTORLIKE2,<span class="keyword">class</span> MAT>
<a name="l00852"></a><a class="code" href="group__stats__grp.html#ga9f4f0d1f3c898ec9b9017aa4632ff977">00852</a> <span class="keyword">inline</span> <span class="keyword">typename</span> VECTORLIKE1<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a> <a class="code" href="group__stats__grp.html#ga9f4f0d1f3c898ec9b9017aa4632ff977" title="Computes the mahalanobis distance of a vector X given the mean MU and the covariance *inverse* COV_in...">mahalanobisDistance</a>(
<a name="l00853"></a>00853 <span class="keyword">const</span> VECTORLIKE1 &X,
<a name="l00854"></a>00854 <span class="keyword">const</span> VECTORLIKE2 &MU,
<a name="l00855"></a>00855 <span class="keyword">const</span> MAT &COV )
<a name="l00856"></a>00856 {
<a name="l00857"></a>00857 <span class="keywordflow">return</span> std::sqrt( <a class="code" href="group__stats__grp.html#ga83890daff9ce929f7acd216a5a248842" title="Computes the squared mahalanobis distance of a vector X given the mean MU and the covariance *inverse...">mahalanobisDistance2</a>(X,MU,COV) );
<a name="l00858"></a>00858 }
<a name="l00859"></a>00859
<a name="l00860"></a>00860 <span class="comment"></span>
<a name="l00861"></a>00861 <span class="comment"> /** Computes the squared mahalanobis distance between two *non-independent* Gaussians, given the two covariance matrices and the vector with the difference of their means.</span>
<a name="l00862"></a>00862 <span class="comment"> * \f[ d^2 = \Delta_\mu^\top (\Sigma_1 + \Sigma_2 - 2 \Sigma_12 )^{-1} \Delta_\mu \f]</span>
<a name="l00863"></a>00863 <span class="comment"> */</span>
<a name="l00864"></a>00864 <span class="keyword">template</span><<span class="keyword">class</span> VECTORLIKE,<span class="keyword">class</span> MAT1,<span class="keyword">class</span> MAT2,<span class="keyword">class</span> MAT3>
<a name="l00865"></a>00865 <span class="keyword">typename</span> VECTORLIKE<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a>
<a name="l00866"></a><a class="code" href="group__stats__grp.html#ga4bdd42229480454da8a1ffa6c31266f4">00866</a> <a class="code" href="group__stats__grp.html#ga83890daff9ce929f7acd216a5a248842" title="Computes the squared mahalanobis distance of a vector X given the mean MU and the covariance *inverse...">mahalanobisDistance2</a>(
<a name="l00867"></a>00867 <span class="keyword">const</span> VECTORLIKE &mean_diffs,
<a name="l00868"></a>00868 <span class="keyword">const</span> MAT1 &COV1,
<a name="l00869"></a>00869 <span class="keyword">const</span> MAT2 &COV2,
<a name="l00870"></a>00870 <span class="keyword">const</span> MAT3 &CROSS_COV12 )
<a name="l00871"></a>00871 {
<a name="l00872"></a>00872 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00873"></a>00873 <span class="preprocessor"> #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)</span>
<a name="l00874"></a>00874 <span class="preprocessor"></span> <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( !mean_diffs.empty() );
<a name="l00875"></a>00875 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( mean_diffs.size()==<a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">size</a>(COV1,1));
<a name="l00876"></a>00876 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( COV1.isSquare() && COV2.isSquare() );
<a name="l00877"></a>00877 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( <a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">size</a>(COV1,1)==<a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">size</a>(COV2,1));
<a name="l00878"></a>00878 <span class="preprocessor"> #endif</span>
<a name="l00879"></a>00879 <span class="preprocessor"></span> <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = <a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">size</a>(COV1,1);
<a name="l00880"></a>00880 MAT1 COV = COV1;
<a name="l00881"></a>00881 COV+=COV2;
<a name="l00882"></a>00882 COV.substract_An(CROSS_COV12,2);
<a name="l00883"></a>00883 MAT1 COV_inv;
<a name="l00884"></a>00884 COV.inv_fast(COV_inv);
<a name="l00885"></a>00885 <span class="keywordflow">return</span> <a class="code" href="namespacemrpt_1_1math.html#aa8357cce481e22376ecadae393167977" title="r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)">multiply_HCHt_scalar</a>(mean_diffs,COV_inv);
<a name="l00886"></a>00886 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00887"></a>00887 }
<a name="l00888"></a>00888 <span class="comment"></span>
<a name="l00889"></a>00889 <span class="comment"> /** Computes the mahalanobis distance between two *non-independent* Gaussians (or independent if CROSS_COV12=NULL), given the two covariance matrices and the vector with the difference of their means.</span>
<a name="l00890"></a>00890 <span class="comment"> * \f[ d = \sqrt{ \Delta_\mu^\top (\Sigma_1 + \Sigma_2 - 2 \Sigma_12 )^{-1} \Delta_\mu } \f]</span>
<a name="l00891"></a>00891 <span class="comment"> */</span>
<a name="l00892"></a>00892 <span class="keyword">template</span><<span class="keyword">class</span> VECTORLIKE,<span class="keyword">class</span> MAT1,<span class="keyword">class</span> MAT2,<span class="keyword">class</span> MAT3> <span class="keyword">inline</span> <span class="keyword">typename</span> VECTORLIKE<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a>
<a name="l00893"></a><a class="code" href="group__stats__grp.html#gad92bbaa6ff21979851cd068b575b1cc4">00893</a> <a class="code" href="group__stats__grp.html#ga9f4f0d1f3c898ec9b9017aa4632ff977" title="Computes the mahalanobis distance of a vector X given the mean MU and the covariance *inverse* COV_in...">mahalanobisDistance</a>(
<a name="l00894"></a>00894 <span class="keyword">const</span> VECTORLIKE &mean_diffs,
<a name="l00895"></a>00895 <span class="keyword">const</span> MAT1 &COV1,
<a name="l00896"></a>00896 <span class="keyword">const</span> MAT2 &COV2,
<a name="l00897"></a>00897 <span class="keyword">const</span> MAT3 &CROSS_COV12 )
<a name="l00898"></a>00898 {
<a name="l00899"></a>00899 <span class="keywordflow">return</span> std::sqrt( <a class="code" href="group__stats__grp.html#ga9f4f0d1f3c898ec9b9017aa4632ff977" title="Computes the mahalanobis distance of a vector X given the mean MU and the covariance *inverse* COV_in...">mahalanobisDistance</a>( mean_diffs, COV1,COV2,CROSS_COV12 ));
<a name="l00900"></a>00900 }
<a name="l00901"></a>00901 <span class="comment"></span>
<a name="l00902"></a>00902 <span class="comment"> /** Computes the squared mahalanobis distance between a point and a Gaussian, given the covariance matrix and the vector with the difference between the mean and the point.</span>
<a name="l00903"></a>00903 <span class="comment"> * \f[ d^2 = \Delta_\mu^\top \Sigma^{-1} \Delta_\mu \f]</span>
<a name="l00904"></a>00904 <span class="comment"> */</span>
<a name="l00905"></a>00905 <span class="keyword">template</span><<span class="keyword">class</span> VECTORLIKE,<span class="keyword">class</span> MATRIXLIKE>
<a name="l00906"></a>00906 <span class="keyword">inline</span> <span class="keyword">typename</span> MATRIXLIKE<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a>
<a name="l00907"></a><a class="code" href="group__stats__grp.html#ga6d3521a4e3b2ff5d55d8762108e6c8e4">00907</a> <a class="code" href="group__stats__grp.html#ga83890daff9ce929f7acd216a5a248842" title="Computes the squared mahalanobis distance of a vector X given the mean MU and the covariance *inverse...">mahalanobisDistance2</a>(<span class="keyword">const</span> VECTORLIKE &delta_mu,<span class="keyword">const</span> MATRIXLIKE &<a class="code" href="namespacemrpt_1_1math.html#a43f4e051fc574fd75b6800ad4fb25037" title="Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...">cov</a>)
<a name="l00908"></a>00908 {
<a name="l00909"></a>00909 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(cov.isSquare())
<a name="l00910"></a>00910 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(cov.getColCount()==delta_mu.size())
<a name="l00911"></a>00911 <span class="keywordflow">return</span> <a class="code" href="namespacemrpt_1_1math.html#aa8357cce481e22376ecadae393167977" title="r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)">multiply_HCHt_scalar</a>(delta_mu,cov.inverse());
<a name="l00912"></a>00912 }
<a name="l00913"></a>00913 <span class="comment"></span>
<a name="l00914"></a>00914 <span class="comment"> /** Computes the mahalanobis distance between a point and a Gaussian, given the covariance matrix and the vector with the difference between the mean and the point.</span>
<a name="l00915"></a>00915 <span class="comment"> * \f[ d^2 = \sqrt( \Delta_\mu^\top \Sigma^{-1} \Delta_\mu ) \f]</span>
<a name="l00916"></a>00916 <span class="comment"> */</span>
<a name="l00917"></a>00917 <span class="keyword">template</span><<span class="keyword">class</span> VECTORLIKE,<span class="keyword">class</span> MATRIXLIKE>
<a name="l00918"></a>00918 <span class="keyword">inline</span> <span class="keyword">typename</span> MATRIXLIKE<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a>
<a name="l00919"></a><a class="code" href="group__stats__grp.html#ga90f3038071d69c2b7dced52af44e4944">00919</a> <a class="code" href="group__stats__grp.html#ga9f4f0d1f3c898ec9b9017aa4632ff977" title="Computes the mahalanobis distance of a vector X given the mean MU and the covariance *inverse* COV_in...">mahalanobisDistance</a>(<span class="keyword">const</span> VECTORLIKE &delta_mu,<span class="keyword">const</span> MATRIXLIKE &<a class="code" href="namespacemrpt_1_1math.html#a43f4e051fc574fd75b6800ad4fb25037" title="Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...">cov</a>)
<a name="l00920"></a>00920 {
<a name="l00921"></a>00921 <span class="keywordflow">return</span> std::sqrt(<a class="code" href="group__stats__grp.html#ga83890daff9ce929f7acd216a5a248842" title="Computes the squared mahalanobis distance of a vector X given the mean MU and the covariance *inverse...">mahalanobisDistance2</a>(delta_mu,cov));
<a name="l00922"></a>00922 }
<a name="l00923"></a>00923 <span class="comment"></span>
<a name="l00924"></a>00924 <span class="comment"> /** Computes the integral of the product of two Gaussians, with means separated by "mean_diffs" and covariances "COV1" and "COV2".</span>
<a name="l00925"></a>00925 <span class="comment"> * \f[ D = \frac{1}{(2 \pi)^{0.5 N} \sqrt{} } \exp( \Delta_\mu^\top (\Sigma_1 + \Sigma_2 - 2 \Sigma_12)^{-1} \Delta_\mu) \f]</span>
<a name="l00926"></a>00926 <span class="comment"> */</span>
<a name="l00927"></a>00927 <span class="keyword">template</span> <<span class="keyword">typename</span> T>
<a name="l00928"></a><a class="code" href="group__stats__grp.html#ga668be07e32223bceb4ff0100f8f860fc">00928</a> T <a class="code" href="group__stats__grp.html#ga668be07e32223bceb4ff0100f8f860fc" title="Computes the integral of the product of two Gaussians, with means separated by "mean_diffs" and covar...">productIntegralTwoGaussians</a>(
<a name="l00929"></a>00929 <span class="keyword">const</span> <a class="code" href="classstd_1_1vector.html">std::vector<T></a> &mean_diffs,
<a name="l00930"></a>00930 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<T></a> &COV1,
<a name="l00931"></a>00931 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<T></a> &COV2
<a name="l00932"></a>00932 )
<a name="l00933"></a>00933 {
<a name="l00934"></a>00934 <span class="keyword">const</span> <span class="keywordtype">size_t</span> vector_dim = mean_diffs.size();
<a name="l00935"></a>00935 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(vector_dim>=1)
<a name="l00936"></a>00936
<a name="l00937"></a>00937 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<T></a> C = COV1;
<a name="l00938"></a>00938 C+= COV2; <span class="comment">// Sum of covs:</span>
<a name="l00939"></a>00939 <span class="keyword">const</span> T cov_det = C.det();
<a name="l00940"></a>00940 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<T></a> C_inv;
<a name="l00941"></a>00941 C.inv_fast(C_inv);
<a name="l00942"></a>00942
<a name="l00943"></a>00943 <span class="keywordflow">return</span> std::pow( <a class="code" href="mrpt__macros_8h.html#a4aa2c43b09c1300c334821f5507d6f71">M_2PI</a>, -0.5*vector_dim ) * (1.0/std::sqrt( cov_det ))
<a name="l00944"></a>00944 * exp( -0.5 * mean_diffs.multiply_HCHt_scalar(C_inv) );
<a name="l00945"></a>00945 }
<a name="l00946"></a>00946 <span class="comment"></span>
<a name="l00947"></a>00947 <span class="comment"> /** Computes the integral of the product of two Gaussians, with means separated by "mean_diffs" and covariances "COV1" and "COV2".</span>
<a name="l00948"></a>00948 <span class="comment"> * \f[ D = \frac{1}{(2 \pi)^{0.5 N} \sqrt{} } \exp( \Delta_\mu^\top (\Sigma_1 + \Sigma_2)^{-1} \Delta_\mu) \f]</span>
<a name="l00949"></a>00949 <span class="comment"> */</span>
<a name="l00950"></a>00950 <span class="keyword">template</span> <<span class="keyword">typename</span> T, <span class="keywordtype">size_t</span> DIM>
<a name="l00951"></a><a class="code" href="group__stats__grp.html#gaa31d217d82b559612958855947f79084">00951</a> T <a class="code" href="group__stats__grp.html#ga668be07e32223bceb4ff0100f8f860fc" title="Computes the integral of the product of two Gaussians, with means separated by "mean_diffs" and covar...">productIntegralTwoGaussians</a>(
<a name="l00952"></a>00952 <span class="keyword">const</span> <a class="code" href="classstd_1_1vector.html">std::vector<T></a> &mean_diffs,
<a name="l00953"></a>00953 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<T,DIM,DIM></a> &COV1,
<a name="l00954"></a>00954 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<T,DIM,DIM></a> &COV2
<a name="l00955"></a>00955 )
<a name="l00956"></a>00956 {
<a name="l00957"></a>00957 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(mean_diffs.size()==DIM);
<a name="l00958"></a>00958
<a name="l00959"></a>00959 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<T,DIM,DIM></a> C = COV1;
<a name="l00960"></a>00960 C+= COV2; <span class="comment">// Sum of covs:</span>
<a name="l00961"></a>00961 <span class="keyword">const</span> T cov_det = C.det();
<a name="l00962"></a>00962 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<T,DIM,DIM></a> C_inv(<a class="code" href="namespacemrpt_1_1math.html#a27e8ae8971ff5aa1c39f1f9be334d73aa28acc66160006cb691487ec89f8d266d">UNINITIALIZED_MATRIX</a>);
<a name="l00963"></a>00963 C.inv_fast(C_inv);
<a name="l00964"></a>00964
<a name="l00965"></a>00965 <span class="keywordflow">return</span> std::pow( <a class="code" href="mrpt__macros_8h.html#a4aa2c43b09c1300c334821f5507d6f71">M_2PI</a>, -0.5*DIM ) * (1.0/std::sqrt( cov_det ))
<a name="l00966"></a>00966 * exp( -0.5 * mean_diffs.multiply_HCHt_scalar(C_inv) );
<a name="l00967"></a>00967 }
<a name="l00968"></a>00968 <span class="comment"></span>
<a name="l00969"></a>00969 <span class="comment"> /** Computes both, the integral of the product of two Gaussians and their square Mahalanobis distance.</span>
<a name="l00970"></a>00970 <span class="comment"> * \sa productIntegralTwoGaussians, mahalanobisDistance2</span>
<a name="l00971"></a>00971 <span class="comment"> */</span>
<a name="l00972"></a>00972 <span class="keyword">template</span> <<span class="keyword">typename</span> T, <span class="keyword">class</span> VECLIKE,<span class="keyword">class</span> MATLIKE1, <span class="keyword">class</span> MATLIKE2>
<a name="l00973"></a><a class="code" href="group__stats__grp.html#ga0080dc1b4ea2d89d56fdde37f2ada9bf">00973</a> <span class="keywordtype">void</span> <a class="code" href="group__stats__grp.html#ga0080dc1b4ea2d89d56fdde37f2ada9bf" title="Computes both, the integral of the product of two Gaussians and their square Mahalanobis distance...">productIntegralAndMahalanobisTwoGaussians</a>(
<a name="l00974"></a>00974 <span class="keyword">const</span> VECLIKE &mean_diffs,
<a name="l00975"></a>00975 <span class="keyword">const</span> MATLIKE1 &COV1,
<a name="l00976"></a>00976 <span class="keyword">const</span> MATLIKE2 &COV2,
<a name="l00977"></a>00977 T &maha2_out,
<a name="l00978"></a>00978 T &intprod_out,
<a name="l00979"></a>00979 <span class="keyword">const</span> MATLIKE1 *CROSS_COV12=NULL
<a name="l00980"></a>00980 )
<a name="l00981"></a>00981 {
<a name="l00982"></a>00982 <span class="keyword">const</span> <span class="keywordtype">size_t</span> vector_dim = mean_diffs.size();
<a name="l00983"></a>00983 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(vector_dim>=1)
<a name="l00984"></a>00984
<a name="l00985"></a>00985 MATLIKE1 C = COV1;
<a name="l00986"></a>00986 C+= COV2; <span class="comment">// Sum of covs:</span>
<a name="l00987"></a>00987 <span class="keywordflow">if</span> (CROSS_COV12) { C-=*CROSS_COV12; C-=*CROSS_COV12; }
<a name="l00988"></a>00988 <span class="keyword">const</span> T cov_det = C.det();
<a name="l00989"></a>00989 MATLIKE1 C_inv;
<a name="l00990"></a>00990 C.inv_fast(C_inv);
<a name="l00991"></a>00991
<a name="l00992"></a>00992 maha2_out = mean_diffs.multiply_HCHt_scalar(C_inv);
<a name="l00993"></a>00993 intprod_out = std::pow( <a class="code" href="mrpt__macros_8h.html#a4aa2c43b09c1300c334821f5507d6f71">M_2PI</a>, -0.5*vector_dim ) * (1.0/std::sqrt( cov_det ))*exp(-0.5*maha2_out);
<a name="l00994"></a>00994 }
<a name="l00995"></a>00995 <span class="comment"></span>
<a name="l00996"></a>00996 <span class="comment"> /** Computes both, the logarithm of the PDF and the square Mahalanobis distance between a point (given by its difference wrt the mean) and a Gaussian.</span>
<a name="l00997"></a>00997 <span class="comment"> * \sa productIntegralTwoGaussians, mahalanobisDistance2, normalPDF, mahalanobisDistance2AndPDF</span>
<a name="l00998"></a>00998 <span class="comment"> */</span>
<a name="l00999"></a>00999 <span class="keyword">template</span> <<span class="keyword">typename</span> T, <span class="keyword">class</span> VECLIKE,<span class="keyword">class</span> MATRIXLIKE>
<a name="l01000"></a><a class="code" href="group__stats__grp.html#ga69052bc18d0891a7b1d9fb02ab525e3b">01000</a> <span class="keywordtype">void</span> <a class="code" href="group__stats__grp.html#ga69052bc18d0891a7b1d9fb02ab525e3b" title="Computes both, the logarithm of the PDF and the square Mahalanobis distance between a point (given by...">mahalanobisDistance2AndLogPDF</a>(
<a name="l01001"></a>01001 <span class="keyword">const</span> VECLIKE &diff_mean,
<a name="l01002"></a>01002 <span class="keyword">const</span> MATRIXLIKE &<a class="code" href="namespacemrpt_1_1math.html#a43f4e051fc574fd75b6800ad4fb25037" title="Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...">cov</a>,
<a name="l01003"></a>01003 T &maha2_out,
<a name="l01004"></a>01004 T &log_pdf_out)
<a name="l01005"></a>01005 {
<a name="l01006"></a>01006 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l01007"></a>01007 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(cov.isSquare())
<a name="l01008"></a>01008 <a class="code" href="mrpt__macros_8h.html#a5ad4d8d68e2f6664f247407bf89aac55" title="Defines an assertion mechanism - only when compiled in debug.">ASSERTDEB_</a>(<span class="keywordtype">size_t</span>(cov.getColCount())==<span class="keywordtype">size_t</span>(diff_mean.size()))
<a name="l01009"></a>01009 MATRIXLIKE C_inv;
<a name="l01010"></a>01010 cov.inv(C_inv);
<a name="l01011"></a>01011 maha2_out = <a class="code" href="namespacemrpt_1_1math.html#aa8357cce481e22376ecadae393167977" title="r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)">multiply_HCHt_scalar</a>(diff_mean,C_inv);
<a name="l01012"></a>01012 log_pdf_out = <span class="keyword">static_cast<</span>typename <a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">MATRIXLIKE::value_type</a><span class="keyword">></span>(-0.5)* (
<a name="l01013"></a>01013 maha2_out+
<a name="l01014"></a>01014 <span class="keyword">static_cast<</span>typename <a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">MATRIXLIKE::value_type</a><span class="keyword">></span>(cov.getColCount())*::log(static_cast<typename MATRIXLIKE::value_type>(<a class="code" href="mrpt__macros_8h.html#a4aa2c43b09c1300c334821f5507d6f71">M_2PI</a>))+
<a name="l01015"></a>01015 ::log(cov.det())
<a name="l01016"></a>01016 );
<a name="l01017"></a>01017 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l01018"></a>01018 }
<a name="l01019"></a>01019 <span class="comment"></span>
<a name="l01020"></a>01020 <span class="comment"> /** Computes both, the PDF and the square Mahalanobis distance between a point (given by its difference wrt the mean) and a Gaussian.</span>
<a name="l01021"></a>01021 <span class="comment"> * \sa productIntegralTwoGaussians, mahalanobisDistance2, normalPDF</span>
<a name="l01022"></a>01022 <span class="comment"> */</span>
<a name="l01023"></a>01023 <span class="keyword">template</span> <<span class="keyword">typename</span> T, <span class="keyword">class</span> VECLIKE,<span class="keyword">class</span> MATRIXLIKE>
<a name="l01024"></a><a class="code" href="group__stats__grp.html#gaf3c8a238490b9bfcaf2f2631fb7c2ed2">01024</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="group__stats__grp.html#gaf3c8a238490b9bfcaf2f2631fb7c2ed2" title="Computes both, the PDF and the square Mahalanobis distance between a point (given by its difference w...">mahalanobisDistance2AndPDF</a>(
<a name="l01025"></a>01025 <span class="keyword">const</span> VECLIKE &diff_mean,
<a name="l01026"></a>01026 <span class="keyword">const</span> MATRIXLIKE &<a class="code" href="namespacemrpt_1_1math.html#a43f4e051fc574fd75b6800ad4fb25037" title="Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...">cov</a>,
<a name="l01027"></a>01027 T &maha2_out,
<a name="l01028"></a>01028 T &pdf_out)
<a name="l01029"></a>01029 {
<a name="l01030"></a>01030 <a class="code" href="group__stats__grp.html#ga69052bc18d0891a7b1d9fb02ab525e3b" title="Computes both, the logarithm of the PDF and the square Mahalanobis distance between a point (given by...">mahalanobisDistance2AndLogPDF</a>(diff_mean,cov,maha2_out,pdf_out);
<a name="l01031"></a>01031 pdf_out = std::exp(pdf_out); <span class="comment">// log to linear</span>
<a name="l01032"></a>01032 }
<a name="l01033"></a>01033 <span class="comment"></span>
<a name="l01034"></a>01034 <span class="comment"> /** @} */</span><span class="comment"></span>
<a name="l01035"></a>01035 <span class="comment"> /** @} */</span> <span class="comment">// end of grouping statistics</span>
<a name="l01036"></a>01036 <span class="comment"></span>
<a name="l01037"></a>01037 <span class="comment"> /** @addtogroup interpolation_grp Interpolation, least-squares fit, splines</span>
<a name="l01038"></a>01038 <span class="comment"> * \ingroup mrpt_base_grp</span>
<a name="l01039"></a>01039 <span class="comment"> * @{ */</span>
<a name="l01040"></a>01040 <span class="comment"></span>
<a name="l01041"></a>01041 <span class="comment"> /** Interpolate a data sequence "ys" ranging from "x0" to "x1" (equally spaced), to obtain the approximation of the sequence at the point "x".</span>
<a name="l01042"></a>01042 <span class="comment"> * If the point "x" is out of the range [x0,x1], the closest extreme "ys" value is returned.</span>
<a name="l01043"></a>01043 <span class="comment"> * \sa spline, interpolate2points</span>
<a name="l01044"></a>01044 <span class="comment"> */</span>
<a name="l01045"></a>01045 <span class="keyword">template</span> <<span class="keyword">class</span> T,<span class="keyword">class</span> VECTOR>
<a name="l01046"></a><a class="code" href="group__interpolation__grp.html#gaeb1c0badda6737f8362ce902e10961dc">01046</a> T <a class="code" href="group__interpolation__grp.html#gaeb1c0badda6737f8362ce902e10961dc" title="Interpolate a data sequence "ys" ranging from "x0" to "x1" (equally spaced), to obtain the approximat...">interpolate</a>(
<a name="l01047"></a>01047 <span class="keyword">const</span> T &x,
<a name="l01048"></a>01048 <span class="keyword">const</span> VECTOR &ys,
<a name="l01049"></a>01049 <span class="keyword">const</span> T &x0,
<a name="l01050"></a>01050 <span class="keyword">const</span> T &x1 )
<a name="l01051"></a>01051 {
<a name="l01052"></a>01052 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l01053"></a>01053 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(x1>x0); <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(!ys.empty());
<a name="l01054"></a>01054 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = ys.size();
<a name="l01055"></a>01055 <span class="keywordflow">if</span> (x<=x0) <span class="keywordflow">return</span> ys[0];
<a name="l01056"></a>01056 <span class="keywordflow">if</span> (x>=x1) <span class="keywordflow">return</span> ys[N-1];
<a name="l01057"></a>01057 <span class="keyword">const</span> T Ax = (x1-x0)/T(N);
<a name="l01058"></a>01058 <span class="keyword">const</span> <span class="keywordtype">size_t</span> i = int( (x-x0)/Ax );
<a name="l01059"></a>01059 <span class="keywordflow">if</span> (i>=N-1) <span class="keywordflow">return</span> ys[N-1];
<a name="l01060"></a>01060 <span class="keyword">const</span> T Ay = ys[i+1]-ys[i];
<a name="l01061"></a>01061 <span class="keywordflow">return</span> ys[i] + (x-(x0+i*Ax))*Ay/Ax;
<a name="l01062"></a>01062 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l01063"></a>01063 }
<a name="l01064"></a>01064 <span class="comment"></span>
<a name="l01065"></a>01065 <span class="comment"> /** Linear interpolation/extrapolation: evaluates at "x" the line (x0,y0)-(x1,y1).</span>
<a name="l01066"></a>01066 <span class="comment"> * If wrap2pi is true, output is wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).</span>
<a name="l01067"></a>01067 <span class="comment"> * \sa spline, interpolate, leastSquareLinearFit</span>
<a name="l01068"></a>01068 <span class="comment"> */</span>
<a name="l01069"></a>01069 <span class="keywordtype">double</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__interpolation__grp.html#ga199ae09bf97644048fe53590964abd1b" title="Linear interpolation/extrapolation: evaluates at "x" the line (x0,y0)-(x1,y1).">interpolate2points</a>(<span class="keyword">const</span> <span class="keywordtype">double</span> x, <span class="keyword">const</span> <span class="keywordtype">double</span> x0, <span class="keyword">const</span> <span class="keywordtype">double</span> y0, <span class="keyword">const</span> <span class="keywordtype">double</span> x1, <span class="keyword">const</span> <span class="keywordtype">double</span> y1, <span class="keywordtype">bool</span> wrap2pi = <span class="keyword">false</span>);
<a name="l01070"></a>01070 <span class="comment"></span>
<a name="l01071"></a>01071 <span class="comment"> /** Interpolates the value of a function in a point "t" given 4 SORTED points where "t" is between the two middle points</span>
<a name="l01072"></a>01072 <span class="comment"> * If wrap2pi is true, output "y" values are wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).</span>
<a name="l01073"></a>01073 <span class="comment"> * \sa leastSquareLinearFit</span>
<a name="l01074"></a>01074 <span class="comment"> */</span>
<a name="l01075"></a>01075 <span class="keywordtype">double</span> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="group__interpolation__grp.html#gadab1f3df3e592268d89214b345f7f816" title="Interpolates the value of a function in a point "t" given 4 SORTED points where "t" is between the tw...">spline</a>(<span class="keyword">const</span> <span class="keywordtype">double</span> <a class="code" href="eigen__plugins_8h.html#a7b88b312dc3827120dbfc60da344625d" title="Transpose.">t</a>, <span class="keyword">const</span> <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> &x, <span class="keyword">const</span> <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> &<a class="code" href="namespace_eigen_1_1internal.html#a3d7a581aeb951248dc6fe114e9e05f07">y</a>, <span class="keywordtype">bool</span> wrap2pi = <span class="keyword">false</span>);
<a name="l01076"></a>01076 <span class="comment"></span>
<a name="l01077"></a>01077 <span class="comment"> /** Interpolates or extrapolates using a least-square linear fit of the set of values "x" and "y", evaluated at a single point "t".</span>
<a name="l01078"></a>01078 <span class="comment"> * The vectors x and y must have size >=2, and all values of "x" must be different.</span>
<a name="l01079"></a>01079 <span class="comment"> * If wrap2pi is true, output "y" values are wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).</span>
<a name="l01080"></a>01080 <span class="comment"> * \sa spline</span>
<a name="l01081"></a>01081 <span class="comment"> * \sa getRegressionLine, getRegressionPlane</span>
<a name="l01082"></a>01082 <span class="comment"> */</span>
<a name="l01083"></a>01083 <span class="keyword">template</span> <<span class="keyword">typename</span> NUMTYPE,<span class="keyword">class</span> VECTORLIKE>
<a name="l01084"></a><a class="code" href="group__interpolation__grp.html#ga1e31a6d4b982eee16bab9ae66c0ee042">01084</a> NUMTYPE <a class="code" href="group__interpolation__grp.html#ga1e31a6d4b982eee16bab9ae66c0ee042" title="Interpolates or extrapolates using a least-square linear fit of the set of values "x" and "y"...">leastSquareLinearFit</a>(<span class="keyword">const</span> NUMTYPE <a class="code" href="eigen__plugins_8h.html#a7b88b312dc3827120dbfc60da344625d" title="Transpose.">t</a>, <span class="keyword">const</span> VECTORLIKE &x, <span class="keyword">const</span> VECTORLIKE &<a class="code" href="namespace_eigen_1_1internal.html#a3d7a581aeb951248dc6fe114e9e05f07">y</a>, <span class="keywordtype">bool</span> wrap2pi = <span class="keyword">false</span>)
<a name="l01085"></a>01085 {
<a name="l01086"></a>01086 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l01087"></a>01087
<a name="l01088"></a>01088 <span class="comment">// http://en.wikipedia.org/wiki/Linear_least_squares</span>
<a name="l01089"></a>01089 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(x.size()==y.size());
<a name="l01090"></a>01090 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(x.size()>1);
<a name="l01091"></a>01091
<a name="l01092"></a>01092 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = x.size();
<a name="l01093"></a>01093
<a name="l01094"></a>01094 <span class="keyword">typedef</span> <span class="keyword">typename</span> VECTORLIKE<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a> NUM;
<a name="l01095"></a>01095
<a name="l01096"></a>01096 <span class="comment">// X= [1 columns of ones, x' ]</span>
<a name="l01097"></a>01097 <span class="keyword">const</span> NUM x_min = x.minimum();
<a name="l01098"></a>01098 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<NUM></a> Xt(2,N);
<a name="l01099"></a>01099 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<N;i++)
<a name="l01100"></a>01100 {
<a name="l01101"></a>01101 Xt.set_unsafe(0,i, 1);
<a name="l01102"></a>01102 Xt.set_unsafe(1,i, x[i]-x_min);
<a name="l01103"></a>01103 }
<a name="l01104"></a>01104
<a name="l01105"></a>01105 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<NUM></a> XtX;
<a name="l01106"></a>01106 XtX.multiply_AAt(Xt);
<a name="l01107"></a>01107
<a name="l01108"></a>01108 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<NUM></a> XtXinv;
<a name="l01109"></a>01109 XtX.inv_fast(XtXinv);
<a name="l01110"></a>01110
<a name="l01111"></a>01111 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<NUM></a> XtXinvXt; <span class="comment">// B = inv(X' * X)*X' (pseudoinverse)</span>
<a name="l01112"></a>01112 XtXinvXt.multiply(XtXinv,Xt);
<a name="l01113"></a>01113
<a name="l01114"></a>01114 VECTORLIKE B;
<a name="l01115"></a>01115 XtXinvXt.multiply_Ab(y,B);
<a name="l01116"></a>01116
<a name="l01117"></a>01117 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(B.size()==2)
<a name="l01118"></a>01118
<a name="l01119"></a>01119 NUM ret = B[0] + B[1]*(t-x_min);
<a name="l01120"></a>01120
<a name="l01121"></a>01121 <span class="comment">// wrap?</span>
<a name="l01122"></a>01122 <span class="keywordflow">if</span> (!wrap2pi)
<a name="l01123"></a>01123 <span class="keywordflow">return</span> ret;
<a name="l01124"></a>01124 <span class="keywordflow">else</span> <span class="keywordflow">return</span> <a class="code" href="group__container__ops__grp.html#gaa2ddb99c5a5532075417f855b1c132fd" title="Modifies the given angle to translate it into the ]-pi,pi] range.">mrpt::math::wrapToPi</a>(ret);
<a name="l01125"></a>01125
<a name="l01126"></a>01126 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l01127"></a>01127 }
<a name="l01128"></a>01128 <span class="comment"></span>
<a name="l01129"></a>01129 <span class="comment"> /** Interpolates or extrapolates using a least-square linear fit of the set of values "x" and "y", evaluated at a sequence of points "ts" and returned at "outs".</span>
<a name="l01130"></a>01130 <span class="comment"> * If wrap2pi is true, output "y" values are wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).</span>
<a name="l01131"></a>01131 <span class="comment"> * \sa spline, getRegressionLine, getRegressionPlane</span>
<a name="l01132"></a>01132 <span class="comment"> */</span>
<a name="l01133"></a>01133 <span class="keyword">template</span> <<span class="keyword">class</span> VECTORLIKE1,<span class="keyword">class</span> VECTORLIKE2,<span class="keyword">class</span> VECTORLIKE3>
<a name="l01134"></a><a class="code" href="group__interpolation__grp.html#ga4939a6f537e90519df7149c550ed71c8">01134</a> <span class="keywordtype">void</span> <a class="code" href="group__interpolation__grp.html#ga1e31a6d4b982eee16bab9ae66c0ee042" title="Interpolates or extrapolates using a least-square linear fit of the set of values "x" and "y"...">leastSquareLinearFit</a>(
<a name="l01135"></a>01135 <span class="keyword">const</span> VECTORLIKE1 &ts,
<a name="l01136"></a>01136 VECTORLIKE2 &outs,
<a name="l01137"></a>01137 <span class="keyword">const</span> VECTORLIKE3 &x,
<a name="l01138"></a>01138 <span class="keyword">const</span> VECTORLIKE3 &<a class="code" href="namespace_eigen_1_1internal.html#a3d7a581aeb951248dc6fe114e9e05f07">y</a>,
<a name="l01139"></a>01139 <span class="keywordtype">bool</span> wrap2pi = <span class="keyword">false</span>)
<a name="l01140"></a>01140 {
<a name="l01141"></a>01141 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l01142"></a>01142
<a name="l01143"></a>01143 <span class="comment">// http://en.wikipedia.org/wiki/Linear_least_squares</span>
<a name="l01144"></a>01144 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(x.size()==y.size());
<a name="l01145"></a>01145 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(x.size()>1);
<a name="l01146"></a>01146
<a name="l01147"></a>01147 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = x.size();
<a name="l01148"></a>01148
<a name="l01149"></a>01149 <span class="comment">// X= [1 columns of ones, x' ]</span>
<a name="l01150"></a>01150 <span class="keyword">typedef</span> <span class="keyword">typename</span> VECTORLIKE3<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a> NUM;
<a name="l01151"></a>01151 <span class="keyword">const</span> NUM x_min = x.minimum();
<a name="l01152"></a>01152 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<NUM></a> Xt(2,N);
<a name="l01153"></a>01153 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<N;i++)
<a name="l01154"></a>01154 {
<a name="l01155"></a>01155 Xt.set_unsafe(0,i, 1);
<a name="l01156"></a>01156 Xt.set_unsafe(1,i, x[i]-x_min);
<a name="l01157"></a>01157 }
<a name="l01158"></a>01158
<a name="l01159"></a>01159 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<NUM></a> XtX;
<a name="l01160"></a>01160 XtX.multiply_AAt(Xt);
<a name="l01161"></a>01161
<a name="l01162"></a>01162 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<NUM></a> XtXinv;
<a name="l01163"></a>01163 XtX.inv_fast(XtXinv);
<a name="l01164"></a>01164
<a name="l01165"></a>01165 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixTemplateNumeric<NUM></a> XtXinvXt; <span class="comment">// B = inv(X' * X)*X' (pseudoinverse)</span>
<a name="l01166"></a>01166 XtXinvXt.multiply(XtXinv,Xt);
<a name="l01167"></a>01167
<a name="l01168"></a>01168 VECTORLIKE3 B;
<a name="l01169"></a>01169 XtXinvXt.multiply_Ab(y,B);
<a name="l01170"></a>01170
<a name="l01171"></a>01171 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>(B.size()==2)
<a name="l01172"></a>01172
<a name="l01173"></a>01173 <span class="keyword">const</span> <span class="keywordtype">size_t</span> tsN = <span class="keywordtype">size_t</span>(ts.size());
<a name="l01174"></a>01174 outs.resize(tsN);
<a name="l01175"></a>01175 <span class="keywordflow">if</span> (!wrap2pi)
<a name="l01176"></a>01176 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<tsN;k++)
<a name="l01177"></a>01177 outs[k] = B[0] + B[1]*(ts[k]-x_min);
<a name="l01178"></a>01178 <span class="keywordflow">else</span>
<a name="l01179"></a>01179 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<tsN;k++)
<a name="l01180"></a>01180 outs[k] = <a class="code" href="group__container__ops__grp.html#gaa2ddb99c5a5532075417f855b1c132fd" title="Modifies the given angle to translate it into the ]-pi,pi] range.">mrpt::math::wrapToPi</a>( B[0] + B[1]*(ts[k]-x_min) );
<a name="l01181"></a>01181 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l01182"></a>01182 }
<a name="l01183"></a>01183 <span class="comment"></span>
<a name="l01184"></a>01184 <span class="comment"> /** @} */</span> <span class="comment">// end grouping interpolation_grp</span>
<a name="l01185"></a>01185
<a name="l01186"></a>01186 } <span class="comment">// End of MATH namespace</span>
<a name="l01187"></a>01187
<a name="l01188"></a>01188 } <span class="comment">// End of namespace</span>
<a name="l01189"></a>01189
<a name="l01190"></a>01190 <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>
