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

MainWindow: Added statistics about how much size the created clouds take in RAM. Avoid caching data on small movements. Export: Fixed meshing checkbox and pipeline combo box not saved/loaded. PreferencesDialog: Added option to disable caching the point clouds. computeNormals(): added viewpoint parameter. mls(): making sure that all returned normals are normalized.

Browse Source
This commit is contained in:
matlabbe
2016-06-12 17:53:35 -04:00
parent cb7c76889d
commit 543b8df045
12 changed files with 288 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -194,6 +194,8 @@ public:
void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
const Transform & groundTruth() const {return groundTruth_;}
long getMemoryUsed() const; // Return memory usage in Bytes
private:
int _id;
double _stamp;

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

@@ -126,28 +126,34 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int normalKSearch = 20);
int normalKSearch = 20,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int normalKSearch = 20);
int normalKSearch = 20,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch = 20);
int normalKSearch = 20,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch = 20);
int normalKSearch = 20,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float maxDepthChangeFactor = 0.02f,
float normalSmoothingSize = 10.0f);
float normalSmoothingSize = 10.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float maxDepthChangeFactor = 0.02f,
float normalSmoothingSize = 10.0f);
float normalSmoothingSize = 10.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,

16
corelib/src/CameraThread.cpp
View File

@@ -193,7 +193,21 @@ void CameraThread::mainLoop()
cv::Mat scan;
if(_scanNormalsK>0)
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK);
// view point
Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
{
viewPoint[0] = data.cameraModels()[0].localTransform().x();
viewPoint[1] = data.cameraModels()[0].localTransform().y();
viewPoint[2] = data.cameraModels()[0].localTransform().z();
}
else if(!data.stereoCameraModel().localTransform().isNull())
{
viewPoint[0] = data.stereoCameraModel().localTransform().x();
viewPoint[1] = data.stereoCameraModel().localTransform().y();
viewPoint[2] = data.stereoCameraModel().localTransform().z();
}
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals);

13
corelib/src/SensorData.cpp
View File

@@ -448,7 +448,6 @@ void SensorData::setUserDataRaw(const cv::Mat & userDataRaw)
return;
}
_userDataRaw = userDataRaw;
_userDataCompressed = cv::Mat();
}
void SensorData::setUserData(const cv::Mat & userData)
@@ -607,5 +606,17 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv:
}
}
long SensorData::getMemoryUsed() const // Return memory usage in Bytes
{
return _imageCompressed.total()*_imageCompressed.elemSize() +
_imageRaw.total()*_imageRaw.elemSize() +
_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize() +
_depthOrRightRaw.total()*_depthOrRightRaw.elemSize() +
_userDataCompressed.total()*_userDataCompressed.elemSize() +
_userDataRaw.total()*_userDataRaw.elemSize() +
_laserScanCompressed.total()*_laserScanCompressed.elemSize() +
_laserScanRaw.total()*_laserScanRaw.elemSize();
}
} // namespace rtabmap

37
corelib/src/util3d_surface.cpp
View File

@@ -596,15 +596,17 @@ pcl::TextureMesh::Ptr createTextureMesh(
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int normalKSearch)
int normalKSearch,
const Eigen::Vector3f & viewPoint)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeNormals(cloud, indices, normalKSearch);
return computeNormals(cloud, indices, normalKSearch, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch)
int normalKSearch,
const Eigen::Vector3f & viewPoint)
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
if(indices->size())
@@ -627,6 +629,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
//}
n.setSearchMethod (tree);
n.setKSearch (normalKSearch);
n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
n.compute (*normals);
return normals;
@@ -634,15 +637,17 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int normalKSearch)
int normalKSearch,
const Eigen::Vector3f & viewPoint)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeNormals(cloud, indices, normalKSearch);
return computeNormals(cloud, indices, normalKSearch, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch)
int normalKSearch,
const Eigen::Vector3f & viewPoint)
{
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
if(indices->size())
@@ -665,6 +670,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
//}
n.setSearchMethod (tree);
n.setKSearch (normalKSearch);
n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
n.compute (*normals);
return normals;
@@ -673,16 +679,18 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float maxDepthChangeFactor,
float normalSmoothingSize)
float normalSmoothingSize,
const Eigen::Vector3f & viewPoint)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize);
return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float maxDepthChangeFactor,
float normalSmoothingSize)
float normalSmoothingSize,
const Eigen::Vector3f & viewPoint)
{
UASSERT(cloud->isOrganized());
@@ -698,6 +706,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
//{
// ne.setIndices(indices);
//}
ne.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
ne.compute(*normals);
return normals;
@@ -783,6 +792,16 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
mls.setSearchMethod (tree);
mls.process (*cloud_with_normals);
// It seems that returned normals are not normalized!? FIXME: Is it a bug only in PCL 1.7.1?
for(unsigned int i=0; i<cloud_with_normals->size(); ++i)
{
Eigen::Vector3f normal(cloud_with_normals->at(i).normal_x, cloud_with_normals->at(i).normal_y, cloud_with_normals->at(i).normal_z);
normal.normalize();
cloud_with_normals->at(i).normal_x = normal[0];
cloud_with_normals->at(i).normal_y = normal[1];
cloud_with_normals->at(i).normal_z = normal[2];
}
return cloud_with_normals;
}