i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

2
corelib/include/rtabmap/core/CameraRGBD.h
View File

@@ -273,7 +273,7 @@ private:
};
/////////////////////////
// CameraOpenNIPCL
// CameraRealSense
/////////////////////////
class RTABMAP_EXP CameraRealSense :
public Camera

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_ */

8
corelib/include/rtabmap/core/util3d_filtering.h
View File

@@ -411,7 +411,6 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
* @param viewpoint from which viewpoint the normals should be estimated (see pcl::NormalEstimation).
* @return the indices of the points which respect the normal constraint.
*/
pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -434,6 +433,13 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
int normalKSearch,
const Eigen::Vector4f & viewpoint);
void RTABMAP_EXP colorMeanFiltering(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudRef,
const pcl::IndicesPtr & indicesRef,
float radiusSearch);
/**
* For convenience.
*/

32
corelib/include/rtabmap/core/util3d_surface.h
View File

@@ -60,6 +60,10 @@ void RTABMAP_EXP createPolygonIndexes(
std::vector<std::set<int> > & neighborPolygons,
std::vector<std::set<int> > & vertexPolygons);
std::list<std::list<int> > RTABMAP_EXP clusterPolygons(
const std::vector<std::set<int> > & neighborPolygons,
int minClusterSize = 0);
std::vector<pcl::Vertices> RTABMAP_EXP organizedFastMesh(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
double angleTolerance = M_PI/16,
@@ -194,33 +198,11 @@ template<typename pointT>
std::vector<pcl::Vertices> normalizePolygonsSide(
const pcl::PointCloud<pointT> & cloud,
const std::vector<pcl::Vertices> & polygons,
const pcl::PointXYZ & viewPoint = pcl::PointXYZ(0,0,0))
{
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;
}
const pcl::PointXYZ & viewPoint = pcl::PointXYZ(0,0,0));
} // namespace util3d
} // namespace rtabmap
#include "rtabmap/core/impl/util3d_surface.hpp"
#endif /* UTIL3D_SURFACE_H_ */