mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-04 00:57:46 +08:00
507 lines
63 KiB
HTML
507 lines
63 KiB
HTML
<!-- HTML header for doxygen 1.9.8-->
|
|
<!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" lang="en-US">
|
|
<head>
|
|
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
|
|
<meta http-equiv="X-UA-Compatible" content="IE=11"/>
|
|
<meta name="generator" content="Doxygen 1.9.8"/>
|
|
<meta name="viewport" content="width=device-width, initial-scale=1"/>
|
|
<title>RTAB-Map: corelib/include/rtabmap/core/impl/util3d_mapping.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="navtree.css" rel="stylesheet" type="text/css"/>
|
|
<script type="text/javascript" src="resize.js"></script>
|
|
<script type="text/javascript" src="navtreedata.js"></script>
|
|
<script type="text/javascript" src="navtree.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" />
|
|
<link href="doxygen-awesome.css" rel="stylesheet" type="text/css"/>
|
|
<link href="custom.css" rel="stylesheet" type="text/css"/>
|
|
<script type="text/javascript">window.RTABMAP_DOC_ROOT="";</script>
|
|
<script type="text/javascript" src="../versions.js"></script>
|
|
<script type="text/javascript" src="version-switcher.js"></script>
|
|
<script type="text/javascript" src="doxygen-awesome-darkmode-toggle.js"></script>
|
|
<script type="text/javascript">DoxygenAwesomeDarkModeToggle.init();</script>
|
|
</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 id="projectrow">
|
|
<td id="projectalign">
|
|
<div id="projectname">RTAB-Map<span id="projectnumber"> 0.23.10</span>
|
|
</div>
|
|
<div id="projectbrief">Real-Time Appearance-Based Mapping</div>
|
|
</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
<!-- end header part -->
|
|
<!-- Generated by Doxygen 1.9.8 -->
|
|
<script type="text/javascript">
|
|
/* @license magnet:?xt=urn:btih:d3d9a9a6595521f9666a5e94cc830dab83b65699&dn=expat.txt MIT */
|
|
var searchBox = new SearchBox("searchBox", "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:d3d9a9a6595521f9666a5e94cc830dab83b65699&dn=expat.txt MIT */
|
|
$(function() {
|
|
initMenu('',true,false,'search.php','Search');
|
|
$(document).ready(function() { init_search(); });
|
|
});
|
|
/* @license-end */
|
|
</script>
|
|
<div id="main-nav"></div>
|
|
<script type="text/javascript">
|
|
/* @license magnet:?xt=urn:btih:d3d9a9a6595521f9666a5e94cc830dab83b65699&dn=expat.txt MIT */
|
|
$(document).ready(function() { init_codefold(0); });
|
|
/* @license-end */
|
|
</script>
|
|
</div><!-- top -->
|
|
<div id="side-nav" class="ui-resizable side-nav-resizable">
|
|
<div id="nav-tree">
|
|
<div id="nav-tree-contents">
|
|
<div id="nav-sync" class="sync"></div>
|
|
</div>
|
|
</div>
|
|
<div id="splitbar" style="-moz-user-select:none;"
|
|
class="ui-resizable-handle">
|
|
</div>
|
|
</div>
|
|
<script type="text/javascript">
|
|
/* @license magnet:?xt=urn:btih:d3d9a9a6595521f9666a5e94cc830dab83b65699&dn=expat.txt MIT */
|
|
$(document).ready(function(){initNavTree('util3d__mapping_8hpp_source.html',''); initResizable(); });
|
|
/* @license-end */
|
|
</script>
|
|
<div id="doc-content">
|
|
<!-- 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">
|
|
<div id="MSearchResults">
|
|
<div class="SRPage">
|
|
<div id="SRIndex">
|
|
<div id="SRResults"></div>
|
|
<div class="SRStatus" id="Loading">Loading...</div>
|
|
<div class="SRStatus" id="Searching">Searching...</div>
|
|
<div class="SRStatus" id="NoMatches">No Matches</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div class="header">
|
|
<div class="headertitle"><div class="title">util3d_mapping.hpp</div></div>
|
|
</div><!--header-->
|
|
<div class="contents">
|
|
<div class="fragment"><div class="line"><a id="l00001" name="l00001"></a><span class="lineno"> 1</span><span class="comment">/*</span></div>
|
|
<div class="line"><a id="l00002" name="l00002"></a><span class="lineno"> 2</span><span class="comment">Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke</span></div>
|
|
<div class="line"><a id="l00003" name="l00003"></a><span class="lineno"> 3</span><span class="comment">All rights reserved.</span></div>
|
|
<div class="line"><a id="l00004" name="l00004"></a><span class="lineno"> 4</span><span class="comment"></span> </div>
|
|
<div class="line"><a id="l00005" name="l00005"></a><span class="lineno"> 5</span><span class="comment">Redistribution and use in source and binary forms, with or without</span></div>
|
|
<div class="line"><a id="l00006" name="l00006"></a><span class="lineno"> 6</span><span class="comment">modification, are permitted provided that the following conditions are met:</span></div>
|
|
<div class="line"><a id="l00007" name="l00007"></a><span class="lineno"> 7</span><span class="comment"> * Redistributions of source code must retain the above copyright</span></div>
|
|
<div class="line"><a id="l00008" name="l00008"></a><span class="lineno"> 8</span><span class="comment"> notice, this list of conditions and the following disclaimer.</span></div>
|
|
<div class="line"><a id="l00009" name="l00009"></a><span class="lineno"> 9</span><span class="comment"> * Redistributions in binary form must reproduce the above copyright</span></div>
|
|
<div class="line"><a id="l00010" name="l00010"></a><span class="lineno"> 10</span><span class="comment"> notice, this list of conditions and the following disclaimer in the</span></div>
|
|
<div class="line"><a id="l00011" name="l00011"></a><span class="lineno"> 11</span><span class="comment"> documentation and/or other materials provided with the distribution.</span></div>
|
|
<div class="line"><a id="l00012" name="l00012"></a><span class="lineno"> 12</span><span class="comment"> * Neither the name of the Universite de Sherbrooke nor the</span></div>
|
|
<div class="line"><a id="l00013" name="l00013"></a><span class="lineno"> 13</span><span class="comment"> names of its contributors may be used to endorse or promote products</span></div>
|
|
<div class="line"><a id="l00014" name="l00014"></a><span class="lineno"> 14</span><span class="comment"> derived from this software without specific prior written permission.</span></div>
|
|
<div class="line"><a id="l00015" name="l00015"></a><span class="lineno"> 15</span><span class="comment"></span> </div>
|
|
<div class="line"><a id="l00016" name="l00016"></a><span class="lineno"> 16</span><span class="comment">THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND</span></div>
|
|
<div class="line"><a id="l00017" name="l00017"></a><span class="lineno"> 17</span><span class="comment">ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED</span></div>
|
|
<div class="line"><a id="l00018" name="l00018"></a><span class="lineno"> 18</span><span class="comment">WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE</span></div>
|
|
<div class="line"><a id="l00019" name="l00019"></a><span class="lineno"> 19</span><span class="comment">DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY</span></div>
|
|
<div class="line"><a id="l00020" name="l00020"></a><span class="lineno"> 20</span><span class="comment">DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES</span></div>
|
|
<div class="line"><a id="l00021" name="l00021"></a><span class="lineno"> 21</span><span class="comment">(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;</span></div>
|
|
<div class="line"><a id="l00022" name="l00022"></a><span class="lineno"> 22</span><span class="comment">LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND</span></div>
|
|
<div class="line"><a id="l00023" name="l00023"></a><span class="lineno"> 23</span><span class="comment">ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT</span></div>
|
|
<div class="line"><a id="l00024" name="l00024"></a><span class="lineno"> 24</span><span class="comment">(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS</span></div>
|
|
<div class="line"><a id="l00025" name="l00025"></a><span class="lineno"> 25</span><span class="comment">SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.</span></div>
|
|
<div class="line"><a id="l00026" name="l00026"></a><span class="lineno"> 26</span><span class="comment">*/</span></div>
|
|
<div class="line"><a id="l00027" name="l00027"></a><span class="lineno"> 27</span> </div>
|
|
<div class="line"><a id="l00028" name="l00028"></a><span class="lineno"> 28</span><span class="preprocessor">#ifndef UTIL3D_MAPPING_HPP_</span></div>
|
|
<div class="line"><a id="l00029" name="l00029"></a><span class="lineno"> 29</span><span class="preprocessor">#define UTIL3D_MAPPING_HPP_</span></div>
|
|
<div class="line"><a id="l00030" name="l00030"></a><span class="lineno"> 30</span> </div>
|
|
<div class="line"><a id="l00031" name="l00031"></a><span class="lineno"> 31</span><span class="preprocessor">#include <rtabmap/core/util3d_filtering.h></span></div>
|
|
<div class="line"><a id="l00032" name="l00032"></a><span class="lineno"> 32</span><span class="preprocessor">#include <rtabmap/core/util3d.h></span></div>
|
|
<div class="line"><a id="l00033" name="l00033"></a><span class="lineno"> 33</span><span class="preprocessor">#include <pcl/common/common.h></span></div>
|
|
<div class="line"><a id="l00034" name="l00034"></a><span class="lineno"> 34</span><span class="preprocessor">#include <pcl/common/centroid.h></span></div>
|
|
<div class="line"><a id="l00035" name="l00035"></a><span class="lineno"> 35</span><span class="preprocessor">#include <pcl/common/io.h></span></div>
|
|
<div class="line"><a id="l00036" name="l00036"></a><span class="lineno"> 36</span> </div>
|
|
<div class="line"><a id="l00037" name="l00037"></a><span class="lineno"> 37</span><span class="keyword">namespace </span><a class="code hl_namespace" href="namespacertabmap.html">rtabmap</a>{</div>
|
|
<div class="line"><a id="l00038" name="l00038"></a><span class="lineno"> 38</span><span class="keyword">namespace </span>util3d{</div>
|
|
<div class="line"><a id="l00039" name="l00039"></a><span class="lineno"> 39</span> </div>
|
|
<div class="line"><a id="l00040" name="l00040"></a><span class="lineno"> 40</span><span class="keyword">template</span><<span class="keyword">typename</span> Po<span class="keywordtype">int</span>T></div>
|
|
<div class="foldopen" id="foldopen00041" data-start="{" data-end="}">
|
|
<div class="line"><a id="l00041" name="l00041"></a><span class="lineno"><a class="line" href="namespacertabmap_1_1util3d.html#a3bdae076cc5054f35393b744f478b980"> 41</a></span><span class="keyword">typename</span> pcl::PointCloud<PointT>::Ptr <a class="code hl_function" href="namespacertabmap_1_1util3d.html#a3bdae076cc5054f35393b744f478b980">projectCloudOnXYPlane</a>(</div>
|
|
<div class="line"><a id="l00042" name="l00042"></a><span class="lineno"> 42</span> <span class="keyword">const</span> <span class="keyword">typename</span> pcl::PointCloud<PointT> & cloud)</div>
|
|
<div class="line"><a id="l00043" name="l00043"></a><span class="lineno"> 43</span>{</div>
|
|
<div class="line"><a id="l00044" name="l00044"></a><span class="lineno"> 44</span> <span class="keyword">typename</span> pcl::PointCloud<PointT>::Ptr output(<span class="keyword">new</span> pcl::PointCloud<PointT>);</div>
|
|
<div class="line"><a id="l00045" name="l00045"></a><span class="lineno"> 45</span> *output = cloud;</div>
|
|
<div class="line"><a id="l00046" name="l00046"></a><span class="lineno"> 46</span> <span class="keywordflow">for</span>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i=0; i<output->size(); ++i)</div>
|
|
<div class="line"><a id="l00047" name="l00047"></a><span class="lineno"> 47</span> {</div>
|
|
<div class="line"><a id="l00048" name="l00048"></a><span class="lineno"> 48</span> output->at(i).z = 0;</div>
|
|
<div class="line"><a id="l00049" name="l00049"></a><span class="lineno"> 49</span> }</div>
|
|
<div class="line"><a id="l00050" name="l00050"></a><span class="lineno"> 50</span> <span class="keywordflow">return</span> output;</div>
|
|
<div class="line"><a id="l00051" name="l00051"></a><span class="lineno"> 51</span>}</div>
|
|
</div>
|
|
<div class="line"><a id="l00052" name="l00052"></a><span class="lineno"> 52</span> </div>
|
|
<div class="line"><a id="l00053" name="l00053"></a><span class="lineno"> 53</span><span class="keyword">template</span><<span class="keyword">typename</span> Po<span class="keywordtype">int</span>T></div>
|
|
<div class="line"><a id="l00054" name="l00054"></a><span class="lineno"> 54</span><span class="keywordtype">void</span> segmentObstaclesFromGround(</div>
|
|
<div class="line"><a id="l00055" name="l00055"></a><span class="lineno"> 55</span> <span class="keyword">const</span> <span class="keyword">typename</span> pcl::PointCloud<PointT>::Ptr & cloud,</div>
|
|
<div class="line"><a id="l00056" name="l00056"></a><span class="lineno"> 56</span> <span class="keyword">const</span> <span class="keyword">typename</span> pcl::IndicesPtr & indices,</div>
|
|
<div class="line"><a id="l00057" name="l00057"></a><span class="lineno"> 57</span> pcl::IndicesPtr & ground,</div>
|
|
<div class="line"><a id="l00058" name="l00058"></a><span class="lineno"> 58</span> pcl::IndicesPtr & obstacles,</div>
|
|
<div class="line"><a id="l00059" name="l00059"></a><span class="lineno"> 59</span> <span class="keywordtype">int</span> normalKSearch,</div>
|
|
<div class="line"><a id="l00060" name="l00060"></a><span class="lineno"> 60</span> <span class="keywordtype">float</span> groundNormalAngle,</div>
|
|
<div class="line"><a id="l00061" name="l00061"></a><span class="lineno"> 61</span> <span class="keywordtype">float</span> clusterRadius,</div>
|
|
<div class="line"><a id="l00062" name="l00062"></a><span class="lineno"> 62</span> <span class="keywordtype">int</span> minClusterSize,</div>
|
|
<div class="line"><a id="l00063" name="l00063"></a><span class="lineno"> 63</span> <span class="keywordtype">bool</span> segmentFlatObstacles,</div>
|
|
<div class="line"><a id="l00064" name="l00064"></a><span class="lineno"> 64</span> <span class="keywordtype">float</span> maxGroundHeight,</div>
|
|
<div class="line"><a id="l00065" name="l00065"></a><span class="lineno"> 65</span> pcl::IndicesPtr * flatObstacles,</div>
|
|
<div class="line"><a id="l00066" name="l00066"></a><span class="lineno"> 66</span> <span class="keyword">const</span> Eigen::Vector4f & viewPoint,</div>
|
|
<div class="line"><a id="l00067" name="l00067"></a><span class="lineno"> 67</span> <span class="keywordtype">float</span> groundNormalsUp)</div>
|
|
<div class="line"><a id="l00068" name="l00068"></a><span class="lineno"> 68</span>{</div>
|
|
<div class="line"><a id="l00069" name="l00069"></a><span class="lineno"> 69</span> ground.reset(<span class="keyword">new</span> std::vector<int>);</div>
|
|
<div class="line"><a id="l00070" name="l00070"></a><span class="lineno"> 70</span> obstacles.reset(<span class="keyword">new</span> std::vector<int>);</div>
|
|
<div class="line"><a id="l00071" name="l00071"></a><span class="lineno"> 71</span> <span class="keywordflow">if</span>(flatObstacles)</div>
|
|
<div class="line"><a id="l00072" name="l00072"></a><span class="lineno"> 72</span> {</div>
|
|
<div class="line"><a id="l00073" name="l00073"></a><span class="lineno"> 73</span> flatObstacles->reset(<span class="keyword">new</span> std::vector<int>);</div>
|
|
<div class="line"><a id="l00074" name="l00074"></a><span class="lineno"> 74</span> }</div>
|
|
<div class="line"><a id="l00075" name="l00075"></a><span class="lineno"> 75</span> </div>
|
|
<div class="line"><a id="l00076" name="l00076"></a><span class="lineno"> 76</span> <span class="keywordflow">if</span>(cloud->size())</div>
|
|
<div class="line"><a id="l00077" name="l00077"></a><span class="lineno"> 77</span> {</div>
|
|
<div class="line"><a id="l00078" name="l00078"></a><span class="lineno"> 78</span> <span class="comment">// Find the ground</span></div>
|
|
<div class="line"><a id="l00079" name="l00079"></a><span class="lineno"> 79</span> pcl::IndicesPtr flatSurfaces = <a class="code hl_function" href="group__PointNormalFiltering.html#ga7c7e3de2ff24fc3aadfb3eb0ae05314d">normalFiltering</a>(</div>
|
|
<div class="line"><a id="l00080" name="l00080"></a><span class="lineno"> 80</span> cloud,</div>
|
|
<div class="line"><a id="l00081" name="l00081"></a><span class="lineno"> 81</span> indices,</div>
|
|
<div class="line"><a id="l00082" name="l00082"></a><span class="lineno"> 82</span> groundNormalAngle,</div>
|
|
<div class="line"><a id="l00083" name="l00083"></a><span class="lineno"> 83</span> Eigen::Vector4f(0,0,1,0),</div>
|
|
<div class="line"><a id="l00084" name="l00084"></a><span class="lineno"> 84</span> normalKSearch,</div>
|
|
<div class="line"><a id="l00085" name="l00085"></a><span class="lineno"> 85</span> viewPoint,</div>
|
|
<div class="line"><a id="l00086" name="l00086"></a><span class="lineno"> 86</span> groundNormalsUp);</div>
|
|
<div class="line"><a id="l00087" name="l00087"></a><span class="lineno"> 87</span> </div>
|
|
<div class="line"><a id="l00088" name="l00088"></a><span class="lineno"> 88</span> <span class="keywordflow">if</span>(flatSurfaces->size() && </div>
|
|
<div class="line"><a id="l00089" name="l00089"></a><span class="lineno"> 89</span> (segmentFlatObstacles || maxGroundHeight != 0.0f || minClusterSize>1))</div>
|
|
<div class="line"><a id="l00090" name="l00090"></a><span class="lineno"> 90</span> {</div>
|
|
<div class="line"><a id="l00091" name="l00091"></a><span class="lineno"> 91</span> <span class="keywordtype">int</span> biggestFlatSurfaceIndex;</div>
|
|
<div class="line"><a id="l00092" name="l00092"></a><span class="lineno"> 92</span> std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = <a class="code hl_function" href="group__ExtractClusters.html#ga62a0d02930f5969cbff707afe01d1bcd">extractClusters</a>(</div>
|
|
<div class="line"><a id="l00093" name="l00093"></a><span class="lineno"> 93</span> cloud,</div>
|
|
<div class="line"><a id="l00094" name="l00094"></a><span class="lineno"> 94</span> flatSurfaces,</div>
|
|
<div class="line"><a id="l00095" name="l00095"></a><span class="lineno"> 95</span> clusterRadius,</div>
|
|
<div class="line"><a id="l00096" name="l00096"></a><span class="lineno"> 96</span> minClusterSize,</div>
|
|
<div class="line"><a id="l00097" name="l00097"></a><span class="lineno"> 97</span> std::numeric_limits<int>::max(),</div>
|
|
<div class="line"><a id="l00098" name="l00098"></a><span class="lineno"> 98</span> &biggestFlatSurfaceIndex);</div>
|
|
<div class="line"><a id="l00099" name="l00099"></a><span class="lineno"> 99</span> </div>
|
|
<div class="line"><a id="l00100" name="l00100"></a><span class="lineno"> 100</span> <span class="comment">// cluster all surfaces for which the centroid is in the Z-range of the bigger surface</span></div>
|
|
<div class="line"><a id="l00101" name="l00101"></a><span class="lineno"> 101</span> <span class="keywordflow">if</span>(clusteredFlatSurfaces.size())</div>
|
|
<div class="line"><a id="l00102" name="l00102"></a><span class="lineno"> 102</span> {</div>
|
|
<div class="line"><a id="l00103" name="l00103"></a><span class="lineno"> 103</span> Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax;</div>
|
|
<div class="line"><a id="l00104" name="l00104"></a><span class="lineno"> 104</span> <span class="keywordflow">if</span>(maxGroundHeight != 0.0f)</div>
|
|
<div class="line"><a id="l00105" name="l00105"></a><span class="lineno"> 105</span> {</div>
|
|
<div class="line"><a id="l00106" name="l00106"></a><span class="lineno"> 106</span> <span class="comment">// Search for biggest surface under max ground height</span></div>
|
|
<div class="line"><a id="l00107" name="l00107"></a><span class="lineno"> 107</span> <span class="keywordtype">size_t</span> points = 0;</div>
|
|
<div class="line"><a id="l00108" name="l00108"></a><span class="lineno"> 108</span> biggestFlatSurfaceIndex = -1;</div>
|
|
<div class="line"><a id="l00109" name="l00109"></a><span class="lineno"> 109</span> <span class="keywordflow">for</span>(<span class="keywordtype">size_t</span> i=0;i<clusteredFlatSurfaces.size();++i)</div>
|
|
<div class="line"><a id="l00110" name="l00110"></a><span class="lineno"> 110</span> {</div>
|
|
<div class="line"><a id="l00111" name="l00111"></a><span class="lineno"> 111</span> Eigen::Vector4f min,max;</div>
|
|
<div class="line"><a id="l00112" name="l00112"></a><span class="lineno"> 112</span> pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(i), min, max);</div>
|
|
<div class="line"><a id="l00113" name="l00113"></a><span class="lineno"> 113</span> <span class="keywordflow">if</span>(min[2]<maxGroundHeight && clusteredFlatSurfaces.at(i)->size() > points)</div>
|
|
<div class="line"><a id="l00114" name="l00114"></a><span class="lineno"> 114</span> {</div>
|
|
<div class="line"><a id="l00115" name="l00115"></a><span class="lineno"> 115</span> points = clusteredFlatSurfaces.at(i)->size();</div>
|
|
<div class="line"><a id="l00116" name="l00116"></a><span class="lineno"> 116</span> biggestFlatSurfaceIndex = i;</div>
|
|
<div class="line"><a id="l00117" name="l00117"></a><span class="lineno"> 117</span> biggestSurfaceMin = min;</div>
|
|
<div class="line"><a id="l00118" name="l00118"></a><span class="lineno"> 118</span> biggestSurfaceMax = max;</div>
|
|
<div class="line"><a id="l00119" name="l00119"></a><span class="lineno"> 119</span> }</div>
|
|
<div class="line"><a id="l00120" name="l00120"></a><span class="lineno"> 120</span> }</div>
|
|
<div class="line"><a id="l00121" name="l00121"></a><span class="lineno"> 121</span> }</div>
|
|
<div class="line"><a id="l00122" name="l00122"></a><span class="lineno"> 122</span> <span class="keywordflow">else</span></div>
|
|
<div class="line"><a id="l00123" name="l00123"></a><span class="lineno"> 123</span> {</div>
|
|
<div class="line"><a id="l00124" name="l00124"></a><span class="lineno"> 124</span> pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), biggestSurfaceMin, biggestSurfaceMax);</div>
|
|
<div class="line"><a id="l00125" name="l00125"></a><span class="lineno"> 125</span> }</div>
|
|
<div class="line"><a id="l00126" name="l00126"></a><span class="lineno"> 126</span> <span class="keywordflow">if</span>(biggestFlatSurfaceIndex>=0)</div>
|
|
<div class="line"><a id="l00127" name="l00127"></a><span class="lineno"> 127</span> {</div>
|
|
<div class="line"><a id="l00128" name="l00128"></a><span class="lineno"> 128</span> ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);</div>
|
|
<div class="line"><a id="l00129" name="l00129"></a><span class="lineno"> 129</span> }</div>
|
|
<div class="line"><a id="l00130" name="l00130"></a><span class="lineno"> 130</span> </div>
|
|
<div class="line"><a id="l00131" name="l00131"></a><span class="lineno"> 131</span> <span class="keywordflow">if</span>(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))</div>
|
|
<div class="line"><a id="l00132" name="l00132"></a><span class="lineno"> 132</span> {</div>
|
|
<div class="line"><a id="l00133" name="l00133"></a><span class="lineno"> 133</span> <span class="keywordflow">for</span>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i=0; i<clusteredFlatSurfaces.size(); ++i)</div>
|
|
<div class="line"><a id="l00134" name="l00134"></a><span class="lineno"> 134</span> {</div>
|
|
<div class="line"><a id="l00135" name="l00135"></a><span class="lineno"> 135</span> <span class="keywordflow">if</span>((<span class="keywordtype">int</span>)i!=biggestFlatSurfaceIndex)</div>
|
|
<div class="line"><a id="l00136" name="l00136"></a><span class="lineno"> 136</span> {</div>
|
|
<div class="line"><a id="l00137" name="l00137"></a><span class="lineno"> 137</span> Eigen::Vector4f centroid(0,0,0,1);</div>
|
|
<div class="line"><a id="l00138" name="l00138"></a><span class="lineno"> 138</span> pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);</div>
|
|
<div class="line"><a id="l00139" name="l00139"></a><span class="lineno"> 139</span> <span class="keywordflow">if</span>((maxGroundHeight!=0.0f && centroid[2] <= maxGroundHeight) ||</div>
|
|
<div class="line"><a id="l00140" name="l00140"></a><span class="lineno"> 140</span> centroid[2] <= biggestSurfaceMax[2] ||</div>
|
|
<div class="line"><a id="l00141" name="l00141"></a><span class="lineno"> 141</span> (maxGroundHeight==0.0f && !segmentFlatObstacles))</div>
|
|
<div class="line"><a id="l00142" name="l00142"></a><span class="lineno"> 142</span> {</div>
|
|
<div class="line"><a id="l00143" name="l00143"></a><span class="lineno"> 143</span> ground = <a class="code hl_function" href="namespacertabmap_1_1util3d.html#aff3c61a08b0fcad7f1c3f7c847190969">util3d::concatenate</a>(ground, clusteredFlatSurfaces.at(i));</div>
|
|
<div class="line"><a id="l00144" name="l00144"></a><span class="lineno"> 144</span> }</div>
|
|
<div class="line"><a id="l00145" name="l00145"></a><span class="lineno"> 145</span> <span class="keywordflow">else</span> <span class="keywordflow">if</span>(flatObstacles)</div>
|
|
<div class="line"><a id="l00146" name="l00146"></a><span class="lineno"> 146</span> {</div>
|
|
<div class="line"><a id="l00147" name="l00147"></a><span class="lineno"> 147</span> *flatObstacles = <a class="code hl_function" href="namespacertabmap_1_1util3d.html#aff3c61a08b0fcad7f1c3f7c847190969">util3d::concatenate</a>(*flatObstacles, clusteredFlatSurfaces.at(i));</div>
|
|
<div class="line"><a id="l00148" name="l00148"></a><span class="lineno"> 148</span> }</div>
|
|
<div class="line"><a id="l00149" name="l00149"></a><span class="lineno"> 149</span> }</div>
|
|
<div class="line"><a id="l00150" name="l00150"></a><span class="lineno"> 150</span> }</div>
|
|
<div class="line"><a id="l00151" name="l00151"></a><span class="lineno"> 151</span> }</div>
|
|
<div class="line"><a id="l00152" name="l00152"></a><span class="lineno"> 152</span> <span class="keywordflow">else</span></div>
|
|
<div class="line"><a id="l00153" name="l00153"></a><span class="lineno"> 153</span> {</div>
|
|
<div class="line"><a id="l00154" name="l00154"></a><span class="lineno"> 154</span> <span class="comment">// reject ground!</span></div>
|
|
<div class="line"><a id="l00155" name="l00155"></a><span class="lineno"> 155</span> ground.reset(<span class="keyword">new</span> std::vector<int>);</div>
|
|
<div class="line"><a id="l00156" name="l00156"></a><span class="lineno"> 156</span> <span class="keywordflow">if</span>(flatObstacles)</div>
|
|
<div class="line"><a id="l00157" name="l00157"></a><span class="lineno"> 157</span> {</div>
|
|
<div class="line"><a id="l00158" name="l00158"></a><span class="lineno"> 158</span> *flatObstacles = flatSurfaces;</div>
|
|
<div class="line"><a id="l00159" name="l00159"></a><span class="lineno"> 159</span> }</div>
|
|
<div class="line"><a id="l00160" name="l00160"></a><span class="lineno"> 160</span> }</div>
|
|
<div class="line"><a id="l00161" name="l00161"></a><span class="lineno"> 161</span> }</div>
|
|
<div class="line"><a id="l00162" name="l00162"></a><span class="lineno"> 162</span> }</div>
|
|
<div class="line"><a id="l00163" name="l00163"></a><span class="lineno"> 163</span> <span class="keywordflow">else</span></div>
|
|
<div class="line"><a id="l00164" name="l00164"></a><span class="lineno"> 164</span> {</div>
|
|
<div class="line"><a id="l00165" name="l00165"></a><span class="lineno"> 165</span> ground = flatSurfaces;</div>
|
|
<div class="line"><a id="l00166" name="l00166"></a><span class="lineno"> 166</span> }</div>
|
|
<div class="line"><a id="l00167" name="l00167"></a><span class="lineno"> 167</span> </div>
|
|
<div class="line"><a id="l00168" name="l00168"></a><span class="lineno"> 168</span> <span class="keywordflow">if</span>(ground->size() != cloud->size())</div>
|
|
<div class="line"><a id="l00169" name="l00169"></a><span class="lineno"> 169</span> {</div>
|
|
<div class="line"><a id="l00170" name="l00170"></a><span class="lineno"> 170</span> <span class="comment">// Remove ground</span></div>
|
|
<div class="line"><a id="l00171" name="l00171"></a><span class="lineno"> 171</span> pcl::IndicesPtr notObstacles = ground;</div>
|
|
<div class="line"><a id="l00172" name="l00172"></a><span class="lineno"> 172</span> <span class="keywordflow">if</span>(indices->size())</div>
|
|
<div class="line"><a id="l00173" name="l00173"></a><span class="lineno"> 173</span> {</div>
|
|
<div class="line"><a id="l00174" name="l00174"></a><span class="lineno"> 174</span> notObstacles = <a class="code hl_function" href="group__ExtractIndices.html#ga8685395807479f06b19d9d41616a03a7">util3d::extractIndices</a>(cloud, indices, <span class="keyword">true</span>);</div>
|
|
<div class="line"><a id="l00175" name="l00175"></a><span class="lineno"> 175</span> notObstacles = <a class="code hl_function" href="namespacertabmap_1_1util3d.html#aff3c61a08b0fcad7f1c3f7c847190969">util3d::concatenate</a>(notObstacles, ground);</div>
|
|
<div class="line"><a id="l00176" name="l00176"></a><span class="lineno"> 176</span> }</div>
|
|
<div class="line"><a id="l00177" name="l00177"></a><span class="lineno"> 177</span> pcl::IndicesPtr otherStuffIndices = <a class="code hl_function" href="group__ExtractIndices.html#ga8685395807479f06b19d9d41616a03a7">util3d::extractIndices</a>(cloud, notObstacles, <span class="keyword">true</span>);</div>
|
|
<div class="line"><a id="l00178" name="l00178"></a><span class="lineno"> 178</span> </div>
|
|
<div class="line"><a id="l00179" name="l00179"></a><span class="lineno"> 179</span> <span class="comment">// If ground height is set, remove obstacles under it</span></div>
|
|
<div class="line"><a id="l00180" name="l00180"></a><span class="lineno"> 180</span> <span class="keywordflow">if</span>(maxGroundHeight != 0.0f)</div>
|
|
<div class="line"><a id="l00181" name="l00181"></a><span class="lineno"> 181</span> {</div>
|
|
<div class="line"><a id="l00182" name="l00182"></a><span class="lineno"> 182</span> otherStuffIndices = <a class="code hl_function" href="group__PassThrough.html#ga7bfc95ed4f7fca124b00707708223473">rtabmap::util3d::passThrough</a>(cloud, otherStuffIndices, <span class="stringliteral">"z"</span>, maxGroundHeight, std::numeric_limits<float>::max());</div>
|
|
<div class="line"><a id="l00183" name="l00183"></a><span class="lineno"> 183</span> }</div>
|
|
<div class="line"><a id="l00184" name="l00184"></a><span class="lineno"> 184</span> </div>
|
|
<div class="line"><a id="l00185" name="l00185"></a><span class="lineno"> 185</span> <span class="comment">//Cluster remaining stuff (obstacles)</span></div>
|
|
<div class="line"><a id="l00186" name="l00186"></a><span class="lineno"> 186</span> <span class="keywordflow">if</span>(otherStuffIndices->size())</div>
|
|
<div class="line"><a id="l00187" name="l00187"></a><span class="lineno"> 187</span> {</div>
|
|
<div class="line"><a id="l00188" name="l00188"></a><span class="lineno"> 188</span> std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = <a class="code hl_function" href="group__ExtractClusters.html#ga62a0d02930f5969cbff707afe01d1bcd">util3d::extractClusters</a>(</div>
|
|
<div class="line"><a id="l00189" name="l00189"></a><span class="lineno"> 189</span> cloud,</div>
|
|
<div class="line"><a id="l00190" name="l00190"></a><span class="lineno"> 190</span> otherStuffIndices,</div>
|
|
<div class="line"><a id="l00191" name="l00191"></a><span class="lineno"> 191</span> clusterRadius,</div>
|
|
<div class="line"><a id="l00192" name="l00192"></a><span class="lineno"> 192</span> minClusterSize);</div>
|
|
<div class="line"><a id="l00193" name="l00193"></a><span class="lineno"> 193</span> </div>
|
|
<div class="line"><a id="l00194" name="l00194"></a><span class="lineno"> 194</span> <span class="comment">// merge indices</span></div>
|
|
<div class="line"><a id="l00195" name="l00195"></a><span class="lineno"> 195</span> obstacles = <a class="code hl_function" href="namespacertabmap_1_1util3d.html#aff3c61a08b0fcad7f1c3f7c847190969">util3d::concatenate</a>(clusteredObstaclesSurfaces);</div>
|
|
<div class="line"><a id="l00196" name="l00196"></a><span class="lineno"> 196</span> }</div>
|
|
<div class="line"><a id="l00197" name="l00197"></a><span class="lineno"> 197</span> }</div>
|
|
<div class="line"><a id="l00198" name="l00198"></a><span class="lineno"> 198</span> }</div>
|
|
<div class="line"><a id="l00199" name="l00199"></a><span class="lineno"> 199</span>}</div>
|
|
<div class="line"><a id="l00200" name="l00200"></a><span class="lineno"> 200</span> </div>
|
|
<div class="line"><a id="l00201" name="l00201"></a><span class="lineno"> 201</span><span class="keyword">template</span><<span class="keyword">typename</span> Po<span class="keywordtype">int</span>T></div>
|
|
<div class="foldopen" id="foldopen00202" data-start="{" data-end="}">
|
|
<div class="line"><a id="l00202" name="l00202"></a><span class="lineno"><a class="line" href="namespacertabmap_1_1util3d.html#a72b271fa98352180241614517ef3b21e"> 202</a></span><span class="keywordtype">void</span> segmentObstaclesFromGround(</div>
|
|
<div class="line"><a id="l00203" name="l00203"></a><span class="lineno"> 203</span> <span class="keyword">const</span> <span class="keyword">typename</span> pcl::PointCloud<PointT>::Ptr & cloud,</div>
|
|
<div class="line"><a id="l00204" name="l00204"></a><span class="lineno"> 204</span> pcl::IndicesPtr & ground,</div>
|
|
<div class="line"><a id="l00205" name="l00205"></a><span class="lineno"> 205</span> pcl::IndicesPtr & obstacles,</div>
|
|
<div class="line"><a id="l00206" name="l00206"></a><span class="lineno"> 206</span> <span class="keywordtype">int</span> normalKSearch,</div>
|
|
<div class="line"><a id="l00207" name="l00207"></a><span class="lineno"> 207</span> <span class="keywordtype">float</span> groundNormalAngle,</div>
|
|
<div class="line"><a id="l00208" name="l00208"></a><span class="lineno"> 208</span> <span class="keywordtype">float</span> clusterRadius,</div>
|
|
<div class="line"><a id="l00209" name="l00209"></a><span class="lineno"> 209</span> <span class="keywordtype">int</span> minClusterSize,</div>
|
|
<div class="line"><a id="l00210" name="l00210"></a><span class="lineno"> 210</span> <span class="keywordtype">bool</span> segmentFlatObstacles,</div>
|
|
<div class="line"><a id="l00211" name="l00211"></a><span class="lineno"> 211</span> <span class="keywordtype">float</span> maxGroundHeight,</div>
|
|
<div class="line"><a id="l00212" name="l00212"></a><span class="lineno"> 212</span> pcl::IndicesPtr * flatObstacles,</div>
|
|
<div class="line"><a id="l00213" name="l00213"></a><span class="lineno"> 213</span> <span class="keyword">const</span> Eigen::Vector4f & viewPoint,</div>
|
|
<div class="line"><a id="l00214" name="l00214"></a><span class="lineno"> 214</span> <span class="keywordtype">float</span> groundNormalsUp)</div>
|
|
<div class="line"><a id="l00215" name="l00215"></a><span class="lineno"> 215</span>{</div>
|
|
<div class="line"><a id="l00216" name="l00216"></a><span class="lineno"> 216</span> pcl::IndicesPtr indices(<span class="keyword">new</span> std::vector<int>);</div>
|
|
<div class="line"><a id="l00217" name="l00217"></a><span class="lineno"> 217</span> segmentObstaclesFromGround<PointT>(</div>
|
|
<div class="line"><a id="l00218" name="l00218"></a><span class="lineno"> 218</span> cloud,</div>
|
|
<div class="line"><a id="l00219" name="l00219"></a><span class="lineno"> 219</span> indices,</div>
|
|
<div class="line"><a id="l00220" name="l00220"></a><span class="lineno"> 220</span> ground,</div>
|
|
<div class="line"><a id="l00221" name="l00221"></a><span class="lineno"> 221</span> obstacles,</div>
|
|
<div class="line"><a id="l00222" name="l00222"></a><span class="lineno"> 222</span> normalKSearch,</div>
|
|
<div class="line"><a id="l00223" name="l00223"></a><span class="lineno"> 223</span> groundNormalAngle,</div>
|
|
<div class="line"><a id="l00224" name="l00224"></a><span class="lineno"> 224</span> clusterRadius,</div>
|
|
<div class="line"><a id="l00225" name="l00225"></a><span class="lineno"> 225</span> minClusterSize,</div>
|
|
<div class="line"><a id="l00226" name="l00226"></a><span class="lineno"> 226</span> segmentFlatObstacles,</div>
|
|
<div class="line"><a id="l00227" name="l00227"></a><span class="lineno"> 227</span> maxGroundHeight,</div>
|
|
<div class="line"><a id="l00228" name="l00228"></a><span class="lineno"> 228</span> flatObstacles,</div>
|
|
<div class="line"><a id="l00229" name="l00229"></a><span class="lineno"> 229</span> viewPoint,</div>
|
|
<div class="line"><a id="l00230" name="l00230"></a><span class="lineno"> 230</span> groundNormalsUp);</div>
|
|
<div class="line"><a id="l00231" name="l00231"></a><span class="lineno"> 231</span>}</div>
|
|
</div>
|
|
<div class="line"><a id="l00232" name="l00232"></a><span class="lineno"> 232</span> </div>
|
|
<div class="line"><a id="l00233" name="l00233"></a><span class="lineno"> 233</span><span class="keyword">template</span><<span class="keyword">typename</span> Po<span class="keywordtype">int</span>T></div>
|
|
<div class="foldopen" id="foldopen00234" data-start="{" data-end="}">
|
|
<div class="line"><a id="l00234" name="l00234"></a><span class="lineno"><a class="line" href="namespacertabmap_1_1util3d.html#ab156e4308c21e118ea0aa164a5ae9812"> 234</a></span><span class="keywordtype">void</span> <a class="code hl_function" href="namespacertabmap_1_1util3d.html#ab156e4308c21e118ea0aa164a5ae9812">occupancy2DFromGroundObstacles</a>(</div>
|
|
<div class="line"><a id="l00235" name="l00235"></a><span class="lineno"> 235</span> <span class="keyword">const</span> <span class="keyword">typename</span> pcl::PointCloud<PointT>::Ptr & cloud,</div>
|
|
<div class="line"><a id="l00236" name="l00236"></a><span class="lineno"> 236</span> <span class="keyword">const</span> pcl::IndicesPtr & groundIndices,</div>
|
|
<div class="line"><a id="l00237" name="l00237"></a><span class="lineno"> 237</span> <span class="keyword">const</span> pcl::IndicesPtr & obstaclesIndices,</div>
|
|
<div class="line"><a id="l00238" name="l00238"></a><span class="lineno"> 238</span> cv::Mat & ground,</div>
|
|
<div class="line"><a id="l00239" name="l00239"></a><span class="lineno"> 239</span> cv::Mat & obstacles,</div>
|
|
<div class="line"><a id="l00240" name="l00240"></a><span class="lineno"> 240</span> <span class="keywordtype">float</span> cellSize)</div>
|
|
<div class="line"><a id="l00241" name="l00241"></a><span class="lineno"> 241</span>{</div>
|
|
<div class="line"><a id="l00242" name="l00242"></a><span class="lineno"> 242</span> <span class="keyword">typename</span> pcl::PointCloud<PointT>::Ptr groundCloud(<span class="keyword">new</span> pcl::PointCloud<PointT>);</div>
|
|
<div class="line"><a id="l00243" name="l00243"></a><span class="lineno"> 243</span> <span class="keyword">typename</span> pcl::PointCloud<PointT>::Ptr obstaclesCloud(<span class="keyword">new</span> pcl::PointCloud<PointT>);</div>
|
|
<div class="line"><a id="l00244" name="l00244"></a><span class="lineno"> 244</span> </div>
|
|
<div class="line"><a id="l00245" name="l00245"></a><span class="lineno"> 245</span> <span class="keywordflow">if</span>(groundIndices->size())</div>
|
|
<div class="line"><a id="l00246" name="l00246"></a><span class="lineno"> 246</span> {</div>
|
|
<div class="line"><a id="l00247" name="l00247"></a><span class="lineno"> 247</span> pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);</div>
|
|
<div class="line"><a id="l00248" name="l00248"></a><span class="lineno"> 248</span> }</div>
|
|
<div class="line"><a id="l00249" name="l00249"></a><span class="lineno"> 249</span> </div>
|
|
<div class="line"><a id="l00250" name="l00250"></a><span class="lineno"> 250</span> <span class="keywordflow">if</span>(obstaclesIndices->size())</div>
|
|
<div class="line"><a id="l00251" name="l00251"></a><span class="lineno"> 251</span> {</div>
|
|
<div class="line"><a id="l00252" name="l00252"></a><span class="lineno"> 252</span> pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);</div>
|
|
<div class="line"><a id="l00253" name="l00253"></a><span class="lineno"> 253</span> }</div>
|
|
<div class="line"><a id="l00254" name="l00254"></a><span class="lineno"> 254</span> </div>
|
|
<div class="line"><a id="l00255" name="l00255"></a><span class="lineno"> 255</span> occupancy2DFromGroundObstacles<PointT>(</div>
|
|
<div class="line"><a id="l00256" name="l00256"></a><span class="lineno"> 256</span> groundCloud,</div>
|
|
<div class="line"><a id="l00257" name="l00257"></a><span class="lineno"> 257</span> obstaclesCloud,</div>
|
|
<div class="line"><a id="l00258" name="l00258"></a><span class="lineno"> 258</span> ground,</div>
|
|
<div class="line"><a id="l00259" name="l00259"></a><span class="lineno"> 259</span> obstacles,</div>
|
|
<div class="line"><a id="l00260" name="l00260"></a><span class="lineno"> 260</span> cellSize);</div>
|
|
<div class="line"><a id="l00261" name="l00261"></a><span class="lineno"> 261</span>}</div>
|
|
</div>
|
|
<div class="line"><a id="l00262" name="l00262"></a><span class="lineno"> 262</span> </div>
|
|
<div class="line"><a id="l00263" name="l00263"></a><span class="lineno"> 263</span><span class="keyword">template</span><<span class="keyword">typename</span> Po<span class="keywordtype">int</span>T></div>
|
|
<div class="foldopen" id="foldopen00264" data-start="{" data-end="}">
|
|
<div class="line"><a id="l00264" name="l00264"></a><span class="lineno"><a class="line" href="namespacertabmap_1_1util3d.html#a9b93d07997a94a78b6be6bd573e39148"> 264</a></span><span class="keywordtype">void</span> <a class="code hl_function" href="namespacertabmap_1_1util3d.html#ab156e4308c21e118ea0aa164a5ae9812">occupancy2DFromGroundObstacles</a>(</div>
|
|
<div class="line"><a id="l00265" name="l00265"></a><span class="lineno"> 265</span> <span class="keyword">const</span> <span class="keyword">typename</span> pcl::PointCloud<PointT>::Ptr & groundCloud,</div>
|
|
<div class="line"><a id="l00266" name="l00266"></a><span class="lineno"> 266</span> <span class="keyword">const</span> <span class="keyword">typename</span> pcl::PointCloud<PointT>::Ptr & obstaclesCloud,</div>
|
|
<div class="line"><a id="l00267" name="l00267"></a><span class="lineno"> 267</span> cv::Mat & ground,</div>
|
|
<div class="line"><a id="l00268" name="l00268"></a><span class="lineno"> 268</span> cv::Mat & obstacles,</div>
|
|
<div class="line"><a id="l00269" name="l00269"></a><span class="lineno"> 269</span> <span class="keywordtype">float</span> cellSize)</div>
|
|
<div class="line"><a id="l00270" name="l00270"></a><span class="lineno"> 270</span>{</div>
|
|
<div class="line"><a id="l00271" name="l00271"></a><span class="lineno"> 271</span> ground = cv::Mat();</div>
|
|
<div class="line"><a id="l00272" name="l00272"></a><span class="lineno"> 272</span> <span class="keywordflow">if</span>(groundCloud->size())</div>
|
|
<div class="line"><a id="l00273" name="l00273"></a><span class="lineno"> 273</span> {</div>
|
|
<div class="line"><a id="l00274" name="l00274"></a><span class="lineno"> 274</span> <span class="comment">//project on XY plane</span></div>
|
|
<div class="line"><a id="l00275" name="l00275"></a><span class="lineno"> 275</span> <span class="keyword">typename</span> pcl::PointCloud<PointT>::Ptr groundCloudProjected;</div>
|
|
<div class="line"><a id="l00276" name="l00276"></a><span class="lineno"> 276</span> groundCloudProjected = <a class="code hl_function" href="namespacertabmap_1_1util3d.html#a3bdae076cc5054f35393b744f478b980">util3d::projectCloudOnXYPlane</a>(*groundCloud);</div>
|
|
<div class="line"><a id="l00277" name="l00277"></a><span class="lineno"> 277</span> <span class="comment">//voxelize to grid cell size</span></div>
|
|
<div class="line"><a id="l00278" name="l00278"></a><span class="lineno"> 278</span> groundCloudProjected = <a class="code hl_function" href="group__VoxelFiltering.html#gad8d88a06174857cf53b1dc2cf2310d89">util3d::voxelize</a>(groundCloudProjected, cellSize);</div>
|
|
<div class="line"><a id="l00279" name="l00279"></a><span class="lineno"> 279</span> </div>
|
|
<div class="line"><a id="l00280" name="l00280"></a><span class="lineno"> 280</span> ground = cv::Mat(1, (<span class="keywordtype">int</span>)groundCloudProjected->size(), CV_32FC2);</div>
|
|
<div class="line"><a id="l00281" name="l00281"></a><span class="lineno"> 281</span> <span class="keywordflow">for</span>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i=0;i<groundCloudProjected->size(); ++i)</div>
|
|
<div class="line"><a id="l00282" name="l00282"></a><span class="lineno"> 282</span> {</div>
|
|
<div class="line"><a id="l00283" name="l00283"></a><span class="lineno"> 283</span> cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();</div>
|
|
<div class="line"><a id="l00284" name="l00284"></a><span class="lineno"> 284</span> ptr[i][0] = groundCloudProjected->at(i).x;</div>
|
|
<div class="line"><a id="l00285" name="l00285"></a><span class="lineno"> 285</span> ptr[i][1] = groundCloudProjected->at(i).y;</div>
|
|
<div class="line"><a id="l00286" name="l00286"></a><span class="lineno"> 286</span> }</div>
|
|
<div class="line"><a id="l00287" name="l00287"></a><span class="lineno"> 287</span> }</div>
|
|
<div class="line"><a id="l00288" name="l00288"></a><span class="lineno"> 288</span> </div>
|
|
<div class="line"><a id="l00289" name="l00289"></a><span class="lineno"> 289</span> obstacles = cv::Mat();</div>
|
|
<div class="line"><a id="l00290" name="l00290"></a><span class="lineno"> 290</span> <span class="keywordflow">if</span>(obstaclesCloud->size())</div>
|
|
<div class="line"><a id="l00291" name="l00291"></a><span class="lineno"> 291</span> {</div>
|
|
<div class="line"><a id="l00292" name="l00292"></a><span class="lineno"> 292</span> <span class="comment">//project on XY plane</span></div>
|
|
<div class="line"><a id="l00293" name="l00293"></a><span class="lineno"> 293</span> <span class="keyword">typename</span> pcl::PointCloud<PointT>::Ptr obstaclesCloudProjected;</div>
|
|
<div class="line"><a id="l00294" name="l00294"></a><span class="lineno"> 294</span> obstaclesCloudProjected = <a class="code hl_function" href="namespacertabmap_1_1util3d.html#a3bdae076cc5054f35393b744f478b980">util3d::projectCloudOnXYPlane</a>(*obstaclesCloud);</div>
|
|
<div class="line"><a id="l00295" name="l00295"></a><span class="lineno"> 295</span> <span class="comment">//voxelize to grid cell size</span></div>
|
|
<div class="line"><a id="l00296" name="l00296"></a><span class="lineno"> 296</span> obstaclesCloudProjected = <a class="code hl_function" href="group__VoxelFiltering.html#gad8d88a06174857cf53b1dc2cf2310d89">util3d::voxelize</a>(obstaclesCloudProjected, cellSize);</div>
|
|
<div class="line"><a id="l00297" name="l00297"></a><span class="lineno"> 297</span> </div>
|
|
<div class="line"><a id="l00298" name="l00298"></a><span class="lineno"> 298</span> obstacles = cv::Mat(1, (<span class="keywordtype">int</span>)obstaclesCloudProjected->size(), CV_32FC2);</div>
|
|
<div class="line"><a id="l00299" name="l00299"></a><span class="lineno"> 299</span> <span class="keywordflow">for</span>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i=0;i<obstaclesCloudProjected->size(); ++i)</div>
|
|
<div class="line"><a id="l00300" name="l00300"></a><span class="lineno"> 300</span> {</div>
|
|
<div class="line"><a id="l00301" name="l00301"></a><span class="lineno"> 301</span> cv::Vec2f * ptr = obstacles.ptr<cv::Vec2f>();</div>
|
|
<div class="line"><a id="l00302" name="l00302"></a><span class="lineno"> 302</span> ptr[i][0] = obstaclesCloudProjected->at(i).x;</div>
|
|
<div class="line"><a id="l00303" name="l00303"></a><span class="lineno"> 303</span> ptr[i][1] = obstaclesCloudProjected->at(i).y;</div>
|
|
<div class="line"><a id="l00304" name="l00304"></a><span class="lineno"> 304</span> }</div>
|
|
<div class="line"><a id="l00305" name="l00305"></a><span class="lineno"> 305</span> }</div>
|
|
<div class="line"><a id="l00306" name="l00306"></a><span class="lineno"> 306</span>}</div>
|
|
</div>
|
|
<div class="line"><a id="l00307" name="l00307"></a><span class="lineno"> 307</span> </div>
|
|
<div class="line"><a id="l00308" name="l00308"></a><span class="lineno"> 308</span><span class="keyword">template</span><<span class="keyword">typename</span> Po<span class="keywordtype">int</span>T></div>
|
|
<div class="foldopen" id="foldopen00309" data-start="{" data-end="}">
|
|
<div class="line"><a id="l00309" name="l00309"></a><span class="lineno"><a class="line" href="namespacertabmap_1_1util3d.html#a7d90f3a00301f9f7c79cd6fc6cd663c2"> 309</a></span><span class="keywordtype">void</span> <a class="code hl_function" href="namespacertabmap_1_1util3d.html#a7d90f3a00301f9f7c79cd6fc6cd663c2">occupancy2DFromCloud3D</a>(</div>
|
|
<div class="line"><a id="l00310" name="l00310"></a><span class="lineno"> 310</span> <span class="keyword">const</span> <span class="keyword">typename</span> pcl::PointCloud<PointT>::Ptr & cloud,</div>
|
|
<div class="line"><a id="l00311" name="l00311"></a><span class="lineno"> 311</span> <span class="keyword">const</span> pcl::IndicesPtr & indices,</div>
|
|
<div class="line"><a id="l00312" name="l00312"></a><span class="lineno"> 312</span> cv::Mat & ground,</div>
|
|
<div class="line"><a id="l00313" name="l00313"></a><span class="lineno"> 313</span> cv::Mat & obstacles,</div>
|
|
<div class="line"><a id="l00314" name="l00314"></a><span class="lineno"> 314</span> <span class="keywordtype">float</span> cellSize,</div>
|
|
<div class="line"><a id="l00315" name="l00315"></a><span class="lineno"> 315</span> <span class="keywordtype">float</span> groundNormalAngle,</div>
|
|
<div class="line"><a id="l00316" name="l00316"></a><span class="lineno"> 316</span> <span class="keywordtype">int</span> minClusterSize,</div>
|
|
<div class="line"><a id="l00317" name="l00317"></a><span class="lineno"> 317</span> <span class="keywordtype">bool</span> segmentFlatObstacles,</div>
|
|
<div class="line"><a id="l00318" name="l00318"></a><span class="lineno"> 318</span> <span class="keywordtype">float</span> maxGroundHeight)</div>
|
|
<div class="line"><a id="l00319" name="l00319"></a><span class="lineno"> 319</span>{</div>
|
|
<div class="line"><a id="l00320" name="l00320"></a><span class="lineno"> 320</span> <span class="keywordflow">if</span>(cloud->size() == 0)</div>
|
|
<div class="line"><a id="l00321" name="l00321"></a><span class="lineno"> 321</span> {</div>
|
|
<div class="line"><a id="l00322" name="l00322"></a><span class="lineno"> 322</span> <span class="keywordflow">return</span>;</div>
|
|
<div class="line"><a id="l00323" name="l00323"></a><span class="lineno"> 323</span> }</div>
|
|
<div class="line"><a id="l00324" name="l00324"></a><span class="lineno"> 324</span> pcl::IndicesPtr groundIndices, obstaclesIndices;</div>
|
|
<div class="line"><a id="l00325" name="l00325"></a><span class="lineno"> 325</span> </div>
|
|
<div class="line"><a id="l00326" name="l00326"></a><span class="lineno"> 326</span> segmentObstaclesFromGround<PointT>(</div>
|
|
<div class="line"><a id="l00327" name="l00327"></a><span class="lineno"> 327</span> cloud,</div>
|
|
<div class="line"><a id="l00328" name="l00328"></a><span class="lineno"> 328</span> indices,</div>
|
|
<div class="line"><a id="l00329" name="l00329"></a><span class="lineno"> 329</span> groundIndices,</div>
|
|
<div class="line"><a id="l00330" name="l00330"></a><span class="lineno"> 330</span> obstaclesIndices,</div>
|
|
<div class="line"><a id="l00331" name="l00331"></a><span class="lineno"> 331</span> 20,</div>
|
|
<div class="line"><a id="l00332" name="l00332"></a><span class="lineno"> 332</span> groundNormalAngle,</div>
|
|
<div class="line"><a id="l00333" name="l00333"></a><span class="lineno"> 333</span> cellSize*2.0f,</div>
|
|
<div class="line"><a id="l00334" name="l00334"></a><span class="lineno"> 334</span> minClusterSize,</div>
|
|
<div class="line"><a id="l00335" name="l00335"></a><span class="lineno"> 335</span> segmentFlatObstacles,</div>
|
|
<div class="line"><a id="l00336" name="l00336"></a><span class="lineno"> 336</span> maxGroundHeight);</div>
|
|
<div class="line"><a id="l00337" name="l00337"></a><span class="lineno"> 337</span> </div>
|
|
<div class="line"><a id="l00338" name="l00338"></a><span class="lineno"> 338</span> occupancy2DFromGroundObstacles<PointT>(</div>
|
|
<div class="line"><a id="l00339" name="l00339"></a><span class="lineno"> 339</span> cloud,</div>
|
|
<div class="line"><a id="l00340" name="l00340"></a><span class="lineno"> 340</span> groundIndices,</div>
|
|
<div class="line"><a id="l00341" name="l00341"></a><span class="lineno"> 341</span> obstaclesIndices,</div>
|
|
<div class="line"><a id="l00342" name="l00342"></a><span class="lineno"> 342</span> ground,</div>
|
|
<div class="line"><a id="l00343" name="l00343"></a><span class="lineno"> 343</span> obstacles,</div>
|
|
<div class="line"><a id="l00344" name="l00344"></a><span class="lineno"> 344</span> cellSize);</div>
|
|
<div class="line"><a id="l00345" name="l00345"></a><span class="lineno"> 345</span>}</div>
|
|
</div>
|
|
<div class="line"><a id="l00346" name="l00346"></a><span class="lineno"> 346</span> </div>
|
|
<div class="line"><a id="l00347" name="l00347"></a><span class="lineno"> 347</span><span class="keyword">template</span><<span class="keyword">typename</span> Po<span class="keywordtype">int</span>T></div>
|
|
<div class="foldopen" id="foldopen00348" data-start="{" data-end="}">
|
|
<div class="line"><a id="l00348" name="l00348"></a><span class="lineno"><a class="line" href="namespacertabmap_1_1util3d.html#a969e2d135e91a6e2ace68850086ae0e9"> 348</a></span><span class="keywordtype">void</span> <a class="code hl_function" href="namespacertabmap_1_1util3d.html#a7d90f3a00301f9f7c79cd6fc6cd663c2">occupancy2DFromCloud3D</a>(</div>
|
|
<div class="line"><a id="l00349" name="l00349"></a><span class="lineno"> 349</span> <span class="keyword">const</span> <span class="keyword">typename</span> pcl::PointCloud<PointT>::Ptr & cloud,</div>
|
|
<div class="line"><a id="l00350" name="l00350"></a><span class="lineno"> 350</span> cv::Mat & ground,</div>
|
|
<div class="line"><a id="l00351" name="l00351"></a><span class="lineno"> 351</span> cv::Mat & obstacles,</div>
|
|
<div class="line"><a id="l00352" name="l00352"></a><span class="lineno"> 352</span> <span class="keywordtype">float</span> cellSize,</div>
|
|
<div class="line"><a id="l00353" name="l00353"></a><span class="lineno"> 353</span> <span class="keywordtype">float</span> groundNormalAngle,</div>
|
|
<div class="line"><a id="l00354" name="l00354"></a><span class="lineno"> 354</span> <span class="keywordtype">int</span> minClusterSize,</div>
|
|
<div class="line"><a id="l00355" name="l00355"></a><span class="lineno"> 355</span> <span class="keywordtype">bool</span> segmentFlatObstacles,</div>
|
|
<div class="line"><a id="l00356" name="l00356"></a><span class="lineno"> 356</span> <span class="keywordtype">float</span> maxGroundHeight)</div>
|
|
<div class="line"><a id="l00357" name="l00357"></a><span class="lineno"> 357</span>{</div>
|
|
<div class="line"><a id="l00358" name="l00358"></a><span class="lineno"> 358</span> pcl::IndicesPtr indices(<span class="keyword">new</span> std::vector<int>);</div>
|
|
<div class="line"><a id="l00359" name="l00359"></a><span class="lineno"> 359</span> occupancy2DFromCloud3D<PointT>(cloud, indices, ground, obstacles, cellSize, groundNormalAngle, minClusterSize, segmentFlatObstacles, maxGroundHeight);</div>
|
|
<div class="line"><a id="l00360" name="l00360"></a><span class="lineno"> 360</span>}</div>
|
|
</div>
|
|
<div class="line"><a id="l00361" name="l00361"></a><span class="lineno"> 361</span> </div>
|
|
<div class="line"><a id="l00362" name="l00362"></a><span class="lineno"> 362</span>}</div>
|
|
<div class="line"><a id="l00363" name="l00363"></a><span class="lineno"> 363</span>}</div>
|
|
<div class="line"><a id="l00364" name="l00364"></a><span class="lineno"> 364</span> </div>
|
|
<div class="line"><a id="l00365" name="l00365"></a><span class="lineno"> 365</span><span class="preprocessor">#endif </span><span class="comment">/* UTIL3D_MAPPING_HPP_ */</span><span class="preprocessor"></span></div>
|
|
<div class="ttc" id="agroup__ExtractClusters_html_ga62a0d02930f5969cbff707afe01d1bcd"><div class="ttname"><a href="group__ExtractClusters.html#ga62a0d02930f5969cbff707afe01d1bcd">rtabmap::util3d::extractClusters</a></div><div class="ttdeci">std::vector< pcl::IndicesPtr > RTABMAP_CORE_EXPORT extractClusters(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float clusterTolerance, int minClusterSize, int maxClusterSize=std::numeric_limits< int >::max(), int *biggestClusterIndex=0)</div><div class="ttdoc">Extract clusters from point cloud of type pcl::PointXYZ.</div></div>
|
|
<div class="ttc" id="agroup__ExtractIndices_html_ga8685395807479f06b19d9d41616a03a7"><div class="ttname"><a href="group__ExtractIndices.html#ga8685395807479f06b19d9d41616a03a7">rtabmap::util3d::extractIndices</a></div><div class="ttdeci">pcl::IndicesPtr RTABMAP_CORE_EXPORT extractIndices(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)</div><div class="ttdoc">Extract indices from point cloud of type pcl::PointXYZ.</div></div>
|
|
<div class="ttc" id="agroup__PassThrough_html_ga7bfc95ed4f7fca124b00707708223473"><div class="ttname"><a href="group__PassThrough.html#ga7bfc95ed4f7fca124b00707708223473">rtabmap::util3d::passThrough</a></div><div class="ttdeci">pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)</div><div class="ttdoc">Performs pass-through filtering on a point cloud of type pcl::PointXYZ and returns filtered indices.</div></div>
|
|
<div class="ttc" id="agroup__PointNormalFiltering_html_ga7c7e3de2ff24fc3aadfb3eb0ae05314d"><div class="ttname"><a href="group__PointNormalFiltering.html#ga7c7e3de2ff24fc3aadfb3eb0ae05314d">rtabmap::util3d::normalFiltering</a></div><div class="ttdeci">pcl::IndicesPtr RTABMAP_CORE_EXPORT normalFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float angleMax, const Eigen::Vector4f &normal, int normalKSearch, const Eigen::Vector4f &viewpoint, float groundNormalsUp=0.0f)</div><div class="ttdoc">Point normal filtering for point cloud of type pcl::PointXYZ.</div></div>
|
|
<div class="ttc" id="agroup__VoxelFiltering_html_gad8d88a06174857cf53b1dc2cf2310d89"><div class="ttname"><a href="group__VoxelFiltering.html#gad8d88a06174857cf53b1dc2cf2310d89">rtabmap::util3d::voxelize</a></div><div class="ttdeci">pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)</div><div class="ttdoc">Performs voxel grid downsampling on a point cloud of type pcl::PointXYZ on provided indices.</div></div>
|
|
<div class="ttc" id="anamespacertabmap_1_1util3d_html_a3bdae076cc5054f35393b744f478b980"><div class="ttname"><a href="namespacertabmap_1_1util3d.html#a3bdae076cc5054f35393b744f478b980">rtabmap::util3d::projectCloudOnXYPlane</a></div><div class="ttdeci">pcl::PointCloud< PointT >::Ptr projectCloudOnXYPlane(const typename pcl::PointCloud< PointT > &cloud)</div><div class="ttdoc">Projects a point cloud onto the XY plane by setting all Z coordinates to zero.</div><div class="ttdef"><b>Definition</b> <a href="util3d__mapping_8hpp_source.html#l00041">util3d_mapping.hpp:41</a></div></div>
|
|
<div class="ttc" id="anamespacertabmap_1_1util3d_html_a7d90f3a00301f9f7c79cd6fc6cd663c2"><div class="ttname"><a href="namespacertabmap_1_1util3d.html#a7d90f3a00301f9f7c79cd6fc6cd663c2">rtabmap::util3d::occupancy2DFromCloud3D</a></div><div class="ttdeci">void occupancy2DFromCloud3D(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, cv::Mat &ground, cv::Mat &obstacles, float cellSize, float groundNormalAngle, int minClusterSize, bool segmentFlatObstacles, float maxGroundHeight)</div><div class="ttdoc">Generates 2D ground and obstacle occupancy data from a 3D point cloud.</div><div class="ttdef"><b>Definition</b> <a href="util3d__mapping_8hpp_source.html#l00309">util3d_mapping.hpp:309</a></div></div>
|
|
<div class="ttc" id="anamespacertabmap_1_1util3d_html_ab156e4308c21e118ea0aa164a5ae9812"><div class="ttname"><a href="namespacertabmap_1_1util3d.html#ab156e4308c21e118ea0aa164a5ae9812">rtabmap::util3d::occupancy2DFromGroundObstacles</a></div><div class="ttdeci">void occupancy2DFromGroundObstacles(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &groundIndices, const pcl::IndicesPtr &obstaclesIndices, cv::Mat &ground, cv::Mat &obstacles, float cellSize)</div><div class="ttdoc">Projects 3D ground and obstacle point clouds onto the 2D XY plane and voxelizes them into 2D occupanc...</div><div class="ttdef"><b>Definition</b> <a href="util3d__mapping_8hpp_source.html#l00234">util3d_mapping.hpp:234</a></div></div>
|
|
<div class="ttc" id="anamespacertabmap_1_1util3d_html_aff3c61a08b0fcad7f1c3f7c847190969"><div class="ttname"><a href="namespacertabmap_1_1util3d.html#aff3c61a08b0fcad7f1c3f7c847190969">rtabmap::util3d::concatenate</a></div><div class="ttdeci">pcl::IndicesPtr RTABMAP_CORE_EXPORT concatenate(const std::vector< pcl::IndicesPtr > &indices)</div><div class="ttdoc">Concatenates multiple sets of indices into a single index vector.</div></div>
|
|
<div class="ttc" id="anamespacertabmap_html"><div class="ttname"><a href="namespacertabmap.html">rtabmap</a></div><div class="ttdef"><b>Definition</b> <a href="BayesFilter_8h_source.html#l00039">BayesFilter.h:39</a></div></div>
|
|
</div><!-- fragment --></div><!-- contents -->
|
|
</div><!-- doc-content -->
|
|
<!-- start footer part -->
|
|
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
|
|
<ul>
|
|
<li class="navelem"><a class="el" href="dir_04d49acea3c42cab2afcd35cb2f2a48f.html">corelib</a></li><li class="navelem"><a class="el" href="dir_e996b11fd4b3a747423759b16a8ba4a6.html">include</a></li><li class="navelem"><a class="el" href="dir_ff01f7bc5399ccf9ff1cc91d1f45726a.html">rtabmap</a></li><li class="navelem"><a class="el" href="dir_884419f21456e80aa47bc1d0c2ab0140.html">core</a></li><li class="navelem"><a class="el" href="dir_67ee77bab1d00e5527b3ba66cff8b822.html">impl</a></li><li class="navelem"><b>util3d_mapping.hpp</b></li>
|
|
<li class="footer">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.8 </li>
|
|
</ul>
|
|
</div>
|
|
</body>
|
|
</html>
|