i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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_ */

39
corelib/src/util3d_filtering.cpp
View File

@@ -1555,6 +1555,45 @@ pcl::IndicesPtr normalFiltering(
return output;
}
void colorMeanFiltering(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudRef,
const pcl::IndicesPtr & indicesRef,
float radiusSearch)
{
UASSERT(radiusSearch>0.0f);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
if(indicesRef->size())
{
tree->setInputCloud(cloudRef, indicesRef);
}
else
{
tree->setInputCloud(cloudRef);
}
for(unsigned int i=0; i<indices->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
pcl::PointXYZRGB & pt = cloud->at(indices->at(i));
if(tree->radiusSearch(pt, radiusSearch, kIndices, kDistances))
{
UASSERT(kIndices.size());
int r=0,g=0,b=0;
for(unsigned int j=0; j<kIndices.size(); ++j)
{
r+=cloudRef->at(kIndices.at(j)).r;
g+=cloudRef->at(kIndices.at(j)).g;
b+=cloudRef->at(kIndices.at(j)).b;
}
pt.r = (unsigned char)(r/kIndices.size());
pt.g = (unsigned char)(g/kIndices.size());
pt.b = (unsigned char)(b/kIndices.size());
}
}
}
std::vector<pcl::IndicesPtr> extractClusters(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float clusterTolerance,

37
corelib/src/util3d_surface.cpp
View File

@@ -100,6 +100,43 @@ void createPolygonIndexes(
}
}
std::list<std::list<int> > clusterPolygons(
const std::vector<std::set<int> > & neighborPolygons,
int minClusterSize)
{
std::set<int> polygonsChecked;
std::list<std::list<int> > clusters;
for(unsigned int i=0; i<neighborPolygons.size(); ++i)
{
if(polygonsChecked.find(i) == polygonsChecked.end())
{
std::list<int> currentCluster;
currentCluster.push_back(i);
polygonsChecked.insert(i);
for(std::list<int>::iterator iter=currentCluster.begin(); iter!=currentCluster.end(); ++iter)
{
// get neighbor polygons
std::set<int> neighbors = neighborPolygons[*iter];
for(std::set<int>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
{
if(polygonsChecked.insert(*jter).second)
{
currentCluster.push_back(*jter);
}
}
}
if(currentCluster.size() > minClusterSize)
{
clusters.push_back(currentCluster);
}
}
}
return clusters;
}
std::vector<pcl::Vertices> organizedFastMesh(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
double angleTolerance,