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

Mesh reconstruction update: fixed normals computation (with and without MLS)

Browse Source
This commit is contained in:
Mathieu Labbé
2015-08-11 17:19:12 -04:00
parent 9c653655bd
commit abdd3de773
16 changed files with 580 additions and 204 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -46,6 +46,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float voxelSize);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
float voxelSize);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP sampling(
@@ -141,6 +144,32 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
float radiusSearch,
int minNeighborsInRadius = 0);
/**
* For convenience.
*/
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
float radiusSearch,
int minNeighborsInRadius = 0);
/**
* Subtract a cloud from another one using radius filtering.
* @param cloud the input cloud.
* @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
* @param cloud the input cloud to subtract.
* @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
* @param radiusSearch the radius in meter.
* @return the indices of the points satisfying the parameters.
*/
pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
float radiusSearch,
int minNeighborsInRadius = 0);
/**
* For convenience.

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

@@ -30,7 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RtabmapExp.h>
#include <pcl/PolygonMesh.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
@@ -49,7 +48,7 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
float gp3MaximumSurfaceAngle = M_PI/4,
float gp3MinimumAngle = M_PI/18,
float gp3MaximumAngle = 2*M_PI/3,
bool gp3NormalConsistency = false);
bool gp3NormalConsistency = true);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -62,7 +61,42 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormalsSmoothed(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float smoothingSearchRadius = 0.025,
bool smoothingPolynomialFit = true);
bool smoothingPolynomialFit = true,
float voxelSize = 0.0f);
void RTABMAP_EXP adjustNormalsToViewPoints(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & viewpoints,
pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud);
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;
}
} // namespace util3d
} // namespace rtabmap

3
corelib/include/rtabmap/core/util3d_transforms.h
View File

@@ -46,6 +46,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Transform & transform);
pcl::PointXYZ RTABMAP_EXP transformPoint(
const pcl::PointXYZ & pt,