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

CameraThread: fixed scan from depth max points with multi-camera

Browse Source
This commit is contained in:
matlabbe
2016-06-21 11:36:50 -04:00
parent 0ec39e3c77
commit 530919c531
2 changed files with 48 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
corelib/src/CameraThread.cpp
View File

@@ -177,44 +177,50 @@ void CameraThread::mainLoop()
pcl::IndicesPtr validIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, _scanMinDepth, validIndices.get());
float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
if(_scanVoxelSize>0.0f)
{
cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
float ratio = float(cloud->size()) / float(validIndices->size());
maxPoints = ratio * maxPoints;
}
else if(!cloud->is_dense)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
cloud = denseCloud;
}
cv::Mat scan;
if(_scanNormalsK>0)
if(validIndices->size())
{
// view point
Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
if(_scanVoxelSize>0.0f)
{
viewPoint[0] = data.cameraModels()[0].localTransform().x();
viewPoint[1] = data.cameraModels()[0].localTransform().y();
viewPoint[2] = data.cameraModels()[0].localTransform().z();
cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
float ratio = float(cloud->size()) / float(validIndices->size());
maxPoints = ratio * maxPoints;
}
else if(!data.stereoCameraModel().localTransform().isNull())
else if(!cloud->is_dense)
{
viewPoint[0] = data.stereoCameraModel().localTransform().x();
viewPoint[1] = data.stereoCameraModel().localTransform().y();
viewPoint[2] = data.stereoCameraModel().localTransform().z();
pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
cloud = denseCloud;
}
if(cloud->size())
{
if(_scanNormalsK>0)
{
// 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);
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud);
}
}
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);
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud);
}
data.setLaserScanRaw(scan, (int)maxPoints, _scanMaxDepth);
info.timeScanFromDepth = timer.ticks();

10
corelib/src/util3d.cpp
View File

@@ -682,6 +682,16 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
UERROR("Camera model %d is invalid", i);
}
}
if(cloud->is_dense && validIndices)
{
//generate indices for all points (they are all valid)
validIndices->resize(cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
{
validIndices->at(i) = i;
}
}
}
else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection())
{