2026-08-06 22:56:24 +00:00
<!-- 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_surface.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" >
2026-09-20 06:51:14 +00:00
< div id = "projectname" > RTAB-Map< span id = "projectnumber" >   0.23.12</ span >
2026-08-06 22:56:24 +00:00
</ 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__surface_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_surface.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" > * util3d_surface.hpp</ span ></ div >
< div class = "line" >< a id = "l00003" name = "l00003" ></ a >< span class = "lineno" > 3</ span >< span class = "comment" > *</ span ></ div >
< div class = "line" >< a id = "l00004" name = "l00004" ></ a >< span class = "lineno" > 4</ span >< span class = "comment" > * Created on: Sep 3, 2016</ span ></ div >
< div class = "line" >< a id = "l00005" name = "l00005" ></ a >< span class = "lineno" > 5</ span >< span class = "comment" > * Author: mathieu</ span ></ div >
< div class = "line" >< a id = "l00006" name = "l00006" ></ a >< span class = "lineno" > 6</ span >< span class = "comment" > */</ span ></ div >
< div class = "line" >< a id = "l00007" name = "l00007" ></ a >< span class = "lineno" > 7</ span > </ div >
< div class = "line" >< a id = "l00008" name = "l00008" ></ a >< span class = "lineno" > 8</ span >< span class = "preprocessor" > #ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_</ span ></ div >
< div class = "line" >< a id = "l00009" name = "l00009" ></ a >< span class = "lineno" > 9</ span >< span class = "preprocessor" > #define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_</ span ></ div >
< div class = "line" >< a id = "l00010" name = "l00010" ></ a >< span class = "lineno" > 10</ span > </ div >
< div class = "line" >< a id = "l00011" name = "l00011" ></ a >< span class = "lineno" > 11</ span >< span class = "preprocessor" > #include < pcl/search/kdtree.h> </ span ></ div >
< div class = "line" >< a id = "l00012" name = "l00012" ></ a >< span class = "lineno" > 12</ span >< span class = "preprocessor" > #include < pcl/conversions.h> </ span ></ div >
< div class = "line" >< a id = "l00013" name = "l00013" ></ a >< span class = "lineno" > 13</ span >< span class = "preprocessor" > #include < < a class = "code" href = "UConversion_8h.html" > rtabmap/utilite/UConversion.h</ a > > </ span ></ div >
< div class = "line" >< a id = "l00014" name = "l00014" ></ a >< span class = "lineno" > 14</ span > </ div >
< div class = "line" >< a id = "l00015" name = "l00015" ></ a >< span class = "lineno" > 15</ span >< span class = "keyword" > namespace </ span >< a class = "code hl_namespace" href = "namespacertabmap.html" > rtabmap</ a > {</ div >
< div class = "line" >< a id = "l00016" name = "l00016" ></ a >< span class = "lineno" > 16</ span > </ div >
< div class = "line" >< a id = "l00017" name = "l00017" ></ a >< span class = "lineno" > 17</ span >< span class = "keyword" > namespace </ span > util3d {</ div >
< div class = "line" >< a id = "l00018" name = "l00018" ></ a >< span class = "lineno" > 18</ span > </ div >
< div class = "line" >< a id = "l00019" name = "l00019" ></ a >< span class = "lineno" > 19</ span >< span class = "keyword" > template</ span > < < span class = "keyword" > typename</ span > po< span class = "keywordtype" > int</ span > T> </ div >
< div class = "line" >< a id = "l00020" name = "l00020" ></ a >< span class = "lineno" > 20</ span > std::vector< pcl::Vertices> normalizePolygonsSide(</ div >
< div class = "line" >< a id = "l00021" name = "l00021" ></ a >< span class = "lineno" > 21</ span > < span class = "keyword" > const</ span > < span class = "keyword" > typename</ span > pcl::PointCloud< pointT> & cloud,</ div >
< div class = "line" >< a id = "l00022" name = "l00022" ></ a >< span class = "lineno" > 22</ span > < span class = "keyword" > const</ span > std::vector< pcl::Vertices> & polygons,</ div >
< div class = "line" >< a id = "l00023" name = "l00023" ></ a >< span class = "lineno" > 23</ span > < span class = "keyword" > const</ span > pcl::PointXYZ & viewPoint)</ div >
< div class = "line" >< a id = "l00024" name = "l00024" ></ a >< span class = "lineno" > 24</ span > {</ div >
< div class = "line" >< a id = "l00025" name = "l00025" ></ a >< span class = "lineno" > 25</ span > std::vector< pcl::Vertices> output(polygons.size());</ div >
< div class = "line" >< a id = "l00026" name = "l00026" ></ a >< span class = "lineno" > 26</ span > < span class = "keywordflow" > for</ span > (< span class = "keywordtype" > unsigned</ span > < span class = "keywordtype" > int</ span > i=0; i< polygons.size(); ++i)</ 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 > pcl::Vertices polygon = polygons[i];</ div >
< div class = "line" >< a id = "l00029" name = "l00029" ></ a >< span class = "lineno" > 29</ span > Eigen::Vector3f v1 = cloud.at(polygon.vertices[1]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();</ div >
< div class = "line" >< a id = "l00030" name = "l00030" ></ a >< span class = "lineno" > 30</ span > Eigen::Vector3f v2 = cloud.at(polygon.vertices[2]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();</ div >
< div class = "line" >< a id = "l00031" name = "l00031" ></ a >< span class = "lineno" > 31</ span > Eigen::Vector3f n = (v1.cross(v2)).normalized();</ div >
< div class = "line" >< a id = "l00032" name = "l00032" ></ a >< span class = "lineno" > 32</ span > </ div >
< div class = "line" >< a id = "l00033" name = "l00033" ></ a >< span class = "lineno" > 33</ span > Eigen::Vector3f p = Eigen::Vector3f(viewPoint.x, viewPoint.y, viewPoint.z) - cloud.at(polygon.vertices[1]).getVector3fMap();</ div >
< div class = "line" >< a id = "l00034" name = "l00034" ></ a >< span class = "lineno" > 34</ span > </ div >
< div class = "line" >< a id = "l00035" name = "l00035" ></ a >< span class = "lineno" > 35</ span > < span class = "keywordtype" > float</ span > result = n.dot(p);</ div >
< div class = "line" >< a id = "l00036" name = "l00036" ></ a >< span class = "lineno" > 36</ span > < span class = "keywordflow" > if</ span > (result < 0)</ div >
< div class = "line" >< a id = "l00037" name = "l00037" ></ a >< span class = "lineno" > 37</ span > {</ div >
< div class = "line" >< a id = "l00038" name = "l00038" ></ a >< span class = "lineno" > 38</ span > < span class = "comment" > //reverse vertices order</ span ></ div >
< div class = "line" >< a id = "l00039" name = "l00039" ></ a >< span class = "lineno" > 39</ span > < span class = "keywordtype" > int</ span > tmp = polygon.vertices[0];</ div >
< div class = "line" >< a id = "l00040" name = "l00040" ></ a >< span class = "lineno" > 40</ span > polygon.vertices[0] = polygon.vertices[2];</ div >
< div class = "line" >< a id = "l00041" name = "l00041" ></ a >< span class = "lineno" > 41</ span > polygon.vertices[2] = tmp;</ div >
< div class = "line" >< a id = "l00042" name = "l00042" ></ a >< span class = "lineno" > 42</ span > }</ 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 > output[i] = polygon;</ div >
< div class = "line" >< a id = "l00045" name = "l00045" ></ a >< span class = "lineno" > 45</ span > }</ div >
< div class = "line" >< a id = "l00046" name = "l00046" ></ a >< span class = "lineno" > 46</ span > < span class = "keywordflow" > return</ span > output;</ 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 > </ div >
< div class = "line" >< a id = "l00049" name = "l00049" ></ a >< span class = "lineno" > 49</ span >< span class = "keyword" > template</ span > < < span class = "keyword" > typename</ span > po< span class = "keywordtype" > int</ span > RGBT> </ div >
< div class = "line" >< a id = "l00050" name = "l00050" ></ a >< span class = "lineno" > 50</ span >< span class = "keywordtype" > void</ span > denseMeshPostProcessing(</ div >
< div class = "line" >< a id = "l00051" name = "l00051" ></ a >< span class = "lineno" > 51</ span > pcl::PolygonMeshPtr & mesh,</ div >
< div class = "line" >< a id = "l00052" name = "l00052" ></ a >< span class = "lineno" > 52</ span > < span class = "keywordtype" > float</ span > meshDecimationFactor,</ div >
< div class = "line" >< a id = "l00053" name = "l00053" ></ a >< span class = "lineno" > 53</ span > < span class = "keywordtype" > int</ span > maximumPolygons,</ div >
< div class = "line" >< a id = "l00054" name = "l00054" ></ a >< span class = "lineno" > 54</ span > < span class = "keyword" > const</ span > < span class = "keyword" > typename</ span > pcl::PointCloud< pointRGBT> ::Ptr & cloud,</ div >
< div class = "line" >< a id = "l00055" name = "l00055" ></ a >< span class = "lineno" > 55</ span > < span class = "keywordtype" > float</ span > transferColorRadius,</ div >
< div class = "line" >< a id = "l00056" name = "l00056" ></ a >< span class = "lineno" > 56</ span > < span class = "keywordtype" > bool</ span > coloredOutput,</ div >
< div class = "line" >< a id = "l00057" name = "l00057" ></ a >< span class = "lineno" > 57</ span > < span class = "keywordtype" > bool</ span > cleanMesh,</ div >
< div class = "line" >< a id = "l00058" name = "l00058" ></ a >< span class = "lineno" > 58</ span > < span class = "keywordtype" > int</ span > minClusterSize,</ div >
< div class = "line" >< a id = "l00059" name = "l00059" ></ a >< span class = "lineno" > 59</ span > ProgressState * progressState)</ div >
< div class = "line" >< a id = "l00060" name = "l00060" ></ a >< span class = "lineno" > 60</ span > {</ div >
< div class = "line" >< a id = "l00061" name = "l00061" ></ a >< span class = "lineno" > 61</ span > < span class = "comment" > // compute normals for the mesh if not already here</ span ></ div >
< div class = "line" >< a id = "l00062" name = "l00062" ></ a >< span class = "lineno" > 62</ span > < span class = "keywordtype" > bool</ span > hasNormals = < span class = "keyword" > false</ span > ;</ div >
< div class = "line" >< a id = "l00063" name = "l00063" ></ a >< span class = "lineno" > 63</ span > < span class = "keywordtype" > bool</ span > hasColors = < span class = "keyword" > false</ span > ;</ div >
< div class = "line" >< a id = "l00064" name = "l00064" ></ a >< span class = "lineno" > 64</ span > < span class = "keywordflow" > for</ span > (< span class = "keywordtype" > unsigned</ span > < span class = "keywordtype" > int</ span > i=0; i< mesh-> cloud.fields.size(); ++i)</ div >
< div class = "line" >< a id = "l00065" name = "l00065" ></ a >< span class = "lineno" > 65</ span > {</ div >
< div class = "line" >< a id = "l00066" name = "l00066" ></ a >< span class = "lineno" > 66</ span > < span class = "keywordflow" > if</ span > (mesh-> cloud.fields[i].name.compare(< span class = "stringliteral" > " normal_x" </ span > ) == 0)</ div >
< div class = "line" >< a id = "l00067" name = "l00067" ></ a >< span class = "lineno" > 67</ span > {</ div >
< div class = "line" >< a id = "l00068" name = "l00068" ></ a >< span class = "lineno" > 68</ span > hasNormals = < span class = "keyword" > true</ span > ;</ div >
< div class = "line" >< a id = "l00069" name = "l00069" ></ a >< span class = "lineno" > 69</ span > }</ div >
< div class = "line" >< a id = "l00070" name = "l00070" ></ a >< span class = "lineno" > 70</ span > < span class = "keywordflow" > else</ span > < span class = "keywordflow" > if</ span > (mesh-> cloud.fields[i].name.compare(< span class = "stringliteral" > " rgb" </ span > ) == 0)</ div >
< div class = "line" >< a id = "l00071" name = "l00071" ></ a >< span class = "lineno" > 71</ span > {</ div >
< div class = "line" >< a id = "l00072" name = "l00072" ></ a >< span class = "lineno" > 72</ span > hasColors = < span class = "keyword" > true</ span > ;</ div >
< div class = "line" >< a id = "l00073" name = "l00073" ></ a >< span class = "lineno" > 73</ span > }</ 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 > (maximumPolygons > 0)</ 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 = "keywordtype" > double</ span > factor = 1.0-double(maximumPolygons)/double(mesh-> polygons.size());</ div >
< div class = "line" >< a id = "l00079" name = "l00079" ></ a >< span class = "lineno" > 79</ span > < span class = "keywordflow" > if</ span > (factor > meshDecimationFactor)</ div >
< div class = "line" >< a id = "l00080" name = "l00080" ></ a >< span class = "lineno" > 80</ span > {</ div >
< div class = "line" >< a id = "l00081" name = "l00081" ></ a >< span class = "lineno" > 81</ span > meshDecimationFactor = factor;</ div >
< div class = "line" >< a id = "l00082" name = "l00082" ></ a >< span class = "lineno" > 82</ span > }</ div >
< div class = "line" >< a id = "l00083" name = "l00083" ></ a >< span class = "lineno" > 83</ span > }</ div >
< div class = "line" >< a id = "l00084" name = "l00084" ></ a >< span class = "lineno" > 84</ span > < span class = "keywordflow" > if</ span > (meshDecimationFactor > 0.0)</ div >
< div class = "line" >< a id = "l00085" name = "l00085" ></ a >< span class = "lineno" > 85</ span > {</ div >
< div class = "line" >< a id = "l00086" name = "l00086" ></ a >< span class = "lineno" > 86</ span > < span class = "keywordtype" > unsigned</ span > < span class = "keywordtype" > int</ span > count = mesh-> polygons.size();</ div >
< div class = "line" >< a id = "l00087" name = "l00087" ></ a >< span class = "lineno" > 87</ span > < span class = "keywordflow" > if</ span > (progressState) progressState-> callback(< a class = "code hl_function" href = "UConversion_8h.html#a59ad5e2b3368d3dd19d73a24159388ce" > uFormat</ a > (< span class = "stringliteral" > " Mesh decimation (factor=%f) from %d polygons..." </ span > ,meshDecimationFactor, (< span class = "keywordtype" > int</ span > )count));</ div >
< div class = "line" >< a id = "l00088" name = "l00088" ></ a >< span class = "lineno" > 88</ span > </ div >
< div class = "line" >< a id = "l00089" name = "l00089" ></ a >< span class = "lineno" > 89</ span > mesh = util3d::meshDecimation(mesh, (< span class = "keywordtype" > float</ span > )meshDecimationFactor);</ div >
< div class = "line" >< a id = "l00090" name = "l00090" ></ a >< span class = "lineno" > 90</ span > < span class = "keywordflow" > if</ span > (progressState) progressState-> callback(< a class = "code hl_function" href = "UConversion_8h.html#a59ad5e2b3368d3dd19d73a24159388ce" > uFormat</ a > (< span class = "stringliteral" > " Mesh decimated (factor=%f) from %d to %d polygons" </ span > , meshDecimationFactor, (< span class = "keywordtype" > int</ span > )count, (< span class = "keywordtype" > int</ span > )mesh-> polygons.size()));</ div >
< div class = "line" >< a id = "l00091" name = "l00091" ></ a >< span class = "lineno" > 91</ span > < span class = "keywordflow" > if</ span > (count < mesh-> polygons.size())</ div >
< div class = "line" >< a id = "l00092" name = "l00092" ></ a >< span class = "lineno" > 92</ span > {</ div >
< div class = "line" >< a id = "l00093" name = "l00093" ></ a >< span class = "lineno" > 93</ span > < span class = "keywordflow" > if</ span > (progressState) progressState-> callback(< a class = "code hl_function" href = "UConversion_8h.html#a59ad5e2b3368d3dd19d73a24159388ce" > uFormat</ a > (< span class = "stringliteral" > " Decimated mesh has more polygons than before!" </ span > ));</ div >
< div class = "line" >< a id = "l00094" name = "l00094" ></ a >< span class = "lineno" > 94</ span > }</ div >
< div class = "line" >< a id = "l00095" name = "l00095" ></ a >< span class = "lineno" > 95</ span > hasNormals = < span class = "keyword" > false</ span > ;</ div >
< div class = "line" >< a id = "l00096" name = "l00096" ></ a >< span class = "lineno" > 96</ span > hasColors = < span class = "keyword" > false</ span > ;</ div >
< div class = "line" >< a id = "l00097" name = "l00097" ></ a >< span class = "lineno" > 97</ span > }</ div >
< div class = "line" >< a id = "l00098" name = "l00098" ></ a >< span class = "lineno" > 98</ span > </ div >
< div class = "line" >< a id = "l00099" name = "l00099" ></ a >< span class = "lineno" > 99</ span > < span class = "keywordflow" > if</ span > (cloud.get()!=0 && </ div >
< div class = "line" >< a id = "l00100" name = "l00100" ></ a >< span class = "lineno" > 100</ span > !hasColors && </ div >
< div class = "line" >< a id = "l00101" name = "l00101" ></ a >< span class = "lineno" > 101</ span > transferColorRadius > = 0.0)</ 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 > < span class = "keywordflow" > if</ span > (progressState) progressState-> callback(< a class = "code hl_function" href = "UConversion_8h.html#a59ad5e2b3368d3dd19d73a24159388ce" > uFormat</ a > (< span class = "stringliteral" > " Transferring color from point cloud to mesh..." </ span > ));</ div >
< div class = "line" >< a id = "l00104" name = "l00104" ></ a >< span class = "lineno" > 104</ span > </ div >
< div class = "line" >< a id = "l00105" name = "l00105" ></ a >< span class = "lineno" > 105</ span > < span class = "comment" > // transfer color from point cloud to mesh</ span ></ div >
< div class = "line" >< a id = "l00106" name = "l00106" ></ a >< span class = "lineno" > 106</ span > < span class = "keyword" > typename</ span > pcl::search::KdTree< pointRGBT> ::Ptr tree (< span class = "keyword" > new</ span > pcl::search::KdTree< pointRGBT> (< span class = "keyword" > true</ span > ));</ div >
< div class = "line" >< a id = "l00107" name = "l00107" ></ a >< span class = "lineno" > 107</ span > tree-> setInputCloud(cloud);</ div >
< div class = "line" >< a id = "l00108" name = "l00108" ></ a >< span class = "lineno" > 108</ span > pcl::PointCloud< pcl::PointXYZRGBNormal> ::Ptr coloredCloud(< span class = "keyword" > new</ span > pcl::PointCloud< pcl::PointXYZRGBNormal> );</ div >
< div class = "line" >< a id = "l00109" name = "l00109" ></ a >< span class = "lineno" > 109</ span > pcl::fromPCLPointCloud2(mesh-> cloud, *coloredCloud);</ div >
< div class = "line" >< a id = "l00110" name = "l00110" ></ a >< span class = "lineno" > 110</ span > std::vector< bool> coloredPts(coloredCloud-> size());</ div >
< div class = "line" >< a id = "l00111" name = "l00111" ></ a >< span class = "lineno" > 111</ span > < span class = "keywordflow" > for</ span > (< span class = "keywordtype" > unsigned</ span > < span class = "keywordtype" > int</ span > i=0; i< coloredCloud-> size(); ++i)</ div >
< div class = "line" >< a id = "l00112" name = "l00112" ></ a >< span class = "lineno" > 112</ span > {</ div >
< div class = "line" >< a id = "l00113" name = "l00113" ></ a >< span class = "lineno" > 113</ span > std::vector< int> kIndices;</ div >
< div class = "line" >< a id = "l00114" name = "l00114" ></ a >< span class = "lineno" > 114</ span > std::vector< float> kDistances;</ div >
< div class = "line" >< a id = "l00115" name = "l00115" ></ a >< span class = "lineno" > 115</ span > pointRGBT pt;</ div >
< div class = "line" >< a id = "l00116" name = "l00116" ></ a >< span class = "lineno" > 116</ span > pt.x = coloredCloud-> at(i).x;</ div >
< div class = "line" >< a id = "l00117" name = "l00117" ></ a >< span class = "lineno" > 117</ span > pt.y = coloredCloud-> at(i).y;</ div >
< div class = "line" >< a id = "l00118" name = "l00118" ></ a >< span class = "lineno" > 118</ span > pt.z = coloredCloud-> at(i).z;</ div >
< div class = "line" >< a id = "l00119" name = "l00119" ></ a >< span class = "lineno" > 119</ span > < span class = "keywordflow" > if</ span > (transferColorRadius > 0.0)</ 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 > tree-> radiusSearch(pt, transferColorRadius, kIndices, kDistances);</ div >
< div class = "line" >< a id = "l00122" name = "l00122" ></ a >< span class = "lineno" > 122</ span > }</ div >
< div class = "line" >< a id = "l00123" name = "l00123" ></ a >< span class = "lineno" > 123</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00124" name = "l00124" ></ a >< span class = "lineno" > 124</ span > {</ div >
< div class = "line" >< a id = "l00125" name = "l00125" ></ a >< span class = "lineno" > 125</ span > tree-> nearestKSearch(pt, 1, kIndices, kDistances);</ div >
< div class = "line" >< a id = "l00126" name = "l00126" ></ a >< span class = "lineno" > 126</ span > }</ div >
< div class = "line" >< a id = "l00127" name = "l00127" ></ a >< span class = "lineno" > 127</ span > < span class = "keywordflow" > if</ span > (kIndices.size())</ div >
< div class = "line" >< a id = "l00128" name = "l00128" ></ a >< span class = "lineno" > 128</ span > {</ div >
< div class = "line" >< a id = "l00129" name = "l00129" ></ a >< span class = "lineno" > 129</ span > < span class = "comment" > //compute average color</ span ></ div >
< div class = "line" >< a id = "l00130" name = "l00130" ></ a >< span class = "lineno" > 130</ span > < span class = "keywordtype" > int</ span > r=0;</ div >
< div class = "line" >< a id = "l00131" name = "l00131" ></ a >< span class = "lineno" > 131</ span > < span class = "keywordtype" > int</ span > g=0;</ div >
< div class = "line" >< a id = "l00132" name = "l00132" ></ a >< span class = "lineno" > 132</ span > < span class = "keywordtype" > int</ span > b=0;</ div >
< div class = "line" >< a id = "l00133" name = "l00133" ></ a >< span class = "lineno" > 133</ span > < span class = "keywordtype" > int</ span > a=0;</ div >
< div class = "line" >< a id = "l00134" name = "l00134" ></ a >< span class = "lineno" > 134</ span > < span class = "keywordflow" > for</ span > (< span class = "keywordtype" > unsigned</ span > < span class = "keywordtype" > int</ span > j=0; j< kIndices.size(); ++j)</ div >
< div class = "line" >< a id = "l00135" name = "l00135" ></ a >< span class = "lineno" > 135</ span > {</ div >
< div class = "line" >< a id = "l00136" name = "l00136" ></ a >< span class = "lineno" > 136</ span > r+=(int)cloud-> at(kIndices[j]).r;</ div >
< div class = "line" >< a id = "l00137" name = "l00137" ></ a >< span class = "lineno" > 137</ span > g+=(int)cloud-> at(kIndices[j]).g;</ div >
< div class = "line" >< a id = "l00138" name = "l00138" ></ a >< span class = "lineno" > 138</ span > b+=(int)cloud-> at(kIndices[j]).b;</ div >
< div class = "line" >< a id = "l00139" name = "l00139" ></ a >< span class = "lineno" > 139</ span > a+=(int)cloud-> at(kIndices[j]).a;</ div >
< div class = "line" >< a id = "l00140" name = "l00140" ></ a >< span class = "lineno" > 140</ span > }</ div >
< div class = "line" >< a id = "l00141" name = "l00141" ></ a >< span class = "lineno" > 141</ span > coloredCloud-> at(i).r = r/kIndices.size();</ div >
< div class = "line" >< a id = "l00142" name = "l00142" ></ a >< span class = "lineno" > 142</ span > coloredCloud-> at(i).g = g/kIndices.size();</ div >
< div class = "line" >< a id = "l00143" name = "l00143" ></ a >< span class = "lineno" > 143</ span > coloredCloud-> at(i).b = b/kIndices.size();</ div >
< div class = "line" >< a id = "l00144" name = "l00144" ></ a >< span class = "lineno" > 144</ span > coloredCloud-> at(i).a = a/kIndices.size();</ div >
< div class = "line" >< a id = "l00145" name = "l00145" ></ a >< span class = "lineno" > 145</ span > coloredPts.at(i) = < span class = "keyword" > true</ span > ;</ 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 > < span class = "keywordflow" > else</ span ></ 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 > < span class = "comment" > //white</ span ></ div >
< div class = "line" >< a id = "l00150" name = "l00150" ></ a >< span class = "lineno" > 150</ span > coloredCloud-> at(i).r = coloredCloud-> at(i).g = coloredCloud-> at(i).b = 255;</ div >
< div class = "line" >< a id = "l00151" name = "l00151" ></ a >< span class = "lineno" > 151</ span > coloredPts.at(i) = < span class = "keyword" > false</ span > ;</ div >
< div class = "line" >< a id = "l00152" name = "l00152" ></ a >< span class = "lineno" > 152</ 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 = "keywordflow" > if</ span > (coloredOutput) {</ div >
< div class = "line" >< a id = "l00155" name = "l00155" ></ a >< span class = "lineno" > 155</ span > pcl::toPCLPointCloud2(*coloredCloud, mesh-> cloud);</ div >
< div class = "line" >< a id = "l00156" name = "l00156" ></ a >< span class = "lineno" > 156</ span > hasColors = < span class = "keyword" > true</ span > ;</ 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 > </ div >
< div class = "line" >< a id = "l00159" name = "l00159" ></ a >< span class = "lineno" > 159</ span > < span class = "comment" > // remove polygons with no color</ span ></ div >
< div class = "line" >< a id = "l00160" name = "l00160" ></ a >< span class = "lineno" > 160</ span > < span class = "keywordflow" > if</ span > (cleanMesh)</ 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 > std::vector< pcl::Vertices> filteredPolygons(mesh-> polygons.size());</ div >
< div class = "line" >< a id = "l00163" name = "l00163" ></ a >< span class = "lineno" > 163</ span > < span class = "keywordtype" > int</ span > oi=0;</ div >
< div class = "line" >< a id = "l00164" name = "l00164" ></ a >< span class = "lineno" > 164</ span > < span class = "keywordflow" > for</ span > (< span class = "keywordtype" > unsigned</ span > < span class = "keywordtype" > int</ span > i=0; i< mesh-> polygons.size(); ++i)</ div >
< div class = "line" >< a id = "l00165" name = "l00165" ></ a >< span class = "lineno" > 165</ span > {</ div >
< div class = "line" >< a id = "l00166" name = "l00166" ></ a >< span class = "lineno" > 166</ span > < span class = "keywordtype" > bool</ span > coloredPolygon = < span class = "keyword" > true</ span > ;</ div >
< div class = "line" >< a id = "l00167" name = "l00167" ></ a >< span class = "lineno" > 167</ span > < span class = "keywordflow" > for</ span > (< span class = "keywordtype" > unsigned</ span > < span class = "keywordtype" > int</ span > j=0; j< mesh-> polygons[i].vertices.size(); ++j)</ div >
< div class = "line" >< a id = "l00168" name = "l00168" ></ a >< span class = "lineno" > 168</ span > {</ div >
< div class = "line" >< a id = "l00169" name = "l00169" ></ a >< span class = "lineno" > 169</ span > < span class = "keywordflow" > if</ span > (!coloredPts.at(mesh-> polygons[i].vertices[j]))</ div >
< div class = "line" >< a id = "l00170" name = "l00170" ></ a >< span class = "lineno" > 170</ span > {</ div >
< div class = "line" >< a id = "l00171" name = "l00171" ></ a >< span class = "lineno" > 171</ span > coloredPolygon = < span class = "keyword" > false</ span > ;</ div >
< div class = "line" >< a id = "l00172" name = "l00172" ></ a >< span class = "lineno" > 172</ span > < span class = "keywordflow" > break</ span > ;</ 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 > }</ div >
< div class = "line" >< a id = "l00175" name = "l00175" ></ a >< span class = "lineno" > 175</ span > < span class = "keywordflow" > if</ span > (coloredPolygon)</ 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 > filteredPolygons[oi++] = mesh-> polygons[i];</ 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 > }</ div >
< div class = "line" >< a id = "l00180" name = "l00180" ></ a >< span class = "lineno" > 180</ span > filteredPolygons.resize(oi);</ div >
< div class = "line" >< a id = "l00181" name = "l00181" ></ a >< span class = "lineno" > 181</ span > mesh-> polygons = filteredPolygons;</ div >
< div class = "line" >< a id = "l00182" name = "l00182" ></ a >< span class = "lineno" > 182</ span > }</ 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 = "keywordflow" > if</ span > (minClusterSize)</ div >
< div class = "line" >< a id = "l00186" name = "l00186" ></ a >< span class = "lineno" > 186</ span > {</ div >
< div class = "line" >< a id = "l00187" name = "l00187" ></ a >< span class = "lineno" > 187</ span > < span class = "keywordflow" > if</ span > (progressState) progressState-> callback(< a class = "code hl_function" href = "UConversion_8h.html#a59ad5e2b3368d3dd19d73a24159388ce" > uFormat</ a > (< span class = "stringliteral" > " Filter small polygon clusters..." </ span > ));</ div >
< div class = "line" >< a id = "l00188" name = "l00188" ></ a >< span class = "lineno" > 188</ span > </ div >
< div class = "line" >< a id = "l00189" name = "l00189" ></ a >< span class = "lineno" > 189</ span > < span class = "comment" > // filter polygons</ span ></ div >
< div class = "line" >< a id = "l00190" name = "l00190" ></ a >< span class = "lineno" > 190</ span > std::vector< std::set< int> > neighbors;</ div >
< div class = "line" >< a id = "l00191" name = "l00191" ></ a >< span class = "lineno" > 191</ span > std::vector< std::set< int> > vertexToPolygons;</ div >
< div class = "line" >< a id = "l00192" name = "l00192" ></ a >< span class = "lineno" > 192</ span > < a class = "code hl_function" href = "namespacertabmap_1_1util3d.html#a9ebe11bdf534e3bdbcb2802d5dd70e62" > util3d::createPolygonIndexes</ a > (mesh-> polygons,</ div >
< div class = "line" >< a id = "l00193" name = "l00193" ></ a >< span class = "lineno" > 193</ span > mesh-> cloud.height*mesh-> cloud.width,</ div >
< div class = "line" >< a id = "l00194" name = "l00194" ></ a >< span class = "lineno" > 194</ span > neighbors,</ div >
< div class = "line" >< a id = "l00195" name = "l00195" ></ a >< span class = "lineno" > 195</ span > vertexToPolygons);</ div >
< div class = "line" >< a id = "l00196" name = "l00196" ></ a >< span class = "lineno" > 196</ span > std::list< std::list< int> > clusters = util3d::clusterPolygons(</ div >
< div class = "line" >< a id = "l00197" name = "l00197" ></ a >< span class = "lineno" > 197</ span > neighbors,</ div >
< div class = "line" >< a id = "l00198" name = "l00198" ></ a >< span class = "lineno" > 198</ span > minClusterSize< 0?0:minClusterSize);</ 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 > std::vector< pcl::Vertices> filteredPolygons(mesh-> polygons.size());</ div >
< div class = "line" >< a id = "l00201" name = "l00201" ></ a >< span class = "lineno" > 201</ span > < span class = "keywordflow" > if</ span > (minClusterSize < 0)</ div >
< div class = "line" >< a id = "l00202" name = "l00202" ></ a >< span class = "lineno" > 202</ span > {</ div >
< div class = "line" >< a id = "l00203" name = "l00203" ></ a >< span class = "lineno" > 203</ span > < span class = "comment" > // only keep the biggest cluster</ span ></ div >
< div class = "line" >< a id = "l00204" name = "l00204" ></ a >< span class = "lineno" > 204</ span > std::list< std::list< int> > ::iterator biggestClusterIndex = clusters.end();</ div >
< div class = "line" >< a id = "l00205" name = "l00205" ></ a >< span class = "lineno" > 205</ span > < span class = "keywordtype" > unsigned</ span > < span class = "keywordtype" > int</ span > biggestClusterSize = 0;</ div >
< div class = "line" >< a id = "l00206" name = "l00206" ></ a >< span class = "lineno" > 206</ span > < span class = "keywordflow" > for</ span > (std::list< std::list< int> > ::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)</ div >
< div class = "line" >< a id = "l00207" name = "l00207" ></ a >< span class = "lineno" > 207</ span > {</ div >
< div class = "line" >< a id = "l00208" name = "l00208" ></ a >< span class = "lineno" > 208</ span > < span class = "keywordflow" > if</ span > (iter-> size() > biggestClusterSize)</ div >
< div class = "line" >< a id = "l00209" name = "l00209" ></ a >< span class = "lineno" > 209</ span > {</ div >
< div class = "line" >< a id = "l00210" name = "l00210" ></ a >< span class = "lineno" > 210</ span > biggestClusterIndex = iter;</ div >
< div class = "line" >< a id = "l00211" name = "l00211" ></ a >< span class = "lineno" > 211</ span > biggestClusterSize = iter-> size();</ div >
< div class = "line" >< a id = "l00212" name = "l00212" ></ a >< span class = "lineno" > 212</ span > }</ div >
< div class = "line" >< a id = "l00213" name = "l00213" ></ a >< span class = "lineno" > 213</ span > }</ div >
< div class = "line" >< a id = "l00214" name = "l00214" ></ a >< span class = "lineno" > 214</ span > < span class = "keywordflow" > if</ span > (biggestClusterIndex != clusters.end())</ 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 > < span class = "keywordtype" > int</ span > oi=0;</ div >
< div class = "line" >< a id = "l00217" name = "l00217" ></ a >< span class = "lineno" > 217</ span > < span class = "keywordflow" > for</ span > (std::list< int> ::iterator jter=biggestClusterIndex-> begin(); jter!=biggestClusterIndex-> end(); ++jter)</ div >
< div class = "line" >< a id = "l00218" name = "l00218" ></ a >< span class = "lineno" > 218</ span > {</ div >
< div class = "line" >< a id = "l00219" name = "l00219" ></ a >< span class = "lineno" > 219</ span > filteredPolygons[oi++] = mesh-> polygons.at(*jter);</ div >
< div class = "line" >< a id = "l00220" name = "l00220" ></ a >< span class = "lineno" > 220</ span > }</ div >
< div class = "line" >< a id = "l00221" name = "l00221" ></ a >< span class = "lineno" > 221</ span > filteredPolygons.resize(oi);</ div >
< div class = "line" >< a id = "l00222" name = "l00222" ></ a >< span class = "lineno" > 222</ span > }</ div >
< div class = "line" >< a id = "l00223" name = "l00223" ></ a >< span class = "lineno" > 223</ span > }</ div >
< div class = "line" >< a id = "l00224" name = "l00224" ></ a >< span class = "lineno" > 224</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00225" name = "l00225" ></ a >< span class = "lineno" > 225</ span > {</ div >
< div class = "line" >< a id = "l00226" name = "l00226" ></ a >< span class = "lineno" > 226</ span > < span class = "keywordtype" > int</ span > oi=0;</ div >
< div class = "line" >< a id = "l00227" name = "l00227" ></ a >< span class = "lineno" > 227</ span > < span class = "keywordflow" > for</ span > (std::list< std::list< int> > ::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)</ div >
< div class = "line" >< a id = "l00228" name = "l00228" ></ a >< span class = "lineno" > 228</ span > {</ div >
< div class = "line" >< a id = "l00229" name = "l00229" ></ a >< span class = "lineno" > 229</ span > < span class = "keywordflow" > for</ span > (std::list< int> ::iterator jter=iter-> begin(); jter!=iter-> end(); ++jter)</ div >
< div class = "line" >< a id = "l00230" name = "l00230" ></ a >< span class = "lineno" > 230</ span > {</ div >
< div class = "line" >< a id = "l00231" name = "l00231" ></ a >< span class = "lineno" > 231</ span > filteredPolygons[oi++] = mesh-> polygons.at(*jter);</ 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 > }</ div >
< div class = "line" >< a id = "l00234" name = "l00234" ></ a >< span class = "lineno" > 234</ span > filteredPolygons.resize(oi);</ div >
< div class = "line" >< a id = "l00235" name = "l00235" ></ a >< span class = "lineno" > 235</ span > }</ div >
< div class = "line" >< a id = "l00236" name = "l00236" ></ a >< span class = "lineno" > 236</ span > </ div >
< div class = "line" >< a id = "l00237" name = "l00237" ></ a >< span class = "lineno" > 237</ span > < span class = "keywordtype" > int</ span > before = (int)mesh-> polygons.size();</ div >
< div class = "line" >< a id = "l00238" name = "l00238" ></ a >< span class = "lineno" > 238</ span > mesh-> polygons = filteredPolygons;</ div >
< div class = "line" >< a id = "l00239" name = "l00239" ></ a >< span class = "lineno" > 239</ span > </ div >
< div class = "line" >< a id = "l00240" name = "l00240" ></ a >< span class = "lineno" > 240</ span > < span class = "keywordflow" > if</ span > (progressState) progressState-> callback(< a class = "code hl_function" href = "UConversion_8h.html#a59ad5e2b3368d3dd19d73a24159388ce" > uFormat</ a > (< span class = "stringliteral" > " Filtered %d polygons." </ span > , before-(< span class = "keywordtype" > int</ span > )mesh-> polygons.size()));</ 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 > </ div >
< div class = "line" >< a id = "l00243" name = "l00243" ></ a >< span class = "lineno" > 243</ span > < span class = "comment" > // compute normals for the mesh if not already here</ span ></ div >
< div class = "line" >< a id = "l00244" name = "l00244" ></ a >< span class = "lineno" > 244</ span > < span class = "keywordflow" > if</ span > (!hasNormals)</ div >
< div class = "line" >< a id = "l00245" name = "l00245" ></ a >< span class = "lineno" > 245</ span > {</ div >
< div class = "line" >< a id = "l00246" name = "l00246" ></ a >< span class = "lineno" > 246</ span > < span class = "comment" > // use polygons</ span ></ div >
< div class = "line" >< a id = "l00247" name = "l00247" ></ a >< span class = "lineno" > 247</ span > < span class = "keywordflow" > if</ span > (hasColors)</ 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 > pcl::PointCloud< pcl::PointXYZRGBNormal> ::Ptr cloud (< span class = "keyword" > new</ span > pcl::PointCloud< pcl::PointXYZRGBNormal> );</ div >
< div class = "line" >< a id = "l00250" name = "l00250" ></ a >< span class = "lineno" > 250</ span > pcl::fromPCLPointCloud2(mesh-> cloud, *cloud);</ 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 > Eigen::Vector3f normal(1,0,0);</ div >
< div class = "line" >< a id = "l00253" name = "l00253" ></ a >< span class = "lineno" > 253</ span > < span class = "keywordflow" > for</ span > (< span class = "keywordtype" > unsigned</ span > < span class = "keywordtype" > int</ span > i=0; i< mesh-> polygons.size(); ++i)</ 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 > pcl::Vertices & v = mesh-> polygons[i];</ div >
< div class = "line" >< a id = "l00256" name = "l00256" ></ a >< span class = "lineno" > 256</ span > < span class = "keywordflow" > if</ span > (!hasNormals)</ div >
< div class = "line" >< a id = "l00257" name = "l00257" ></ a >< span class = "lineno" > 257</ span > {</ div >
< div class = "line" >< a id = "l00258" name = "l00258" ></ a >< span class = "lineno" > 258</ span > < a class = "code hl_define" href = "ULogger_8h.html#ab3436168e27946669bbd4c76e4c4007e" > UASSERT</ a > (v.vertices.size()> 2);</ div >
< div class = "line" >< a id = "l00259" name = "l00259" ></ a >< span class = "lineno" > 259</ span > Eigen::Vector3f v0(</ div >
< div class = "line" >< a id = "l00260" name = "l00260" ></ a >< span class = "lineno" > 260</ span > cloud-> at(v.vertices[1]).x - cloud-> at(v.vertices[0]).x,</ div >
< div class = "line" >< a id = "l00261" name = "l00261" ></ a >< span class = "lineno" > 261</ span > cloud-> at(v.vertices[1]).y - cloud-> at(v.vertices[0]).y,</ div >
< div class = "line" >< a id = "l00262" name = "l00262" ></ a >< span class = "lineno" > 262</ span > cloud-> at(v.vertices[1]).z - cloud-> at(v.vertices[0]).z);</ div >
< div class = "line" >< a id = "l00263" name = "l00263" ></ a >< span class = "lineno" > 263</ span > < span class = "keywordtype" > int</ span > last = v.vertices.size()-1;</ div >
< div class = "line" >< a id = "l00264" name = "l00264" ></ a >< span class = "lineno" > 264</ span > Eigen::Vector3f v1(</ div >
< div class = "line" >< a id = "l00265" name = "l00265" ></ a >< span class = "lineno" > 265</ span > cloud-> at(v.vertices[last]).x - cloud-> at(v.vertices[0]).x,</ div >
< div class = "line" >< a id = "l00266" name = "l00266" ></ a >< span class = "lineno" > 266</ span > cloud-> at(v.vertices[last]).y - cloud-> at(v.vertices[0]).y,</ div >
< div class = "line" >< a id = "l00267" name = "l00267" ></ a >< span class = "lineno" > 267</ span > cloud-> at(v.vertices[last]).z - cloud-> at(v.vertices[0]).z);</ div >
< div class = "line" >< a id = "l00268" name = "l00268" ></ a >< span class = "lineno" > 268</ span > normal = v0.cross(v1);</ div >
< div class = "line" >< a id = "l00269" name = "l00269" ></ a >< span class = "lineno" > 269</ span > normal.normalize();</ 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 > < span class = "comment" > // flat normal (per face)</ span ></ div >
< div class = "line" >< a id = "l00272" name = "l00272" ></ a >< span class = "lineno" > 272</ span > < span class = "keywordflow" > for</ span > (< span class = "keywordtype" > unsigned</ span > < span class = "keywordtype" > int</ span > j=0; j< v.vertices.size(); ++j)</ 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 > cloud-> at(v.vertices[j]).normal_x = normal[0];</ div >
< div class = "line" >< a id = "l00275" name = "l00275" ></ a >< span class = "lineno" > 275</ span > cloud-> at(v.vertices[j]).normal_y = normal[1];</ div >
< div class = "line" >< a id = "l00276" name = "l00276" ></ a >< span class = "lineno" > 276</ span > cloud-> at(v.vertices[j]).normal_z = normal[2];</ div >
< div class = "line" >< a id = "l00277" name = "l00277" ></ a >< span class = "lineno" > 277</ span > }</ div >
< div class = "line" >< a id = "l00278" name = "l00278" ></ a >< span class = "lineno" > 278</ span > }</ div >
< div class = "line" >< a id = "l00279" name = "l00279" ></ a >< span class = "lineno" > 279</ span > pcl::toPCLPointCloud2 (*cloud, mesh-> cloud);</ div >
< div class = "line" >< a id = "l00280" name = "l00280" ></ a >< span class = "lineno" > 280</ span > }</ div >
< div class = "line" >< a id = "l00281" name = "l00281" ></ a >< span class = "lineno" > 281</ span > < span class = "keywordflow" > else</ span ></ 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 > pcl::PointCloud< pcl::PointNormal> ::Ptr cloud (< span class = "keyword" > new</ span > pcl::PointCloud< pcl::PointNormal> );</ div >
< div class = "line" >< a id = "l00284" name = "l00284" ></ a >< span class = "lineno" > 284</ span > pcl::fromPCLPointCloud2(mesh-> cloud, *cloud);</ div >
< div class = "line" >< a id = "l00285" name = "l00285" ></ a >< span class = "lineno" > 285</ span > </ div >
< div class = "line" >< a id = "l00286" name = "l00286" ></ a >< span class = "lineno" > 286</ span > < span class = "keywordflow" > for</ span > (< span class = "keywordtype" > unsigned</ span > < span class = "keywordtype" > int</ span > i=0; i< mesh-> polygons.size(); ++i)</ 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 > pcl::Vertices & v = mesh-> polygons[i];</ div >
< div class = "line" >< a id = "l00289" name = "l00289" ></ a >< span class = "lineno" > 289</ span > < a class = "code hl_define" href = "ULogger_8h.html#ab3436168e27946669bbd4c76e4c4007e" > UASSERT</ a > (v.vertices.size()> 2);</ div >
< div class = "line" >< a id = "l00290" name = "l00290" ></ a >< span class = "lineno" > 290</ span > Eigen::Vector3f v0(</ div >
< div class = "line" >< a id = "l00291" name = "l00291" ></ a >< span class = "lineno" > 291</ span > cloud-> at(v.vertices[1]).x - cloud-> at(v.vertices[0]).x,</ div >
< div class = "line" >< a id = "l00292" name = "l00292" ></ a >< span class = "lineno" > 292</ span > cloud-> at(v.vertices[1]).y - cloud-> at(v.vertices[0]).y,</ div >
< div class = "line" >< a id = "l00293" name = "l00293" ></ a >< span class = "lineno" > 293</ span > cloud-> at(v.vertices[1]).z - cloud-> at(v.vertices[0]).z);</ div >
< div class = "line" >< a id = "l00294" name = "l00294" ></ a >< span class = "lineno" > 294</ span > < span class = "keywordtype" > int</ span > last = v.vertices.size()-1;</ div >
< div class = "line" >< a id = "l00295" name = "l00295" ></ a >< span class = "lineno" > 295</ span > Eigen::Vector3f v1(</ div >
< div class = "line" >< a id = "l00296" name = "l00296" ></ a >< span class = "lineno" > 296</ span > cloud-> at(v.vertices[last]).x - cloud-> at(v.vertices[0]).x,</ div >
< div class = "line" >< a id = "l00297" name = "l00297" ></ a >< span class = "lineno" > 297</ span > cloud-> at(v.vertices[last]).y - cloud-> at(v.vertices[0]).y,</ div >
< div class = "line" >< a id = "l00298" name = "l00298" ></ a >< span class = "lineno" > 298</ span > cloud-> at(v.vertices[last]).z - cloud-> at(v.vertices[0]).z);</ div >
< div class = "line" >< a id = "l00299" name = "l00299" ></ a >< span class = "lineno" > 299</ span > Eigen::Vector3f normal = v0.cross(v1);</ div >
< div class = "line" >< a id = "l00300" name = "l00300" ></ a >< span class = "lineno" > 300</ span > normal.normalize();</ div >
< div class = "line" >< a id = "l00301" name = "l00301" ></ a >< span class = "lineno" > 301</ span > < span class = "comment" > // flat normal (per face)</ span ></ div >
< div class = "line" >< a id = "l00302" name = "l00302" ></ a >< span class = "lineno" > 302</ span > < span class = "keywordflow" > for</ span > (< span class = "keywordtype" > unsigned</ span > < span class = "keywordtype" > int</ span > j=0; j< v.vertices.size(); ++j)</ div >
< div class = "line" >< a id = "l00303" name = "l00303" ></ a >< span class = "lineno" > 303</ span > {</ div >
< div class = "line" >< a id = "l00304" name = "l00304" ></ a >< span class = "lineno" > 304</ span > cloud-> at(v.vertices[j]).normal_x = normal[0];</ div >
< div class = "line" >< a id = "l00305" name = "l00305" ></ a >< span class = "lineno" > 305</ span > cloud-> at(v.vertices[j]).normal_y = normal[1];</ div >
< div class = "line" >< a id = "l00306" name = "l00306" ></ a >< span class = "lineno" > 306</ span > cloud-> at(v.vertices[j]).normal_z = normal[2];</ 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 > }</ div >
< div class = "line" >< a id = "l00309" name = "l00309" ></ a >< span class = "lineno" > 309</ span > pcl::toPCLPointCloud2 (*cloud, mesh-> cloud);</ div >
< div class = "line" >< a id = "l00310" name = "l00310" ></ a >< span class = "lineno" > 310</ span > }</ div >
< div class = "line" >< a id = "l00311" name = "l00311" ></ a >< span class = "lineno" > 311</ span > }</ div >
< div class = "line" >< a id = "l00312" name = "l00312" ></ a >< span class = "lineno" > 312</ span > }</ div >
< div class = "line" >< a id = "l00313" name = "l00313" ></ a >< span class = "lineno" > 313</ span > </ div >
< div class = "line" >< a id = "l00314" name = "l00314" ></ a >< span class = "lineno" > 314</ span >< span class = "keyword" > template</ span > < < span class = "keyword" > typename</ span > Po< span class = "keywordtype" > int</ span > T> </ div >
< div class = "line" >< a id = "l00315" name = "l00315" ></ a >< span class = "lineno" > 315</ span >< span class = "keywordtype" > bool</ span > intersectRayMesh(</ div >
< div class = "line" >< a id = "l00316" name = "l00316" ></ a >< span class = "lineno" > 316</ span > < span class = "keyword" > const</ span > Eigen::Vector3f & origin,</ div >
< div class = "line" >< a id = "l00317" name = "l00317" ></ a >< span class = "lineno" > 317</ span > < span class = "keyword" > const</ span > Eigen::Vector3f & dir,</ div >
< div class = "line" >< a id = "l00318" name = "l00318" ></ a >< span class = "lineno" > 318</ span > < span class = "keyword" > const</ span > < span class = "keyword" > typename</ span > pcl::PointCloud< PointT> & cloud,</ div >
< div class = "line" >< a id = "l00319" name = "l00319" ></ a >< span class = "lineno" > 319</ span > < span class = "keyword" > const</ span > std::vector< pcl::Vertices> & polygons,</ div >
< div class = "line" >< a id = "l00320" name = "l00320" ></ a >< span class = "lineno" > 320</ span > < span class = "keywordtype" > bool</ span > ignoreBackFaces,</ div >
< div class = "line" >< a id = "l00321" name = "l00321" ></ a >< span class = "lineno" > 321</ span > < span class = "keywordtype" > float</ span > & distance,</ div >
< div class = "line" >< a id = "l00322" name = "l00322" ></ a >< span class = "lineno" > 322</ span > Eigen::Vector3f & normal,</ div >
< div class = "line" >< a id = "l00323" name = "l00323" ></ a >< span class = "lineno" > 323</ span > < span class = "keywordtype" > int</ span > & index)</ div >
< div class = "line" >< a id = "l00324" name = "l00324" ></ a >< span class = "lineno" > 324</ span > {</ div >
< div class = "line" >< a id = "l00325" name = "l00325" ></ a >< span class = "lineno" > 325</ span > < span class = "keywordtype" > bool</ span > intersect = < span class = "keyword" > false</ span > ;</ div >
< div class = "line" >< a id = "l00326" name = "l00326" ></ a >< span class = "lineno" > 326</ span > distance = std::numeric_limits< float> ::max();</ div >
< div class = "line" >< a id = "l00327" name = "l00327" ></ a >< span class = "lineno" > 327</ span > < span class = "keywordflow" > for</ span > (< span class = "keywordtype" > size_t</ span > i = 0; i < polygons.size(); ++i)</ div >
< div class = "line" >< a id = "l00328" name = "l00328" ></ a >< span class = "lineno" > 328</ span > {</ div >
< div class = "line" >< a id = "l00329" name = "l00329" ></ a >< span class = "lineno" > 329</ span > < span class = "keyword" > const</ span > pcl::Vertices & vert = polygons.at(i);</ div >
< div class = "line" >< a id = "l00330" name = "l00330" ></ a >< span class = "lineno" > 330</ span > < a class = "code hl_define" href = "ULogger_8h.html#ab3436168e27946669bbd4c76e4c4007e" > UASSERT</ a > (vert.vertices.size()==3);</ div >
< div class = "line" >< a id = "l00331" name = "l00331" ></ a >< span class = "lineno" > 331</ span > < span class = "keywordtype" > float</ span > d;</ div >
< div class = "line" >< a id = "l00332" name = "l00332" ></ a >< span class = "lineno" > 332</ span > Eigen::Vector3f n;</ div >
< div class = "line" >< a id = "l00333" name = "l00333" ></ a >< span class = "lineno" > 333</ span > < span class = "keywordflow" > if</ span > (< a class = "code hl_function" href = "namespacertabmap_1_1util3d.html#ab138227ab6c0511088e846b2276e7afc" > intersectRayTriangle</ a > (origin, dir,</ div >
< div class = "line" >< a id = "l00334" name = "l00334" ></ a >< span class = "lineno" > 334</ span > cloud.at(vert.vertices.at(0)).getVector3fMap(),</ div >
< div class = "line" >< a id = "l00335" name = "l00335" ></ a >< span class = "lineno" > 335</ span > cloud.at(vert.vertices.at(1)).getVector3fMap(),</ div >
< div class = "line" >< a id = "l00336" name = "l00336" ></ a >< span class = "lineno" > 336</ span > cloud.at(vert.vertices.at(2)).getVector3fMap(), d, n) && </ div >
< div class = "line" >< a id = "l00337" name = "l00337" ></ a >< span class = "lineno" > 337</ span > d < distance && </ div >
< div class = "line" >< a id = "l00338" name = "l00338" ></ a >< span class = "lineno" > 338</ span > (!ignoreBackFaces || n.dot(dir)< 0))</ div >
< div class = "line" >< a id = "l00339" name = "l00339" ></ a >< span class = "lineno" > 339</ span > {</ div >
< div class = "line" >< a id = "l00340" name = "l00340" ></ a >< span class = "lineno" > 340</ span > distance = d;</ div >
< div class = "line" >< a id = "l00341" name = "l00341" ></ a >< span class = "lineno" > 341</ span > index = i;</ div >
< div class = "line" >< a id = "l00342" name = "l00342" ></ a >< span class = "lineno" > 342</ span > normal = n;</ div >
< div class = "line" >< a id = "l00343" name = "l00343" ></ a >< span class = "lineno" > 343</ span > intersect |= < span class = "keyword" > true</ span > ;</ div >
< div class = "line" >< a id = "l00344" name = "l00344" ></ a >< span class = "lineno" > 344</ span > }</ div >
< div class = "line" >< a id = "l00345" name = "l00345" ></ a >< span class = "lineno" > 345</ span > }</ div >
< div class = "line" >< a id = "l00346" name = "l00346" ></ a >< span class = "lineno" > 346</ span > normal.normalize();</ div >
< div class = "line" >< a id = "l00347" name = "l00347" ></ a >< span class = "lineno" > 347</ span > < span class = "keywordflow" > return</ span > intersect;</ div >
< div class = "line" >< a id = "l00348" name = "l00348" ></ a >< span class = "lineno" > 348</ span > }</ div >
< div class = "line" >< a id = "l00349" name = "l00349" ></ a >< span class = "lineno" > 349</ span > </ div >
< div class = "line" >< a id = "l00350" name = "l00350" ></ a >< span class = "lineno" > 350</ span > }</ div >
< div class = "line" >< a id = "l00351" name = "l00351" ></ a >< span class = "lineno" > 351</ span > </ div >
< div class = "line" >< a id = "l00352" name = "l00352" ></ a >< span class = "lineno" > 352</ span > }</ div >
< div class = "line" >< a id = "l00353" name = "l00353" ></ a >< span class = "lineno" > 353</ span > </ div >
< div class = "line" >< a id = "l00354" name = "l00354" ></ a >< span class = "lineno" > 354</ span > </ div >
< div class = "line" >< a id = "l00355" name = "l00355" ></ a >< span class = "lineno" > 355</ span >< span class = "preprocessor" > #endif </ span >< span class = "comment" > /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_ */</ span >< span class = "preprocessor" ></ span ></ div >
< div class = "ttc" id = "aUConversion_8h_html" >< div class = "ttname" >< a href = "UConversion_8h.html" > UConversion.h</ a ></ div >< div class = "ttdoc" > Some conversion functions.</ div ></ div >
< div class = "ttc" id = "aUConversion_8h_html_a59ad5e2b3368d3dd19d73a24159388ce" >< div class = "ttname" >< a href = "UConversion_8h.html#a59ad5e2b3368d3dd19d73a24159388ce" > uFormat</ a ></ div >< div class = "ttdeci" > std::string UTILITE_EXPORT uFormat(const char *fmt,...)</ div ></ div >
< div class = "ttc" id = "aULogger_8h_html_ab3436168e27946669bbd4c76e4c4007e" >< div class = "ttname" >< a href = "ULogger_8h.html#ab3436168e27946669bbd4c76e4c4007e" > UASSERT</ a ></ div >< div class = "ttdeci" > #define UASSERT(condition)</ div >< div class = "ttdef" >< b > Definition</ b > < a href = "ULogger_8h_source.html#l00066" > ULogger.h:66</ a ></ div ></ div >
< div class = "ttc" id = "anamespacertabmap_1_1util3d_html_a9ebe11bdf534e3bdbcb2802d5dd70e62" >< div class = "ttname" >< a href = "namespacertabmap_1_1util3d.html#a9ebe11bdf534e3bdbcb2802d5dd70e62" > rtabmap::util3d::createPolygonIndexes</ a ></ div >< div class = "ttdeci" > void RTABMAP_CORE_EXPORT createPolygonIndexes(const std::vector< pcl::Vertices > & polygons, int cloudSize, std::vector< std::set< int > > & neighborPolygons, std::vector< std::set< int > > & vertexPolygons)</ div >< div class = "ttdoc" > Given a set of polygons, create two indexes: polygons to neighbor polygons and vertices to polygons.</ div ></ div >
< div class = "ttc" id = "anamespacertabmap_1_1util3d_html_ab138227ab6c0511088e846b2276e7afc" >< div class = "ttname" >< a href = "namespacertabmap_1_1util3d.html#ab138227ab6c0511088e846b2276e7afc" > rtabmap::util3d::intersectRayTriangle</ a ></ div >< div class = "ttdeci" > bool RTABMAP_CORE_EXPORT intersectRayTriangle(const Eigen::Vector3f & p, const Eigen::Vector3f & dir, const Eigen::Vector3f & v0, const Eigen::Vector3f & v1, const Eigen::Vector3f & v2, float & distance, Eigen::Vector3f & normal)</ div ></ div >
2026-08-23 20:27:42 +00:00
< 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#l00042" > BayesFilter.h:42</ a ></ div ></ div >
2026-08-06 22:56:24 +00:00
</ 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_surface.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 >
