i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

DatabaseViewer: showing normals if scans have 6 channels

Browse Source
This commit is contained in:
matlabbe
2016-09-07 17:14:09 -04:00
parent ff4300d525
commit ce1acd9d44
1 changed files with 45 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
guilib/src/DatabaseViewer.cpp
View File

@@ -1832,17 +1832,14 @@ void DatabaseViewer::view3DLaserScans()
dbDriver_->getNodeData(iter->first, data); dbDriver_->getNodeData(iter->first, data);
cv::Mat scan; cv::Mat scan;
data.uncompressDataConst(0, 0, &scan); data.uncompressDataConst(0, 0, &scan);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
UASSERT(scan.empty() || scan.type()==CV_32FC2 || scan.type() == CV_32FC3);
if(downsamplingStepSize>1) if(!scan.empty())
{ {
scan = util3d::downsample(scan, downsamplingStepSize); if(downsamplingStepSize>1)
} {
cloud = util3d::laserScanToPointCloud(scan, data.laserScanInfo().localTransform()); scan = util3d::downsample(scan, downsamplingStepSize);
}
if(cloud->size())
{
QColor color = Qt::red; QColor color = Qt::red;
int mapId, weight; int mapId, weight;
Transform odomPose, groundTruth; Transform odomPose, groundTruth;
@@ -1853,20 +1850,25 @@ void DatabaseViewer::view3DLaserScans()
color = (Qt::GlobalColor)(mapId % 12 + 7 ); color = (Qt::GlobalColor)(mapId % 12 + 7 );
} }
int normalK = uStr2Int(ui_->parameters_toolbox->getParameters().at(Parameters::kIcpPointToPlaneNormalNeighbors())); if(scan.channels() == 6)
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, normalK); {
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>); pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
pcl::concatenateFields(*cloud, *normals, *cloudNormals); cloud = util3d::laserScanToPointCloudNormal(scan, data.laserScanInfo().localTransform());
viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
viewer->addCloud(uFormat("cloud%d", iter->first), cloudNormals, pose, color); }
else
UINFO("Generated %d (%d points)", iter->first, cloud->size()); {
progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size())); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::laserScanToPointCloud(scan, data.laserScanInfo().localTransform());
viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
}
UINFO("Generated %d (%d points)", iter->first, scan.cols);
progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(scan.cols));
} }
else else
{ {
UINFO("Empty cloud %d", iter->first); UINFO("Empty scan %d", iter->first);
progressDialog.appendText(QString("Empty cloud %1").arg(iter->first)); progressDialog.appendText(QString("Empty scan %1").arg(iter->first));
} }
progressDialog.incrementStep(); progressDialog.incrementStep();
QApplication::processEvents(); QApplication::processEvents();
@@ -2599,11 +2601,16 @@ void DatabaseViewer::update(int value,
} }
//add scan //add scan
if(ui_->checkBox_showScan->isChecked()) if(ui_->checkBox_showScan->isChecked() && data.laserScanRaw().cols)
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanInfo().localTransform()); if(data.laserScanRaw().channels() == 6)
if(scan->size())
{ {
pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(data.laserScanRaw(), data.laserScanInfo().localTransform());
view3D->addCloud("scan", scan, pose, Qt::yellow);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanInfo().localTransform());
view3D->addCloud("scan", scan, pose, Qt::yellow); view3D->addCloud("scan", scan, pose, Qt::yellow);
} }
} }
@@ -3559,25 +3566,31 @@ void DatabaseViewer::updateConstraintView(
} }
// Added loop closure scans // Added loop closure scans
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB; constraintsViewer_->removeCloud("scan0");
scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform()); constraintsViewer_->removeCloud("scan1");
scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), dataTo.laserScanInfo().localTransform()); if(dataFrom.laserScanRaw().channels() == 6)
scanB = rtabmap::util3d::transformPointCloud(scanB, t);
if(scanA->size())
{ {
constraintsViewer_->addCloud("scan0", scanA, Transform::getIdentity(), Qt::yellow); pcl::PointCloud<pcl::PointNormal>::Ptr scan;
scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
constraintsViewer_->addCloud("scan0", scan, Transform::getIdentity(), Qt::yellow);
} }
else else
{ {
constraintsViewer_->removeCloud("scan0"); pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
scan = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
constraintsViewer_->addCloud("scan0", scan, Transform::getIdentity(), Qt::yellow);
} }
if(scanB->size()) if(dataTo.laserScanRaw().channels() == 6)
{ {
constraintsViewer_->addCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta); pcl::PointCloud<pcl::PointNormal>::Ptr scan;
scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
constraintsViewer_->addCloud("scan1", scan, Transform::getIdentity(), Qt::magenta);
} }
else else
{ {
constraintsViewer_->removeCloud("scan1"); pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
scan = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
constraintsViewer_->addCloud("scan1", scan, Transform::getIdentity(), Qt::magenta);
} }
} }
else else