<!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>ops_matrices.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">ops_matrices.h</div> </div>
</div>
<div class="contents">
<a href="ops__matrices_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_matrix_ops_H</span>
<a name="l00029"></a>00029 <span class="preprocessor"></span><span class="preprocessor">#define mrpt_math_matrix_ops_H</span>
<a name="l00030"></a>00030 <span class="preprocessor"></span>
<a name="l00031"></a>00031 <span class="preprocessor">#include <<a class="code" href="math__frwds_8h.html">mrpt/math/math_frwds.h</a>></span> <span class="comment">// Fordward declarations</span>
<a name="l00032"></a>00032
<a name="l00033"></a>00033 <span class="preprocessor">#include <<a class="code" href="_c_matrix_8h.html">mrpt/math/CMatrix.h</a>></span>
<a name="l00034"></a>00034 <span class="preprocessor">#include <<a class="code" href="_c_matrix_d_8h.html">mrpt/math/CMatrixD.h</a>></span>
<a name="l00035"></a>00035 <span class="preprocessor">#include <<a class="code" href="_c_stream_8h.html">mrpt/utils/CStream.h</a>></span>
<a name="l00036"></a>00036
<a name="l00037"></a>00037 <span class="preprocessor">#include <<a class="code" href="_c_matrix_template_numeric_8h.html">mrpt/math/CMatrixTemplateNumeric.h</a>></span>
<a name="l00038"></a>00038 <span class="preprocessor">#include <<a class="code" href="_c_matrix_fixed_numeric_8h.html">mrpt/math/CMatrixFixedNumeric.h</a>></span>
<a name="l00039"></a>00039
<a name="l00040"></a>00040 <span class="preprocessor">#include <<a class="code" href="ops__containers_8h.html" title="This file implements several operations that operate element-wise on individual or pairs of container...">mrpt/math/ops_containers.h</a>></span> <span class="comment">// Many generic operations</span>
<a name="l00041"></a>00041
<a name="l00042"></a>00042 <span class="preprocessor">#include <<a class="code" href="metaprogramming_8h.html">mrpt/utils/metaprogramming.h</a>></span> <span class="comment">// for copy_container_typecasting()</span>
<a name="l00043"></a>00043
<a name="l00044"></a>00044 <span class="comment"></span>
<a name="l00045"></a>00045 <span class="comment">/** \file ops_matrices.h</span>
<a name="l00046"></a>00046 <span class="comment"> * This file implements miscelaneous matrix and matrix/vector operations, plus internal functions in mrpt::math::detail</span>
<a name="l00047"></a>00047 <span class="comment"> *</span>
<a name="l00048"></a>00048 <span class="comment"> */</span>
<a name="l00049"></a>00049
<a name="l00050"></a>00050 <span class="keyword">namespace </span>mrpt
<a name="l00051"></a>00051 {
<a name="l00052"></a>00052 <span class="keyword">namespace </span>math
<a name="l00053"></a>00053 {<span class="comment"></span>
<a name="l00054"></a>00054 <span class="comment"> /** \addtogroup container_ops_grp</span>
<a name="l00055"></a>00055 <span class="comment"> * @{ */</span>
<a name="l00056"></a>00056 <span class="comment"></span>
<a name="l00057"></a>00057 <span class="comment"> /** @name Operators for binary streaming of MRPT matrices</span>
<a name="l00058"></a>00058 <span class="comment"> @{ */</span>
<a name="l00059"></a>00059 <span class="comment"></span>
<a name="l00060"></a>00060 <span class="comment"> /** Read operator from a CStream. The format is compatible with that of CMatrix & CMatrixD */</span>
<a name="l00061"></a>00061 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> NROWS,<span class="keywordtype">size_t</span> NCOLS>
<a name="l00062"></a><a class="code" href="group__container__ops__grp.html#ga56a2bcbd3e333c77935a7e54d6eb9466">00062</a> mrpt<a class="code" href="classmrpt_1_1utils_1_1_c_stream.html" title="This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...">::utils::CStream</a> &<a class="code" href="namespacemrpt_1_1math.html#ab55e39255f4b544e7693867d22a70b91">operator>></a>(<a class="code" href="classmrpt_1_1utils_1_1_c_stream.html" title="This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...">mrpt::utils::CStream</a> &in, <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<float,NROWS,NCOLS></a> & M) {
<a name="l00063"></a>00063 <a class="code" href="classmrpt_1_1math_1_1_c_matrix.html" title="This class is a "CSerializable" wrapper for "CMatrixFloat".">CMatrix</a> aux;
<a name="l00064"></a>00064 in.<a class="code" href="classmrpt_1_1utils_1_1_c_stream.html#a290510e87656433dfd093cc31d34371c" title="Reads an object from stream, its class determined at runtime, and returns a smart pointer to the obje...">ReadObject</a>(&aux);
<a name="l00065"></a>00065 <a class="code" href="mrpt__macros_8h.html#ad30ea0382c594c0e2efe88212e9352b0">ASSERTMSG_</a>(M.<a class="code" href="class_eigen_1_1_plain_object_base.html#a6268375015b1413e43d624d1b3207285">cols</a>()==aux.<a class="code" href="class_eigen_1_1_plain_object_base.html#a6268375015b1413e43d624d1b3207285">cols</a>() && M.<a class="code" href="class_eigen_1_1_plain_object_base.html#a4f740be94c28777819aecb8d47e3bdb4">rows</a>()==aux.<a class="code" href="class_eigen_1_1_plain_object_base.html#a4f740be94c28777819aecb8d47e3bdb4">rows</a>(), <a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Size mismatch: deserialized is %ux%u, expected is %ux%u"</span>,(<span class="keywordtype">unsigned</span>)aux.getRowCount(),(unsigned)aux.getColCount(),(unsigned)NROWS,(<span class="keywordtype">unsigned</span>)NCOLS))
<a name="l00066"></a>00066 M = aux;
<a name="l00067"></a>00067 <span class="keywordflow">return</span> in;
<a name="l00068"></a>00068 }<span class="comment"></span>
<a name="l00069"></a>00069 <span class="comment"> /** Read operator from a CStream. The format is compatible with that of CMatrix & CMatrixD */</span>
<a name="l00070"></a>00070 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> NROWS,<span class="keywordtype">size_t</span> NCOLS>
<a name="l00071"></a><a class="code" href="group__container__ops__grp.html#ga745fc9af0a39075b935a4b84c0b392b1">00071</a> mrpt<a class="code" href="classmrpt_1_1utils_1_1_c_stream.html" title="This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...">::utils::CStream</a> &<a class="code" href="namespacemrpt_1_1math.html#ab55e39255f4b544e7693867d22a70b91">operator>></a>(<a class="code" href="classmrpt_1_1utils_1_1_c_stream.html" title="This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...">mrpt::utils::CStream</a> &in, <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<double,NROWS,NCOLS></a> & M) {
<a name="l00072"></a>00072 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_d.html" title="This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".">CMatrixD</a> aux;
<a name="l00073"></a>00073 in.<a class="code" href="classmrpt_1_1utils_1_1_c_stream.html#a290510e87656433dfd093cc31d34371c" title="Reads an object from stream, its class determined at runtime, and returns a smart pointer to the obje...">ReadObject</a>(&aux);
<a name="l00074"></a>00074 <a class="code" href="mrpt__macros_8h.html#ad30ea0382c594c0e2efe88212e9352b0">ASSERTMSG_</a>(M.<a class="code" href="class_eigen_1_1_plain_object_base.html#a6268375015b1413e43d624d1b3207285">cols</a>()==aux.cols() && M.<a class="code" href="class_eigen_1_1_plain_object_base.html#a4f740be94c28777819aecb8d47e3bdb4">rows</a>()==aux.rows(), <a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Size mismatch: deserialized is %ux%u, expected is %ux%u"</span>,(<span class="keywordtype">unsigned</span>)aux.getRowCount(),(unsigned)aux.getColCount(),(unsigned)NROWS,(<span class="keywordtype">unsigned</span>)NCOLS))
<a name="l00075"></a>00075 M = aux;
<a name="l00076"></a>00076 <span class="keywordflow">return</span> in;
<a name="l00077"></a>00077 }
<a name="l00078"></a>00078 <span class="comment"></span>
<a name="l00079"></a>00079 <span class="comment"> /** Write operator for writing into a CStream. The format is compatible with that of CMatrix & CMatrixD */</span>
<a name="l00080"></a>00080 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> NROWS,<span class="keywordtype">size_t</span> NCOLS>
<a name="l00081"></a><a class="code" href="group__container__ops__grp.html#ga22527dc2d242a466ca2bcd8f1656594b">00081</a> mrpt<a class="code" href="classmrpt_1_1utils_1_1_c_stream.html" title="This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...">::utils::CStream</a> &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
<a name="l00082"></a>00082 <a class="code" href="classmrpt_1_1math_1_1_c_matrix.html" title="This class is a "CSerializable" wrapper for "CMatrixFloat".">CMatrix</a> aux = <a class="code" href="namespacemrpt_1_1math.html#a46578d070e41e17dead613002e755aa3" title="Declares a matrix of float numbers (non serializable).">CMatrixFloat</a>(M);
<a name="l00083"></a>00083 out.WriteObject(&aux);
<a name="l00084"></a>00084 <span class="keywordflow">return</span> out;
<a name="l00085"></a>00085 }<span class="comment"></span>
<a name="l00086"></a>00086 <span class="comment"> /** Write operator for writing into a CStream. The format is compatible with that of CMatrix & CMatrixD */</span>
<a name="l00087"></a>00087 <span class="keyword">template</span> <<span class="keywordtype">size_t</span> NROWS,<span class="keywordtype">size_t</span> NCOLS>
<a name="l00088"></a><a class="code" href="group__container__ops__grp.html#ga0a1091c4d09339131c04d59c9b534a58">00088</a> mrpt<a class="code" href="classmrpt_1_1utils_1_1_c_stream.html" title="This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...">::utils::CStream</a> &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
<a name="l00089"></a>00089 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_d.html" title="This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".">CMatrixD</a> aux = <a class="code" href="namespacemrpt_1_1math.html#a3814c2b868f059d6a7ab0d8ecd2311d6" title="Declares a matrix of double numbers (non serializable).">CMatrixDouble</a>(M);
<a name="l00090"></a>00090 out.WriteObject(&aux);
<a name="l00091"></a>00091 <span class="keywordflow">return</span> out;
<a name="l00092"></a>00092 }
<a name="l00093"></a>00093 <span class="comment"></span>
<a name="l00094"></a>00094 <span class="comment"> /** @} */</span> <span class="comment">// end MRPT matrices stream operators</span>
<a name="l00095"></a>00095
<a name="l00096"></a>00096 <span class="comment"></span>
<a name="l00097"></a>00097 <span class="comment"> /** @name Operators for text streaming of MRPT matrices</span>
<a name="l00098"></a>00098 <span class="comment"> @{ */</span>
<a name="l00099"></a>00099
<a name="l00100"></a>00100 <span class="comment"></span>
<a name="l00101"></a>00101 <span class="comment"> /** Dumps the matrix to a text ostream, adding a final "\n" to Eigen's default output. */</span>
<a name="l00102"></a>00102 <span class="keyword">template</span> <<span class="keyword">typename</span> T,<span class="keywordtype">size_t</span> NROWS,<span class="keywordtype">size_t</span> NCOLS>
<a name="l00103"></a><a class="code" href="group__container__ops__grp.html#ga91b22de0057ca91e09f29a4b56e26976">00103</a> <span class="keyword">inline</span> std<a class="code" href="classstd_1_1ostream.html" title="STL class.">::ostream</a> & operator << (std::ostream & s, const CMatrixFixedNumeric<T,NROWS,NCOLS>& m)
<a name="l00104"></a>00104 {
<a name="l00105"></a>00105 Eigen::IOFormat fmt; fmt.<a class="code" href="struct_eigen_1_1_i_o_format.html#abd7f0810c3f1413a018cd359c4ec384b">matSuffix</a>=<span class="stringliteral">"\n"</span>;
<a name="l00106"></a>00106 <span class="keywordflow">return</span> s << m.format(fmt);
<a name="l00107"></a>00107 }
<a name="l00108"></a>00108 <span class="comment"></span>
<a name="l00109"></a>00109 <span class="comment"> /** Dumps the matrix to a text ostream, adding a final "\n" to Eigen's default output. */</span>
<a name="l00110"></a>00110 <span class="keyword">template</span><<span class="keyword">typename</span> T>
<a name="l00111"></a><a class="code" href="group__container__ops__grp.html#ga76d0eeef79e0687814bdee2a24a4c070">00111</a> <span class="keyword">inline</span> std<a class="code" href="classstd_1_1ostream.html" title="STL class.">::ostream</a> & operator << (std::ostream & s, const CMatrixTemplateNumeric<T>& m)
<a name="l00112"></a>00112 {
<a name="l00113"></a>00113 Eigen::IOFormat fmt; fmt.<a class="code" href="struct_eigen_1_1_i_o_format.html#abd7f0810c3f1413a018cd359c4ec384b">matSuffix</a>=<span class="stringliteral">"\n"</span>;
<a name="l00114"></a>00114 <span class="keywordflow">return</span> s << m.format(fmt);
<a name="l00115"></a>00115 }
<a name="l00116"></a>00116 <span class="comment"></span>
<a name="l00117"></a>00117 <span class="comment"> /** Dumps the vector as a row to a text ostream, with the format: "[v1 v2 v3... vN]" */</span>
<a name="l00118"></a>00118 <span class="keyword">template</span><<span class="keyword">typename</span> T>
<a name="l00119"></a><a class="code" href="group__container__ops__grp.html#ga19bbefb659826b1e442bd8cf628b1507">00119</a> <span class="keyword">inline</span> std<a class="code" href="classstd_1_1ostream.html" title="STL class.">::ostream</a> & operator << (std::ostream & s, const mrpt::dynamicsize_vector<T>& m)
<a name="l00120"></a>00120 {
<a name="l00121"></a>00121 Eigen::IOFormat fmt; fmt.<a class="code" href="struct_eigen_1_1_i_o_format.html#aa97a2793595db0f308ad001d7c3d5042">rowSeparator</a>=<span class="stringliteral">" "</span>; fmt.<a class="code" href="struct_eigen_1_1_i_o_format.html#a84ef75e032b1d786d7bae864c3111a94">matPrefix</a>=<span class="stringliteral">"["</span>; fmt.<a class="code" href="struct_eigen_1_1_i_o_format.html#abd7f0810c3f1413a018cd359c4ec384b">matSuffix</a>=<span class="stringliteral">"]"</span>;
<a name="l00122"></a>00122 <span class="keywordflow">return</span> s << m.format(fmt);
<a name="l00123"></a>00123 }
<a name="l00124"></a>00124 <span class="comment"></span>
<a name="l00125"></a>00125 <span class="comment"> /** @} */</span> <span class="comment">// end MRPT matrices stream operators</span>
<a name="l00126"></a>00126
<a name="l00127"></a>00127 <span class="comment"></span>
<a name="l00128"></a>00128 <span class="comment"> /** Transpose operator for matrices */</span>
<a name="l00129"></a>00129 <span class="keyword">template</span> <<span class="keyword">class</span> Derived>
<a name="l00130"></a><a class="code" href="group__container__ops__grp.html#gab02e2e15de033aa2c5f47096964361ec">00130</a> <span class="keyword">inline</span> <span class="keyword">const</span> <span class="keyword">typename</span> Eigen::MatrixBase<Derived>::AdjointReturnType <a class="code" href="group__container__ops__grp.html#gab02e2e15de033aa2c5f47096964361ec" title="Transpose operator for matrices.">operator ~</a>(<span class="keyword">const</span> <a class="code" href="class_eigen_1_1_matrix_base.html">Eigen::MatrixBase<Derived></a> &m) {
<a name="l00131"></a>00131 <span class="keywordflow">return</span> m.<a class="code" href="class_eigen_1_1_matrix_base.html#abb43f33ace14ac23e2678311a46f10fc">adjoint</a>();
<a name="l00132"></a>00132 }
<a name="l00133"></a>00133 <span class="comment"></span>
<a name="l00134"></a>00134 <span class="comment"> /** Unary inversion operator. */</span>
<a name="l00135"></a>00135 <span class="keyword">template</span> <<span class="keyword">class</span> Derived>
<a name="l00136"></a><a class="code" href="group__container__ops__grp.html#gae6aca84fb7a8bb647f606d59facd1e66">00136</a> <span class="keyword">inline</span> <span class="keyword">typename</span> Eigen::MatrixBase<Derived>::PlainObject <a class="code" href="group__container__ops__grp.html#gae6aca84fb7a8bb647f606d59facd1e66" title="Unary inversion operator.">operator !</a>(<span class="keyword">const</span> <a class="code" href="class_eigen_1_1_matrix_base.html">Eigen::MatrixBase<Derived></a> &m) {
<a name="l00137"></a>00137 <span class="keywordflow">return</span> m.<a class="code" href="class_eigen_1_1_matrix_base.html#a7f3f8b129246e3712dce005c72ecdfb6">inv</a>();
<a name="l00138"></a>00138 }
<a name="l00139"></a>00139 <span class="comment"></span>
<a name="l00140"></a>00140 <span class="comment"> /** @} */</span> <span class="comment">// end MRPT matrices operators</span>
<a name="l00141"></a>00141
<a name="l00142"></a>00142 <span class="comment"></span>
<a name="l00143"></a>00143 <span class="comment"> /** R = H * C * H^t (with C symmetric) */</span>
<a name="l00144"></a>00144 <span class="keyword">template</span> <<span class="keyword">typename</span> MAT_H, <span class="keyword">typename</span> MAT_C, <span class="keyword">typename</span> MAT_R>
<a name="l00145"></a><a class="code" href="namespacemrpt_1_1math.html#ad9a424a973f0598f255924a9c89c07f7">00145</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="namespacemrpt_1_1math.html#ad9a424a973f0598f255924a9c89c07f7" title="R = H * C * H^t (with C symmetric)">multiply_HCHt</a>(
<a name="l00146"></a>00146 <span class="keyword">const</span> MAT_H &H,
<a name="l00147"></a>00147 <span class="keyword">const</span> MAT_C &C,
<a name="l00148"></a>00148 MAT_R &R,
<a name="l00149"></a>00149 <span class="keywordtype">bool</span> accumResultInOutput ) <span class="comment">// bool allow_submatrix_mult)</span>
<a name="l00150"></a>00150 {
<a name="l00151"></a>00151 <span class="keywordflow">if</span> (accumResultInOutput)
<a name="l00152"></a>00152 R += ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
<a name="l00153"></a>00153 <span class="keywordflow">else</span>
<a name="l00154"></a>00154 R = ( (H * C.template selfadjointView<Eigen::Lower>()).eval() * H.adjoint()).eval().template selfadjointView<Eigen::Lower>();
<a name="l00155"></a>00155 }
<a name="l00156"></a>00156 <span class="comment"></span>
<a name="l00157"></a>00157 <span class="comment"> /** r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C) */</span>
<a name="l00158"></a>00158 <span class="keyword">template</span> <<span class="keyword">typename</span> VECTOR_H, <span class="keyword">typename</span> MAT_C>
<a name="l00159"></a>00159 <span class="keyword">typename</span> MAT_C<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a>
<a name="l00160"></a><a class="code" href="namespacemrpt_1_1math.html#aa8357cce481e22376ecadae393167977">00160</a> <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>(<span class="keyword">const</span> VECTOR_H &H, <span class="keyword">const</span> MAT_C &C)
<a name="l00161"></a>00161 {
<a name="l00162"></a>00162 <span class="keywordflow">return</span> (H.matrix().adjoint() * C * H.matrix()).eval()(0,0);
<a name="l00163"></a>00163 }
<a name="l00164"></a>00164 <span class="comment"></span>
<a name="l00165"></a>00165 <span class="comment"> /** R = H^t * C * H (with C symmetric) */</span>
<a name="l00166"></a>00166 <span class="keyword">template</span> <<span class="keyword">typename</span> MAT_H, <span class="keyword">typename</span> MAT_C, <span class="keyword">typename</span> MAT_R>
<a name="l00167"></a><a class="code" href="namespacemrpt_1_1math.html#a49f1d85587f29a9d6691973452a3b2a0">00167</a> <span class="keywordtype">void</span> <a class="code" href="namespacemrpt_1_1math.html#a49f1d85587f29a9d6691973452a3b2a0" title="R = H^t * C * H (with C symmetric)">multiply_HtCH</a>(
<a name="l00168"></a>00168 <span class="keyword">const</span> MAT_H &H,
<a name="l00169"></a>00169 <span class="keyword">const</span> MAT_C &C,
<a name="l00170"></a>00170 MAT_R &R,
<a name="l00171"></a>00171 <span class="keywordtype">bool</span> accumResultInOutput) <span class="comment">// bool allow_submatrix_mult)</span>
<a name="l00172"></a>00172 {
<a name="l00173"></a>00173 <span class="keywordflow">if</span> (accumResultInOutput)
<a name="l00174"></a>00174 R += ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
<a name="l00175"></a>00175 <span class="keywordflow">else</span>
<a name="l00176"></a>00176 R = ( (H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() * H).eval().template selfadjointView<Eigen::Lower>();
<a name="l00177"></a>00177 }
<a name="l00178"></a>00178
<a name="l00179"></a>00179 <span class="comment"></span>
<a name="l00180"></a>00180 <span class="comment"> /** Computes the mean vector and covariance from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.</span>
<a name="l00181"></a>00181 <span class="comment"> * \param v The set of data as a NxM matrix, of types: CMatrixTemplateNumeric or CMatrixFixedNumeric</span>
<a name="l00182"></a>00182 <span class="comment"> * \param out_mean The output M-vector for the estimated mean.</span>
<a name="l00183"></a>00183 <span class="comment"> * \param out_cov The output MxM matrix for the estimated covariance matrix, this can also be either a fixed-size of dynamic size matrix.</span>
<a name="l00184"></a>00184 <span class="comment"> * \sa mrpt::math::meanAndCovVec, math::mean,math::stddev, math::cov</span>
<a name="l00185"></a>00185 <span class="comment"> */</span>
<a name="l00186"></a>00186 <span class="keyword">template</span><<span class="keyword">class</span> MAT_IN,<span class="keyword">class</span> VECTOR, <span class="keyword">class</span> MAT_OUT>
<a name="l00187"></a><a class="code" href="namespacemrpt_1_1math.html#ae8d48c1203173296453a12b174ed932c">00187</a> <span class="keywordtype">void</span> <a class="code" href="namespacemrpt_1_1math.html#ae8d48c1203173296453a12b174ed932c" title="Computes the mean vector and covariance from a list of samples in an NxM matrix, where each row is a ...">meanAndCovMat</a>(
<a name="l00188"></a>00188 <span class="keyword">const</span> MAT_IN & v,
<a name="l00189"></a>00189 VECTOR & out_mean,
<a name="l00190"></a>00190 MAT_OUT & out_cov
<a name="l00191"></a>00191 )
<a name="l00192"></a>00192 {
<a name="l00193"></a>00193 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = v.rows();
<a name="l00194"></a>00194 <a class="code" href="mrpt__macros_8h.html#ad30ea0382c594c0e2efe88212e9352b0">ASSERTMSG_</a>(N>0,<span class="stringliteral">"The input matrix contains no elements"</span>);
<a name="l00195"></a>00195 <span class="keyword">const</span> <span class="keywordtype">double</span> N_inv = 1.0/N;
<a name="l00196"></a>00196
<a name="l00197"></a>00197 <span class="keyword">const</span> <span class="keywordtype">size_t</span> M = v.cols();
<a name="l00198"></a>00198 <a class="code" href="mrpt__macros_8h.html#ad30ea0382c594c0e2efe88212e9352b0">ASSERTMSG_</a>(M>0,<span class="stringliteral">"The input matrix contains rows of length 0"</span>);
<a name="l00199"></a>00199
<a name="l00200"></a>00200 <span class="comment">// First: Compute the mean</span>
<a name="l00201"></a>00201 out_mean.assign(M,0);
<a name="l00202"></a>00202 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<N;i++)
<a name="l00203"></a>00203 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0;j<M;j++)
<a name="l00204"></a>00204 out_mean[j]+=v.coeff(i,j);
<a name="l00205"></a>00205 out_mean*=N_inv;
<a name="l00206"></a>00206
<a name="l00207"></a>00207 <span class="comment">// Second: Compute the covariance</span>
<a name="l00208"></a>00208 <span class="comment">// Save only the above-diagonal part, then after averaging</span>
<a name="l00209"></a>00209 <span class="comment">// duplicate that part to the other half.</span>
<a name="l00210"></a>00210 out_cov.zeros(M,M);
<a name="l00211"></a>00211 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<N;i++)
<a name="l00212"></a>00212 {
<a name="l00213"></a>00213 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0;j<M;j++)
<a name="l00214"></a>00214 out_cov.get_unsafe(j,j)+=<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(v.get_unsafe(i,j)-out_mean[j]);
<a name="l00215"></a>00215
<a name="l00216"></a>00216 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0;j<M;j++)
<a name="l00217"></a>00217 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=j+1;k<M;k++)
<a name="l00218"></a>00218 out_cov.get_unsafe(j,k)+=(v.get_unsafe(i,j)-out_mean[j])*(v.get_unsafe(i,k)-out_mean[k]);
<a name="l00219"></a>00219 }
<a name="l00220"></a>00220 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j=0;j<M;j++)
<a name="l00221"></a>00221 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=j+1;k<M;k++)
<a name="l00222"></a>00222 out_cov.get_unsafe(k,j) = out_cov.get_unsafe(j,k);
<a name="l00223"></a>00223 out_cov*=N_inv;
<a name="l00224"></a>00224 }
<a name="l00225"></a>00225 <span class="comment"></span>
<a name="l00226"></a>00226 <span class="comment"> /** Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.</span>
<a name="l00227"></a>00227 <span class="comment"> * \param v The set of data, as a NxM matrix.</span>
<a name="l00228"></a>00228 <span class="comment"> * \param out_cov The output MxM matrix for the estimated covariance matrix.</span>
<a name="l00229"></a>00229 <span class="comment"> * \sa math::mean,math::stddev, math::cov</span>
<a name="l00230"></a>00230 <span class="comment"> */</span>
<a name="l00231"></a>00231 <span class="keyword">template</span><<span class="keyword">class</span> MATRIX>
<a name="l00232"></a>00232 <span class="keyword">inline</span> Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime>
<a name="l00233"></a><a class="code" href="namespacemrpt_1_1math.html#a43f4e051fc574fd75b6800ad4fb25037">00233</a> <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>( <span class="keyword">const</span> MATRIX &v )
<a name="l00234"></a>00234 {
<a name="l00235"></a>00235 Eigen::Matrix<double,MATRIX::ColsAtCompileTime,1> m;
<a name="l00236"></a>00236 Eigen::Matrix<typename MATRIX::Scalar,MATRIX::ColsAtCompileTime,MATRIX::ColsAtCompileTime> C;
<a name="l00237"></a>00237 <a class="code" href="namespacemrpt_1_1math.html#ae8d48c1203173296453a12b174ed932c" title="Computes the mean vector and covariance from a list of samples in an NxM matrix, where each row is a ...">meanAndCovMat</a>(v,m,C);
<a name="l00238"></a>00238 <span class="keywordflow">return</span> C;
<a name="l00239"></a>00239 }
<a name="l00240"></a>00240 <span class="comment"></span>
<a name="l00241"></a>00241 <span class="comment"> /** A useful macro for saving matrixes to a file while debugging. */</span>
<a name="l00242"></a><a class="code" href="ops__matrices_8h.html#a462cb509d2376fdf42659d059685ca08">00242</a> <span class="preprocessor"> #define SAVE_MATRIX(M) M.saveToTextFile(#M ".txt");</span>
<a name="l00243"></a>00243 <span class="preprocessor"></span>
<a name="l00244"></a>00244 <span class="comment"></span>
<a name="l00245"></a>00245 <span class="comment"> /** Only for vectors/arrays "v" of length3, compute out = A * Skew(v), where Skew(v) is the skew symmetric matric generated from \a v (see mrpt::math::skew_symmetric3)</span>
<a name="l00246"></a>00246 <span class="comment"> */</span>
<a name="l00247"></a>00247 <span class="keyword">template</span> <<span class="keyword">class</span> MAT_A,<span class="keyword">class</span> SKEW_3VECTOR,<span class="keyword">class</span> MAT_OUT>
<a name="l00248"></a><a class="code" href="namespacemrpt_1_1math.html#ad21f2859644b754aba9a1392503a9776">00248</a> <span class="keywordtype">void</span> <a class="code" href="namespacemrpt_1_1math.html#ad21f2859644b754aba9a1392503a9776" title="Only for vectors/arrays "v" of length3, compute out = A * Skew(v), where Skew(v) is the skew symmetri...">multiply_A_skew3</a>(<span class="keyword">const</span> MAT_A &A,<span class="keyword">const</span> SKEW_3VECTOR &v, MAT_OUT &out)
<a name="l00249"></a>00249 {
<a name="l00250"></a>00250 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00251"></a>00251 <a class="code" href="mrpt__macros_8h.html#a02c6e78d47a0bae723824559846cc673">ASSERT_EQUAL_</a>(<a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">size</a>(A,2),3)
<a name="l00252"></a>00252 <a class="code" href="mrpt__macros_8h.html#a02c6e78d47a0bae723824559846cc673">ASSERT_EQUAL_</a>(v.size(),3)
<a name="l00253"></a>00253 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = <a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">size</a>(A,1);
<a name="l00254"></a>00254 out.setSize(N,3);
<a name="l00255"></a>00255 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<N;i++)
<a name="l00256"></a>00256 {
<a name="l00257"></a>00257 out.set_unsafe(i,0, A.get_unsafe(i,1)*v[2]-A.get_unsafe(i,2)*v[1] );
<a name="l00258"></a>00258 out.set_unsafe(i,1,-A.get_unsafe(i,0)*v[2]+A.get_unsafe(i,2)*v[0] );
<a name="l00259"></a>00259 out.set_unsafe(i,2, A.get_unsafe(i,0)*v[1]-A.get_unsafe(i,1)*v[0] );
<a name="l00260"></a>00260 }
<a name="l00261"></a>00261 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00262"></a>00262 }
<a name="l00263"></a>00263 <span class="comment"></span>
<a name="l00264"></a>00264 <span class="comment"> /** Only for vectors/arrays "v" of length3, compute out = Skew(v) * A, where Skew(v) is the skew symmetric matric generated from \a v (see mrpt::math::skew_symmetric3)</span>
<a name="l00265"></a>00265 <span class="comment"> */</span>
<a name="l00266"></a>00266 <span class="keyword">template</span> <<span class="keyword">class</span> SKEW_3VECTOR,<span class="keyword">class</span> MAT_A,<span class="keyword">class</span> MAT_OUT>
<a name="l00267"></a><a class="code" href="namespacemrpt_1_1math.html#a134f1d14fa12f25cbc82fa36faf59393">00267</a> <span class="keywordtype">void</span> <a class="code" href="namespacemrpt_1_1math.html#a134f1d14fa12f25cbc82fa36faf59393" title="Only for vectors/arrays "v" of length3, compute out = Skew(v) * A, where Skew(v) is the skew symmetri...">multiply_skew3_A</a>(<span class="keyword">const</span> SKEW_3VECTOR &v, <span class="keyword">const</span> MAT_A &A,MAT_OUT &out)
<a name="l00268"></a>00268 {
<a name="l00269"></a>00269 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00270"></a>00270 <a class="code" href="mrpt__macros_8h.html#a02c6e78d47a0bae723824559846cc673">ASSERT_EQUAL_</a>(<a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">size</a>(A,1),3)
<a name="l00271"></a>00271 <a class="code" href="mrpt__macros_8h.html#a02c6e78d47a0bae723824559846cc673">ASSERT_EQUAL_</a>(v.size(),3)
<a name="l00272"></a>00272 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = <a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">size</a>(A,2);
<a name="l00273"></a>00273 out.setSize(3,N);
<a name="l00274"></a>00274 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<N;i++)
<a name="l00275"></a>00275 {
<a name="l00276"></a>00276 out.set_unsafe(0,i,-A.get_unsafe(1,i)*v[2]+A.get_unsafe(2,i)*v[1] );
<a name="l00277"></a>00277 out.set_unsafe(1,i, A.get_unsafe(0,i)*v[2]-A.get_unsafe(2,i)*v[0] );
<a name="l00278"></a>00278 out.set_unsafe(2,i,-A.get_unsafe(0,i)*v[1]+A.get_unsafe(1,i)*v[0] );
<a name="l00279"></a>00279 }
<a name="l00280"></a>00280 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00281"></a>00281 }
<a name="l00282"></a>00282
<a name="l00283"></a>00283
<a name="l00284"></a>00284 <span class="comment">// ------ Implementatin of detail functions -------------</span>
<a name="l00285"></a>00285 <span class="keyword">namespace </span>detail
<a name="l00286"></a>00286 {<span class="comment"></span>
<a name="l00287"></a>00287 <span class="comment"> /** Extract a submatrix - The output matrix must be set to the required size before call. */</span>
<a name="l00288"></a>00288 <span class="keyword">template</span> <<span class="keyword">class</span> MATORG, <span class="keyword">class</span> MATDEST>
<a name="l00289"></a><a class="code" href="namespacemrpt_1_1math_1_1detail.html#aa19b24dfb9f42a5339d2abf59dc97f27">00289</a> <span class="keywordtype">void</span> <a class="code" href="namespacemrpt_1_1math_1_1detail.html#aa19b24dfb9f42a5339d2abf59dc97f27" title="Extract a submatrix - The output matrix must be set to the required size before call.">extractMatrix</a>(
<a name="l00290"></a>00290 <span class="keyword">const</span> MATORG &M,
<a name="l00291"></a>00291 <span class="keyword">const</span> <span class="keywordtype">size_t</span> first_row,
<a name="l00292"></a>00292 <span class="keyword">const</span> <span class="keywordtype">size_t</span> first_col,
<a name="l00293"></a>00293 MATDEST &outMat)
<a name="l00294"></a>00294 {
<a name="l00295"></a>00295 <span class="keyword">const</span> <span class="keywordtype">size_t</span> NR = outMat.getRowCount();
<a name="l00296"></a>00296 <span class="keyword">const</span> <span class="keywordtype">size_t</span> NC = outMat.getColCount();
<a name="l00297"></a>00297 <a class="code" href="mrpt__macros_8h.html#a9cadce51acb48cccb17428421c952dbf">ASSERT_BELOWEQ_</a>( first_row+NR, M.getRowCount() )
<a name="l00298"></a>00298 <a class="code" href="mrpt__macros_8h.html#a9cadce51acb48cccb17428421c952dbf">ASSERT_BELOWEQ_</a>( first_col+NC, M.getColCount() )
<a name="l00299"></a>00299 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> r=0;r<NR;r++)
<a name="l00300"></a>00300 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> c=0;c<NC;c++)
<a name="l00301"></a>00301 outMat.get_unsafe(r,c) = M.get_unsafe(first_row+r,first_col+c);
<a name="l00302"></a>00302 }
<a name="l00303"></a>00303
<a name="l00304"></a>00304 } <span class="comment">// end of detail namespace</span>
<a name="l00305"></a>00305 <span class="comment"></span>
<a name="l00306"></a>00306 <span class="comment"> /** @} */</span> <span class="comment">// end of grouping</span>
<a name="l00307"></a>00307
<a name="l00308"></a>00308 } <span class="comment">// End of math namespace</span>
<a name="l00309"></a>00309 } <span class="comment">// End of mrpt namespace</span>
<a name="l00310"></a>00310
<a name="l00311"></a>00311
<a name="l00312"></a>00312 <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>
