i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added min polygon cluster size option (DbViewer and GUI export)

Browse Source
This commit is contained in:
matlabbe
2016-09-03 18:59:43 -04:00
parent 0848b03127
commit 7f2c118f8d
10 changed files with 602 additions and 284 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
corelib/include/rtabmap/core/impl/util3d_surface.hpp Normal file
View File

@@ -0,0 +1,50 @@
/*
* util3d_surface.hpp
*
* Created on: Sep 3, 2016
* Author: mathieu
*/
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_
#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_
namespace rtabmap {
namespace util3d {
template<typename pointT>
std::vector<pcl::Vertices> normalizePolygonsSide(
const pcl::PointCloud<pointT> & cloud,
const std::vector<pcl::Vertices> & polygons,
const pcl::PointXYZ & viewPoint)
{
std::vector<pcl::Vertices> output(polygons.size());
for(unsigned int i=0; i<polygons.size(); ++i)
{
pcl::Vertices polygon = polygons[i];
Eigen::Vector3f v1 = cloud.at(polygon.vertices[1]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
Eigen::Vector3f v2 = cloud.at(polygon.vertices[2]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
Eigen::Vector3f n = (v1.cross(v2)).normalized();
Eigen::Vector3f p = Eigen::Vector3f(viewPoint.x, viewPoint.y, viewPoint.z) - cloud.at(polygon.vertices[1]).getVector3fMap();
float result = n.dot(p);
if(result < 0)
{
//reverse vertices order
int tmp = polygon.vertices[0];
polygon.vertices[0] = polygon.vertices[2];
polygon.vertices[2] = tmp;
}
output[i] = polygon;
}
return output;
}
}
}
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_ */