<!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>CNetworkOfPoses_impl.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">CNetworkOfPoses_impl.h</div> </div>
</div>
<div class="contents">
<a href="_c_network_of_poses__impl_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 CONSTRAINED_POSE_NETWORK_IMPL_H</span>
<a name="l00029"></a>00029 <span class="preprocessor"></span><span class="preprocessor">#define CONSTRAINED_POSE_NETWORK_IMPL_H</span>
<a name="l00030"></a>00030 <span class="preprocessor"></span>
<a name="l00031"></a>00031 <span class="preprocessor">#include <<a class="code" href="dijkstra_8h.html">mrpt/graphs/dijkstra.h</a>></span>
<a name="l00032"></a>00032 <span class="preprocessor">#include <<a class="code" href="_c_text_file_lines_parser_8h.html">mrpt/utils/CTextFileLinesParser.h</a>></span>
<a name="l00033"></a>00033
<a name="l00034"></a>00034
<a name="l00035"></a>00035 <span class="keyword">namespace </span>mrpt
<a name="l00036"></a>00036 {
<a name="l00037"></a>00037 <span class="keyword">namespace </span>graphs
<a name="l00038"></a>00038 {
<a name="l00039"></a>00039 <span class="keyword">namespace </span>detail
<a name="l00040"></a>00040 {
<a name="l00041"></a>00041 <span class="keyword">using namespace </span>std;
<a name="l00042"></a>00042 <span class="keyword">using namespace </span>mrpt;
<a name="l00043"></a>00043 <span class="keyword">using namespace </span>mrpt::utils;
<a name="l00044"></a>00044 <span class="keyword">using namespace </span>mrpt::poses;
<a name="l00045"></a>00045 <span class="keyword">using namespace </span>mrpt::graphs;
<a name="l00046"></a>00046
<a name="l00047"></a>00047 <span class="keyword">template</span> <<span class="keyword">class</span> POSE_PDF> <span class="keyword">struct </span>TPosePDFHelper
<a name="l00048"></a>00048 {
<a name="l00049"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper.html#a8e0784970badec493cd9ffe191c863d9">00049</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper.html#a8e0784970badec493cd9ffe191c863d9">copyFrom2D</a>(POSE_PDF &p, <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html" title="A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...">CPosePDFGaussianInf</a> &pdf ) { p.copyFrom( pdf ); }
<a name="l00050"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper.html#a5ac3e7678b40178ee4ec11a6e3540eea">00050</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper.html#a5ac3e7678b40178ee4ec11a6e3540eea">copyFrom3D</a>(POSE_PDF &p, <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...">CPose3DPDFGaussianInf</a> &pdf ) { p.copyFrom( pdf ); }
<a name="l00051"></a>00051 };
<a name="l00052"></a>00052 <span class="keyword">template</span> <> <span class="keyword">struct </span>TPosePDFHelper<<a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html" title="A class used to store a 2D pose.">CPose2D</a>>
<a name="l00053"></a>00053 {
<a name="l00054"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper_3_01_c_pose2_d_01_4.html#ad891d90fbb0b58157104ab819369ee05">00054</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper_3_01_c_pose2_d_01_4.html#ad891d90fbb0b58157104ab819369ee05">copyFrom2D</a>(<a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html" title="A class used to store a 2D pose.">CPose2D</a> &p, <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html" title="A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...">CPosePDFGaussianInf</a> &pdf ) { p = pdf.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#ad7ca9a20eeb844d44b72d306e3185a1d" title="The mean value.">mean</a>; }
<a name="l00055"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper_3_01_c_pose2_d_01_4.html#a22bd8c921e1536ebefcefd0b8d75b7b1">00055</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper_3_01_c_pose2_d_01_4.html#a22bd8c921e1536ebefcefd0b8d75b7b1">copyFrom3D</a>(<a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html" title="A class used to store a 2D pose.">CPose2D</a> &p, <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...">CPose3DPDFGaussianInf</a> &pdf ) { p = <a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html" title="A class used to store a 2D pose.">CPose2D</a>(pdf.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a0780ff6283d03e426947004c517c9792" title="The mean value.">mean</a>); }
<a name="l00056"></a>00056 };
<a name="l00057"></a>00057 <span class="keyword">template</span> <> <span class="keyword">struct </span>TPosePDFHelper<<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a>>
<a name="l00058"></a>00058 {
<a name="l00059"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper_3_01_c_pose3_d_01_4.html#a4fa6bfe1d605b3b1c64e2057f7bf4463">00059</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper_3_01_c_pose3_d_01_4.html#a4fa6bfe1d605b3b1c64e2057f7bf4463">copyFrom2D</a>(<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> &p, <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html" title="A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...">CPosePDFGaussianInf</a> &pdf ) { p = pdf.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#ad7ca9a20eeb844d44b72d306e3185a1d" title="The mean value.">mean</a>; }
<a name="l00060"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper_3_01_c_pose3_d_01_4.html#a0accf84779aacbf0d25fe29fcd524f4d">00060</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper_3_01_c_pose3_d_01_4.html#a0accf84779aacbf0d25fe29fcd524f4d">copyFrom3D</a>(<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> &p, <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...">CPose3DPDFGaussianInf</a> &pdf ) { p = pdf.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a0780ff6283d03e426947004c517c9792" title="The mean value.">mean</a>; }
<a name="l00061"></a>00061 };
<a name="l00062"></a>00062 <span class="comment"></span>
<a name="l00063"></a>00063 <span class="comment"> /// a helper struct with static template functions \sa CNetworkOfPoses</span>
<a name="l00064"></a>00064 <span class="comment"></span> <span class="keyword">template</span> <<span class="keyword">class</span> graph_t>
<a name="l00065"></a>00065 <span class="keyword">struct </span>graph_ops
<a name="l00066"></a>00066 {
<a name="l00067"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a8112dc167c5f49f66398bda33b26bba5">00067</a> <span class="keyword">static</span> <span class="keywordtype">void</span> write_VERTEX_line(<span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1utils.html#a718b4f99645b7e9f6501c9b7bb2a2fe7" title="The type for node IDs in graphs of different types.">TNodeID</a> <span class="keywordtype">id</span>, <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html" title="A class used to store a 2D pose.">CPose2D</a> &p, <a class="code" href="classstd_1_1ofstream.html" title="STL class.">std::ofstream</a> &f)
<a name="l00068"></a>00068 {
<a name="l00069"></a>00069 <span class="comment">// VERTEX2 id x y phi</span>
<a name="l00070"></a>00070 f << <span class="stringliteral">"VERTEX2 "</span> << <span class="keywordtype">id</span> << <a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">" %.04f %.04f %.04f\n"</span>,p.x(),p.y(),p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html#ab9a8af6ad8281b613bbc37cb3f0c9bb5" title="Get the phi angle of the 2D pose (in radians)">phi</a>() );
<a name="l00071"></a>00071 }
<a name="l00072"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#add7eee12162d8064e2502bbc06c07b6e">00072</a> <span class="keyword">static</span> <span class="keywordtype">void</span> write_VERTEX_line(<span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1utils.html#a718b4f99645b7e9f6501c9b7bb2a2fe7" title="The type for node IDs in graphs of different types.">TNodeID</a> <span class="keywordtype">id</span>, <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> &p, <a class="code" href="classstd_1_1ofstream.html" title="STL class.">std::ofstream</a> &f)
<a name="l00073"></a>00073 {
<a name="l00074"></a>00074 <span class="comment">// VERTEX3 id x y z roll pitch yaw</span>
<a name="l00075"></a>00075 <span class="comment">// **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.</span>
<a name="l00076"></a>00076 f << <span class="stringliteral">"VERTEX3 "</span> << <span class="keywordtype">id</span> << <a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">" %.04f %.04f %.04f %.04f %.04f %.04f\n"</span>,p.x(),p.y(),p.z(),p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#a47c00788772e211ca9efa4a59153b19b" title="Get the ROLL angle (in radians)">roll</a>(),p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#abed455c10a98c4b7463e49ca73dd1ac1" title="Get the PITCH angle (in radians)">pitch</a>(),p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#ae6df3cca6719010c12d5d70f7059cb3f" title="Get the YAW angle (in radians)">yaw</a>() );
<a name="l00077"></a>00077 }
<a name="l00078"></a>00078
<a name="l00079"></a>00079
<a name="l00080"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#ad3310f8ee563971958e74ad450ffd465">00080</a> <span class="keyword">static</span> <span class="keywordtype">void</span> write_EDGE_line( <span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1utils.html#aee71d7beb4d61406566af3847410d0e4" title="A pair of node IDs.">TPairNodeIDs</a> &edgeIDs,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html" title="A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...">CPosePDFGaussianInf</a> & edge, <a class="code" href="classstd_1_1ofstream.html" title="STL class.">std::ofstream</a> &f)
<a name="l00081"></a>00081 {
<a name="l00082"></a>00082 <span class="comment">// EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp inf_xp inf_yp</span>
<a name="l00083"></a>00083 <span class="comment">// **CAUTION** TORO docs say "from_id" "to_id" in the opposite order, but it seems from the data that this is the correct expected format.</span>
<a name="l00084"></a>00084 f << <span class="stringliteral">"EDGE2 "</span> << edgeIDs.first << <span class="stringliteral">" "</span> << edgeIDs.second << <span class="stringliteral">" "</span> <<
<a name="l00085"></a>00085 <span class="comment">//format(" %.06f %.06f %.06f %e %e %e %e %e %e\n",</span>
<a name="l00086"></a>00086 edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#ad7ca9a20eeb844d44b72d306e3185a1d" title="The mean value.">mean</a>.x()<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#ad7ca9a20eeb844d44b72d306e3185a1d" title="The mean value.">mean</a>.y()<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#ad7ca9a20eeb844d44b72d306e3185a1d" title="The mean value.">mean</a>.<a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html#ab9a8af6ad8281b613bbc37cb3f0c9bb5" title="Get the phi angle of the 2D pose (in radians)">phi</a>()<<<span class="stringliteral">" "</span><<
<a name="l00087"></a>00087 edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#a587a5280102e9da590535670249e0c0c" title="The inverse of the 3x3 covariance matrix (the "information" matrix)">cov_inv</a>(0,0)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#a587a5280102e9da590535670249e0c0c" title="The inverse of the 3x3 covariance matrix (the "information" matrix)">cov_inv</a>(0,1)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#a587a5280102e9da590535670249e0c0c" title="The inverse of the 3x3 covariance matrix (the "information" matrix)">cov_inv</a>(1,1)<<<span class="stringliteral">" "</span><<
<a name="l00088"></a>00088 edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#a587a5280102e9da590535670249e0c0c" title="The inverse of the 3x3 covariance matrix (the "information" matrix)">cov_inv</a>(2,2)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#a587a5280102e9da590535670249e0c0c" title="The inverse of the 3x3 covariance matrix (the "information" matrix)">cov_inv</a>(0,2)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#a587a5280102e9da590535670249e0c0c" title="The inverse of the 3x3 covariance matrix (the "information" matrix)">cov_inv</a>(1,2) << endl;
<a name="l00089"></a>00089 }
<a name="l00090"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a3825bdd90cab0b3ec2473a6a0d7525b0">00090</a> <span class="keyword">static</span> <span class="keywordtype">void</span> write_EDGE_line( <span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1utils.html#aee71d7beb4d61406566af3847410d0e4" title="A pair of node IDs.">TPairNodeIDs</a> &edgeIDs,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...">CPose3DPDFGaussianInf</a> & edge, <a class="code" href="classstd_1_1ofstream.html" title="STL class.">std::ofstream</a> &f)
<a name="l00091"></a>00091 {
<a name="l00092"></a>00092 <span class="comment">// EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66</span>
<a name="l00093"></a>00093 <span class="comment">// **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.</span>
<a name="l00094"></a>00094 <span class="comment">// **CAUTION** TORO docs say "from_id" "to_id" in the opposite order, but it seems from the data that this is the correct expected format.</span>
<a name="l00095"></a>00095 f << <span class="stringliteral">"EDGE3 "</span> << edgeIDs.first << <span class="stringliteral">" "</span> << edgeIDs.second << <span class="stringliteral">" "</span> <<
<a name="l00096"></a>00096 <span class="comment">//format(" %.06f %.06f %.06f %.06f %.06f %.06f %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e\n",</span>
<a name="l00097"></a>00097 edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a0780ff6283d03e426947004c517c9792" title="The mean value.">mean</a>.x()<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a0780ff6283d03e426947004c517c9792" title="The mean value.">mean</a>.y()<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a0780ff6283d03e426947004c517c9792" title="The mean value.">mean</a>.z()<<<span class="stringliteral">" "</span><<
<a name="l00098"></a>00098 edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a0780ff6283d03e426947004c517c9792" title="The mean value.">mean</a>.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#a47c00788772e211ca9efa4a59153b19b" title="Get the ROLL angle (in radians)">roll</a>()<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a0780ff6283d03e426947004c517c9792" title="The mean value.">mean</a>.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#abed455c10a98c4b7463e49ca73dd1ac1" title="Get the PITCH angle (in radians)">pitch</a>()<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a0780ff6283d03e426947004c517c9792" title="The mean value.">mean</a>.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#ae6df3cca6719010c12d5d70f7059cb3f" title="Get the YAW angle (in radians)">yaw</a>()<<<span class="stringliteral">" "</span><<
<a name="l00099"></a>00099 edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(0,0)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(0,1)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(0,2)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(0,5)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(0,4)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(0,3)<<<span class="stringliteral">" "</span><<
<a name="l00100"></a>00100 edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(1,1)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(1,2)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(1,5)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(1,4)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(1,3)<<<span class="stringliteral">" "</span><<
<a name="l00101"></a>00101 edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(2,2)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(2,5)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(2,4)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(2,3)<<<span class="stringliteral">" "</span><<
<a name="l00102"></a>00102 edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(5,5)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(5,4)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(5,3)<<<span class="stringliteral">" "</span><<
<a name="l00103"></a>00103 edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(4,4)<<<span class="stringliteral">" "</span><<edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(4,3)<<<span class="stringliteral">" "</span><<
<a name="l00104"></a>00104 edge.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>(3,3) << endl;
<a name="l00105"></a>00105 }
<a name="l00106"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a725a9efe48b3d782805ce507fa36b6fc">00106</a> <span class="keyword">static</span> <span class="keywordtype">void</span> write_EDGE_line( <span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1utils.html#aee71d7beb4d61406566af3847410d0e4" title="A pair of node IDs.">TPairNodeIDs</a> &edgeIDs,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 2D pose ...">CPosePDFGaussian</a> & edge, <a class="code" href="classstd_1_1ofstream.html" title="STL class.">std::ofstream</a> &f)
<a name="l00107"></a>00107 {
<a name="l00108"></a>00108 <a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html" title="A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...">CPosePDFGaussianInf</a> p;
<a name="l00109"></a>00109 p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#a97da760ecf3ca8377deaca8de450f608" title="Copy operator, translating if necesary (for example, between particles and gaussian representations)...">copyFrom</a>(edge);
<a name="l00110"></a>00110 write_EDGE_line(edgeIDs,p,f);
<a name="l00111"></a>00111 }
<a name="l00112"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a5a44e943a1fe03e6fbf526096da50732">00112</a> <span class="keyword">static</span> <span class="keywordtype">void</span> write_EDGE_line( <span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1utils.html#aee71d7beb4d61406566af3847410d0e4" title="A pair of node IDs.">TPairNodeIDs</a> &edgeIDs,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose ...">CPose3DPDFGaussian</a> & edge, <a class="code" href="classstd_1_1ofstream.html" title="STL class.">std::ofstream</a> &f)
<a name="l00113"></a>00113 {
<a name="l00114"></a>00114 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...">CPose3DPDFGaussianInf</a> p;
<a name="l00115"></a>00115 p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#abdc7f66d1a53adaf5c0259a5ee03a6cb" title="Copy operator, translating if necesary (for example, between particles and gaussian representations)...">copyFrom</a>(edge);
<a name="l00116"></a>00116 write_EDGE_line(edgeIDs,p,f);
<a name="l00117"></a>00117 }
<a name="l00118"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a81a4a56efe3ac32d78318d36f32b4c38">00118</a> <span class="keyword">static</span> <span class="keywordtype">void</span> write_EDGE_line( <span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1utils.html#aee71d7beb4d61406566af3847410d0e4" title="A pair of node IDs.">TPairNodeIDs</a> &edgeIDs,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html" title="A class used to store a 2D pose.">CPose2D</a> & edge, <a class="code" href="classstd_1_1ofstream.html" title="STL class.">std::ofstream</a> &f)
<a name="l00119"></a>00119 {
<a name="l00120"></a>00120 <a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html" title="A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...">CPosePDFGaussianInf</a> p;
<a name="l00121"></a>00121 p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#ad7ca9a20eeb844d44b72d306e3185a1d" title="The mean value.">mean</a> = edge;
<a name="l00122"></a>00122 p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#a587a5280102e9da590535670249e0c0c" title="The inverse of the 3x3 covariance matrix (the "information" matrix)">cov_inv</a>.unit(3,1.0);
<a name="l00123"></a>00123 write_EDGE_line(edgeIDs,p,f);
<a name="l00124"></a>00124 }
<a name="l00125"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#aa9de5e17c1440c9b362e331d64b6689d">00125</a> <span class="keyword">static</span> <span class="keywordtype">void</span> write_EDGE_line( <span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1utils.html#aee71d7beb4d61406566af3847410d0e4" title="A pair of node IDs.">TPairNodeIDs</a> &edgeIDs,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> & edge, <a class="code" href="classstd_1_1ofstream.html" title="STL class.">std::ofstream</a> &f)
<a name="l00126"></a>00126 {
<a name="l00127"></a>00127 <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...">CPose3DPDFGaussianInf</a> p;
<a name="l00128"></a>00128 p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a0780ff6283d03e426947004c517c9792" title="The mean value.">mean</a> = edge;
<a name="l00129"></a>00129 p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>.unit(6,1.0);
<a name="l00130"></a>00130 write_EDGE_line(edgeIDs,p,f);
<a name="l00131"></a>00131 }
<a name="l00132"></a>00132
<a name="l00133"></a>00133
<a name="l00134"></a>00134 <span class="comment">// =================================================================</span>
<a name="l00135"></a>00135 <span class="comment">// save_graph_of_poses_from_text_file</span>
<a name="l00136"></a>00136 <span class="comment">// =================================================================</span>
<a name="l00137"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a474ba91ba0d9fdb438a204bec62c9c85">00137</a> <span class="keyword">static</span> <span class="keywordtype">void</span> save_graph_of_poses_from_text_file(<span class="keyword">const</span> graph_t *g, <span class="keyword">const</span> <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a> &fil)
<a name="l00138"></a>00138 {
<a name="l00139"></a>00139 <span class="keyword">typedef</span> <span class="keyword">typename</span> graph_t::constraint_t CPOSE;
<a name="l00140"></a>00140
<a name="l00141"></a>00141 std<a class="code" href="classstd_1_1ofstream.html" title="STL class.">::ofstream</a> f;
<a name="l00142"></a>00142 f.open(fil.c_str());
<a name="l00143"></a>00143 <span class="keywordflow">if</span> (!f.is_open())
<a name="l00144"></a>00144 <a class="code" href="mrpt__macros_8h.html#a61a8d46146210ee20fa1ff423257a5ec">THROW_EXCEPTION_CUSTOM_MSG1</a>(<span class="stringliteral">"Error opening file '%s' for writing"</span>,fil.c_str());
<a name="l00145"></a>00145
<a name="l00146"></a>00146 <span class="comment">// 1st: Nodes</span>
<a name="l00147"></a>00147 <span class="keywordflow">for</span> (<span class="keyword">typename</span> <a class="code" href="eigen__plugins_8h.html#a8dbda719917732693c56cee228465ed9">graph_t::global_poses_t::const_iterator</a> itNod = g->nodes.begin();itNod!=g->nodes.end();++itNod)
<a name="l00148"></a>00148 write_VERTEX_line(itNod->first, itNod->second, f);
<a name="l00149"></a>00149
<a name="l00150"></a>00150 <span class="comment">// 2nd: Edges:</span>
<a name="l00151"></a>00151 <span class="keywordflow">for</span> (<span class="keyword">typename</span> <a class="code" href="eigen__plugins_8h.html#a8dbda719917732693c56cee228465ed9">graph_t::const_iterator</a> it=g->begin();it!=g->end();++it)
<a name="l00152"></a>00152 <span class="keywordflow">if</span> (it->first.first!=it->first.second) <span class="comment">// Ignore self-edges, typically from importing files with EQUIV's</span>
<a name="l00153"></a>00153 write_EDGE_line( it->first, it->second, f);
<a name="l00154"></a>00154
<a name="l00155"></a>00155 } <span class="comment">// end save_graph</span>
<a name="l00156"></a>00156
<a name="l00157"></a>00157
<a name="l00158"></a>00158 <span class="comment">// =================================================================</span>
<a name="l00159"></a>00159 <span class="comment">// load_graph_of_poses_from_text_file</span>
<a name="l00160"></a>00160 <span class="comment">// =================================================================</span>
<a name="l00161"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#aeeab6b6c21ff53356d0e5f5229aef3fc">00161</a> <span class="keyword">static</span> <span class="keywordtype">void</span> load_graph_of_poses_from_text_file(graph_t *g, <span class="keyword">const</span> <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a> &fil)
<a name="l00162"></a>00162 {
<a name="l00163"></a>00163 <span class="keyword">typedef</span> <span class="keyword">typename</span> graph_t::constraint_t CPOSE;
<a name="l00164"></a>00164
<a name="l00165"></a>00165 <a class="code" href="classstd_1_1set.html" title="STL class.">set<string></a> alreadyWarnedUnknowns; <span class="comment">// for unknown line types, show a warning to cerr just once.</span>
<a name="l00166"></a>00166
<a name="l00167"></a>00167 <span class="comment">// First, empty the graph:</span>
<a name="l00168"></a>00168 g->clear();
<a name="l00169"></a>00169
<a name="l00170"></a>00170 <span class="comment">// Determine if it's a 2D or 3D graph, just to raise an error if loading a 3D graph in a 2D one, since</span>
<a name="l00171"></a>00171 <span class="comment">// it would be an unintentional loss of information:</span>
<a name="l00172"></a>00172 <span class="keyword">const</span> <span class="keywordtype">bool</span> graph_is_3D = CPOSE::is_3D();
<a name="l00173"></a>00173
<a name="l00174"></a>00174 <a class="code" href="classmrpt_1_1utils_1_1_c_text_file_lines_parser.html" title="A class for parsing text files, returning each non-empty and non-comment line, along its line number...">CTextFileLinesParser</a> filParser(fil); <span class="comment">// raises an exception on error</span>
<a name="l00175"></a>00175
<a name="l00176"></a>00176 <span class="comment">// -------------------------------------------</span>
<a name="l00177"></a>00177 <span class="comment">// 1st PASS: Read EQUIV entries only</span>
<a name="l00178"></a>00178 <span class="comment">// since processing them AFTER loading the data</span>
<a name="l00179"></a>00179 <span class="comment">// is much much slower.</span>
<a name="l00180"></a>00180 <span class="comment">// -------------------------------------------</span>
<a name="l00181"></a>00181 <a class="code" href="classstd_1_1map.html" title="STL class.">map<TNodeID,TNodeID></a> lstEquivs; <span class="comment">// List of EQUIV entries: NODEID -> NEWNODEID. NEWNODEID will be always the lowest ID number.</span>
<a name="l00182"></a>00182
<a name="l00183"></a>00183 <span class="comment">// Read & process lines each at once until EOF:</span>
<a name="l00184"></a>00184 <a class="code" href="classstd_1_1istringstream.html" title="STL class.">istringstream</a> s;
<a name="l00185"></a>00185 <span class="keywordflow">while</span> (filParser.<a class="code" href="classmrpt_1_1utils_1_1_c_text_file_lines_parser.html#ac92ed19f521269dbfbdf5c6d2e698813" title="Reads from the file and return the next (non-comment) line, as a std::string.">getNextLine</a>(s))
<a name="l00186"></a>00186 {
<a name="l00187"></a>00187 <span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> lineNum = filParser.<a class="code" href="classmrpt_1_1utils_1_1_c_text_file_lines_parser.html#a8297fb868c0ca7948c94b46e6ef7432f" title="Return the line number of the last line returned with getNextLine.">getCurrentLineNumber</a>();
<a name="l00188"></a>00188 <span class="keyword">const</span> <span class="keywordtype">string</span> lin = s.str();
<a name="l00189"></a>00189
<a name="l00190"></a>00190 <span class="keywordtype">string</span> key;
<a name="l00191"></a>00191 <span class="keywordflow">if</span> ( !(s >> key) || key.empty() )
<a name="l00192"></a>00192 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Line %u: Can't read string for entry type in: '%s'"</span>, lineNum, lin.c_str() ) );
<a name="l00193"></a>00193
<a name="l00194"></a>00194 <span class="keywordflow">if</span> ( <a class="code" href="group__string__manage.html#ga554856afe84963ca30164dfe8fc9b754" title="Return true if the two strings are equal (case insensitive)">strCmpI</a>(key,<span class="stringliteral">"EQUIV"</span>) )
<a name="l00195"></a>00195 {
<a name="l00196"></a>00196 <span class="comment">// Process these ones at the end, for now store in a list:</span>
<a name="l00197"></a>00197 <a class="code" href="namespacemrpt_1_1utils.html#a718b4f99645b7e9f6501c9b7bb2a2fe7" title="The type for node IDs in graphs of different types.">TNodeID</a> id1,id2;
<a name="l00198"></a>00198 <span class="keywordflow">if</span> (!(s>> id1 >> id2))
<a name="l00199"></a>00199 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Line %u: Can't read id1 & id2 in EQUIV line: '%s'"</span>, lineNum, lin.c_str() ) );
<a name="l00200"></a>00200 lstEquivs[std::max(id1,id2)] = std::min(id1,id2);
<a name="l00201"></a>00201 }
<a name="l00202"></a>00202 } <span class="comment">// end 1st pass</span>
<a name="l00203"></a>00203
<a name="l00204"></a>00204 <span class="comment">// -------------------------------------------</span>
<a name="l00205"></a>00205 <span class="comment">// 2nd PASS: Read all other entries</span>
<a name="l00206"></a>00206 <span class="comment">// -------------------------------------------</span>
<a name="l00207"></a>00207 filParser.<a class="code" href="classmrpt_1_1utils_1_1_c_text_file_lines_parser.html#aa8d6ea6a30acda060f8733bd05023b91" title="Reset the read pointer to the beginning of the file.">rewind</a>();
<a name="l00208"></a>00208
<a name="l00209"></a>00209 <span class="comment">// Read & process lines each at once until EOF:</span>
<a name="l00210"></a>00210 <span class="keywordflow">while</span> (filParser.<a class="code" href="classmrpt_1_1utils_1_1_c_text_file_lines_parser.html#ac92ed19f521269dbfbdf5c6d2e698813" title="Reads from the file and return the next (non-comment) line, as a std::string.">getNextLine</a>(s))
<a name="l00211"></a>00211 {
<a name="l00212"></a>00212 <span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> lineNum = filParser.<a class="code" href="classmrpt_1_1utils_1_1_c_text_file_lines_parser.html#a8297fb868c0ca7948c94b46e6ef7432f" title="Return the line number of the last line returned with getNextLine.">getCurrentLineNumber</a>();
<a name="l00213"></a>00213 <span class="keyword">const</span> <span class="keywordtype">string</span> lin = s.str();
<a name="l00214"></a>00214
<a name="l00215"></a>00215 <span class="comment">// Recognized strings:</span>
<a name="l00216"></a>00216 <span class="comment">// VERTEX2 id x y phi</span>
<a name="l00217"></a>00217 <span class="comment">// EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp inf_xp inf_yp</span>
<a name="l00218"></a>00218 <span class="comment">// VERTEX3 id x y z roll pitch yaw</span>
<a name="l00219"></a>00219 <span class="comment">// EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66</span>
<a name="l00220"></a>00220 <span class="comment">// EQUIV id1 id2</span>
<a name="l00221"></a>00221 <span class="keywordtype">string</span> key;
<a name="l00222"></a>00222 <span class="keywordflow">if</span> ( !(s >> key) || key.empty() )
<a name="l00223"></a>00223 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Line %u: Can't read string for entry type in: '%s'"</span>, lineNum, lin.c_str() ) );
<a name="l00224"></a>00224
<a name="l00225"></a>00225 <span class="keywordflow">if</span> ( <a class="code" href="group__string__manage.html#ga554856afe84963ca30164dfe8fc9b754" title="Return true if the two strings are equal (case insensitive)">strCmpI</a>(key,<span class="stringliteral">"VERTEX2"</span>) )
<a name="l00226"></a>00226 {
<a name="l00227"></a>00227 <a class="code" href="namespacemrpt_1_1utils.html#a718b4f99645b7e9f6501c9b7bb2a2fe7" title="The type for node IDs in graphs of different types.">TNodeID</a> id;
<a name="l00228"></a>00228 <a class="code" href="structmrpt_1_1math_1_1_t_pose2_d.html" title="Lightweight 2D pose.">TPose2D</a> p2D;
<a name="l00229"></a>00229 <span class="keywordflow">if</span> (!(s>> <span class="keywordtype">id</span> >> p2D.<a class="code" href="structmrpt_1_1math_1_1_t_pose2_d.html#a4e0f59996ff467539d21a402d8251264" title="X coordinate.">x</a> >> p2D.<a class="code" href="structmrpt_1_1math_1_1_t_pose2_d.html#a47f362c442c15e7dccee6704cef4f387" title="Y coordinate.">y</a> >> p2D.<a class="code" href="structmrpt_1_1math_1_1_t_pose2_d.html#a0a82abf23dbae9f3e78be72aed1c9f9f" title="Phi coordinate.">phi</a>))
<a name="l00230"></a>00230 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Line %u: Error parsing VERTEX2 line: '%s'"</span>, lineNum, lin.c_str() ) );
<a name="l00231"></a>00231
<a name="l00232"></a>00232 <span class="comment">// Make sure the node is new:</span>
<a name="l00233"></a>00233 <span class="keywordflow">if</span> (g->nodes.find(<span class="keywordtype">id</span>)!=g->nodes.end())
<a name="l00234"></a>00234 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Line %u: Error, duplicated verted ID %u in line: '%s'"</span>, lineNum, static_cast<unsigned int>(<span class="keywordtype">id</span>), lin.c_str() ) );
<a name="l00235"></a>00235
<a name="l00236"></a>00236 <span class="comment">// EQUIV? Replace ID by new one.</span>
<a name="l00237"></a>00237 {
<a name="l00238"></a>00238 <span class="keyword">const</span> <a class="code" href="classstd_1_1map_1_1const__iterator.html" title="STL iterator class.">map<TNodeID,TNodeID>::const_iterator</a> itEq = lstEquivs.find(<span class="keywordtype">id</span>);
<a name="l00239"></a>00239 <span class="keywordflow">if</span> (itEq!=lstEquivs.end()) <span class="keywordtype">id</span> = itEq->second;
<a name="l00240"></a>00240 }
<a name="l00241"></a>00241
<a name="l00242"></a>00242 <span class="comment">// Add to map: ID -> absolute pose:</span>
<a name="l00243"></a>00243 <span class="keywordflow">if</span> (g->nodes.find(<span class="keywordtype">id</span>)==g->nodes.end())
<a name="l00244"></a>00244 {
<a name="l00245"></a>00245 <span class="keyword">typename</span> <a class="code" href="classmrpt_1_1graphs_1_1_c_network_of_poses.html" title="A directed graph of pose constraints, with edges being the relative pose between pairs of nodes inden...">CNetworkOfPoses<CPOSE>::constraint_t::type_value</a> & newNode = g-><a class="code" href="classmrpt_1_1graphs_1_1_c_network_of_poses.html#ad7ed942c40716b8c387205ed4bb5423a" title="The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position...">nodes</a>[id];
<a name="l00246"></a>00246 newNode = <a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html" title="A class used to store a 2D pose.">CPose2D</a>(p2D); <span class="comment">// Auto converted to CPose3D if needed</span>
<a name="l00247"></a>00247 }
<a name="l00248"></a>00248 }
<a name="l00249"></a>00249 <span class="keywordflow">else</span> <span class="keywordflow">if</span> ( <a class="code" href="group__string__manage.html#ga554856afe84963ca30164dfe8fc9b754" title="Return true if the two strings are equal (case insensitive)">strCmpI</a>(key,<span class="stringliteral">"VERTEX3"</span>) )
<a name="l00250"></a>00250 {
<a name="l00251"></a>00251 <span class="keywordflow">if</span> (!graph_is_3D)
<a name="l00252"></a>00252 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Line %u: Try to load VERTEX3 into a 2D graph: '%s'"</span>, lineNum, lin.c_str() ) );
<a name="l00253"></a>00253
<a name="l00254"></a>00254 <span class="comment">// VERTEX3 id x y z roll pitch yaw</span>
<a name="l00255"></a>00255 <a class="code" href="namespacemrpt_1_1utils.html#a718b4f99645b7e9f6501c9b7bb2a2fe7" title="The type for node IDs in graphs of different types.">TNodeID</a> id;
<a name="l00256"></a>00256 <a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html" title="Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).">TPose3D</a> p3D;
<a name="l00257"></a>00257 <span class="comment">// **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.</span>
<a name="l00258"></a>00258 <span class="keywordflow">if</span> (!(s>> <span class="keywordtype">id</span> >> p3D.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html#a58c4aff2e15ffe59fcd033a1b8a9a233" title="X coordinate.">x</a> >> p3D.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html#a0fc71229e42e72ead9652312ae69259a" title="Y coordinate.">y</a> >> p3D.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html#a62c86a3ad1d59501afca2b30cd4f9d76" title="Z coordinate.">z</a> >> p3D.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html#a6ca364705b1001f112cf423ad68b5a98" title="Roll coordinate (rotation angle over X coordinate).">roll</a> >> p3D.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html#acab2927c55a45e830e741220339bc580" title="Pitch coordinate (rotation angle over Y axis).">pitch</a> >> p3D.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html#af57c4381902066aac35d93746ed9dff9" title="Yaw coordinate (rotation angle over Z axis).">yaw</a> ))
<a name="l00259"></a>00259 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Line %u: Error parsing VERTEX3 line: '%s'"</span>, lineNum, lin.c_str() ) );
<a name="l00260"></a>00260
<a name="l00261"></a>00261 <span class="comment">// Make sure the node is new:</span>
<a name="l00262"></a>00262 <span class="keywordflow">if</span> (g->nodes.find(<span class="keywordtype">id</span>)!=g->nodes.end())
<a name="l00263"></a>00263 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Line %u: Error, duplicated verted ID %u in line: '%s'"</span>, lineNum, static_cast<unsigned int>(<span class="keywordtype">id</span>), lin.c_str() ) );
<a name="l00264"></a>00264
<a name="l00265"></a>00265 <span class="comment">// EQUIV? Replace ID by new one.</span>
<a name="l00266"></a>00266 {
<a name="l00267"></a>00267 <span class="keyword">const</span> <a class="code" href="classstd_1_1map_1_1const__iterator.html" title="STL iterator class.">map<TNodeID,TNodeID>::const_iterator</a> itEq = lstEquivs.find(<span class="keywordtype">id</span>);
<a name="l00268"></a>00268 <span class="keywordflow">if</span> (itEq!=lstEquivs.end()) <span class="keywordtype">id</span> = itEq->second;
<a name="l00269"></a>00269 }
<a name="l00270"></a>00270
<a name="l00271"></a>00271 <span class="comment">// Add to map: ID -> absolute pose:</span>
<a name="l00272"></a>00272 <span class="keywordflow">if</span> (g->nodes.find(<span class="keywordtype">id</span>)==g->nodes.end())
<a name="l00273"></a>00273 {
<a name="l00274"></a>00274 g->nodes[id] = <span class="keyword">typename</span> <a class="code" href="classmrpt_1_1graphs_1_1_c_network_of_poses.html" title="A directed graph of pose constraints, with edges being the relative pose between pairs of nodes inden...">CNetworkOfPoses<CPOSE>::constraint_t::type_value</a>( <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a>(p3D) ); <span class="comment">// Auto converted to CPose2D if needed</span>
<a name="l00275"></a>00275 }
<a name="l00276"></a>00276 }
<a name="l00277"></a>00277 <span class="keywordflow">else</span> <span class="keywordflow">if</span> ( <a class="code" href="group__string__manage.html#ga554856afe84963ca30164dfe8fc9b754" title="Return true if the two strings are equal (case insensitive)">strCmpI</a>(key,<span class="stringliteral">"EDGE2"</span>) )
<a name="l00278"></a>00278 {
<a name="l00279"></a>00279 <span class="comment">// EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp inf_xp inf_yp</span>
<a name="l00280"></a>00280 <span class="comment">// s00 s01 s11 s22 s02 s12</span>
<a name="l00281"></a>00281 <span class="comment">// Read values are:</span>
<a name="l00282"></a>00282 <span class="comment">// [ s00 s01 s02 ]</span>
<a name="l00283"></a>00283 <span class="comment">// [ - s11 s12 ]</span>
<a name="l00284"></a>00284 <span class="comment">// [ - - s22 ]</span>
<a name="l00285"></a>00285 <span class="comment">//</span>
<a name="l00286"></a>00286 <a class="code" href="namespacemrpt_1_1utils.html#a718b4f99645b7e9f6501c9b7bb2a2fe7" title="The type for node IDs in graphs of different types.">TNodeID</a> to_id, from_id;
<a name="l00287"></a>00287 <span class="keywordflow">if</span> (!(s>> from_id >> to_id ))
<a name="l00288"></a>00288 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Line %u: Error parsing EDGE2 line: '%s'"</span>, lineNum, lin.c_str() ) );
<a name="l00289"></a>00289
<a name="l00290"></a>00290 <span class="comment">// EQUIV? Replace ID by new one.</span>
<a name="l00291"></a>00291 {
<a name="l00292"></a>00292 <span class="keyword">const</span> <a class="code" href="classstd_1_1map_1_1const__iterator.html" title="STL iterator class.">map<TNodeID,TNodeID>::const_iterator</a> itEq = lstEquivs.find(to_id);
<a name="l00293"></a>00293 <span class="keywordflow">if</span> (itEq!=lstEquivs.end()) to_id = itEq->second;
<a name="l00294"></a>00294 }
<a name="l00295"></a>00295 {
<a name="l00296"></a>00296 <span class="keyword">const</span> <a class="code" href="classstd_1_1map_1_1const__iterator.html" title="STL iterator class.">map<TNodeID,TNodeID>::const_iterator</a> itEq = lstEquivs.find(from_id);
<a name="l00297"></a>00297 <span class="keywordflow">if</span> (itEq!=lstEquivs.end()) from_id = itEq->second;
<a name="l00298"></a>00298 }
<a name="l00299"></a>00299
<a name="l00300"></a>00300 <span class="keywordflow">if</span> (from_id!=to_id) <span class="comment">// Don't load self-edges! (probably come from an EQUIV)</span>
<a name="l00301"></a>00301 {
<a name="l00302"></a>00302 <a class="code" href="structmrpt_1_1math_1_1_t_pose2_d.html" title="Lightweight 2D pose.">TPose2D</a> Ap_mean;
<a name="l00303"></a>00303 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html">CMatrixDouble33</a> Ap_cov_inv;
<a name="l00304"></a>00304 <span class="keywordflow">if</span> (!(s>>
<a name="l00305"></a>00305 Ap_mean.<a class="code" href="structmrpt_1_1math_1_1_t_pose2_d.html#a4e0f59996ff467539d21a402d8251264" title="X coordinate.">x</a> >> Ap_mean.<a class="code" href="structmrpt_1_1math_1_1_t_pose2_d.html#a47f362c442c15e7dccee6704cef4f387" title="Y coordinate.">y</a> >> Ap_mean.<a class="code" href="structmrpt_1_1math_1_1_t_pose2_d.html#a0a82abf23dbae9f3e78be72aed1c9f9f" title="Phi coordinate.">phi</a> >>
<a name="l00306"></a>00306 Ap_cov_inv(0,0) >> Ap_cov_inv(0,1) >> Ap_cov_inv(1,1) >>
<a name="l00307"></a>00307 Ap_cov_inv(2,2) >> Ap_cov_inv(0,2) >> Ap_cov_inv(1,2) ))
<a name="l00308"></a>00308 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Line %u: Error parsing EDGE2 line: '%s'"</span>, lineNum, lin.c_str() ) );
<a name="l00309"></a>00309
<a name="l00310"></a>00310 <span class="comment">// Complete low triangular part of inf matrix:</span>
<a name="l00311"></a>00311 Ap_cov_inv(1,0) = Ap_cov_inv(0,1);
<a name="l00312"></a>00312 Ap_cov_inv(2,0) = Ap_cov_inv(0,2);
<a name="l00313"></a>00313 Ap_cov_inv(2,1) = Ap_cov_inv(1,2);
<a name="l00314"></a>00314
<a name="l00315"></a>00315 <span class="comment">// Convert to 2D cov, 3D cov or 3D inv_cov as needed:</span>
<a name="l00316"></a>00316 <span class="keyword">typename</span> <a class="code" href="classmrpt_1_1graphs_1_1_c_network_of_poses.html" title="A directed graph of pose constraints, with edges being the relative pose between pairs of nodes inden...">CNetworkOfPoses<CPOSE>::edge_t</a> newEdge;
<a name="l00317"></a>00317 <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper.html">TPosePDFHelper<CPOSE>::copyFrom2D</a>(newEdge, <a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html" title="A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...">CPosePDFGaussianInf</a>( <a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html" title="A class used to store a 2D pose.">CPose2D</a>(Ap_mean), Ap_cov_inv ) );
<a name="l00318"></a>00318 g->insertEdge(from_id, to_id, newEdge);
<a name="l00319"></a>00319 }
<a name="l00320"></a>00320 }
<a name="l00321"></a>00321 <span class="keywordflow">else</span> <span class="keywordflow">if</span> ( <a class="code" href="group__string__manage.html#ga554856afe84963ca30164dfe8fc9b754" title="Return true if the two strings are equal (case insensitive)">strCmpI</a>(key,<span class="stringliteral">"EDGE3"</span>) )
<a name="l00322"></a>00322 {
<a name="l00323"></a>00323 <span class="keywordflow">if</span> (!graph_is_3D)
<a name="l00324"></a>00324 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Line %u: Try to load EDGE3 into a 2D graph: '%s'"</span>, lineNum, lin.c_str() ) );
<a name="l00325"></a>00325
<a name="l00326"></a>00326 <span class="comment">// EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66</span>
<a name="l00327"></a>00327 <a class="code" href="namespacemrpt_1_1utils.html#a718b4f99645b7e9f6501c9b7bb2a2fe7" title="The type for node IDs in graphs of different types.">TNodeID</a> to_id, from_id;
<a name="l00328"></a>00328 <span class="keywordflow">if</span> (!(s>> from_id >> to_id ))
<a name="l00329"></a>00329 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Line %u: Error parsing EDGE3 line: '%s'"</span>, lineNum, lin.c_str() ) );
<a name="l00330"></a>00330
<a name="l00331"></a>00331 <span class="comment">// EQUIV? Replace ID by new one.</span>
<a name="l00332"></a>00332 {
<a name="l00333"></a>00333 <span class="keyword">const</span> <a class="code" href="classstd_1_1map_1_1const__iterator.html" title="STL iterator class.">map<TNodeID,TNodeID>::const_iterator</a> itEq = lstEquivs.find(to_id);
<a name="l00334"></a>00334 <span class="keywordflow">if</span> (itEq!=lstEquivs.end()) to_id = itEq->second;
<a name="l00335"></a>00335 }
<a name="l00336"></a>00336 {
<a name="l00337"></a>00337 <span class="keyword">const</span> <a class="code" href="classstd_1_1map_1_1const__iterator.html" title="STL iterator class.">map<TNodeID,TNodeID>::const_iterator</a> itEq = lstEquivs.find(from_id);
<a name="l00338"></a>00338 <span class="keywordflow">if</span> (itEq!=lstEquivs.end()) from_id = itEq->second;
<a name="l00339"></a>00339 }
<a name="l00340"></a>00340
<a name="l00341"></a>00341 <span class="keywordflow">if</span> (from_id!=to_id) <span class="comment">// Don't load self-edges! (probably come from an EQUIV)</span>
<a name="l00342"></a>00342 {
<a name="l00343"></a>00343 <a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html" title="Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).">TPose3D</a> Ap_mean;
<a name="l00344"></a>00344 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html">CMatrixDouble66</a> Ap_cov_inv;
<a name="l00345"></a>00345 <span class="comment">// **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.</span>
<a name="l00346"></a>00346 <span class="keywordflow">if</span> (!(s>> Ap_mean.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html#a58c4aff2e15ffe59fcd033a1b8a9a233" title="X coordinate.">x</a> >> Ap_mean.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html#a0fc71229e42e72ead9652312ae69259a" title="Y coordinate.">y</a> >> Ap_mean.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html#a62c86a3ad1d59501afca2b30cd4f9d76" title="Z coordinate.">z</a> >> Ap_mean.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html#a6ca364705b1001f112cf423ad68b5a98" title="Roll coordinate (rotation angle over X coordinate).">roll</a> >> Ap_mean.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html#acab2927c55a45e830e741220339bc580" title="Pitch coordinate (rotation angle over Y axis).">pitch</a> >> Ap_mean.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d.html#af57c4381902066aac35d93746ed9dff9" title="Yaw coordinate (rotation angle over Z axis).">yaw</a> ))
<a name="l00347"></a>00347 <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Line %u: Error parsing EDGE3 line: '%s'"</span>, lineNum, lin.c_str() ) );
<a name="l00348"></a>00348
<a name="l00349"></a>00349 <span class="comment">// **CAUTION** Indices are shuffled to the change YAW(3) <-> ROLL(5) in the order of the data.</span>
<a name="l00350"></a>00350 <span class="keywordflow">if</span> (!(s>>
<a name="l00351"></a>00351 Ap_cov_inv(0,0) >> Ap_cov_inv(0,1) >> Ap_cov_inv(0,2) >> Ap_cov_inv(0,5) >> Ap_cov_inv(0,4) >> Ap_cov_inv(0,3) >>
<a name="l00352"></a>00352 Ap_cov_inv(1,1) >> Ap_cov_inv(1,2) >> Ap_cov_inv(1,5) >> Ap_cov_inv(1,4) >> Ap_cov_inv(1,3) >>
<a name="l00353"></a>00353 Ap_cov_inv(2,2) >> Ap_cov_inv(2,5) >> Ap_cov_inv(2,4) >> Ap_cov_inv(2,3) >>
<a name="l00354"></a>00354 Ap_cov_inv(5,5) >> Ap_cov_inv(5,4) >> Ap_cov_inv(5,3) >>
<a name="l00355"></a>00355 Ap_cov_inv(4,4) >> Ap_cov_inv(4,3) >>
<a name="l00356"></a>00356 Ap_cov_inv(3,3) ))
<a name="l00357"></a>00357 {
<a name="l00358"></a>00358 <span class="comment">// Cov may be omitted in the file:</span>
<a name="l00359"></a>00359 Ap_cov_inv.unit(6,1.0);
<a name="l00360"></a>00360
<a name="l00361"></a>00361 <span class="keywordflow">if</span> (alreadyWarnedUnknowns.find(<span class="stringliteral">"MISSING_3D"</span>)==alreadyWarnedUnknowns.end())
<a name="l00362"></a>00362 {
<a name="l00363"></a>00363 alreadyWarnedUnknowns.insert(<span class="stringliteral">"MISSING_3D"</span>);
<a name="l00364"></a>00364 cerr << <span class="stringliteral">"[CNetworkOfPoses::loadFromTextFile] "</span> << fil << <span class="stringliteral">":"</span> << lineNum << <span class="stringliteral">": Warning: Information matrix missing, assuming unity.\n"</span>;
<a name="l00365"></a>00365 }
<a name="l00366"></a>00366 }
<a name="l00367"></a>00367 <span class="keywordflow">else</span>
<a name="l00368"></a>00368 {
<a name="l00369"></a>00369 <span class="comment">// Complete low triangular part of inf matrix:</span>
<a name="l00370"></a>00370 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> r=1;r<6;r++)
<a name="l00371"></a>00371 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> c=0;c<r;c++)
<a name="l00372"></a>00372 Ap_cov_inv(r,c) = Ap_cov_inv(c,r);
<a name="l00373"></a>00373 }
<a name="l00374"></a>00374
<a name="l00375"></a>00375 <span class="comment">// Convert as needed:</span>
<a name="l00376"></a>00376 <span class="keyword">typename</span> <a class="code" href="classmrpt_1_1graphs_1_1_c_network_of_poses.html" title="A directed graph of pose constraints, with edges being the relative pose between pairs of nodes inden...">CNetworkOfPoses<CPOSE>::edge_t</a> newEdge;
<a name="l00377"></a>00377 <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1_t_pose_p_d_f_helper.html">TPosePDFHelper<CPOSE>::copyFrom3D</a>(newEdge, <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...">CPose3DPDFGaussianInf</a>( <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a>(Ap_mean), Ap_cov_inv ) );
<a name="l00378"></a>00378 g->insertEdge(from_id, to_id, newEdge);
<a name="l00379"></a>00379 }
<a name="l00380"></a>00380 }
<a name="l00381"></a>00381 <span class="keywordflow">else</span> <span class="keywordflow">if</span> ( <a class="code" href="group__string__manage.html#ga554856afe84963ca30164dfe8fc9b754" title="Return true if the two strings are equal (case insensitive)">strCmpI</a>(key,<span class="stringliteral">"EQUIV"</span>) )
<a name="l00382"></a>00382 {
<a name="l00383"></a>00383 <span class="comment">// Already read in the 1st pass.</span>
<a name="l00384"></a>00384 }
<a name="l00385"></a>00385 <span class="keywordflow">else</span>
<a name="l00386"></a>00386 { <span class="comment">// Unknown entry: Warn the user just once:</span>
<a name="l00387"></a>00387 <span class="keywordflow">if</span> (alreadyWarnedUnknowns.find(key)==alreadyWarnedUnknowns.end())
<a name="l00388"></a>00388 {
<a name="l00389"></a>00389 alreadyWarnedUnknowns.insert(key);
<a name="l00390"></a>00390 cerr << <span class="stringliteral">"[CNetworkOfPoses::loadFromTextFile] "</span> << fil << <span class="stringliteral">":"</span> << lineNum << <span class="stringliteral">": Warning: unknown entry type: "</span> << key << endl;
<a name="l00391"></a>00391 }
<a name="l00392"></a>00392 }
<a name="l00393"></a>00393 } <span class="comment">// end while</span>
<a name="l00394"></a>00394
<a name="l00395"></a>00395 } <span class="comment">// end load_graph</span>
<a name="l00396"></a>00396
<a name="l00397"></a>00397
<a name="l00398"></a>00398 <span class="comment">// --------------------------------------------------------------------------------</span>
<a name="l00399"></a>00399 <span class="comment">// Implements: collapseDuplicatedEdges</span>
<a name="l00400"></a>00400 <span class="comment">//</span>
<a name="l00401"></a>00401 <span class="comment">// Look for duplicated edges (even in opposite directions) between all pairs of nodes and fuse them.</span>
<a name="l00402"></a>00402 <span class="comment">// Upon return, only one edge remains between each pair of nodes with the mean</span>
<a name="l00403"></a>00403 <span class="comment">// & covariance (or information matrix) corresponding to the Bayesian fusion of all the Gaussians.</span>
<a name="l00404"></a>00404 <span class="comment">// --------------------------------------------------------------------------------</span>
<a name="l00405"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#ae91f066adcf90c12827dfddfdbf449df">00405</a> <span class="keyword">static</span> <span class="keywordtype">size_t</span> graph_of_poses_collapse_dup_edges(graph_t *g)
<a name="l00406"></a>00406 {
<a name="l00407"></a>00407 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00408"></a>00408 <span class="keyword">typedef</span> <span class="keyword">typename</span> graph_t<a class="code" href="eigen__plugins_8h.html#a39c5d6430ea9395ae7ae729dd0c3f18c">::edges_map_t::iterator</a> TEdgeIterator;
<a name="l00409"></a>00409
<a name="l00410"></a>00410 <span class="comment">// Data structure: (id1,id2) -> all edges between them</span>
<a name="l00411"></a>00411 <span class="comment">// (with id1 < id2)</span>
<a name="l00412"></a>00412 <span class="keyword">typedef</span> <a class="code" href="classstd_1_1map.html" title="STL class.">map<pair<TNodeID,TNodeID></a>, <a class="code" href="classstd_1_1vector.html" title="STL class.">vector<TEdgeIterator></a> > TListAllEdges; <span class="comment">// For god's sake... when will ALL compilers support auto!! :'-(</span>
<a name="l00413"></a>00413 TListAllEdges lstAllEdges;
<a name="l00414"></a>00414
<a name="l00415"></a>00415 <span class="comment">// Clasify all edges to identify duplicated ones:</span>
<a name="l00416"></a>00416 <span class="keywordflow">for</span> (TEdgeIterator itEd=g->edges.begin();itEd!=g->edges.end();++itEd)
<a name="l00417"></a>00417 {
<a name="l00418"></a>00418 <span class="comment">// Build a pair <id1,id2> with id1 < id2:</span>
<a name="l00419"></a>00419 <span class="keyword">const</span> pair<TNodeID,TNodeID> arc_id = make_pair( std::min(itEd->first.first,itEd->first.second),std::max(itEd->first.first,itEd->first.second) );
<a name="l00420"></a>00420 <span class="comment">// get (or create the first time) the list of edges between them:</span>
<a name="l00421"></a>00421 <a class="code" href="classstd_1_1vector.html" title="STL class.">vector<TEdgeIterator></a> &lstEdges = lstAllEdges[arc_id];
<a name="l00422"></a>00422 <span class="comment">// And add this one:</span>
<a name="l00423"></a>00423 lstEdges.push_back(itEd);
<a name="l00424"></a>00424 }
<a name="l00425"></a>00425
<a name="l00426"></a>00426 <span class="comment">// Now, remove all but the first edge:</span>
<a name="l00427"></a>00427 <span class="keywordtype">size_t</span> nRemoved = 0;
<a name="l00428"></a>00428 <span class="keywordflow">for</span> (<span class="keyword">typename</span> <a class="code" href="eigen__plugins_8h.html#a8dbda719917732693c56cee228465ed9">TListAllEdges::const_iterator</a> it=lstAllEdges.begin();it!=lstAllEdges.end();++it)
<a name="l00429"></a>00429 {
<a name="l00430"></a>00430 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = it->second.size();
<a name="l00431"></a>00431 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=1;i<N;i++) <span class="comment">// i=0 is NOT removed</span>
<a name="l00432"></a>00432 g->edges.erase( it->second[i] );
<a name="l00433"></a>00433
<a name="l00434"></a>00434 <span class="keywordflow">if</span> (N>=2) nRemoved+=N-1;
<a name="l00435"></a>00435 }
<a name="l00436"></a>00436
<a name="l00437"></a>00437 <span class="keywordflow">return</span> nRemoved;
<a name="l00438"></a>00438 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00439"></a>00439 } <span class="comment">// end of graph_of_poses_collapse_dup_edges</span>
<a name="l00440"></a>00440
<a name="l00441"></a>00441 <span class="comment">// --------------------------------------------------------------------------------</span>
<a name="l00442"></a>00442 <span class="comment">// Implements: dijkstra_nodes_estimate</span>
<a name="l00443"></a>00443 <span class="comment">//</span>
<a name="l00444"></a>00444 <span class="comment">// Compute a simple estimation of the global coordinates of each node just from the information in all edges, sorted in a Dijkstra tree based on the current "root" node.</span>
<a name="l00445"></a>00445 <span class="comment">// Note that "global" coordinates are with respect to the node with the ID specified in \a root.</span>
<a name="l00446"></a>00446 <span class="comment">// --------------------------------------------------------------------------------</span>
<a name="l00447"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#aa88281493fab5c014128aed5a72a8579">00447</a> <span class="keyword">static</span> <span class="keywordtype">void</span> graph_of_poses_dijkstra_init(graph_t *g)
<a name="l00448"></a>00448 {
<a name="l00449"></a>00449 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00450"></a>00450
<a name="l00451"></a>00451 <span class="comment">// Do Dijkstra shortest path from "root" to all other nodes:</span>
<a name="l00452"></a>00452 <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1graphs_1_1_c_dijkstra.html" title="The Dijkstra algorithm for finding the shortest path between a given source node in a (weighted) dire...">CDijkstra<graph_t,typename graph_t::maps_implementation_t></a> dijkstra_t;
<a name="l00453"></a>00453 <span class="keyword">typedef</span> <span class="keyword">typename</span> graph_t::constraint_t constraint_t;
<a name="l00454"></a>00454
<a name="l00455"></a>00455 dijkstra_t dijkstra(*g, g->root);
<a name="l00456"></a>00456
<a name="l00457"></a>00457 <span class="comment">// Get the tree representation of the graph and traverse it</span>
<a name="l00458"></a>00458 <span class="comment">// from its root toward the leafs:</span>
<a name="l00459"></a>00459 <span class="keyword">typename</span> dijkstra_t::tree_graph_t treeView;
<a name="l00460"></a>00460 dijkstra.getTreeGraph(treeView);
<a name="l00461"></a>00461
<a name="l00462"></a>00462 <span class="comment">// This visitor class performs the real job of</span>
<a name="l00463"></a>00463 <span class="keyword">struct </span>VisitorComputePoses : <span class="keyword">public</span> dijkstra_t::tree_graph_t::Visitor
<a name="l00464"></a>00464 {
<a name="l00465"></a>00465 graph_t * m_g; <span class="comment">// The original graph</span>
<a name="l00466"></a>00466
<a name="l00467"></a>00467 VisitorComputePoses(graph_t *g) : m_g(g) { }
<a name="l00468"></a>00468 <span class="keyword">virtual</span> <span class="keywordtype">void</span> OnVisitNode( <span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1utils.html#a718b4f99645b7e9f6501c9b7bb2a2fe7" title="The type for node IDs in graphs of different types.">TNodeID</a> parent_id, <span class="keyword">const</span> <span class="keyword">typename</span> dijkstra_t::tree_graph_t::Visitor::tree_t::TEdgeInfo &edge_to_child, <span class="keyword">const</span> <span class="keywordtype">size_t</span> depth_level )
<a name="l00469"></a>00469 {
<a name="l00470"></a>00470 <span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1utils.html#a718b4f99645b7e9f6501c9b7bb2a2fe7" title="The type for node IDs in graphs of different types.">TNodeID</a> child_id = edge_to_child.id;
<a name="l00471"></a>00471
<a name="l00472"></a>00472 <span class="comment">// Compute the pose of "child_id" as parent_pose (+) edge_delta_pose,</span>
<a name="l00473"></a>00473 <span class="comment">// taking into account that that edge may be in reverse order and then have to invert the delta_pose:</span>
<a name="l00474"></a>00474 <span class="keywordflow">if</span> ( (!edge_to_child.reverse && !m_g->edges_store_inverse_poses) ||
<a name="l00475"></a>00475 ( edge_to_child.reverse && m_g->edges_store_inverse_poses)
<a name="l00476"></a>00476 )
<a name="l00477"></a>00477 { <span class="comment">// pose_child = p_parent (+) p_delta</span>
<a name="l00478"></a>00478 m_g->nodes[child_id].composeFrom( m_g->nodes[parent_id], edge_to_child.data->getPoseMean() );
<a name="l00479"></a>00479 }
<a name="l00480"></a>00480 <span class="keywordflow">else</span>
<a name="l00481"></a>00481 { <span class="comment">// pose_child = p_parent (+) [(-)p_delta]</span>
<a name="l00482"></a>00482 m_g->nodes[child_id].composeFrom( m_g->nodes[parent_id], - edge_to_child.data->getPoseMean() );
<a name="l00483"></a>00483 }
<a name="l00484"></a>00484 }
<a name="l00485"></a>00485 };
<a name="l00486"></a>00486
<a name="l00487"></a>00487 <span class="comment">// Remove all global poses but for the root node, which is the origin:</span>
<a name="l00488"></a>00488 g->nodes.clear();
<a name="l00489"></a>00489 g->nodes[g->root] = <span class="keyword">typename</span> constraint_t::type_value(); <span class="comment">// Typ: CPose2D() or CPose3D()</span>
<a name="l00490"></a>00490
<a name="l00491"></a>00491 <span class="comment">// Run the visit thru all nodes in the tree:</span>
<a name="l00492"></a>00492 VisitorComputePoses myVisitor(g);
<a name="l00493"></a>00493 treeView.visitBreadthFirst(treeView.root, myVisitor);
<a name="l00494"></a>00494
<a name="l00495"></a>00495 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00496"></a>00496 }
<a name="l00497"></a>00497
<a name="l00498"></a>00498
<a name="l00499"></a>00499 <span class="comment">// Auxiliary funcs:</span>
<a name="l00500"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a0b89a4b738cdd4c0eade5f1356835bbe">00500</a> <span class="keyword">template</span> <<span class="keyword">class</span> VEC> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">double</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a0b89a4b738cdd4c0eade5f1356835bbe">auxMaha2Dist</a>(VEC &err,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html" title="A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...">CPosePDFGaussianInf</a> &p) {
<a name="l00501"></a>00501 <a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42" title="Modifies the given angle to translate it into the ]-pi,pi] range.">math::wrapToPiInPlace</a>(err[2]);
<a name="l00502"></a>00502 <span class="keywordflow">return</span> <a class="code" href="eigen__plugins_8h.html#afda0882e4db840bc1d8e9bc13bc80e76">mrpt::math::multiply_HCHt_scalar</a>(err,p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian_inf.html#a587a5280102e9da590535670249e0c0c" title="The inverse of the 3x3 covariance matrix (the "information" matrix)">cov_inv</a>); <span class="comment">// err^t*cov_inv*err</span>
<a name="l00503"></a>00503 }
<a name="l00504"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a7d28203bc5557281bca8cebe8eab285e">00504</a> <span class="keyword">template</span> <<span class="keyword">class</span> VEC> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">double</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a7d28203bc5557281bca8cebe8eab285e">auxMaha2Dist</a>(VEC &err,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...">CPose3DPDFGaussianInf</a> &p) {
<a name="l00505"></a>00505 <a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42" title="Modifies the given angle to translate it into the ]-pi,pi] range.">math::wrapToPiInPlace</a>(err[3]);
<a name="l00506"></a>00506 <a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42" title="Modifies the given angle to translate it into the ]-pi,pi] range.">math::wrapToPiInPlace</a>(err[4]);
<a name="l00507"></a>00507 <a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42" title="Modifies the given angle to translate it into the ]-pi,pi] range.">math::wrapToPiInPlace</a>(err[5]);
<a name="l00508"></a>00508 <span class="keywordflow">return</span> <a class="code" href="eigen__plugins_8h.html#afda0882e4db840bc1d8e9bc13bc80e76">mrpt::math::multiply_HCHt_scalar</a>(err,p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian_inf.html#a8f7a64a3fab152f45c505cd8d6c1ab3a" title="The inverse of the 6x6 covariance matrix.">cov_inv</a>); <span class="comment">// err^t*cov_inv*err</span>
<a name="l00509"></a>00509 }
<a name="l00510"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a4fcf5f1ad2be8648babd9e0f5fb3c7ad">00510</a> <span class="keyword">template</span> <<span class="keyword">class</span> VEC> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">double</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a4fcf5f1ad2be8648babd9e0f5fb3c7ad">auxMaha2Dist</a>(VEC &err,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 2D pose ...">CPosePDFGaussian</a> &p) {
<a name="l00511"></a>00511 <a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42" title="Modifies the given angle to translate it into the ]-pi,pi] range.">math::wrapToPiInPlace</a>(err[2]);
<a name="l00512"></a>00512 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html">CMatrixDouble33</a> COV_INV(<a class="code" href="namespacemrpt_1_1math.html#a27e8ae8971ff5aa1c39f1f9be334d73aa28acc66160006cb691487ec89f8d266d">UNINITIALIZED_MATRIX</a>);
<a name="l00513"></a>00513 p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose_p_d_f_gaussian.html#ae0e92d7dc7b32e02e087f4ff900435cc" title="The 3x3 covariance matrix.">cov</a>.inv(COV_INV);
<a name="l00514"></a>00514 <span class="keywordflow">return</span> <a class="code" href="eigen__plugins_8h.html#afda0882e4db840bc1d8e9bc13bc80e76">mrpt::math::multiply_HCHt_scalar</a>(err,COV_INV); <span class="comment">// err^t*cov_inv*err</span>
<a name="l00515"></a>00515 }
<a name="l00516"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a2faffda59ec85e83475969c434e20dec">00516</a> <span class="keyword">template</span> <<span class="keyword">class</span> VEC> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">double</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a2faffda59ec85e83475969c434e20dec">auxMaha2Dist</a>(VEC &err,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian.html" title="Declares a class that represents a Probability Density function (PDF) of a 3D pose ...">CPose3DPDFGaussian</a> &p) {
<a name="l00517"></a>00517 <a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42" title="Modifies the given angle to translate it into the ]-pi,pi] range.">math::wrapToPiInPlace</a>(err[3]);
<a name="l00518"></a>00518 <a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42" title="Modifies the given angle to translate it into the ]-pi,pi] range.">math::wrapToPiInPlace</a>(err[4]);
<a name="l00519"></a>00519 <a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42" title="Modifies the given angle to translate it into the ]-pi,pi] range.">math::wrapToPiInPlace</a>(err[5]);
<a name="l00520"></a>00520 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html">CMatrixDouble66</a> COV_INV(<a class="code" href="namespacemrpt_1_1math.html#a27e8ae8971ff5aa1c39f1f9be334d73aa28acc66160006cb691487ec89f8d266d">UNINITIALIZED_MATRIX</a>);
<a name="l00521"></a>00521 p.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_p_d_f_gaussian.html#a425a06a63e1c4a14de416369d42fb6d8" title="The 6x6 covariance matrix.">cov</a>.inv(COV_INV);
<a name="l00522"></a>00522 <span class="keywordflow">return</span> <a class="code" href="eigen__plugins_8h.html#afda0882e4db840bc1d8e9bc13bc80e76">mrpt::math::multiply_HCHt_scalar</a>(err,COV_INV); <span class="comment">// err^t*cov_inv*err</span>
<a name="l00523"></a>00523 }
<a name="l00524"></a>00524 <span class="comment">// These two are for simulating maha2 distances for non-PDF types: fallback to squared-norm:</span>
<a name="l00525"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#aed75beb83f51c06fc6da49087d644dcd">00525</a> <span class="keyword">template</span> <<span class="keyword">class</span> VEC> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">double</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#aed75beb83f51c06fc6da49087d644dcd">auxMaha2Dist</a>(VEC &err,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html" title="A class used to store a 2D pose.">CPose2D</a> &p) {
<a name="l00526"></a>00526 <a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42" title="Modifies the given angle to translate it into the ]-pi,pi] range.">math::wrapToPiInPlace</a>(err[2]);
<a name="l00527"></a>00527 <span class="keywordflow">return</span> <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(err[0])+<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(err[1])+<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(err[2]);
<a name="l00528"></a>00528 }
<a name="l00529"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#accd7d9890ff98a7b0ffe40feb42f3ee8">00529</a> <span class="keyword">template</span> <<span class="keyword">class</span> VEC> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">double</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#accd7d9890ff98a7b0ffe40feb42f3ee8">auxMaha2Dist</a>(VEC &err,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> &p) {
<a name="l00530"></a>00530 <a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42" title="Modifies the given angle to translate it into the ]-pi,pi] range.">math::wrapToPiInPlace</a>(err[3]);
<a name="l00531"></a>00531 <a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42" title="Modifies the given angle to translate it into the ]-pi,pi] range.">math::wrapToPiInPlace</a>(err[4]);
<a name="l00532"></a>00532 <a class="code" href="group__container__ops__grp.html#ga69ca7c53c45b1f99c69b45f40ef41e42" title="Modifies the given angle to translate it into the ]-pi,pi] range.">math::wrapToPiInPlace</a>(err[5]);
<a name="l00533"></a>00533 <span class="keywordflow">return</span> <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(err[0])+<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(err[1])+<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(err[2])+<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(err[3])+<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(err[4])+<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(err[5]);
<a name="l00534"></a>00534 }
<a name="l00535"></a>00535
<a name="l00536"></a>00536
<a name="l00537"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a87d3286dfdae46b9166b08a58b878945">00537</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">double</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a87d3286dfdae46b9166b08a58b878945">auxEuclid2Dist</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html" title="A class used to store a 2D pose.">CPose2D</a> &p1,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html" title="A class used to store a 2D pose.">CPose2D</a> &p2) {
<a name="l00538"></a>00538 <span class="keywordflow">return</span>
<a name="l00539"></a>00539 <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(p1.x()-p2.x())+
<a name="l00540"></a>00540 <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(p1.y()-p2.y())+
<a name="l00541"></a>00541 <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>( <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>(p1.<a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html#ab9a8af6ad8281b613bbc37cb3f0c9bb5" title="Get the phi angle of the 2D pose (in radians)">phi</a>()-p2.<a class="code" href="classmrpt_1_1poses_1_1_c_pose2_d.html#ab9a8af6ad8281b613bbc37cb3f0c9bb5" title="Get the phi angle of the 2D pose (in radians)">phi</a>() ) );
<a name="l00542"></a>00542 }
<a name="l00543"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a6c430c39256d207b5afb0d4929b53296">00543</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">double</span> <a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a6c430c39256d207b5afb0d4929b53296">auxEuclid2Dist</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> &p1,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> &p2) {
<a name="l00544"></a>00544 <span class="keywordflow">return</span>
<a name="l00545"></a>00545 <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(p1.x()-p2.x())+
<a name="l00546"></a>00546 <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(p1.y()-p2.y())+
<a name="l00547"></a>00547 <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(p1.z()-p2.z())+
<a name="l00548"></a>00548 <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>( <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>(p1.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#ae6df3cca6719010c12d5d70f7059cb3f" title="Get the YAW angle (in radians)">yaw</a>()-p2.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#ae6df3cca6719010c12d5d70f7059cb3f" title="Get the YAW angle (in radians)">yaw</a>() ) )+
<a name="l00549"></a>00549 <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>( <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>(p1.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#abed455c10a98c4b7463e49ca73dd1ac1" title="Get the PITCH angle (in radians)">pitch</a>()-p2.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#abed455c10a98c4b7463e49ca73dd1ac1" title="Get the PITCH angle (in radians)">pitch</a>() ) )+
<a name="l00550"></a>00550 <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>( <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>(p1.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#a47c00788772e211ca9efa4a59153b19b" title="Get the ROLL angle (in radians)">roll</a>()-p2.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html#a47c00788772e211ca9efa4a59153b19b" title="Get the ROLL angle (in radians)">roll</a>() ) );
<a name="l00551"></a>00551 }
<a name="l00552"></a>00552
<a name="l00553"></a>00553 <span class="comment">// --------------------------------------------------------------------------------</span>
<a name="l00554"></a>00554 <span class="comment">// Implements: detail::graph_edge_sqerror</span>
<a name="l00555"></a>00555 <span class="comment">//</span>
<a name="l00556"></a>00556 <span class="comment">// Compute the square error of a single edge, in comparison to the nodes global poses.</span>
<a name="l00557"></a>00557 <span class="comment">// --------------------------------------------------------------------------------</span>
<a name="l00558"></a><a class="code" href="structmrpt_1_1graphs_1_1detail_1_1graph__ops.html#a5b6bf07385a82cdf9fa091103033d85d">00558</a> <span class="keyword">static</span> <span class="keywordtype">double</span> graph_edge_sqerror(
<a name="l00559"></a>00559 <span class="keyword">const</span> graph_t *g,
<a name="l00560"></a>00560 <span class="keyword">const</span> <span class="keyword">typename</span> <a class="code" href="classstd_1_1multimap_1_1const__iterator.html" title="STL iterator class.">mrpt::graphs::CDirectedGraph<typename graph_t::constraint_t>::edges_map_t::const_iterator</a> &itEdge,
<a name="l00561"></a>00561 <span class="keywordtype">bool</span> ignoreCovariances )
<a name="l00562"></a>00562 {
<a name="l00563"></a>00563 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00564"></a>00564
<a name="l00565"></a>00565 <span class="comment">// Get node IDs:</span>
<a name="l00566"></a>00566 <span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1utils.html#a718b4f99645b7e9f6501c9b7bb2a2fe7" title="The type for node IDs in graphs of different types.">TNodeID</a> from_id = itEdge->first.first;
<a name="l00567"></a>00567 <span class="keyword">const</span> <a class="code" href="namespacemrpt_1_1utils.html#a718b4f99645b7e9f6501c9b7bb2a2fe7" title="The type for node IDs in graphs of different types.">TNodeID</a> to_id = itEdge->first.second;
<a name="l00568"></a>00568
<a name="l00569"></a>00569 <span class="comment">// And their global poses as stored in "nodes"</span>
<a name="l00570"></a>00570 <span class="keyword">typename</span> graph_t<a class="code" href="eigen__plugins_8h.html#a8dbda719917732693c56cee228465ed9">::global_poses_t::const_iterator</a> itPoseFrom = g->nodes.find(from_id);
<a name="l00571"></a>00571 <span class="keyword">typename</span> graph_t<a class="code" href="eigen__plugins_8h.html#a8dbda719917732693c56cee228465ed9">::global_poses_t::const_iterator</a> itPoseTo = g->nodes.find(to_id);
<a name="l00572"></a>00572 <a class="code" href="mrpt__macros_8h.html#ad30ea0382c594c0e2efe88212e9352b0">ASSERTMSG_</a>(itPoseFrom!=g->nodes.end(), <a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Node %u doesn't have a global pose in 'nodes'."</span>, static_cast<unsigned int>(from_id)))
<a name="l00573"></a>00573 <a class="code" href="mrpt__macros_8h.html#ad30ea0382c594c0e2efe88212e9352b0">ASSERTMSG_</a>(itPoseTo!=g->nodes.end(), <a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Node %u doesn't have a global pose in 'nodes'."</span>, static_cast<unsigned int>(to_id)))
<a name="l00574"></a>00574
<a name="l00575"></a>00575 <span class="comment">// The global poses:</span>
<a name="l00576"></a>00576 <span class="keyword">typedef</span> <span class="keyword">typename</span> graph_t::constraint_t constraint_t;
<a name="l00577"></a>00577
<a name="l00578"></a>00578 <span class="keyword">const</span> <span class="keyword">typename</span> constraint_t::type_value &from_mean = itPoseFrom->second;
<a name="l00579"></a>00579 <span class="keyword">const</span> <span class="keyword">typename</span> constraint_t::type_value &to_mean = itPoseTo->second;
<a name="l00580"></a>00580
<a name="l00581"></a>00581 <span class="comment">// The delta_pose as stored in the edge:</span>
<a name="l00582"></a>00582 <span class="keyword">const</span> constraint_t &edge_delta_pose = itEdge->second;
<a name="l00583"></a>00583 <span class="keyword">const</span> <span class="keyword">typename</span> constraint_t::type_value &edge_delta_pose_mean = edge_delta_pose.getPoseMean();
<a name="l00584"></a>00584
<a name="l00585"></a>00585 <span class="keywordflow">if</span> (ignoreCovariances)
<a name="l00586"></a>00586 { <span class="comment">// Square Euclidean distance: Just use the mean values, ignore covs.</span>
<a name="l00587"></a>00587 <span class="comment">// from_plus_delta = from_mean (+) edge_delta_pose_mean</span>
<a name="l00588"></a>00588 <span class="keyword">typename</span> constraint_t::type_value from_plus_delta(<a class="code" href="namespacemrpt_1_1poses.html#ac46e1fb95d438a441b6f396d9c79f430aefbfdff7ad639a9a8019af9de5f3c592">UNINITIALIZED_POSE</a>);
<a name="l00589"></a>00589 from_plus_delta.composeFrom(from_mean, edge_delta_pose_mean);
<a name="l00590"></a>00590
<a name="l00591"></a>00591 <span class="comment">// (auxMaha2Dist will also take into account the 2PI wrapping)</span>
<a name="l00592"></a>00592 <span class="keywordflow">return</span> auxEuclid2Dist(from_plus_delta,to_mean);
<a name="l00593"></a>00593 }
<a name="l00594"></a>00594 <span class="keywordflow">else</span>
<a name="l00595"></a>00595 {
<a name="l00596"></a>00596 <span class="comment">// Square Mahalanobis distance</span>
<a name="l00597"></a>00597 <span class="comment">// from_plus_delta = from_mean (+) edge_delta_pose (as a Gaussian)</span>
<a name="l00598"></a>00598 constraint_t from_plus_delta = edge_delta_pose;
<a name="l00599"></a>00599 from_plus_delta.changeCoordinatesReference(from_mean);
<a name="l00600"></a>00600
<a name="l00601"></a>00601 <span class="comment">// "from_plus_delta" is now a 3D or 6D Gaussian, to be compared to "to_mean":</span>
<a name="l00602"></a>00602 <span class="comment">// We want to compute the squared Mahalanobis distance:</span>
<a name="l00603"></a>00603 <span class="comment">// err^t * INV_COV * err</span>
<a name="l00604"></a>00604 <span class="comment">//</span>
<a name="l00605"></a>00605 <a class="code" href="classmrpt_1_1math_1_1_c_array_double.html" title="A partial specialization of CArrayNumeric for double numbers.">CArrayDouble<constraint_t::type_value::static_size></a> err;
<a name="l00606"></a>00606 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<constraint_t<a class="code" href="eigen__plugins_8h.html#a99fb83031ce9923c84392b4e92f956b5ad4a8f9f8c603d29a20ca6f9c056dd4da">::type_value::static_size</a>;i++)
<a name="l00607"></a>00607 err[i] = from_plus_delta.getPoseMean()[i] - to_mean[i];
<a name="l00608"></a>00608
<a name="l00609"></a>00609 <span class="comment">// (auxMaha2Dist will also take into account the 2PI wrapping)</span>
<a name="l00610"></a>00610 <span class="keywordflow">return</span> auxMaha2Dist(err,from_plus_delta);
<a name="l00611"></a>00611 }
<a name="l00612"></a>00612 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00613"></a>00613 }
<a name="l00614"></a>00614
<a name="l00615"></a>00615 }; <span class="comment">// end of graph_ops<graph_t></span>
<a name="l00616"></a>00616
<a name="l00617"></a>00617 }<span class="comment">// end NS</span>
<a name="l00618"></a>00618 }<span class="comment">// end NS</span>
<a name="l00619"></a>00619 } <span class="comment">// end NS</span>
<a name="l00620"></a>00620
<a name="l00621"></a>00621 <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>
