mirror
/
small_gicp
mirror of https://github.com/koide3/small_gicp.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
small_gicp/doc_cpp/index.html

489 lines
44 KiB
HTML
Raw Blame History

<!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: small_gicp</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>
</div><!-- top -->
<!-- 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 class="PageDoc"><div class="header">
<div class="headertitle">
<div class="title"><a class="el" href="namespacesmall__gicp.html">small_gicp</a> </div> </div>
</div><!--header-->
<div class="contents">
<div class="textblock"><p>Expand <a class="anchor" id="md__home_runner_work_small_gicp_small_gicp_README"></a> <b><a class="el" href="namespacesmall__gicp.html">small_gicp</a></b> is a header-only C++ library providing efficient and parallelized algorithms for fine point cloud registration (ICP, Point-to-Plane ICP, GICP, VGICP, etc.). It is a refined and optimized version of its predecessor, <a href="https://github.com/SMRT-AIST/fast_gicp">fast_gicp</a>, re-written from scratch with the following features.</p>
<ul>
<li><b>Highly Optimized</b> : The core registration algorithm implementation has been further optimized from fast_gicp, achieving up to <b>2x speed gain</b>.</li>
<li><b>Fully parallerized</b> : <a class="el" href="namespacesmall__gicp.html">small_gicp</a> offers parallel implementations of several preprocessing algorithms, making the entire registration process parallelized (e.g., Downsampling, KdTree construction, Normal/Covariance estimation). It supports <a href="https://www.openmp.org/">OpenMP</a> and <a href="https://github.com/oneapi-src/oneTBB">Intel TBB</a> as parallelism backends.</li>
<li><b>Minimum dependencies</b> : The library requires only <a href="https://eigen.tuxfamily.org/">Eigen</a> along with the bundled <a href="https://github.com/jlblancoc/nanoflann">nanoflann</a> and <a href="https://github.com/strasdat/Sophus">Sophus</a>. Optionally, it supports a <a href="https://pointclouds.org/">PCL</a> registration interface for use as a drop-in replacement</li>
<li><b>Customizable</b> : <a class="el" href="namespacesmall__gicp.html">small_gicp</a> allows the integration of any custom point cloud class into the registration algorithm via traits. Its template-based implementation enables customization of the registration process with original correspondence estimators and registration factors.</li>
<li><b>Python bindings</b> : By being isolated from PCL, <a class="el" href="namespacesmall__gicp.html">small_gicp</a>'s Python bindings are more portable and can be used seamlessly with other libraries such as Open3D.</li>
</ul>
<p>Note that GPU-based implementations are NOT included in this package.</p>
<p>If you find this package useful for your project, please consider leaving a comment <a href="https://github.com/koide3/small_gicp/issues/3">here</a>. It would help the author receive recognition in his organization and keep working on this project.</p>
<p><a href="https://github.com/koide3/small_gicp/actions/workflows/build-linux.yml"><img src="https://github.com/koide3/small_gicp/actions/workflows/build-linux.yml/badge.svg" alt="Build(Linux)" style="pointer-events: none;" class="inline"/></a> <a href="https://github.com/koide3/small_gicp/actions/workflows/build-macos.yml"><img src="https://github.com/koide3/small_gicp/actions/workflows/build-macos.yml/badge.svg" alt="macos" style="pointer-events: none;" class="inline"/></a> <a href="https://github.com/koide3/small_gicp/actions/workflows/build-windows.yml"><img src="https://github.com/koide3/small_gicp/actions/workflows/build-windows.yml/badge.svg" alt="Build(Windows)" style="pointer-events: none;" class="inline"/></a> <a href="https://github.com/koide3/small_gicp/actions/workflows/test.yml"><img src="https://github.com/koide3/small_gicp/actions/workflows/test.yml/badge.svg" alt="Test" style="pointer-events: none;" class="inline"/></a> <a href="https://codecov.io/gh/koide3/small_gicp"><img src="https://codecov.io/gh/koide3/small_gicp/graph/badge.svg?token=PCVIUP2Z33" alt="codecov" style="pointer-events: none;" class="inline"/></a></p>
<h1><a class="anchor" id="autotoc_md1"></a>
Requirements</h1>
<p>This library uses C++17 features. The PCL interface is not compatible with PCL older than 1.11 that uses <code>boost::shared_ptr</code>.</p>
<h1><a class="anchor" id="autotoc_md2"></a>
Dependencies</h1>
<ul>
<li>[Mandatory] <a href="https://eigen.tuxfamily.org/">Eigen</a>, <a href="https://github.com/jlblancoc/nanoflann">nanoflann</a> (<a href="include/small_gicp/ann/kdtree.hpp">bundled</a>), <a href="https://github.com/strasdat/Sophus">Sophus</a> (<a href="include/small_gicp/util/lie.hpp">bundled</a>)</li>
<li>[Optional] <a href="https://www.openmp.org/">OpenMP</a>, <a href="https://www.intel.com/content/www/us/en/developer/tools/oneapi/onetbb.html">Intel TBB</a>, <a href="https://pointclouds.org/">PCL</a></li>
</ul>
<h1><a class="anchor" id="autotoc_md3"></a>
Installation</h1>
<h2><a class="anchor" id="autotoc_md4"></a>
C++</h2>
<p><a class="el" href="namespacesmall__gicp.html">small_gicp</a> is a header-only library. You can just download and drop it in your project directory to use it.</p>
<p>If you need only basic point cloud registration functions, you can build and install the helper library as follows.</p>
<div class="fragment"><div class="line">sudo apt-get install libeigen3-dev libomp-dev</div>
<div class="line"> </div>
<div class="line">cd small_gicp</div>
<div class="line">mkdir build &amp;&amp; cd build</div>
<div class="line">cmake .. -DCMAKE_BUILD_TYPE=Release &amp;&amp; make -j</div>
<div class="line">sudo make install</div>
</div><!-- fragment --><h2><a class="anchor" id="autotoc_md5"></a>
Python (Linux / Windows / MacOS)</h2>
<h3><a class="anchor" id="autotoc_md6"></a>
Install from &lt;a href="https://pypi.org/project/small-gicp/"&gt;PyPI&lt;/a&gt;</h3>
<div class="fragment"><div class="line">pip install small_gicp</div>
</div><!-- fragment --><h3><a class="anchor" id="autotoc_md7"></a>
Install from source</h3>
<div class="fragment"><div class="line">cd small_gicp</div>
<div class="line">pip install .</div>
<div class="line"> </div>
<div class="line"># [Optional (linux)] Install stubs for autocomplete (If you know a better way, let me know...)</div>
<div class="line">pip install pybind11-stubgen</div>
<div class="line">cd ~/.local/lib/python3.10/site-packages</div>
<div class="line">pybind11-stubgen -o . --ignore-invalid=all small_gicp</div>
</div><!-- fragment --><h1><a class="anchor" id="autotoc_md8"></a>
Documentation</h1>
<ul>
<li>Auto-generated docs: <a href="https://koide3.github.io/small_gicp/doc_cpp/index.html">C++</a> <a href="https://koide3.github.io/small_gicp/doc_py/index.html">Python</a></li>
</ul>
<h1><a class="anchor" id="autotoc_md9"></a>
Usage (C++)</h1>
<p>The following examples assume <code>using namespace <a class="el" href="namespacesmall__gicp.html">small_gicp</a></code> is placed somewhere.</p>
<h2><a class="anchor" id="autotoc_md10"></a>
Using helper library (&lt;a href="src/example/01_basic_registration.cpp"&gt;01_basic_registration.cpp&lt;/a&gt;)</h2>
<p>The helper library (<code><a class="el" href="registration__helper_8hpp.html">registration_helper.hpp</a></code>) enables easily processing point clouds represented as <code>std::vector&lt;Eigen::Vector(3|4)(f|d)&gt;</code>. &lt;details&gt; </p>
<p><code><a class="el" href="namespacesmall__gicp.html#aaaac24cfd2f82c7978ccc8cb66474cf4" title="Align point clouds.">small_gicp::align</a></code> takes two point clouds (<code>std::vectors</code> of <code>Eigen::Vector(3|4)(f|d)</code>) and returns a registration result (estimated transformation and some information on the optimization result). This is the easiest way to use <a class="el" href="namespacesmall__gicp.html">small_gicp</a> but causes an overhead for duplicated preprocessing.</p>
<div class="fragment"><div class="line"><span class="preprocessor">#include &lt;<a class="code" href="registration__helper_8hpp.html">small_gicp/registration/registration_helper.hpp</a>&gt;</span></div>
<div class="line"> </div>
<div class="line">std::vector&lt;Eigen::Vector3d&gt; target_points = ...; <span class="comment">// Any of Eigen::Vector(3|4)(f|d) can be used</span></div>
<div class="line">std::vector&lt;Eigen::Vector3d&gt; source_points = ...; <span class="comment">// </span></div>
<div class="line"> </div>
<div class="line">RegistrationSetting setting;</div>
<div class="line">setting.num_threads = 4; <span class="comment">// Number of threads to be used</span></div>
<div class="line">setting.downsampling_resolution = 0.25; <span class="comment">// Downsampling resolution</span></div>
<div class="line">setting.max_correspondence_distance = 1.0; <span class="comment">// Maximum correspondence distance between points (e.g., triming threshold)</span></div>
<div class="line"> </div>
<div class="line">Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity();</div>
<div class="line">RegistrationResult result = <a class="code" href="namespacesmall__gicp.html#aaaac24cfd2f82c7978ccc8cb66474cf4">align</a>(target_points, source_points, init_T_target_source, setting);</div>
<div class="line"> </div>
<div class="line">Eigen::Isometry3d T = result.T_target_source; <span class="comment">// Estimated transformation</span></div>
<div class="line"><span class="keywordtype">size_t</span> num_inliers = result.num_inliers; <span class="comment">// Number of inlier source points</span></div>
<div class="line">Eigen::Matrix&lt;double, 6, 6&gt; H = result.H; <span class="comment">// Final Hessian matrix (6x6)</span></div>
<div class="ttc" id="anamespacesmall__gicp_html_aaaac24cfd2f82c7978ccc8cb66474cf4"><div class="ttname"><a href="namespacesmall__gicp.html#aaaac24cfd2f82c7978ccc8cb66474cf4">small_gicp::align</a></div><div class="ttdeci">RegistrationResult align(const std::vector&lt; Eigen::Matrix&lt; T, D, 1 &gt;&gt; &amp;target, const std::vector&lt; Eigen::Matrix&lt; T, D, 1 &gt;&gt; &amp;source, const Eigen::Isometry3d &amp;init_T=Eigen::Isometry3d::Identity(), const RegistrationSetting &amp;setting=RegistrationSetting())</div><div class="ttdoc">Align point clouds.</div></div>
<div class="ttc" id="aregistration__helper_8hpp_html"><div class="ttname"><a href="registration__helper_8hpp.html">registration_helper.hpp</a></div></div>
</div><!-- fragment --><p>There is also a way to perform preprocessing and registration separately. This enables saving time for preprocessing in case registration is performed several times for the same point cloud (e.g., typical odometry estimation based on scan-to-scan matching).</p>
<div class="fragment"><div class="line"><span class="preprocessor">#include &lt;<a class="code" href="registration__helper_8hpp.html">small_gicp/registration/registration_helper.hpp</a>&gt;</span></div>
<div class="line"> </div>
<div class="line">std::vector&lt;Eigen::Vector3d&gt; target_points = ...; <span class="comment">// Any of Eigen::Vector(3|4)(f|d) can be used</span></div>
<div class="line">std::vector&lt;Eigen::Vector3d&gt; source_points = ...; <span class="comment">// </span></div>
<div class="line"> </div>
<div class="line"><span class="keywordtype">int</span> num_threads = 4; <span class="comment">// Number of threads to be used</span></div>
<div class="line"><span class="keywordtype">double</span> downsampling_resolution = 0.25; <span class="comment">// Downsampling resolution</span></div>
<div class="line"><span class="keywordtype">int</span> num_neighbors = 10; <span class="comment">// Number of neighbor points used for normal and covariance estimation</span></div>
<div class="line"> </div>
<div class="line"><span class="comment">// std::pair&lt;PointCloud::Ptr, KdTree&lt;PointCloud&gt;::Ptr&gt;</span></div>
<div class="line"><span class="keyword">auto</span> [target, target_tree] = <a class="code" href="namespacesmall__gicp.html#a30d5bdb69c71e494dc85a8556faae960">preprocess_points</a>(target_points, downsampling_resolution, num_neighbors, num_threads);</div>
<div class="line"><span class="keyword">auto</span> [source, source_tree] = <a class="code" href="namespacesmall__gicp.html#a30d5bdb69c71e494dc85a8556faae960">preprocess_points</a>(source_points, downsampling_resolution, num_neighbors, num_threads);</div>
<div class="line"> </div>
<div class="line">RegistrationSetting setting;</div>
<div class="line">setting.num_threads = num_threads;</div>
<div class="line">setting.max_correspondence_distance = 1.0; <span class="comment">// Maximum correspondence distance between points (e.g., triming threshold)</span></div>
<div class="line"> </div>
<div class="line">Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity();</div>
<div class="line">RegistrationResult result = <a class="code" href="namespacesmall__gicp.html#aaaac24cfd2f82c7978ccc8cb66474cf4">align</a>(*target, *source, *target_tree, init_T_target_source, setting);</div>
<div class="line"> </div>
<div class="line">Eigen::Isometry3d T = result.T_target_source; <span class="comment">// Estimated transformation</span></div>
<div class="line"><span class="keywordtype">size_t</span> num_inliers = result.num_inliers; <span class="comment">// Number of inlier source points</span></div>
<div class="line">Eigen::Matrix&lt;double, 6, 6&gt; H = result.H; <span class="comment">// Final Hessian matrix (6x6)</span></div>
<div class="ttc" id="anamespacesmall__gicp_html_a30d5bdb69c71e494dc85a8556faae960"><div class="ttname"><a href="namespacesmall__gicp.html#a30d5bdb69c71e494dc85a8556faae960">small_gicp::preprocess_points</a></div><div class="ttdeci">std::pair&lt; PointCloud::Ptr, std::shared_ptr&lt; KdTree&lt; PointCloud &gt; &gt; &gt; preprocess_points(const PointCloud &amp;points, double downsampling_resolution, int num_neighbors=10, int num_threads=4)</div><div class="ttdoc">Preprocess point cloud (downsampling, kdtree creation, and normal and covariance estimation).</div></div>
</div><!-- fragment --><p>&lt;/details&gt;</p>
<h2><a class="anchor" id="autotoc_md11"></a>
Using PCL interface (&lt;a href="src/example/02_basic_registration_pcl.cpp"&gt;02_basic_registration_pcl.cpp&lt;/a&gt;)</h2>
<p>The PCL interface allows using <a class="el" href="namespacesmall__gicp.html">small_gicp</a> as a drop-in replacement for <code>pcl::Registration</code>. It is also possible to directly feed <code>pcl::PointCloud</code> to algorithms implemented in <a class="el" href="namespacesmall__gicp.html">small_gicp</a>.</p>
<p>&lt;details&gt; </p>
<p>Expand </p>
<div class="fragment"><div class="line"><span class="preprocessor">#include &lt;<a class="code" href="pcl__registration_8hpp.html">small_gicp/pcl/pcl_registration.hpp</a>&gt;</span></div>
<div class="line"> </div>
<div class="line">pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr raw_target = ...;</div>
<div class="line">pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr raw_source = ...;</div>
<div class="line"> </div>
<div class="line"><span class="comment">// small_gicp::voxelgrid_downsampling can directly operate on pcl::PointCloud.</span></div>
<div class="line">pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr target = <a class="code" href="namespacesmall__gicp.html#afedf95a15244b9cb2aa0e105b9a501ee">voxelgrid_sampling_omp</a>(*raw_target, 0.25);</div>
<div class="line">pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr source = <a class="code" href="namespacesmall__gicp.html#afedf95a15244b9cb2aa0e105b9a501ee">voxelgrid_sampling_omp</a>(*raw_source, 0.25);</div>
<div class="line"> </div>
<div class="line"><span class="comment">// RegistrationPCL is derived from pcl::Registration and has mostly the same interface as pcl::GeneralizedIterativeClosestPoint.</span></div>
<div class="line">RegistrationPCL&lt;pcl::PointXYZ, pcl::PointXYZ&gt; reg;</div>
<div class="line">reg.setNumThreads(4);</div>
<div class="line">reg.setCorrespondenceRandomness(20);</div>
<div class="line">reg.setMaxCorrespondenceDistance(1.0);</div>
<div class="line">reg.setVoxelResolution(1.0);</div>
<div class="line">reg.setRegistrationType(<span class="stringliteral">&quot;VGICP&quot;</span>); <span class="comment">// or &quot;GICP&quot; (default = &quot;GICP&quot;)</span></div>
<div class="line"> </div>
<div class="line"><span class="comment">// Set input point clouds.</span></div>
<div class="line">reg.setInputTarget(target);</div>
<div class="line">reg.setInputSource(source);</div>
<div class="line"> </div>
<div class="line"><span class="comment">// Align point clouds.</span></div>
<div class="line"><span class="keyword">auto</span> aligned = pcl::make_shared&lt;pcl::PointCloud&lt;pcl::PointXYZ&gt;&gt;();</div>
<div class="line">reg.align(*aligned);</div>
<div class="line"> </div>
<div class="line"><span class="comment">// Swap source and target and align again.</span></div>
<div class="line"><span class="comment">// This is useful when you want to re-use preprocessed point clouds for successive registrations (e.g., odometry estimation).</span></div>
<div class="line">reg.swapSourceAndTarget();</div>
<div class="line">reg.align(*aligned);</div>
<div class="ttc" id="anamespacesmall__gicp_html_afedf95a15244b9cb2aa0e105b9a501ee"><div class="ttname"><a href="namespacesmall__gicp.html#afedf95a15244b9cb2aa0e105b9a501ee">small_gicp::voxelgrid_sampling_omp</a></div><div class="ttdeci">std::shared_ptr&lt; OutputPointCloud &gt; voxelgrid_sampling_omp(const InputPointCloud &amp;points, double leaf_size, int num_threads=4)</div><div class="ttdoc">Voxel grid downsampling with OpenMP backend.</div><div class="ttdef"><b>Definition:</b> downsampling_omp.hpp:26</div></div>
<div class="ttc" id="apcl__registration_8hpp_html"><div class="ttname"><a href="pcl__registration_8hpp.html">pcl_registration.hpp</a></div></div>
</div><!-- fragment --><p>It is also possible to directly feed <code>pcl::PointCloud</code> to <code><a class="el" href="structsmall__gicp_1_1Registration.html" title="Point cloud registration.">small_gicp::Registration</a></code>. Because all preprocessed data are exposed in this way, you can easily re-use them to obtain the best efficiency.</p>
<div class="fragment"><div class="line"><span class="preprocessor">#include &lt;<a class="code" href="pcl__point_8hpp.html">small_gicp/pcl/pcl_point.hpp</a>&gt;</span></div>
<div class="line"><span class="preprocessor">#include &lt;<a class="code" href="pcl__point__traits_8hpp.html">small_gicp/pcl/pcl_point_traits.hpp</a>&gt;</span></div>
<div class="line"> </div>
<div class="line">pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr raw_target = ...;</div>
<div class="line">pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr raw_source = ...;</div>
<div class="line"> </div>
<div class="line"><span class="comment">// Downsample points and convert them into pcl::PointCloud&lt;pcl::PointCovariance&gt;.</span></div>
<div class="line">pcl::PointCloud&lt;pcl::PointCovariance&gt;::Ptr target = voxelgrid_sampling_omp&lt;pcl::PointCloud&lt;pcl::PointXYZ&gt;, pcl::PointCloud&lt;pcl::PointCovariance&gt;&gt;(*raw_target, 0.25);</div>
<div class="line">pcl::PointCloud&lt;pcl::PointCovariance&gt;::Ptr source = voxelgrid_sampling_omp&lt;pcl::PointCloud&lt;pcl::PointXYZ&gt;, pcl::PointCloud&lt;pcl::PointCovariance&gt;&gt;(*raw_source, 0.25);</div>
<div class="line"> </div>
<div class="line"><span class="comment">// Estimate covariances of points.</span></div>
<div class="line"><span class="keyword">const</span> <span class="keywordtype">int</span> num_threads = 4;</div>
<div class="line"><span class="keyword">const</span> <span class="keywordtype">int</span> num_neighbors = 20;</div>
<div class="line"><a class="code" href="namespacesmall__gicp.html#adf8333664b03248cbcb20a9f17346b72">estimate_covariances_omp</a>(*target, num_neighbors, num_threads);</div>
<div class="line"><a class="code" href="namespacesmall__gicp.html#adf8333664b03248cbcb20a9f17346b72">estimate_covariances_omp</a>(*source, num_neighbors, num_threads);</div>
<div class="line"> </div>
<div class="line"><span class="comment">// Create KdTree for target and source.</span></div>
<div class="line"><span class="keyword">auto</span> target_tree = std::make_shared&lt;KdTree&lt;pcl::PointCloud&lt;pcl::PointCovariance&gt;&gt;&gt;(target, KdTreeBuilderOMP(num_threads));</div>
<div class="line"><span class="keyword">auto</span> source_tree = std::make_shared&lt;KdTree&lt;pcl::PointCloud&lt;pcl::PointCovariance&gt;&gt;&gt;(source, KdTreeBuilderOMP(num_threads));</div>
<div class="line"> </div>
<div class="line">Registration&lt;GICPFactor, ParallelReductionOMP&gt; registration;</div>
<div class="line">registration.reduction.num_threads = num_threads;</div>
<div class="line">registration.rejector.max_dist_sq = 1.0;</div>
<div class="line"> </div>
<div class="line"><span class="comment">// Align point clouds. Note that the input point clouds are pcl::PointCloud&lt;pcl::PointCovariance&gt;.</span></div>
<div class="line"><span class="keyword">auto</span> result = registration.align(*target, *source, *target_tree, Eigen::Isometry3d::Identity());</div>
<div class="ttc" id="anamespacesmall__gicp_html_adf8333664b03248cbcb20a9f17346b72"><div class="ttname"><a href="namespacesmall__gicp.html#adf8333664b03248cbcb20a9f17346b72">small_gicp::estimate_covariances_omp</a></div><div class="ttdeci">void estimate_covariances_omp(PointCloud &amp;cloud, int num_neighbors=20, int num_threads=4)</div><div class="ttdoc">Estimate point covariances with OpenMP.</div><div class="ttdef"><b>Definition:</b> normal_estimation_omp.hpp:58</div></div>
<div class="ttc" id="apcl__point_8hpp_html"><div class="ttname"><a href="pcl__point_8hpp.html">pcl_point.hpp</a></div></div>
<div class="ttc" id="apcl__point__traits_8hpp_html"><div class="ttname"><a href="pcl__point__traits_8hpp.html">pcl_point_traits.hpp</a></div></div>
</div><!-- fragment --><p>&lt;/details&gt;</p>
<h2><a class="anchor" id="autotoc_md12"></a>
Using &lt;tt&gt;Registration&lt;/tt&gt; template (&lt;a href="src/example/03_registration_template.cpp"&gt;03_registration_template.cpp&lt;/a&gt;)</h2>
<p>If you want to fine-control and customize the registration process, use <code><a class="el" href="structsmall__gicp_1_1Registration.html" title="Point cloud registration.">small_gicp::Registration</a></code> template that allows modifying the inner algorithms and parameters. &lt;details&gt; </p>
<p>Expand </p>
<div class="fragment"><div class="line"><span class="preprocessor">#include &lt;<a class="code" href="kdtree__omp_8hpp.html">small_gicp/ann/kdtree_omp.hpp</a>&gt;</span></div>
<div class="line"><span class="preprocessor">#include &lt;<a class="code" href="point__cloud_8hpp.html">small_gicp/points/point_cloud.hpp</a>&gt;</span></div>
<div class="line"><span class="preprocessor">#include &lt;<a class="code" href="gicp__factor_8hpp.html">small_gicp/factors/gicp_factor.hpp</a>&gt;</span></div>
<div class="line"><span class="preprocessor">#include &lt;<a class="code" href="normal__estimation__omp_8hpp.html">small_gicp/util/normal_estimation_omp.hpp</a>&gt;</span></div>
<div class="line"><span class="preprocessor">#include &lt;<a class="code" href="reduction__omp_8hpp.html">small_gicp/registration/reduction_omp.hpp</a>&gt;</span></div>
<div class="line"><span class="preprocessor">#include &lt;<a class="code" href="registration_8hpp.html">small_gicp/registration/registration.hpp</a>&gt;</span></div>
<div class="line"> </div>
<div class="line">std::vector&lt;Eigen::Vector3d&gt; target_points = ...; <span class="comment">// Any of Eigen::Vector(3|4)(f|d) can be used</span></div>
<div class="line">std::vector&lt;Eigen::Vector3d&gt; source_points = ...; <span class="comment">// </span></div>
<div class="line"> </div>
<div class="line"><span class="keywordtype">int</span> num_threads = 4;</div>
<div class="line"><span class="keywordtype">double</span> downsampling_resolution = 0.25;</div>
<div class="line"><span class="keywordtype">int</span> num_neighbors = 10;</div>
<div class="line"><span class="keywordtype">double</span> max_correspondence_distance = 1.0;</div>
<div class="line"> </div>
<div class="line"><span class="comment">// Convert to small_gicp::PointCloud</span></div>
<div class="line"><span class="keyword">auto</span> target = std::make_shared&lt;PointCloud&gt;(target_points);</div>
<div class="line"><span class="keyword">auto</span> source = std::make_shared&lt;PointCloud&gt;(source_points);</div>
<div class="line"> </div>
<div class="line"><span class="comment">// Downsampling</span></div>
<div class="line">target = <a class="code" href="namespacesmall__gicp.html#afedf95a15244b9cb2aa0e105b9a501ee">voxelgrid_sampling_omp</a>(*target, downsampling_resolution, num_threads);</div>
<div class="line">source = <a class="code" href="namespacesmall__gicp.html#afedf95a15244b9cb2aa0e105b9a501ee">voxelgrid_sampling_omp</a>(*source, downsampling_resolution, num_threads);</div>
<div class="line"> </div>
<div class="line"><span class="comment">// Create KdTree</span></div>
<div class="line"><span class="keyword">auto</span> target_tree = std::make_shared&lt;KdTree&lt;PointCloud&gt;&gt;(target, KdTreeBuilderOMP(num_threads));</div>
<div class="line"><span class="keyword">auto</span> source_tree = std::make_shared&lt;KdTree&lt;PointCloud&gt;&gt;(source, KdTreeBuilderOMP(num_threads));</div>
<div class="line"> </div>
<div class="line"><span class="comment">// Estimate point covariances</span></div>
<div class="line"><a class="code" href="namespacesmall__gicp.html#adf8333664b03248cbcb20a9f17346b72">estimate_covariances_omp</a>(*target, *target_tree, num_neighbors, num_threads);</div>
<div class="line"><a class="code" href="namespacesmall__gicp.html#adf8333664b03248cbcb20a9f17346b72">estimate_covariances_omp</a>(*source, *source_tree, num_neighbors, num_threads);</div>
<div class="line"> </div>
<div class="line"><span class="comment">// GICP + OMP-based parallel reduction</span></div>
<div class="line">Registration&lt;GICPFactor, ParallelReductionOMP&gt; registration;</div>
<div class="line">registration.reduction.num_threads = num_threads;</div>
<div class="line">registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance;</div>
<div class="line"> </div>
<div class="line"><span class="comment">// Align point clouds</span></div>
<div class="line">Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity();</div>
<div class="line"><span class="keyword">auto</span> result = registration.align(*target, *source, *target_tree, init_T_target_source);</div>
<div class="line"> </div>
<div class="line">Eigen::Isometry3d T = result.T_target_source; <span class="comment">// Estimated transformation</span></div>
<div class="line"><span class="keywordtype">size_t</span> num_inliers = result.num_inliers; <span class="comment">// Number of inlier source points</span></div>
<div class="line">Eigen::Matrix&lt;double, 6, 6&gt; H = result.H; <span class="comment">// Final Hessian matrix (6x6)</span></div>
<div class="ttc" id="agicp__factor_8hpp_html"><div class="ttname"><a href="gicp__factor_8hpp.html">gicp_factor.hpp</a></div></div>
<div class="ttc" id="akdtree__omp_8hpp_html"><div class="ttname"><a href="kdtree__omp_8hpp.html">kdtree_omp.hpp</a></div></div>
<div class="ttc" id="anormal__estimation__omp_8hpp_html"><div class="ttname"><a href="normal__estimation__omp_8hpp.html">normal_estimation_omp.hpp</a></div></div>
<div class="ttc" id="apoint__cloud_8hpp_html"><div class="ttname"><a href="point__cloud_8hpp.html">point_cloud.hpp</a></div></div>
<div class="ttc" id="areduction__omp_8hpp_html"><div class="ttname"><a href="reduction__omp_8hpp.html">reduction_omp.hpp</a></div></div>
<div class="ttc" id="aregistration_8hpp_html"><div class="ttname"><a href="registration_8hpp.html">registration.hpp</a></div></div>
</div><!-- fragment --><p>See <a href="src/example/03_registration_template.cpp">03_registration_template.cpp</a> for more detailed customization examples.</p>
<p>&lt;/details&gt;</p>
<h2><a class="anchor" id="autotoc_md13"></a>
Cookbook</h2>
<ul>
<li><a href="src/benchmark/odometry_benchmark_small_gicp_omp.cpp">Standard scan-to-scan GICP matching odometry</a></li>
<li><a href="src/benchmark/odometry_benchmark_small_gicp_tbb_flow.cpp">Extremely scalable scan-to-scan matching odometry with data flow graph</a></li>
<li><a href="src/benchmark/odometry_benchmark_small_gicp_model_tbb.cpp">Scan-to-model matching odometry with incremental voxelmap (GICP + iVox)</a></li>
<li><a href="src/benchmark/odometry_benchmark_small_vgicp_model_tbb.cpp">Scan-to-model matching odometry with incremental Gaussian voxelmap (VGICP)</a></li>
</ul>
<h1><a class="anchor" id="autotoc_md14"></a>
Usage (Python) &lt;a href="src/example/basic_registration.py"&gt;basic_registration.py&lt;/a&gt;</h1>
<p>&lt;details&gt; </p>
<p>Expand </p>
<p>Example A : Perform registration with numpy arrays</p>
<div class="fragment"><div class="line"># Align two point clouds using various ICP-like algorithms.</div>
<div class="line"># </div>
<div class="line"># Parameters</div>
<div class="line"># ----------</div>
<div class="line"># target_points : NDArray[np.float64]</div>
<div class="line"># Nx3 or Nx4 matrix representing the target point cloud.</div>
<div class="line"># source_points : NDArray[np.float64]</div>
<div class="line"># Nx3 or Nx4 matrix representing the source point cloud.</div>
<div class="line"># init_T_target_source : np.ndarray[np.float64]</div>
<div class="line"># 4x4 matrix representing the initial transformation from target to source.</div>
<div class="line"># registration_type : str = &#39;GICP&#39;</div>
<div class="line"># Type of registration algorithm to use (&#39;ICP&#39;, &#39;PLANE_ICP&#39;, &#39;GICP&#39;, &#39;VGICP&#39;).</div>
<div class="line"># voxel_resolution : float = 1.0</div>
<div class="line"># Resolution of voxels used for correspondence search (used only in VGICP).</div>
<div class="line"># downsampling_resolution : float = 0.25</div>
<div class="line"># Resolution for downsampling the point clouds.</div>
<div class="line"># max_correspondence_distance : float = 1.0</div>
<div class="line"># Maximum distance for matching points between point clouds.</div>
<div class="line"># num_threads : int = 1</div>
<div class="line"># Number of threads to use for parallel processing.</div>
<div class="line"># </div>
<div class="line"># Returns</div>
<div class="line"># -------</div>
<div class="line"># RegistrationResult</div>
<div class="line"># Object containing the final transformation matrix and convergence status.</div>
<div class="line">result = small_gicp.align(target_raw_numpy, source_raw_numpy, downsampling_resolution=0.25)</div>
<div class="line"> </div>
<div class="line">result.T_target_source # Estimated transformation (4x4 numpy array)</div>
<div class="line">result.converged # If true, the optimization converged successfully</div>
<div class="line">result.iterations # Number of iterations the optimization took</div>
<div class="line">result.num_inliers # Number of inlier points</div>
<div class="line">result.H # Final Hessian matrix (6x6 matrix)</div>
<div class="line">result.b # Final information vector (6D vector)</div>
<div class="line">result.e # Final error (float)</div>
</div><!-- fragment --><p>Example B : Perform preprocessing and registration separately</p>
<div class="fragment"><div class="line"># Preprocess point cloud (downsampling, kdtree construction, and normal/covariance estimation)</div>
<div class="line">#</div>
<div class="line"># Parameters</div>
<div class="line"># ----------</div>
<div class="line"># points : NDArray[np.float64]</div>
<div class="line"># Nx3 or Nx4 matrix representing the point cloud.</div>
<div class="line"># downsampling_resolution : float = 0.1</div>
<div class="line"># Resolution for downsampling the point clouds.</div>
<div class="line"># num_neighbors : int = 20</div>
<div class="line"># Number of neighbor points to usefor point normal/covariance estimation.</div>
<div class="line"># num_threads : int = 1</div>
<div class="line"># Number of threads to use for parallel processing.</div>
<div class="line"># </div>
<div class="line"># Returns</div>
<div class="line"># -------</div>
<div class="line"># PointCloud</div>
<div class="line"># Downsampled point cloud with estimated normals and covariances.</div>
<div class="line"># KdTree</div>
<div class="line"># KdTree for the downsampled point cloud</div>
<div class="line">target, target_tree = small_gicp.preprocess_points(target_raw_numpy, downsampling_resolution=0.25)</div>
<div class="line">source, source_tree = small_gicp.preprocess_points(source_raw_numpy, downsampling_resolution=0.25)</div>
<div class="line"> </div>
<div class="line"># `target` and `source` are small_gicp.PointCloud with the following methods</div>
<div class="line">target.size() # Number of points</div>
<div class="line">target.points() # Nx4 numpy array [x, y, z, 1] x N</div>
<div class="line">target.normals() # Nx4 numpy array [nx, ny, nz, 0] x N</div>
<div class="line">target.covs() # Array of 4x4 covariance matrices</div>
<div class="line"> </div>
<div class="line"># Align two point clouds using specified ICP-like algorithms, utilizing point cloud and KD-tree inputs.</div>
<div class="line">#</div>
<div class="line"># Parameters</div>
<div class="line"># ----------</div>
<div class="line"># target : PointCloud::ConstPtr</div>
<div class="line"># Pointer to the target point cloud.</div>
<div class="line"># source : PointCloud::ConstPtr</div>
<div class="line"># Pointer to the source point cloud.</div>
<div class="line"># target_tree : KdTree&lt;PointCloud&gt;::ConstPtr, optional</div>
<div class="line"># Pointer to the KD-tree of the target for nearest neighbor search. If nullptr, a new tree is built.</div>
<div class="line"># init_T_target_source : NDArray[np.float64]</div>
<div class="line"># 4x4 matrix representing the initial transformation from target to source.</div>
<div class="line"># registration_type : str = &#39;GICP&#39;</div>
<div class="line"># Type of registration algorithm to use (&#39;ICP&#39;, &#39;PLANE_ICP&#39;, &#39;GICP&#39;).</div>
<div class="line"># max_correspondence_distance : float = 1.0</div>
<div class="line"># Maximum distance for corresponding point pairs.</div>
<div class="line"># num_threads : int = 1</div>
<div class="line"># Number of threads to use for computation.</div>
<div class="line"># </div>
<div class="line"># Returns</div>
<div class="line"># -------</div>
<div class="line"># RegistrationResult</div>
<div class="line"># Object containing the final transformation matrix and convergence status.</div>
<div class="line">result = small_gicp.align(target, source, target_tree)</div>
</div><!-- fragment --><p>Example C : Perform each of preprocessing steps one-by-one</p>
<div class="fragment"><div class="line"># Convert numpy arrays (Nx3 or Nx4) to small_gicp.PointCloud</div>
<div class="line">target_raw = small_gicp.PointCloud(target_raw_numpy)</div>
<div class="line">source_raw = small_gicp.PointCloud(source_raw_numpy)</div>
<div class="line"> </div>
<div class="line"># Downsampling</div>
<div class="line">target = small_gicp.voxelgrid_sampling(target_raw, 0.25)</div>
<div class="line">source = small_gicp.voxelgrid_sampling(source_raw, 0.25)</div>
<div class="line"> </div>
<div class="line"># KdTree construction</div>
<div class="line">target_tree = small_gicp.KdTree(target)</div>
<div class="line">source_tree = small_gicp.KdTree(source)</div>
<div class="line"> </div>
<div class="line"># Estimate covariances</div>
<div class="line">small_gicp.estimate_covariances(target, target_tree)</div>
<div class="line">small_gicp.estimate_covariances(source, source_tree)</div>
<div class="line"> </div>
<div class="line"># Align point clouds</div>
<div class="line">result = small_gicp.align(target, source, target_tree)</div>
</div><!-- fragment --><p>Example D: Example with Open3D</p>
<div class="fragment"><div class="line">target_o3d = open3d.io.read_point_cloud(&#39;small_gicp/data/target.ply&#39;).paint_uniform_color([0, 1, 0])</div>
<div class="line">source_o3d = open3d.io.read_point_cloud(&#39;small_gicp/data/source.ply&#39;).paint_uniform_color([0, 0, 1])</div>
<div class="line"> </div>
<div class="line">target, target_tree = small_gicp.preprocess_points(numpy.asarray(target_o3d.points), downsampling_resolution=0.25)</div>
<div class="line">source, source_tree = small_gicp.preprocess_points(numpy.asarray(source_o3d.points), downsampling_resolution=0.25)</div>
<div class="line">result = small_gicp.align(target, source, target_tree)</div>
<div class="line"> </div>
<div class="line">source_o3d.transform(result.T_target_source)</div>
<div class="line">open3d.visualization.draw_geometries([target_o3d, source_o3d])</div>
</div><!-- fragment --><p>&lt;/details&gt;</p>
<h2><a class="anchor" id="autotoc_md15"></a>
Cookbook</h2>
<ul>
<li><a href="src/example/kitti_odometry.py">Scan-to-scan and scan-to-model GICP matching odometry on KITTI</a></li>
</ul>
<h1><a class="anchor" id="autotoc_md16"></a>
Running examples</h1>
<h2><a class="anchor" id="autotoc_md17"></a>
C++</h2>
<div class="fragment"><div class="line">cd small_gicp</div>
<div class="line">mkdir build &amp;&amp; cd build</div>
<div class="line">cmake .. -DBUILD_EXAMPLES=ON &amp;&amp; make -j</div>
<div class="line"> </div>
<div class="line">cd ..</div>
<div class="line">./build/01_basic_registration</div>
<div class="line">./build/02_basic_registration_pcl</div>
<div class="line">./build/03_registration_template</div>
</div><!-- fragment --><h2><a class="anchor" id="autotoc_md18"></a>
Python</h2>
<div class="fragment"><div class="line">cd small_gicp</div>
<div class="line">pip install .</div>
<div class="line"> </div>
<div class="line">python3 src/example/basic_registration.py</div>
</div><!-- fragment --><h1><a class="anchor" id="autotoc_md19"></a>
@ref /home/runner/work/small_gicp/small_gicp/BENCHMARK.md "Benchmark"</h1>
<p>Processing speed comparison between <a class="el" href="namespacesmall__gicp.html">small_gicp</a> and Open3D (<a href="(https://youtu.be/LNESzGXPr4c?feature=shared)">youtube</a>). <a href="https://youtu.be/LNESzGXPr4c?feature=shared"><img src="https://github.com/koide3/small_gicp/assets/31344317/7959edd6-f0e4-4318-b4c1-a3f8755c407f" alt="small_comp" class="inline"/></a></p>
<h2><a class="anchor" id="autotoc_md20"></a>
Downsampling</h2>
<ul>
<li>Single-threaded <code><a class="el" href="namespacesmall__gicp.html#a1f96f233873fdc58bc241a8d26b0b774" title="Voxelgrid downsampling. This function computes exact average of points in each voxel,...">small_gicp::voxelgrid_sampling</a></code> is about <b>1.3x faster</b> than <code>pcl::VoxelGrid</code>.</li>
<li>Multi-threaded <code><a class="el" href="namespacesmall__gicp.html#af99df9601fa3d294463327902f51669b" title="Voxel grid downsampling with TBB backend.">small_gicp::voxelgrid_sampling_tbb</a></code> (6 threads) is about <b>3.2x faster</b> than <code>pcl::VoxelGrid</code>.</li>
<li><code><a class="el" href="namespacesmall__gicp.html#a1f96f233873fdc58bc241a8d26b0b774" title="Voxelgrid downsampling. This function computes exact average of points in each voxel,...">small_gicp::voxelgrid_sampling</a></code> provides accurate downsampling results that are nearly identical to those of <code>pcl::VoxelGrid</code>, while <code>pcl::ApproximateVoxelGrid</code> can produce spurious points (up to 2x more points).</li>
<li><code><a class="el" href="namespacesmall__gicp.html#a1f96f233873fdc58bc241a8d26b0b774" title="Voxelgrid downsampling. This function computes exact average of points in each voxel,...">small_gicp::voxelgrid_sampling</a></code> can handle larger point clouds with finer voxel resolutions compared to <code>pcl::VoxelGrid</code>. For a point cloud with a width of 1000m, the minimum voxel resolution can be <b>0.5 mm</b>.</li>
</ul>
<p><img src="docs/assets/downsampling_comp.png" alt="downsampling_comp" class="inline"/></p>
<h2><a class="anchor" id="autotoc_md21"></a>
KdTree construction</h2>
<ul>
<li>Multi-threaded implementation (TBB and OMP) can be up to <b>6x faster</b> than the single-threaded version. The single-thread version performs almost equivalently to nanoflann.</li>
<li>The new KdTree implementation demonstrates good scalability due to its well-balanced task assignment.</li>
<li>This benchmark compares only the construction time (query time is not included). Nearest neighbor queries are included and evaluated in the following odometry estimation evaluation.</li>
</ul>
<p><img src="docs/assets/kdtree_time.png" alt="kdtree_time" class="inline"/></p>
<h2><a class="anchor" id="autotoc_md22"></a>
Odometry estimation</h2>
<ul>
<li>Single-thread <code>small_gicp::GICP</code> is about <b>2.4x and 1.9x faster</b> than <code>pcl::GICP</code> and <code>fast_gicp::GICP</code>, respectively.</li>
<li><code><a class="el" href="namespacesmall__gicp.html">small_gicp</a>::(GICP|VGICP)</code> demonstrates better multi-threaded scalability compared to <code>fast_gicp::(GICP|VGICP)</code>.</li>
<li><code>small_gicp::GICP</code> parallelized with <a href="src/benchmark/odometry_benchmark_small_gicp_tbb_flow.cpp">TBB flow graph</a> shows excellent scalability in many-threads scenarios (**~128 threads**), though with some latency degradation.</li>
<li>Outputs of <code>small_gicp::GICP</code> are almost identical to those of <code>fast_gicp::GICP</code>.</li>
</ul>
<p><img src="docs/assets/odometry_time.png" alt="odometry_time" class="inline"/></p>
<h1><a class="anchor" id="autotoc_md23"></a>
License</h1>
<p>This package is released under the MIT license.</p>
<p>If you find this package useful for your project, please consider leaving a comment <a href="https://github.com/koide3/small_gicp/issues/3">here</a>. It would help the author receive recognition in his organization and keep working on this project.</p>
<h1><a class="anchor" id="autotoc_md24"></a>
Contact</h1>
<p><a href="https://staff.aist.go.jp/k.koide/">Kenji Koide</a>, National Institute of Advanced Industrial Science and Technology (AIST) </p>
</div></div><!-- PageDoc -->
</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>
Reference in New Issue View Git Blame Copy Permalink