mirror of https://github.com/koide3/small_gicp.git
234 lines
40 KiB
HTML
234 lines
40 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/util/normal_estimation.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&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&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_65c55b74f1fe3d7e17a0effca76d06ab.html">util</a></li> </ul>
|
|
</div>
|
|
</div><!-- top -->
|
|
<div class="header">
|
|
<div class="headertitle">
|
|
<div class="title">normal_estimation.hpp</div> </div>
|
|
</div><!--header-->
|
|
<div class="contents">
|
|
<a href="normal__estimation_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> <span class="comment">// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide</span></div>
|
|
<div class="line"><a name="l00002"></a><span class="lineno"> 2</span> <span class="comment">// SPDX-License-Identifier: MIT</span></div>
|
|
<div class="line"><a name="l00003"></a><span class="lineno"> 3</span> <span class="preprocessor">#pragma once</span></div>
|
|
<div class="line"><a name="l00004"></a><span class="lineno"> 4</span>  </div>
|
|
<div class="line"><a name="l00005"></a><span class="lineno"> 5</span> <span class="preprocessor">#include <Eigen/Eigen></span></div>
|
|
<div class="line"><a name="l00006"></a><span class="lineno"> 6</span> <span class="preprocessor">#include <<a class="code" href="kdtree_8hpp.html">small_gicp/ann/kdtree.hpp</a>></span></div>
|
|
<div class="line"><a name="l00007"></a><span class="lineno"> 7</span>  </div>
|
|
<div class="line"><a name="l00008"></a><span class="lineno"> 8</span> <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>  </div>
|
|
<div class="line"><a name="l00012"></a><span class="lineno"> 12</span> <span class="keyword">template</span> <<span class="keyword">typename</span> Po<span class="keywordtype">int</span>Cloud></div>
|
|
<div class="line"><a name="l00013"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1NormalSetter.html"> 13</a></span> <span class="keyword">struct </span><a class="code" href="structsmall__gicp_1_1NormalSetter.html">NormalSetter</a> {</div>
|
|
<div class="line"><a name="l00015"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1NormalSetter.html#aa779d77c915daa28a4a71865da8c8c1f"> 15</a></span>  <span class="keyword">static</span> <span class="keywordtype">void</span> <a class="code" href="structsmall__gicp_1_1NormalSetter.html#aa779d77c915daa28a4a71865da8c8c1f">set_invalid</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <span class="keywordtype">size_t</span> i) { <a class="code" href="namespacesmall__gicp_1_1traits.html#a5647854e2c446d240366fca679aa8f38">traits::set_normal</a>(cloud, i, Eigen::Vector4d::Zero()); }</div>
|
|
<div class="line"><a name="l00016"></a><span class="lineno"> 16</span>  </div>
|
|
<div class="line"><a name="l00018"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1NormalSetter.html#aaa6b4e2dad59c1f1fac6cafb0e2240aa"> 18</a></span>  <span class="keyword">static</span> <span class="keywordtype">void</span> <a class="code" href="structsmall__gicp_1_1NormalSetter.html#aaa6b4e2dad59c1f1fac6cafb0e2240aa">set</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <span class="keywordtype">size_t</span> i, <span class="keyword">const</span> Eigen::Matrix3d& eigenvectors) {</div>
|
|
<div class="line"><a name="l00019"></a><span class="lineno"> 19</span>  <span class="keyword">const</span> Eigen::Vector4d <a class="code" href="namespacesmall__gicp_1_1traits.html#ad162655a1c08776aa4c94c2fa1ebf863">normal</a> = (Eigen::Vector4d() << eigenvectors.col(0).normalized(), 0.0).finished();</div>
|
|
<div class="line"><a name="l00020"></a><span class="lineno"> 20</span>  <span class="keywordflow">if</span> (<a class="code" href="namespacesmall__gicp_1_1traits.html#a66b660d3a0c2e81cdf06abc8efebdad1">traits::point</a>(cloud, i).dot(<a class="code" href="namespacesmall__gicp_1_1traits.html#ad162655a1c08776aa4c94c2fa1ebf863">normal</a>) > 0) {</div>
|
|
<div class="line"><a name="l00021"></a><span class="lineno"> 21</span>  <a class="code" href="namespacesmall__gicp_1_1traits.html#a5647854e2c446d240366fca679aa8f38">traits::set_normal</a>(cloud, i, -<a class="code" href="namespacesmall__gicp_1_1traits.html#ad162655a1c08776aa4c94c2fa1ebf863">normal</a>);</div>
|
|
<div class="line"><a name="l00022"></a><span class="lineno"> 22</span>  } <span class="keywordflow">else</span> {</div>
|
|
<div class="line"><a name="l00023"></a><span class="lineno"> 23</span>  <a class="code" href="namespacesmall__gicp_1_1traits.html#a5647854e2c446d240366fca679aa8f38">traits::set_normal</a>(cloud, i, <a class="code" href="namespacesmall__gicp_1_1traits.html#ad162655a1c08776aa4c94c2fa1ebf863">normal</a>);</div>
|
|
<div class="line"><a name="l00024"></a><span class="lineno"> 24</span>  }</div>
|
|
<div class="line"><a name="l00025"></a><span class="lineno"> 25</span>  }</div>
|
|
<div class="line"><a name="l00026"></a><span class="lineno"> 26</span> };</div>
|
|
<div class="line"><a name="l00027"></a><span class="lineno"> 27</span>  </div>
|
|
<div class="line"><a name="l00030"></a><span class="lineno"> 30</span> <span class="keyword">template</span> <<span class="keyword">typename</span> Po<span class="keywordtype">int</span>Cloud></div>
|
|
<div class="line"><a name="l00031"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1CovarianceSetter.html"> 31</a></span> <span class="keyword">struct </span><a class="code" href="structsmall__gicp_1_1CovarianceSetter.html">CovarianceSetter</a> {</div>
|
|
<div class="line"><a name="l00033"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1CovarianceSetter.html#a7dad95aaf6108d09cd6865aa4ce48236"> 33</a></span>  <span class="keyword">static</span> <span class="keywordtype">void</span> <a class="code" href="structsmall__gicp_1_1CovarianceSetter.html#a7dad95aaf6108d09cd6865aa4ce48236">set_invalid</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <span class="keywordtype">size_t</span> i) {</div>
|
|
<div class="line"><a name="l00034"></a><span class="lineno"> 34</span>  Eigen::Matrix4d <a class="code" href="namespacesmall__gicp_1_1traits.html#ac9146681bcef16a4c73d3fd05ad295c7">cov</a> = Eigen::Matrix4d::Identity();</div>
|
|
<div class="line"><a name="l00035"></a><span class="lineno"> 35</span>  <a class="code" href="namespacesmall__gicp_1_1traits.html#ac9146681bcef16a4c73d3fd05ad295c7">cov</a>(3, 3) = 0.0;</div>
|
|
<div class="line"><a name="l00036"></a><span class="lineno"> 36</span>  <a class="code" href="namespacesmall__gicp_1_1traits.html#a3e3d8420d93214163aec5cfea1ff6016">traits::set_cov</a>(cloud, i, <a class="code" href="namespacesmall__gicp_1_1traits.html#ac9146681bcef16a4c73d3fd05ad295c7">cov</a>);</div>
|
|
<div class="line"><a name="l00037"></a><span class="lineno"> 37</span>  }</div>
|
|
<div class="line"><a name="l00038"></a><span class="lineno"> 38</span>  </div>
|
|
<div class="line"><a name="l00040"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1CovarianceSetter.html#a6d659b970363e842cbeb28d487cbdea2"> 40</a></span>  <span class="keyword">static</span> <span class="keywordtype">void</span> <a class="code" href="structsmall__gicp_1_1CovarianceSetter.html#a6d659b970363e842cbeb28d487cbdea2">set</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <span class="keywordtype">size_t</span> i, <span class="keyword">const</span> Eigen::Matrix3d& eigenvectors) {</div>
|
|
<div class="line"><a name="l00041"></a><span class="lineno"> 41</span>  <span class="keyword">const</span> Eigen::Vector3d values(1e-3, 1.0, 1.0);</div>
|
|
<div class="line"><a name="l00042"></a><span class="lineno"> 42</span>  Eigen::Matrix4d <a class="code" href="namespacesmall__gicp_1_1traits.html#ac9146681bcef16a4c73d3fd05ad295c7">cov</a> = Eigen::Matrix4d::Zero();</div>
|
|
<div class="line"><a name="l00043"></a><span class="lineno"> 43</span>  <a class="code" href="namespacesmall__gicp_1_1traits.html#ac9146681bcef16a4c73d3fd05ad295c7">cov</a>.block<3, 3>(0, 0) = eigenvectors * values.asDiagonal() * eigenvectors.transpose();</div>
|
|
<div class="line"><a name="l00044"></a><span class="lineno"> 44</span>  <a class="code" href="namespacesmall__gicp_1_1traits.html#a3e3d8420d93214163aec5cfea1ff6016">traits::set_cov</a>(cloud, i, <a class="code" href="namespacesmall__gicp_1_1traits.html#ac9146681bcef16a4c73d3fd05ad295c7">cov</a>);</div>
|
|
<div class="line"><a name="l00045"></a><span class="lineno"> 45</span>  }</div>
|
|
<div class="line"><a name="l00046"></a><span class="lineno"> 46</span> };</div>
|
|
<div class="line"><a name="l00047"></a><span class="lineno"> 47</span>  </div>
|
|
<div class="line"><a name="l00050"></a><span class="lineno"> 50</span> <span class="keyword">template</span> <<span class="keyword">typename</span> Po<span class="keywordtype">int</span>Cloud></div>
|
|
<div class="line"><a name="l00051"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1NormalCovarianceSetter.html"> 51</a></span> <span class="keyword">struct </span><a class="code" href="structsmall__gicp_1_1NormalCovarianceSetter.html">NormalCovarianceSetter</a> {</div>
|
|
<div class="line"><a name="l00053"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1NormalCovarianceSetter.html#a88d3357939cc97beaf51e659b0f0b075"> 53</a></span>  <span class="keyword">static</span> <span class="keywordtype">void</span> <a class="code" href="structsmall__gicp_1_1NormalCovarianceSetter.html#a88d3357939cc97beaf51e659b0f0b075">set_invalid</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <span class="keywordtype">size_t</span> i) {</div>
|
|
<div class="line"><a name="l00054"></a><span class="lineno"> 54</span>  <a class="code" href="structsmall__gicp_1_1NormalSetter.html#aa779d77c915daa28a4a71865da8c8c1f">NormalSetter<PointCloud>::set_invalid</a>(cloud, i);</div>
|
|
<div class="line"><a name="l00055"></a><span class="lineno"> 55</span>  <a class="code" href="structsmall__gicp_1_1CovarianceSetter.html#a7dad95aaf6108d09cd6865aa4ce48236">CovarianceSetter<PointCloud>::set_invalid</a>(cloud, i);</div>
|
|
<div class="line"><a name="l00056"></a><span class="lineno"> 56</span>  }</div>
|
|
<div class="line"><a name="l00057"></a><span class="lineno"> 57</span>  </div>
|
|
<div class="line"><a name="l00059"></a><span class="lineno"><a class="line" href="structsmall__gicp_1_1NormalCovarianceSetter.html#a5ccf6387684fd6d8c6fb1627a3f1e112"> 59</a></span>  <span class="keyword">static</span> <span class="keywordtype">void</span> <a class="code" href="structsmall__gicp_1_1NormalCovarianceSetter.html#a5ccf6387684fd6d8c6fb1627a3f1e112">set</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <span class="keywordtype">size_t</span> i, <span class="keyword">const</span> Eigen::Matrix3d& eigenvectors) {</div>
|
|
<div class="line"><a name="l00060"></a><span class="lineno"> 60</span>  <a class="code" href="structsmall__gicp_1_1NormalSetter.html#aaa6b4e2dad59c1f1fac6cafb0e2240aa">NormalSetter<PointCloud>::set</a>(cloud, i, eigenvectors);</div>
|
|
<div class="line"><a name="l00061"></a><span class="lineno"> 61</span>  <a class="code" href="structsmall__gicp_1_1CovarianceSetter.html#a6d659b970363e842cbeb28d487cbdea2">CovarianceSetter<PointCloud>::set</a>(cloud, i, eigenvectors);</div>
|
|
<div class="line"><a name="l00062"></a><span class="lineno"> 62</span>  }</div>
|
|
<div class="line"><a name="l00063"></a><span class="lineno"> 63</span> };</div>
|
|
<div class="line"><a name="l00064"></a><span class="lineno"> 64</span>  </div>
|
|
<div class="line"><a name="l00065"></a><span class="lineno"> 65</span> <span class="keyword">template</span> <<span class="keyword">typename</span> Setter, <span class="keyword">typename</span> Po<span class="keywordtype">int</span>Cloud, <span class="keyword">typename</span> Tree></div>
|
|
<div class="line"><a name="l00066"></a><span class="lineno"><a class="line" href="namespacesmall__gicp.html#a501687927eb1e20bc346907c925f9d7d"> 66</a></span> <span class="keywordtype">void</span> <a class="code" href="namespacesmall__gicp.html#a501687927eb1e20bc346907c925f9d7d">estimate_local_features</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, Tree& kdtree, <span class="keywordtype">int</span> num_neighbors, <span class="keywordtype">size_t</span> point_index) {</div>
|
|
<div class="line"><a name="l00067"></a><span class="lineno"> 67</span>  std::vector<size_t> k_indices(num_neighbors);</div>
|
|
<div class="line"><a name="l00068"></a><span class="lineno"> 68</span>  std::vector<double> k_sq_dists(num_neighbors);</div>
|
|
<div class="line"><a name="l00069"></a><span class="lineno"> 69</span>  <span class="keyword">const</span> <span class="keywordtype">size_t</span> n = kdtree.knn_search(<a class="code" href="namespacesmall__gicp_1_1traits.html#a66b660d3a0c2e81cdf06abc8efebdad1">traits::point</a>(cloud, point_index), num_neighbors, k_indices.data(), k_sq_dists.data());</div>
|
|
<div class="line"><a name="l00070"></a><span class="lineno"> 70</span>  </div>
|
|
<div class="line"><a name="l00071"></a><span class="lineno"> 71</span>  <span class="keywordflow">if</span> (n < 5) {</div>
|
|
<div class="line"><a name="l00072"></a><span class="lineno"> 72</span>  <span class="comment">// Insufficient number of neighbors</span></div>
|
|
<div class="line"><a name="l00073"></a><span class="lineno"> 73</span>  Setter::set_invalid(cloud, point_index);</div>
|
|
<div class="line"><a name="l00074"></a><span class="lineno"> 74</span>  <span class="keywordflow">return</span>;</div>
|
|
<div class="line"><a name="l00075"></a><span class="lineno"> 75</span>  }</div>
|
|
<div class="line"><a name="l00076"></a><span class="lineno"> 76</span>  </div>
|
|
<div class="line"><a name="l00077"></a><span class="lineno"> 77</span>  Eigen::Vector4d sum_points = Eigen::Vector4d::Zero();</div>
|
|
<div class="line"><a name="l00078"></a><span class="lineno"> 78</span>  Eigen::Matrix4d sum_cross = Eigen::Matrix4d::Zero();</div>
|
|
<div class="line"><a name="l00079"></a><span class="lineno"> 79</span>  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i < n; i++) {</div>
|
|
<div class="line"><a name="l00080"></a><span class="lineno"> 80</span>  <span class="keyword">const</span> <span class="keyword">auto</span>& pt = <a class="code" href="namespacesmall__gicp_1_1traits.html#a66b660d3a0c2e81cdf06abc8efebdad1">traits::point</a>(cloud, k_indices[i]);</div>
|
|
<div class="line"><a name="l00081"></a><span class="lineno"> 81</span>  sum_points += pt;</div>
|
|
<div class="line"><a name="l00082"></a><span class="lineno"> 82</span>  sum_cross += pt * pt.transpose();</div>
|
|
<div class="line"><a name="l00083"></a><span class="lineno"> 83</span>  }</div>
|
|
<div class="line"><a name="l00084"></a><span class="lineno"> 84</span>  </div>
|
|
<div class="line"><a name="l00085"></a><span class="lineno"> 85</span>  <span class="keyword">const</span> Eigen::Vector4d mean = sum_points / n;</div>
|
|
<div class="line"><a name="l00086"></a><span class="lineno"> 86</span>  <span class="keyword">const</span> Eigen::Matrix4d <a class="code" href="namespacesmall__gicp_1_1traits.html#ac9146681bcef16a4c73d3fd05ad295c7">cov</a> = (sum_cross - mean * sum_points.transpose()) / n;</div>
|
|
<div class="line"><a name="l00087"></a><span class="lineno"> 87</span>  </div>
|
|
<div class="line"><a name="l00088"></a><span class="lineno"> 88</span>  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig;</div>
|
|
<div class="line"><a name="l00089"></a><span class="lineno"> 89</span>  eig.computeDirect(<a class="code" href="namespacesmall__gicp_1_1traits.html#ac9146681bcef16a4c73d3fd05ad295c7">cov</a>.block<3, 3>(0, 0));</div>
|
|
<div class="line"><a name="l00090"></a><span class="lineno"> 90</span>  </div>
|
|
<div class="line"><a name="l00091"></a><span class="lineno"> 91</span>  Setter::set(cloud, point_index, eig.eigenvectors());</div>
|
|
<div class="line"><a name="l00092"></a><span class="lineno"> 92</span> }</div>
|
|
<div class="line"><a name="l00093"></a><span class="lineno"> 93</span>  </div>
|
|
<div class="line"><a name="l00094"></a><span class="lineno"> 94</span> <span class="keyword">template</span> <<span class="keyword">typename</span> Setter, <span class="keyword">typename</span> Po<span class="keywordtype">int</span>Cloud, <span class="keyword">typename</span> KdTree></div>
|
|
<div class="line"><a name="l00095"></a><span class="lineno"><a class="line" href="namespacesmall__gicp.html#a94b5c528b2407e7892765d500ac4bc68"> 95</a></span> <span class="keywordtype">void</span> <a class="code" href="namespacesmall__gicp.html#a501687927eb1e20bc346907c925f9d7d">estimate_local_features</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <span class="keyword">const</span> <a class="code" href="structsmall__gicp_1_1KdTree.html">KdTree</a>& kdtree, <span class="keywordtype">int</span> num_neighbors) {</div>
|
|
<div class="line"><a name="l00096"></a><span class="lineno"> 96</span>  <a class="code" href="namespacesmall__gicp_1_1traits.html#ae35e34068062341652ed9dbf7c3eba25">traits::resize</a>(cloud, <a class="code" href="namespacesmall__gicp_1_1traits.html#a3c4ae38ff52330d6e624f4971c291fac">traits::size</a>(cloud));</div>
|
|
<div class="line"><a name="l00097"></a><span class="lineno"> 97</span>  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i < <a class="code" href="namespacesmall__gicp_1_1traits.html#a3c4ae38ff52330d6e624f4971c291fac">traits::size</a>(cloud); i++) {</div>
|
|
<div class="line"><a name="l00098"></a><span class="lineno"> 98</span>  estimate_local_features<Setter>(cloud, kdtree, num_neighbors, i);</div>
|
|
<div class="line"><a name="l00099"></a><span class="lineno"> 99</span>  }</div>
|
|
<div class="line"><a name="l00100"></a><span class="lineno"> 100</span> }</div>
|
|
<div class="line"><a name="l00101"></a><span class="lineno"> 101</span>  </div>
|
|
<div class="line"><a name="l00102"></a><span class="lineno"> 102</span> <span class="keyword">template</span> <<span class="keyword">typename</span> Setter, <span class="keyword">typename</span> Po<span class="keywordtype">int</span>Cloud></div>
|
|
<div class="line"><a name="l00103"></a><span class="lineno"><a class="line" href="namespacesmall__gicp.html#a429f8aa891c416007816c83a09566600"> 103</a></span> <span class="keywordtype">void</span> <a class="code" href="namespacesmall__gicp.html#a501687927eb1e20bc346907c925f9d7d">estimate_local_features</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <span class="keywordtype">int</span> num_neighbors) {</div>
|
|
<div class="line"><a name="l00104"></a><span class="lineno"> 104</span>  <a class="code" href="namespacesmall__gicp_1_1traits.html#ae35e34068062341652ed9dbf7c3eba25">traits::resize</a>(cloud, <a class="code" href="namespacesmall__gicp_1_1traits.html#a3c4ae38ff52330d6e624f4971c291fac">traits::size</a>(cloud));</div>
|
|
<div class="line"><a name="l00105"></a><span class="lineno"> 105</span>  </div>
|
|
<div class="line"><a name="l00106"></a><span class="lineno"> 106</span>  <a class="code" href="structsmall__gicp_1_1UnsafeKdTree.html">UnsafeKdTree<PointCloud></a> kdtree(cloud);</div>
|
|
<div class="line"><a name="l00107"></a><span class="lineno"> 107</span>  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i < <a class="code" href="namespacesmall__gicp_1_1traits.html#a3c4ae38ff52330d6e624f4971c291fac">traits::size</a>(cloud); i++) {</div>
|
|
<div class="line"><a name="l00108"></a><span class="lineno"> 108</span>  estimate_local_features<Setter>(cloud, kdtree, num_neighbors, i);</div>
|
|
<div class="line"><a name="l00109"></a><span class="lineno"> 109</span>  }</div>
|
|
<div class="line"><a name="l00110"></a><span class="lineno"> 110</span> }</div>
|
|
<div class="line"><a name="l00111"></a><span class="lineno"> 111</span>  </div>
|
|
<div class="line"><a name="l00117"></a><span class="lineno"> 117</span> <span class="keyword">template</span> <<span class="keyword">typename</span> Po<span class="keywordtype">int</span>Cloud></div>
|
|
<div class="line"><a name="l00118"></a><span class="lineno"><a class="line" href="namespacesmall__gicp.html#a0dbfb10fa4698c80cda854ceb6561e50"> 118</a></span> <span class="keywordtype">void</span> <a class="code" href="namespacesmall__gicp.html#a0dbfb10fa4698c80cda854ceb6561e50">estimate_normals</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <span class="keywordtype">int</span> num_neighbors = 20) {</div>
|
|
<div class="line"><a name="l00119"></a><span class="lineno"> 119</span>  estimate_local_features<NormalSetter<PointCloud>>(cloud, num_neighbors);</div>
|
|
<div class="line"><a name="l00120"></a><span class="lineno"> 120</span> }</div>
|
|
<div class="line"><a name="l00121"></a><span class="lineno"> 121</span>  </div>
|
|
<div class="line"><a name="l00128"></a><span class="lineno"> 128</span> <span class="keyword">template</span> <<span class="keyword">typename</span> Po<span class="keywordtype">int</span>Cloud, <span class="keyword">typename</span> KdTree></div>
|
|
<div class="line"><a name="l00129"></a><span class="lineno"><a class="line" href="namespacesmall__gicp.html#af962c06fc787c664b3647b53ec05d3d4"> 129</a></span> <span class="keywordtype">void</span> <a class="code" href="namespacesmall__gicp.html#a0dbfb10fa4698c80cda854ceb6561e50">estimate_normals</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <a class="code" href="structsmall__gicp_1_1KdTree.html">KdTree</a>& kdtree, <span class="keywordtype">int</span> num_neighbors = 20) {</div>
|
|
<div class="line"><a name="l00130"></a><span class="lineno"> 130</span>  estimate_local_features<NormalSetter<PointCloud>>(cloud, kdtree, num_neighbors);</div>
|
|
<div class="line"><a name="l00131"></a><span class="lineno"> 131</span> }</div>
|
|
<div class="line"><a name="l00132"></a><span class="lineno"> 132</span>  </div>
|
|
<div class="line"><a name="l00138"></a><span class="lineno"> 138</span> <span class="keyword">template</span> <<span class="keyword">typename</span> Po<span class="keywordtype">int</span>Cloud></div>
|
|
<div class="line"><a name="l00139"></a><span class="lineno"><a class="line" href="namespacesmall__gicp.html#a273f7ce3257fad200bcf76d2f2357d34"> 139</a></span> <span class="keywordtype">void</span> <a class="code" href="namespacesmall__gicp.html#a273f7ce3257fad200bcf76d2f2357d34">estimate_covariances</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <span class="keywordtype">int</span> num_neighbors = 20) {</div>
|
|
<div class="line"><a name="l00140"></a><span class="lineno"> 140</span>  estimate_local_features<CovarianceSetter<PointCloud>>(cloud, num_neighbors);</div>
|
|
<div class="line"><a name="l00141"></a><span class="lineno"> 141</span> }</div>
|
|
<div class="line"><a name="l00142"></a><span class="lineno"> 142</span>  </div>
|
|
<div class="line"><a name="l00149"></a><span class="lineno"> 149</span> <span class="keyword">template</span> <<span class="keyword">typename</span> Po<span class="keywordtype">int</span>Cloud, <span class="keyword">typename</span> KdTree></div>
|
|
<div class="line"><a name="l00150"></a><span class="lineno"><a class="line" href="namespacesmall__gicp.html#a45e5a9a3ca76f402d8203c9d0c9d341b"> 150</a></span> <span class="keywordtype">void</span> <a class="code" href="namespacesmall__gicp.html#a273f7ce3257fad200bcf76d2f2357d34">estimate_covariances</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <a class="code" href="structsmall__gicp_1_1KdTree.html">KdTree</a>& kdtree, <span class="keywordtype">int</span> num_neighbors = 20) {</div>
|
|
<div class="line"><a name="l00151"></a><span class="lineno"> 151</span>  estimate_local_features<CovarianceSetter<PointCloud>>(cloud, kdtree, num_neighbors);</div>
|
|
<div class="line"><a name="l00152"></a><span class="lineno"> 152</span> }</div>
|
|
<div class="line"><a name="l00153"></a><span class="lineno"> 153</span>  </div>
|
|
<div class="line"><a name="l00159"></a><span class="lineno"> 159</span> <span class="keyword">template</span> <<span class="keyword">typename</span> Po<span class="keywordtype">int</span>Cloud></div>
|
|
<div class="line"><a name="l00160"></a><span class="lineno"><a class="line" href="namespacesmall__gicp.html#a1be100924358411fa177c79973642b1b"> 160</a></span> <span class="keywordtype">void</span> <a class="code" href="namespacesmall__gicp.html#a1be100924358411fa177c79973642b1b">estimate_normals_covariances</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <span class="keywordtype">int</span> num_neighbors = 20) {</div>
|
|
<div class="line"><a name="l00161"></a><span class="lineno"> 161</span>  estimate_local_features<NormalCovarianceSetter<PointCloud>>(cloud, num_neighbors);</div>
|
|
<div class="line"><a name="l00162"></a><span class="lineno"> 162</span> }</div>
|
|
<div class="line"><a name="l00163"></a><span class="lineno"> 163</span>  </div>
|
|
<div class="line"><a name="l00170"></a><span class="lineno"> 170</span> <span class="keyword">template</span> <<span class="keyword">typename</span> Po<span class="keywordtype">int</span>Cloud, <span class="keyword">typename</span> KdTree></div>
|
|
<div class="line"><a name="l00171"></a><span class="lineno"><a class="line" href="namespacesmall__gicp.html#a15382a59a6d46a77bf335387fb81477d"> 171</a></span> <span class="keywordtype">void</span> <a class="code" href="namespacesmall__gicp.html#a1be100924358411fa177c79973642b1b">estimate_normals_covariances</a>(<a class="code" href="structsmall__gicp_1_1PointCloud.html">PointCloud</a>& cloud, <a class="code" href="structsmall__gicp_1_1KdTree.html">KdTree</a>& kdtree, <span class="keywordtype">int</span> num_neighbors = 20) {</div>
|
|
<div class="line"><a name="l00172"></a><span class="lineno"> 172</span>  estimate_local_features<NormalCovarianceSetter<PointCloud>>(cloud, kdtree, num_neighbors);</div>
|
|
<div class="line"><a name="l00173"></a><span class="lineno"> 173</span> }</div>
|
|
<div class="line"><a name="l00174"></a><span class="lineno"> 174</span>  </div>
|
|
<div class="line"><a name="l00175"></a><span class="lineno"> 175</span> } <span class="comment">// namespace small_gicp</span></div>
|
|
<div class="ttc" id="akdtree_8hpp_html"><div class="ttname"><a href="kdtree_8hpp.html">kdtree.hpp</a></div></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_1_1traits_html_a3c4ae38ff52330d6e624f4971c291fac"><div class="ttname"><a href="namespacesmall__gicp_1_1traits.html#a3c4ae38ff52330d6e624f4971c291fac">small_gicp::traits::size</a></div><div class="ttdeci">size_t size(const T &points)</div><div class="ttdoc">Get the number of points.</div><div class="ttdef"><b>Definition:</b> traits.hpp:16</div></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_1_1traits_html_a3e3d8420d93214163aec5cfea1ff6016"><div class="ttname"><a href="namespacesmall__gicp_1_1traits.html#a3e3d8420d93214163aec5cfea1ff6016">small_gicp::traits::set_cov</a></div><div class="ttdeci">void set_cov(T &points, size_t i, const Eigen::Matrix4d &cov)</div><div class="ttdoc">Set i-th covariance. Only the top-left 3x3 matrix should be filled.</div><div class="ttdef"><b>Definition:</b> traits.hpp:76</div></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_1_1traits_html_a5647854e2c446d240366fca679aa8f38"><div class="ttname"><a href="namespacesmall__gicp_1_1traits.html#a5647854e2c446d240366fca679aa8f38">small_gicp::traits::set_normal</a></div><div class="ttdeci">void set_normal(T &points, size_t i, const Eigen::Vector4d &pt)</div><div class="ttdoc">Set i-th normal. (nx, nz, nz, 0)</div><div class="ttdef"><b>Definition:</b> traits.hpp:70</div></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_1_1traits_html_a66b660d3a0c2e81cdf06abc8efebdad1"><div class="ttname"><a href="namespacesmall__gicp_1_1traits.html#a66b660d3a0c2e81cdf06abc8efebdad1">small_gicp::traits::point</a></div><div class="ttdeci">auto point(const T &points, size_t i)</div><div class="ttdoc">Get i-th point. 4D vector is used to take advantage of SIMD intrinsics. The last element must be fill...</div><div class="ttdef"><b>Definition:</b> traits.hpp:40</div></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_1_1traits_html_ac9146681bcef16a4c73d3fd05ad295c7"><div class="ttname"><a href="namespacesmall__gicp_1_1traits.html#ac9146681bcef16a4c73d3fd05ad295c7">small_gicp::traits::cov</a></div><div class="ttdeci">auto cov(const T &points, size_t i)</div><div class="ttdoc">Get i-th covariance. Only the top-left 3x3 matrix is filled, and the bottom row and the right col mus...</div><div class="ttdef"><b>Definition:</b> traits.hpp:52</div></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_1_1traits_html_ad162655a1c08776aa4c94c2fa1ebf863"><div class="ttname"><a href="namespacesmall__gicp_1_1traits.html#ad162655a1c08776aa4c94c2fa1ebf863">small_gicp::traits::normal</a></div><div class="ttdeci">auto normal(const T &points, size_t i)</div><div class="ttdoc">Get i-th normal. 4D vector is used to take advantage of SIMD intrinsics. The last element must be fil...</div><div class="ttdef"><b>Definition:</b> traits.hpp:46</div></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_1_1traits_html_ae35e34068062341652ed9dbf7c3eba25"><div class="ttname"><a href="namespacesmall__gicp_1_1traits.html#ae35e34068062341652ed9dbf7c3eba25">small_gicp::traits::resize</a></div><div class="ttdeci">void resize(T &points, size_t n)</div><div class="ttdoc">Resize the point cloud (this function should resize all attributes)</div><div class="ttdef"><b>Definition:</b> traits.hpp:58</div></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="anamespacesmall__gicp_html_a0dbfb10fa4698c80cda854ceb6561e50"><div class="ttname"><a href="namespacesmall__gicp.html#a0dbfb10fa4698c80cda854ceb6561e50">small_gicp::estimate_normals</a></div><div class="ttdeci">void estimate_normals(PointCloud &cloud, int num_neighbors=20)</div><div class="ttdoc">Estimate point normals.</div><div class="ttdef"><b>Definition:</b> normal_estimation.hpp:118</div></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_html_a1be100924358411fa177c79973642b1b"><div class="ttname"><a href="namespacesmall__gicp.html#a1be100924358411fa177c79973642b1b">small_gicp::estimate_normals_covariances</a></div><div class="ttdeci">void estimate_normals_covariances(PointCloud &cloud, int num_neighbors=20)</div><div class="ttdoc">Estimate point normals and covariances.</div><div class="ttdef"><b>Definition:</b> normal_estimation.hpp:160</div></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_html_a273f7ce3257fad200bcf76d2f2357d34"><div class="ttname"><a href="namespacesmall__gicp.html#a273f7ce3257fad200bcf76d2f2357d34">small_gicp::estimate_covariances</a></div><div class="ttdeci">void estimate_covariances(PointCloud &cloud, int num_neighbors=20)</div><div class="ttdoc">Estimate point covariances.</div><div class="ttdef"><b>Definition:</b> normal_estimation.hpp:139</div></div>
|
|
<div class="ttc" id="anamespacesmall__gicp_html_a501687927eb1e20bc346907c925f9d7d"><div class="ttname"><a href="namespacesmall__gicp.html#a501687927eb1e20bc346907c925f9d7d">small_gicp::estimate_local_features</a></div><div class="ttdeci">void estimate_local_features(PointCloud &cloud, Tree &kdtree, int num_neighbors, size_t point_index)</div><div class="ttdef"><b>Definition:</b> normal_estimation.hpp:66</div></div>
|
|
<div class="ttc" id="astructsmall__gicp_1_1CovarianceSetter_html"><div class="ttname"><a href="structsmall__gicp_1_1CovarianceSetter.html">small_gicp::CovarianceSetter</a></div><div class="ttdoc">Computes point covariances from eigenvectors and sets them to the point cloud.</div><div class="ttdef"><b>Definition:</b> normal_estimation.hpp:31</div></div>
|
|
<div class="ttc" id="astructsmall__gicp_1_1CovarianceSetter_html_a6d659b970363e842cbeb28d487cbdea2"><div class="ttname"><a href="structsmall__gicp_1_1CovarianceSetter.html#a6d659b970363e842cbeb28d487cbdea2">small_gicp::CovarianceSetter::set</a></div><div class="ttdeci">static void set(PointCloud &cloud, size_t i, const Eigen::Matrix3d &eigenvectors)</div><div class="ttdoc">Compute and set the covariance to the point cloud.</div><div class="ttdef"><b>Definition:</b> normal_estimation.hpp:40</div></div>
|
|
<div class="ttc" id="astructsmall__gicp_1_1CovarianceSetter_html_a7dad95aaf6108d09cd6865aa4ce48236"><div class="ttname"><a href="structsmall__gicp_1_1CovarianceSetter.html#a7dad95aaf6108d09cd6865aa4ce48236">small_gicp::CovarianceSetter::set_invalid</a></div><div class="ttdeci">static void set_invalid(PointCloud &cloud, size_t i)</div><div class="ttdoc">Handle invalid case (too few points).</div><div class="ttdef"><b>Definition:</b> normal_estimation.hpp:33</div></div>
|
|
<div class="ttc" id="astructsmall__gicp_1_1KdTree_html"><div class="ttname"><a href="structsmall__gicp_1_1KdTree.html">small_gicp::KdTree</a></div><div class="ttdoc">"Safe" KdTree that holds the ownership of the input points.</div><div class="ttdef"><b>Definition:</b> kdtree.hpp:245</div></div>
|
|
<div class="ttc" id="astructsmall__gicp_1_1NormalCovarianceSetter_html"><div class="ttname"><a href="structsmall__gicp_1_1NormalCovarianceSetter.html">small_gicp::NormalCovarianceSetter</a></div><div class="ttdoc">Computes point normals and covariances from eigenvectors and sets them to the point cloud.</div><div class="ttdef"><b>Definition:</b> normal_estimation.hpp:51</div></div>
|
|
<div class="ttc" id="astructsmall__gicp_1_1NormalCovarianceSetter_html_a5ccf6387684fd6d8c6fb1627a3f1e112"><div class="ttname"><a href="structsmall__gicp_1_1NormalCovarianceSetter.html#a5ccf6387684fd6d8c6fb1627a3f1e112">small_gicp::NormalCovarianceSetter::set</a></div><div class="ttdeci">static void set(PointCloud &cloud, size_t i, const Eigen::Matrix3d &eigenvectors)</div><div class="ttdoc">Compute and set the normal and covariance to the point cloud.</div><div class="ttdef"><b>Definition:</b> normal_estimation.hpp:59</div></div>
|
|
<div class="ttc" id="astructsmall__gicp_1_1NormalCovarianceSetter_html_a88d3357939cc97beaf51e659b0f0b075"><div class="ttname"><a href="structsmall__gicp_1_1NormalCovarianceSetter.html#a88d3357939cc97beaf51e659b0f0b075">small_gicp::NormalCovarianceSetter::set_invalid</a></div><div class="ttdeci">static void set_invalid(PointCloud &cloud, size_t i)</div><div class="ttdoc">Handle invalid case (too few points).</div><div class="ttdef"><b>Definition:</b> normal_estimation.hpp:53</div></div>
|
|
<div class="ttc" id="astructsmall__gicp_1_1NormalSetter_html"><div class="ttname"><a href="structsmall__gicp_1_1NormalSetter.html">small_gicp::NormalSetter</a></div><div class="ttdoc">Computes point normals from eigenvectors and sets them to the point cloud.</div><div class="ttdef"><b>Definition:</b> normal_estimation.hpp:13</div></div>
|
|
<div class="ttc" id="astructsmall__gicp_1_1NormalSetter_html_aa779d77c915daa28a4a71865da8c8c1f"><div class="ttname"><a href="structsmall__gicp_1_1NormalSetter.html#aa779d77c915daa28a4a71865da8c8c1f">small_gicp::NormalSetter::set_invalid</a></div><div class="ttdeci">static void set_invalid(PointCloud &cloud, size_t i)</div><div class="ttdoc">Handle invalid case (too few points).</div><div class="ttdef"><b>Definition:</b> normal_estimation.hpp:15</div></div>
|
|
<div class="ttc" id="astructsmall__gicp_1_1NormalSetter_html_aaa6b4e2dad59c1f1fac6cafb0e2240aa"><div class="ttname"><a href="structsmall__gicp_1_1NormalSetter.html#aaa6b4e2dad59c1f1fac6cafb0e2240aa">small_gicp::NormalSetter::set</a></div><div class="ttdeci">static void set(PointCloud &cloud, size_t i, const Eigen::Matrix3d &eigenvectors)</div><div class="ttdoc">Compute and set the normal to the point cloud.</div><div class="ttdef"><b>Definition:</b> normal_estimation.hpp:18</div></div>
|
|
<div class="ttc" id="astructsmall__gicp_1_1PointCloud_html"><div class="ttname"><a href="structsmall__gicp_1_1PointCloud.html">small_gicp::PointCloud</a></div><div class="ttdoc">Point cloud.</div><div class="ttdef"><b>Definition:</b> point_cloud.hpp:15</div></div>
|
|
<div class="ttc" id="astructsmall__gicp_1_1UnsafeKdTree_html"><div class="ttname"><a href="structsmall__gicp_1_1UnsafeKdTree.html">small_gicp::UnsafeKdTree</a></div><div class="ttdoc">"Unsafe" KdTree.</div><div class="ttdef"><b>Definition:</b> kdtree.hpp:137</div></div>
|
|
</div><!-- fragment --></div><!-- contents -->
|
|
<!-- start footer part -->
|
|
<hr class="footer"/><address class="footer"><small>
|
|
Generated by <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>
|