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

API achange (0.11.8): computeNormals returns only pcl::Normal cloud, not pcl::PointNormal or pcl::PointXYZRGBNormal types. MainWindow: Normals are not kept in cache to save RAM. ProgressDialog: check if auto-close is still checked when close() slot is called.

Browse Source
This commit is contained in:
matlabbe
2016-06-12 13:51:49 -04:00
parent 9f296c67b2
commit cb7c76889d
19 changed files with 342 additions and 287 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -121,28 +121,29 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
const std::map<int, Transform> & poses,
const std::map<int, CameraModel> & cameraModels,
const std::map<int, cv::Mat> & images,
const std::string & tmpDirectory = ".");
const std::string & tmpDirectory = ".",
int kNormalSearch = 20); // if mesh doesn't have normals, compute them with k neighbors
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int normalKSearch = 20);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int normalKSearch = 20);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch = 20);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch = 20);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float maxDepthChangeFactor = 0.02f,
float normalSmoothingSize = 10.0f);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float maxDepthChangeFactor = 0.02f,

6
corelib/src/CameraRGB.cpp
View File

@@ -42,6 +42,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_surface.h>
#include <pcl/common/io.h>
#include <iostream>
#include <fstream>
#include <cmath>
@@ -661,7 +663,9 @@ SensorData CameraImages::captureImage()
}
if(_scanNormalsK > 0 && cloud->size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, _scanNormalsK);
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals);
}
else

5
corelib/src/CameraThread.cpp
View File

@@ -193,7 +193,10 @@ void CameraThread::mainLoop()
cv::Mat scan;
if(_scanNormalsK>0)
{
scan = util3d::laserScanFromPointCloud(*util3d::computeNormals(cloud, _scanNormalsK));
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals);
}
else
{

11
corelib/src/RegistrationIcp.cpp
View File

@@ -200,8 +200,15 @@ Transform RegistrationIcp::computeTransformationImpl(
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
if(_pointToPlane) // ICP Point To Plane, only in 3D
{
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::Normal>::Ptr normals;
normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals);
normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*toCloudFiltered, *normals, *toCloudNormals);
std::vector<int> indices;
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);

4
corelib/src/util3d.cpp
View File

@@ -1441,7 +1441,9 @@ cv::Mat loadScan(
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = loadCloud(path, Transform::getIdentity(), downsampleStep, voxelSize);
if(normalsK > 0 && cloud->size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, normalsK);
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, normalsK);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals, transform);
}
else

70
corelib/src/util3d_surface.cpp
View File

@@ -500,7 +500,8 @@ pcl::TextureMesh::Ptr createTextureMesh(
const std::map<int, Transform> & poses,
const std::map<int, CameraModel> & cameraModels,
const std::map<int, cv::Mat> & images,
const std::string & tmpDirectory)
const std::string & tmpDirectory,
int kNormalSearch)
{
pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
textureMesh->cloud = mesh->cloud;
@@ -583,26 +584,28 @@ pcl::TextureMesh::Ptr createTextureMesh(
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals = computeNormals(cloud, 20);
pcl::PointCloud<pcl::Normal>::Ptr normals = computeNormals(cloud, kNormalSearch);
// Concatenate the XYZ and normal fields
pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
pcl::concatenateFields (*cloud, *normals, *cloudWithNormals);
pcl::toPCLPointCloud2 (*cloudWithNormals, textureMesh->cloud);
}
return textureMesh;
}
pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int normalKSearch)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeNormals(cloud, indices, normalKSearch);
}
pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
if(indices->size())
{
@@ -617,35 +620,30 @@ pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
n.setInputCloud (cloud);
if(indices->size())
{
n.setIndices(indices);
}
// Commented: Keep the output normals size the same as the input cloud
//if(indices->size())
//{
// n.setIndices(indices);
//}
n.setSearchMethod (tree);
n.setKSearch (normalKSearch);
n.compute (*normals);
//* normals should not contain the point normals + surface curvatures
// Concatenate the XYZ and normal fields*
pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
//* cloud_with_normals = cloud + normals*/
return cloud_with_normals;
return normals;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int normalKSearch)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeNormals(cloud, indices, normalKSearch);
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
if(indices->size())
{
@@ -660,23 +658,19 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
n.setInputCloud (cloud);
if(indices->size())
{
n.setIndices(indices);
}
// Commented: Keep the output normals size the same as the input cloud
//if(indices->size())
//{
// n.setIndices(indices);
//}
n.setSearchMethod (tree);
n.setKSearch (normalKSearch);
n.compute (*normals);
//* normals should not contain the point normals + surface curvatures
// Concatenate the XYZ and normal fields*
pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
//* cloud_with_normals = cloud + normals*/
return cloud_with_normals;
return normals;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float maxDepthChangeFactor,
float normalSmoothingSize)
@@ -684,7 +678,7 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
pcl::IndicesPtr indices(new std::vector<int>);
return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize);
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float maxDepthChangeFactor,
@@ -692,8 +686,6 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
{
UASSERT(cloud->isOrganized());
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
// Normal estimation
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
@@ -701,16 +693,14 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
ne.setNormalSmoothingSize(normalSmoothingSize);
ne.setInputCloud(cloud);
if(indices->size())
{
ne.setIndices(indices);
}
// Commented: Keep the output normals size the same as the input cloud
//if(indices->size())
//{
// ne.setIndices(indices);
//}
ne.compute(*normals);
// Concatenate the XYZ and normal fields
pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
return cloud_with_normals;
return normals;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(