mirror of https://github.com/koide3/small_gicp.git
174 lines
20 KiB
HTML
174 lines
20 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/downsampling_tbb.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">downsampling_tbb.hpp</div> </div>
|
|
</div><!--header-->
|
|
<div class="contents">
|
|
<a href="downsampling__tbb_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 <atomic></span></div>
|
|
<div class="line"><a name="l00006"></a><span class="lineno"> 6</span> <span class="preprocessor">#include <memory></span></div>
|
|
<div class="line"><a name="l00007"></a><span class="lineno"> 7</span> <span class="preprocessor">#include <iostream></span></div>
|
|
<div class="line"><a name="l00008"></a><span class="lineno"> 8</span>  </div>
|
|
<div class="line"><a name="l00009"></a><span class="lineno"> 9</span> <span class="preprocessor">#include <tbb/tbb.h></span></div>
|
|
<div class="line"><a name="l00010"></a><span class="lineno"> 10</span> <span class="preprocessor">#include <<a class="code" href="points_2traits_8hpp.html">small_gicp/points/traits.hpp</a>></span></div>
|
|
<div class="line"><a name="l00011"></a><span class="lineno"> 11</span> <span class="preprocessor">#include <<a class="code" href="fast__floor_8hpp.html">small_gicp/util/fast_floor.hpp</a>></span></div>
|
|
<div class="line"><a name="l00012"></a><span class="lineno"> 12</span> <span class="preprocessor">#include <<a class="code" href="vector3i__hash_8hpp.html">small_gicp/util/vector3i_hash.hpp</a>></span></div>
|
|
<div class="line"><a name="l00013"></a><span class="lineno"> 13</span>  </div>
|
|
<div class="line"><a name="l00014"></a><span class="lineno"> 14</span> <span class="keyword">namespace </span><a class="code" href="namespacesmall__gicp.html">small_gicp</a> {</div>
|
|
<div class="line"><a name="l00015"></a><span class="lineno"> 15</span>  </div>
|
|
<div class="line"><a name="l00025"></a><span class="lineno"> 25</span> <span class="keyword">template</span> <<span class="keyword">typename</span> InputPo<span class="keywordtype">int</span>Cloud, <span class="keyword">typename</span> OutputPo<span class="keywordtype">int</span>Cloud = InputPo<span class="keywordtype">int</span>Cloud></div>
|
|
<div class="line"><a name="l00026"></a><span class="lineno"><a class="line" href="namespacesmall__gicp.html#af99df9601fa3d294463327902f51669b"> 26</a></span> std::shared_ptr<OutputPointCloud> <a class="code" href="namespacesmall__gicp.html#af99df9601fa3d294463327902f51669b">voxelgrid_sampling_tbb</a>(<span class="keyword">const</span> InputPointCloud& points, <span class="keywordtype">double</span> leaf_size) {</div>
|
|
<div class="line"><a name="l00027"></a><span class="lineno"> 27</span>  <span class="keywordflow">if</span> (<a class="code" href="namespacesmall__gicp_1_1traits.html#a3c4ae38ff52330d6e624f4971c291fac">traits::size</a>(points) == 0) {</div>
|
|
<div class="line"><a name="l00028"></a><span class="lineno"> 28</span>  <span class="keywordflow">return</span> std::make_shared<OutputPointCloud>();</div>
|
|
<div class="line"><a name="l00029"></a><span class="lineno"> 29</span>  }</div>
|
|
<div class="line"><a name="l00030"></a><span class="lineno"> 30</span>  </div>
|
|
<div class="line"><a name="l00031"></a><span class="lineno"> 31</span>  <span class="keyword">const</span> <span class="keywordtype">double</span> inv_leaf_size = 1.0 / leaf_size;</div>
|
|
<div class="line"><a name="l00032"></a><span class="lineno"> 32</span>  </div>
|
|
<div class="line"><a name="l00033"></a><span class="lineno"> 33</span>  constexpr std::uint64_t invalid_coord = std::numeric_limits<std::uint64_t>::max();</div>
|
|
<div class="line"><a name="l00034"></a><span class="lineno"> 34</span>  constexpr <span class="keywordtype">int</span> coord_bit_size = 21; <span class="comment">// Bits to represent each voxel coordinate (pack 21x3 = 63bits in 64bit int)</span></div>
|
|
<div class="line"><a name="l00035"></a><span class="lineno"> 35</span>  constexpr <span class="keywordtype">size_t</span> coord_bit_mask = (1 << 21) - 1; <span class="comment">// Bit mask</span></div>
|
|
<div class="line"><a name="l00036"></a><span class="lineno"> 36</span>  constexpr <span class="keywordtype">int</span> coord_offset = 1 << (coord_bit_size - 1); <span class="comment">// Coordinate offset to make values positive</span></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>  std::vector<std::pair<std::uint64_t, size_t>> coord_pt(<a class="code" href="namespacesmall__gicp_1_1traits.html#a3c4ae38ff52330d6e624f4971c291fac">traits::size</a>(points));</div>
|
|
<div class="line"><a name="l00039"></a><span class="lineno"> 39</span>  tbb::parallel_for(tbb::blocked_range<size_t>(0, <a class="code" href="namespacesmall__gicp_1_1traits.html#a3c4ae38ff52330d6e624f4971c291fac">traits::size</a>(points), 64), [&](<span class="keyword">const</span> tbb::blocked_range<size_t>& range) {</div>
|
|
<div class="line"><a name="l00040"></a><span class="lineno"> 40</span>  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = range.begin(); i != range.end(); i++) {</div>
|
|
<div class="line"><a name="l00041"></a><span class="lineno"> 41</span>  const Eigen::Array4i coord = fast_floor(traits::point(points, i) * inv_leaf_size) + coord_offset;</div>
|
|
<div class="line"><a name="l00042"></a><span class="lineno"> 42</span>  if ((coord < 0).any() || (coord > coord_bit_mask).any()) {</div>
|
|
<div class="line"><a name="l00043"></a><span class="lineno"> 43</span>  std::cerr << <span class="stringliteral">"warning: voxel coord is out of range!!"</span> << std::endl;</div>
|
|
<div class="line"><a name="l00044"></a><span class="lineno"> 44</span>  coord_pt[i] = {invalid_coord, i};</div>
|
|
<div class="line"><a name="l00045"></a><span class="lineno"> 45</span>  continue;</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="l00048"></a><span class="lineno"> 48</span>  <span class="comment">// Compute voxel coord bits (0|1bit, z|21bit, y|21bit, x|21bit)</span></div>
|
|
<div class="line"><a name="l00049"></a><span class="lineno"> 49</span>  <span class="keyword">const</span> std::uint64_t bits = <span class="comment">//</span></div>
|
|
<div class="line"><a name="l00050"></a><span class="lineno"> 50</span>  (<span class="keyword">static_cast<</span>std::uint64_t<span class="keyword">></span>(coord[0] & coord_bit_mask) << (coord_bit_size * 0)) | <span class="comment">//</span></div>
|
|
<div class="line"><a name="l00051"></a><span class="lineno"> 51</span>  (<span class="keyword">static_cast<</span>std::uint64_t<span class="keyword">></span>(coord[1] & coord_bit_mask) << (coord_bit_size * 1)) | <span class="comment">//</span></div>
|
|
<div class="line"><a name="l00052"></a><span class="lineno"> 52</span>  (<span class="keyword">static_cast<</span>std::uint64_t<span class="keyword">></span>(coord[2] & coord_bit_mask) << (coord_bit_size * 2));</div>
|
|
<div class="line"><a name="l00053"></a><span class="lineno"> 53</span>  coord_pt[i] = {bits, i};</div>
|
|
<div class="line"><a name="l00054"></a><span class="lineno"> 54</span>  }</div>
|
|
<div class="line"><a name="l00055"></a><span class="lineno"> 55</span>  });</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>  <span class="comment">// Sort by voxel coords</span></div>
|
|
<div class="line"><a name="l00058"></a><span class="lineno"> 58</span>  tbb::parallel_sort(coord_pt, [](<span class="keyword">const</span> <span class="keyword">auto</span>& lhs, <span class="keyword">const</span> <span class="keyword">auto</span>& rhs) { <span class="keywordflow">return</span> lhs.first < rhs.first; });</div>
|
|
<div class="line"><a name="l00059"></a><span class="lineno"> 59</span>  </div>
|
|
<div class="line"><a name="l00060"></a><span class="lineno"> 60</span>  <span class="keyword">auto</span> downsampled = std::make_shared<OutputPointCloud>();</div>
|
|
<div class="line"><a name="l00061"></a><span class="lineno"> 61</span>  <a class="code" href="namespacesmall__gicp_1_1traits.html#ae35e34068062341652ed9dbf7c3eba25">traits::resize</a>(*downsampled, <a class="code" href="namespacesmall__gicp_1_1traits.html#a3c4ae38ff52330d6e624f4971c291fac">traits::size</a>(points));</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>  <span class="comment">// Take block-wise sum</span></div>
|
|
<div class="line"><a name="l00064"></a><span class="lineno"> 64</span>  <span class="keyword">const</span> <span class="keywordtype">int</span> block_size = 2048;</div>
|
|
<div class="line"><a name="l00065"></a><span class="lineno"> 65</span>  std::atomic_uint64_t num_points = 0;</div>
|
|
<div class="line"><a name="l00066"></a><span class="lineno"> 66</span>  tbb::parallel_for(tbb::blocked_range<size_t>(0, <a class="code" href="namespacesmall__gicp_1_1traits.html#a3c4ae38ff52330d6e624f4971c291fac">traits::size</a>(points), block_size), [&](<span class="keyword">const</span> tbb::blocked_range<size_t>& range) {</div>
|
|
<div class="line"><a name="l00067"></a><span class="lineno"> 67</span>  std::vector<Eigen::Vector4d> sub_points;</div>
|
|
<div class="line"><a name="l00068"></a><span class="lineno"> 68</span>  sub_points.reserve(block_size);</div>
|
|
<div class="line"><a name="l00069"></a><span class="lineno"> 69</span>  </div>
|
|
<div class="line"><a name="l00070"></a><span class="lineno"> 70</span>  Eigen::Vector4d sum_pt = <a class="code" href="namespacesmall__gicp_1_1traits.html#a66b660d3a0c2e81cdf06abc8efebdad1">traits::point</a>(points, coord_pt[range.begin()].second);</div>
|
|
<div class="line"><a name="l00071"></a><span class="lineno"> 71</span>  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = range.begin() + 1; i != range.end(); i++) {</div>
|
|
<div class="line"><a name="l00072"></a><span class="lineno"> 72</span>  <span class="keywordflow">if</span> (coord_pt[i].first == invalid_coord) {</div>
|
|
<div class="line"><a name="l00073"></a><span class="lineno"> 73</span>  <span class="keywordflow">continue</span>;</div>
|
|
<div class="line"><a name="l00074"></a><span class="lineno"> 74</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>  <span class="keywordflow">if</span> (coord_pt[i - 1].first != coord_pt[i].first) {</div>
|
|
<div class="line"><a name="l00077"></a><span class="lineno"> 77</span>  sub_points.emplace_back(sum_pt / sum_pt.w());</div>
|
|
<div class="line"><a name="l00078"></a><span class="lineno"> 78</span>  sum_pt.setZero();</div>
|
|
<div class="line"><a name="l00079"></a><span class="lineno"> 79</span>  }</div>
|
|
<div class="line"><a name="l00080"></a><span class="lineno"> 80</span>  sum_pt += <a class="code" href="namespacesmall__gicp_1_1traits.html#a66b660d3a0c2e81cdf06abc8efebdad1">traits::point</a>(points, coord_pt[i].second);</div>
|
|
<div class="line"><a name="l00081"></a><span class="lineno"> 81</span>  }</div>
|
|
<div class="line"><a name="l00082"></a><span class="lineno"> 82</span>  sub_points.emplace_back(sum_pt / sum_pt.w());</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>  <span class="keyword">const</span> <span class="keywordtype">size_t</span> point_index_begin = num_points.fetch_add(sub_points.size());</div>
|
|
<div class="line"><a name="l00085"></a><span class="lineno"> 85</span>  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i < sub_points.size(); i++) {</div>
|
|
<div class="line"><a name="l00086"></a><span class="lineno"> 86</span>  <a class="code" href="namespacesmall__gicp_1_1traits.html#ad663f28d1c31b400619cf2c87cf7725a">traits::set_point</a>(*downsampled, point_index_begin + i, sub_points[i]);</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>  });</div>
|
|
<div class="line"><a name="l00089"></a><span class="lineno"> 89</span>  </div>
|
|
<div class="line"><a name="l00090"></a><span class="lineno"> 90</span>  <a class="code" href="namespacesmall__gicp_1_1traits.html#ae35e34068062341652ed9dbf7c3eba25">traits::resize</a>(*downsampled, num_points);</div>
|
|
<div class="line"><a name="l00091"></a><span class="lineno"> 91</span>  </div>
|
|
<div class="line"><a name="l00092"></a><span class="lineno"> 92</span>  <span class="keywordflow">return</span> downsampled;</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>  </div>
|
|
<div class="line"><a name="l00095"></a><span class="lineno"> 95</span> } <span class="comment">// namespace small_gicp</span></div>
|
|
<div class="ttc" id="afast__floor_8hpp_html"><div class="ttname"><a href="fast__floor_8hpp.html">fast_floor.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_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_ad663f28d1c31b400619cf2c87cf7725a"><div class="ttname"><a href="namespacesmall__gicp_1_1traits.html#ad663f28d1c31b400619cf2c87cf7725a">small_gicp::traits::set_point</a></div><div class="ttdeci">void set_point(T &points, size_t i, const Eigen::Vector4d &pt)</div><div class="ttdoc">Set i-th point. (x, y, z, 1)</div><div class="ttdef"><b>Definition:</b> traits.hpp:64</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_af99df9601fa3d294463327902f51669b"><div class="ttname"><a href="namespacesmall__gicp.html#af99df9601fa3d294463327902f51669b">small_gicp::voxelgrid_sampling_tbb</a></div><div class="ttdeci">std::shared_ptr< OutputPointCloud > voxelgrid_sampling_tbb(const InputPointCloud &points, double leaf_size)</div><div class="ttdoc">Voxel grid downsampling with TBB backend.</div><div class="ttdef"><b>Definition:</b> downsampling_tbb.hpp:26</div></div>
|
|
<div class="ttc" id="apoints_2traits_8hpp_html"><div class="ttname"><a href="points_2traits_8hpp.html">traits.hpp</a></div></div>
|
|
<div class="ttc" id="avector3i__hash_8hpp_html"><div class="ttname"><a href="vector3i__hash_8hpp.html">vector3i_hash.hpp</a></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>
|