small_gicp/doc_cpp/general__factor_8hpp_source...

148 lines
21 KiB
HTML

<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "https://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.9.1"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>small_gicp: /home/runner/work/small_gicp/small_gicp/include/small_gicp/factors/general_factor.hpp Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
<tbody>
<tr style="height: 56px;">
<td id="projectalign" style="padding-left: 0.5em;">
<div id="projectname">small_gicp
</div>
</td>
</tr>
</tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.9.1 -->
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&amp;dn=gpl-2.0.txt GPL-v2 */
var searchBox = new SearchBox("searchBox", "search",false,'Search','.html');
/* @license-end */
</script>
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&amp;dn=gpl-2.0.txt GPL-v2 */
$(function() {
initMenu('',true,false,'search.php','Search');
$(document).ready(function() { init_search(); });
});
/* @license-end */</script>
<div id="main-nav"></div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
onmouseover="return searchBox.OnSearchSelectShow()"
onmouseout="return searchBox.OnSearchSelectHide()"
onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>
<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0"
name="MSearchResults" id="MSearchResults">
</iframe>
</div>
<div id="nav-path" class="navpath">
<ul>
<li class="navelem"><a class="el" href="dir_d44c64559bbebec7f509842c48db8b23.html">include</a></li><li class="navelem"><a class="el" href="dir_8f93768a68ffc2367e7100c40b4e616a.html">small_gicp</a></li><li class="navelem"><a class="el" href="dir_10d2ca31331a920e163404c3842c11af.html">factors</a></li> </ul>
</div>
</div><!-- top -->
<div class="header">
<div class="headertitle">
<div class="title">general_factor.hpp</div> </div>
</div><!--header-->
<div class="contents">
<a href="general__factor_8hpp.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span>&#160;<span class="comment">// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide</span></div>
<div class="line"><a name="l00002"></a><span class="lineno"> 2</span>&#160;<span class="comment">// SPDX-License-Identifier: MIT</span></div>
<div class="line"><a name="l00003"></a><span class="lineno"> 3</span>&#160;<span class="preprocessor">#pragma once</span></div>
<div class="line"><a name="l00004"></a><span class="lineno"> 4</span>&#160; </div>
<div class="line"><a name="l00005"></a><span class="lineno"> 5</span>&#160;<span class="preprocessor">#include &lt;Eigen/Core&gt;</span></div>
<div class="line"><a name="l00006"></a><span class="lineno"> 6</span>&#160;<span class="preprocessor">#include &lt;Eigen/Geometry&gt;</span></div>
<div class="line"><a name="l00007"></a><span class="lineno"> 7</span>&#160; </div>
<div class="line"><a name="l00008"></a><span class="lineno"> 8</span>&#160;<span class="keyword">namespace </span><a class="code" href="namespacesmall__gicp.html">small_gicp</a> {</div>
<div class="line"><a name="l00009"></a><span class="lineno"> 9</span>&#160; </div>
<div class="line"><a name="l00011"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1NullFactor.html"> 11</a></span>&#160;<span class="keyword">struct </span><a class="code" href="structsmall__gicp_1_1NullFactor.html">NullFactor</a> {</div>
<div class="line"><a name="l00012"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1NullFactor.html#a6742c27af29602f47cccbf1f1e6008d1"> 12</a></span>&#160; <a class="code" href="structsmall__gicp_1_1NullFactor.html#a6742c27af29602f47cccbf1f1e6008d1">NullFactor</a>() = <span class="keywordflow">default</span>;</div>
<div class="line"><a name="l00013"></a><span class="lineno"> 13</span>&#160; </div>
<div class="line"><a name="l00022"></a><span class="lineno"> 22</span>&#160; <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TargetPo<span class="keywordtype">int</span>Cloud, <span class="keyword">typename</span> SourcePo<span class="keywordtype">int</span>Cloud, <span class="keyword">typename</span> TargetTree&gt;</div>
<div class="line"><a name="l00023"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1NullFactor.html#aba0c65232d230e75b9e1fe2a583edbff"> 23</a></span>&#160; <span class="keywordtype">void</span> <a class="code" href="structsmall__gicp_1_1NullFactor.html#aba0c65232d230e75b9e1fe2a583edbff">update_linearized_system</a>(</div>
<div class="line"><a name="l00024"></a><span class="lineno"> 24</span>&#160; <span class="keyword">const</span> TargetPointCloud&amp; target,</div>
<div class="line"><a name="l00025"></a><span class="lineno"> 25</span>&#160; <span class="keyword">const</span> SourcePointCloud&amp; source,</div>
<div class="line"><a name="l00026"></a><span class="lineno"> 26</span>&#160; <span class="keyword">const</span> TargetTree&amp; target_tree,</div>
<div class="line"><a name="l00027"></a><span class="lineno"> 27</span>&#160; <span class="keyword">const</span> Eigen::Isometry3d&amp; T,</div>
<div class="line"><a name="l00028"></a><span class="lineno"> 28</span>&#160; Eigen::Matrix&lt;double, 6, 6&gt;* H,</div>
<div class="line"><a name="l00029"></a><span class="lineno"> 29</span>&#160; Eigen::Matrix&lt;double, 6, 1&gt;* b,</div>
<div class="line"><a name="l00030"></a><span class="lineno"> 30</span>&#160; <span class="keywordtype">double</span>* e)<span class="keyword"> const </span>{}</div>
<div class="line"><a name="l00031"></a><span class="lineno"> 31</span>&#160; </div>
<div class="line"><a name="l00037"></a><span class="lineno"> 37</span>&#160; <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TargetPo<span class="keywordtype">int</span>Cloud, <span class="keyword">typename</span> SourcePo<span class="keywordtype">int</span>Cloud&gt;</div>
<div class="line"><a name="l00038"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1NullFactor.html#a5f4db46bbbb7a3772d043f47d516c527"> 38</a></span>&#160; <span class="keywordtype">void</span> <a class="code" href="structsmall__gicp_1_1NullFactor.html#a5f4db46bbbb7a3772d043f47d516c527">update_error</a>(<span class="keyword">const</span> TargetPointCloud&amp; target, <span class="keyword">const</span> SourcePointCloud&amp; source, <span class="keyword">const</span> Eigen::Isometry3d&amp; T, <span class="keywordtype">double</span>* e)<span class="keyword"> const </span>{}</div>
<div class="line"><a name="l00039"></a><span class="lineno"> 39</span>&#160;};</div>
<div class="line"><a name="l00040"></a><span class="lineno"> 40</span>&#160; </div>
<div class="line"><a name="l00043"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1RestrictDoFFactor.html"> 43</a></span>&#160;<span class="keyword">struct </span><a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html">RestrictDoFFactor</a> {</div>
<div class="line"><a name="l00045"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1RestrictDoFFactor.html#a40be8166c4ccc4d03afa7e7a15069fc5"> 45</a></span>&#160; <a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html#a40be8166c4ccc4d03afa7e7a15069fc5">RestrictDoFFactor</a>() {</div>
<div class="line"><a name="l00046"></a><span class="lineno"> 46</span>&#160; <a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html#a6da7f26b78bc86e79648dc5df041cf83">lambda</a> = 1e9;</div>
<div class="line"><a name="l00047"></a><span class="lineno"> 47</span>&#160; <a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html#afb80d9e38ca89bd986356c5976d513a3">mask</a>.setOnes();</div>
<div class="line"><a name="l00048"></a><span class="lineno"> 48</span>&#160; }</div>
<div class="line"><a name="l00049"></a><span class="lineno"> 49</span>&#160; </div>
<div class="line"><a name="l00051"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1RestrictDoFFactor.html#a77d66bdd37aef1b1b6d40715a2ac9a01"> 51</a></span>&#160; <span class="keywordtype">void</span> <a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html#a77d66bdd37aef1b1b6d40715a2ac9a01">set_rotation_mask</a>(<span class="keyword">const</span> Eigen::Array3d&amp; rot_mask) { <a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html#afb80d9e38ca89bd986356c5976d513a3">mask</a>.head&lt;3&gt;() = rot_mask; }</div>
<div class="line"><a name="l00052"></a><span class="lineno"> 52</span>&#160; </div>
<div class="line"><a name="l00054"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1RestrictDoFFactor.html#af6c8bfcf2c38b9dd3e8835b23e91fdee"> 54</a></span>&#160; <span class="keywordtype">void</span> <a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html#af6c8bfcf2c38b9dd3e8835b23e91fdee">set_translation_mask</a>(<span class="keyword">const</span> Eigen::Array3d&amp; trans_mask) { <a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html#afb80d9e38ca89bd986356c5976d513a3">mask</a>.tail&lt;3&gt;() = trans_mask; }</div>
<div class="line"><a name="l00055"></a><span class="lineno"> 55</span>&#160; </div>
<div class="line"><a name="l00057"></a><span class="lineno"> 57</span>&#160; <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TargetPo<span class="keywordtype">int</span>Cloud, <span class="keyword">typename</span> SourcePo<span class="keywordtype">int</span>Cloud, <span class="keyword">typename</span> TargetTree&gt;</div>
<div class="line"><a name="l00058"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1RestrictDoFFactor.html#a977474000ad7bc28eca1102024f48a31"> 58</a></span>&#160; <span class="keywordtype">void</span> <a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html#a977474000ad7bc28eca1102024f48a31">update_linearized_system</a>(</div>
<div class="line"><a name="l00059"></a><span class="lineno"> 59</span>&#160; <span class="keyword">const</span> TargetPointCloud&amp; target,</div>
<div class="line"><a name="l00060"></a><span class="lineno"> 60</span>&#160; <span class="keyword">const</span> SourcePointCloud&amp; source,</div>
<div class="line"><a name="l00061"></a><span class="lineno"> 61</span>&#160; <span class="keyword">const</span> TargetTree&amp; target_tree,</div>
<div class="line"><a name="l00062"></a><span class="lineno"> 62</span>&#160; <span class="keyword">const</span> Eigen::Isometry3d&amp; T,</div>
<div class="line"><a name="l00063"></a><span class="lineno"> 63</span>&#160; Eigen::Matrix&lt;double, 6, 6&gt;* H,</div>
<div class="line"><a name="l00064"></a><span class="lineno"> 64</span>&#160; Eigen::Matrix&lt;double, 6, 1&gt;* b,</div>
<div class="line"><a name="l00065"></a><span class="lineno"> 65</span>&#160; <span class="keywordtype">double</span>* e)<span class="keyword"> const </span>{</div>
<div class="line"><a name="l00066"></a><span class="lineno"> 66</span>&#160; *H += <a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html#a6da7f26b78bc86e79648dc5df041cf83">lambda</a> * (<a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html#afb80d9e38ca89bd986356c5976d513a3">mask</a> - 1.0).abs().matrix().asDiagonal();</div>
<div class="line"><a name="l00067"></a><span class="lineno"> 67</span>&#160; }</div>
<div class="line"><a name="l00068"></a><span class="lineno"> 68</span>&#160; </div>
<div class="line"><a name="l00070"></a><span class="lineno"> 70</span>&#160; <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TargetPo<span class="keywordtype">int</span>Cloud, <span class="keyword">typename</span> SourcePo<span class="keywordtype">int</span>Cloud&gt;</div>
<div class="line"><a name="l00071"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1RestrictDoFFactor.html#aa61891a0ad977a574f8e9cdb6ca3fa38"> 71</a></span>&#160; <span class="keywordtype">void</span> <a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html#aa61891a0ad977a574f8e9cdb6ca3fa38">update_error</a>(<span class="keyword">const</span> TargetPointCloud&amp; target, <span class="keyword">const</span> SourcePointCloud&amp; source, <span class="keyword">const</span> Eigen::Isometry3d&amp; T, <span class="keywordtype">double</span>* e)<span class="keyword"> const </span>{}</div>
<div class="line"><a name="l00072"></a><span class="lineno"> 72</span>&#160; </div>
<div class="line"><a name="l00073"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1RestrictDoFFactor.html#a6da7f26b78bc86e79648dc5df041cf83"> 73</a></span>&#160; <span class="keywordtype">double</span> <a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html#a6da7f26b78bc86e79648dc5df041cf83">lambda</a>; </div>
<div class="line"><a name="l00074"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1RestrictDoFFactor.html#afb80d9e38ca89bd986356c5976d513a3"> 74</a></span>&#160; Eigen::Array&lt;double, 6, 1&gt; <a class="code" href="structsmall__gicp_1_1RestrictDoFFactor.html#afb80d9e38ca89bd986356c5976d513a3">mask</a>; </div>
<div class="line"><a name="l00075"></a><span class="lineno"> 75</span>&#160;};</div>
<div class="line"><a name="l00076"></a><span class="lineno"> 76</span>&#160; </div>
<div class="line"><a name="l00077"></a><span class="lineno"> 77</span>&#160;} <span class="comment">// namespace small_gicp</span></div>
<div class="ttc" id="anamespacesmall__gicp_html"><div class="ttname"><a href="namespacesmall__gicp.html">small_gicp</a></div><div class="ttdef"><b>Definition:</b> flat_container.hpp:12</div></div>
<div class="ttc" id="astructsmall__gicp_1_1NullFactor_html"><div class="ttname"><a href="structsmall__gicp_1_1NullFactor.html">small_gicp::NullFactor</a></div><div class="ttdoc">Null factor that gives no constraints.</div><div class="ttdef"><b>Definition:</b> general_factor.hpp:11</div></div>
<div class="ttc" id="astructsmall__gicp_1_1NullFactor_html_a5f4db46bbbb7a3772d043f47d516c527"><div class="ttname"><a href="structsmall__gicp_1_1NullFactor.html#a5f4db46bbbb7a3772d043f47d516c527">small_gicp::NullFactor::update_error</a></div><div class="ttdeci">void update_error(const TargetPointCloud &amp;target, const SourcePointCloud &amp;source, const Eigen::Isometry3d &amp;T, double *e) const</div><div class="ttdoc">Update error consisting of per-point factors.</div><div class="ttdef"><b>Definition:</b> general_factor.hpp:38</div></div>
<div class="ttc" id="astructsmall__gicp_1_1NullFactor_html_a6742c27af29602f47cccbf1f1e6008d1"><div class="ttname"><a href="structsmall__gicp_1_1NullFactor.html#a6742c27af29602f47cccbf1f1e6008d1">small_gicp::NullFactor::NullFactor</a></div><div class="ttdeci">NullFactor()=default</div></div>
<div class="ttc" id="astructsmall__gicp_1_1NullFactor_html_aba0c65232d230e75b9e1fe2a583edbff"><div class="ttname"><a href="structsmall__gicp_1_1NullFactor.html#aba0c65232d230e75b9e1fe2a583edbff">small_gicp::NullFactor::update_linearized_system</a></div><div class="ttdeci">void update_linearized_system(const TargetPointCloud &amp;target, const SourcePointCloud &amp;source, const TargetTree &amp;target_tree, const Eigen::Isometry3d &amp;T, Eigen::Matrix&lt; double, 6, 6 &gt; *H, Eigen::Matrix&lt; double, 6, 1 &gt; *b, double *e) const</div><div class="ttdoc">Update linearized system consisting of linearized per-point factors.</div><div class="ttdef"><b>Definition:</b> general_factor.hpp:23</div></div>
<div class="ttc" id="astructsmall__gicp_1_1RestrictDoFFactor_html"><div class="ttname"><a href="structsmall__gicp_1_1RestrictDoFFactor.html">small_gicp::RestrictDoFFactor</a></div><div class="ttdoc">Factor to restrict the degrees of freedom of optimization (e.g., fixing roll, pitch rotation).</div><div class="ttdef"><b>Definition:</b> general_factor.hpp:43</div></div>
<div class="ttc" id="astructsmall__gicp_1_1RestrictDoFFactor_html_a40be8166c4ccc4d03afa7e7a15069fc5"><div class="ttname"><a href="structsmall__gicp_1_1RestrictDoFFactor.html#a40be8166c4ccc4d03afa7e7a15069fc5">small_gicp::RestrictDoFFactor::RestrictDoFFactor</a></div><div class="ttdeci">RestrictDoFFactor()</div><div class="ttdoc">Constructor.</div><div class="ttdef"><b>Definition:</b> general_factor.hpp:45</div></div>
<div class="ttc" id="astructsmall__gicp_1_1RestrictDoFFactor_html_a6da7f26b78bc86e79648dc5df041cf83"><div class="ttname"><a href="structsmall__gicp_1_1RestrictDoFFactor.html#a6da7f26b78bc86e79648dc5df041cf83">small_gicp::RestrictDoFFactor::lambda</a></div><div class="ttdeci">double lambda</div><div class="ttdoc">Regularization parameter (Increasing this makes the constraint stronger)</div><div class="ttdef"><b>Definition:</b> general_factor.hpp:73</div></div>
<div class="ttc" id="astructsmall__gicp_1_1RestrictDoFFactor_html_a77d66bdd37aef1b1b6d40715a2ac9a01"><div class="ttname"><a href="structsmall__gicp_1_1RestrictDoFFactor.html#a77d66bdd37aef1b1b6d40715a2ac9a01">small_gicp::RestrictDoFFactor::set_rotation_mask</a></div><div class="ttdeci">void set_rotation_mask(const Eigen::Array3d &amp;rot_mask)</div><div class="ttdoc">Set rotation mask. (1.0 = active, 0.0 = inactive)</div><div class="ttdef"><b>Definition:</b> general_factor.hpp:51</div></div>
<div class="ttc" id="astructsmall__gicp_1_1RestrictDoFFactor_html_a977474000ad7bc28eca1102024f48a31"><div class="ttname"><a href="structsmall__gicp_1_1RestrictDoFFactor.html#a977474000ad7bc28eca1102024f48a31">small_gicp::RestrictDoFFactor::update_linearized_system</a></div><div class="ttdeci">void update_linearized_system(const TargetPointCloud &amp;target, const SourcePointCloud &amp;source, const TargetTree &amp;target_tree, const Eigen::Isometry3d &amp;T, Eigen::Matrix&lt; double, 6, 6 &gt; *H, Eigen::Matrix&lt; double, 6, 1 &gt; *b, double *e) const</div><div class="ttdoc">Update linearized system consisting of linearized per-point factors.</div><div class="ttdef"><b>Definition:</b> general_factor.hpp:58</div></div>
<div class="ttc" id="astructsmall__gicp_1_1RestrictDoFFactor_html_aa61891a0ad977a574f8e9cdb6ca3fa38"><div class="ttname"><a href="structsmall__gicp_1_1RestrictDoFFactor.html#aa61891a0ad977a574f8e9cdb6ca3fa38">small_gicp::RestrictDoFFactor::update_error</a></div><div class="ttdeci">void update_error(const TargetPointCloud &amp;target, const SourcePointCloud &amp;source, const Eigen::Isometry3d &amp;T, double *e) const</div><div class="ttdoc">Update error consisting of per-point factors.</div><div class="ttdef"><b>Definition:</b> general_factor.hpp:71</div></div>
<div class="ttc" id="astructsmall__gicp_1_1RestrictDoFFactor_html_af6c8bfcf2c38b9dd3e8835b23e91fdee"><div class="ttname"><a href="structsmall__gicp_1_1RestrictDoFFactor.html#af6c8bfcf2c38b9dd3e8835b23e91fdee">small_gicp::RestrictDoFFactor::set_translation_mask</a></div><div class="ttdeci">void set_translation_mask(const Eigen::Array3d &amp;trans_mask)</div><div class="ttdoc">Set translation mask. (1.0 = active, 0.0 = inactive)</div><div class="ttdef"><b>Definition:</b> general_factor.hpp:54</div></div>
<div class="ttc" id="astructsmall__gicp_1_1RestrictDoFFactor_html_afb80d9e38ca89bd986356c5976d513a3"><div class="ttname"><a href="structsmall__gicp_1_1RestrictDoFFactor.html#afb80d9e38ca89bd986356c5976d513a3">small_gicp::RestrictDoFFactor::mask</a></div><div class="ttdeci">Eigen::Array&lt; double, 6, 1 &gt; mask</div><div class="ttdoc">Mask for restricting DoF (rx, ry, rz, tx, ty, tz)</div><div class="ttdef"><b>Definition:</b> general_factor.hpp:74</div></div>
</div><!-- fragment --></div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by&#160;<a href="https://www.doxygen.org/index.html"><img class="footer" src="doxygen.svg" width="104" height="31" alt="doxygen"/></a> 1.9.1
</small></address>
</body>
</html>