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

Added multi-camera feature

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-29 14:46:48 -04:00
parent e6923daf1c
commit c6d0d47b1c
51 changed files with 2833 additions and 2297 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
guilib/src/LoopClosureViewer.cpp
View File

@@ -106,74 +106,14 @@ void LoopClosureViewer::updateView(const Transform & transform)
ui_->label_transform->setText(QString("(%1)").arg(t.prettyPrint().c_str()));
if(!t.isNull())
{
//cloud 3d
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
if(sA_.getDepthRaw().type() == CV_8UC1)
{
cloudA = util3d::cloudFromStereoImages(
sA_.getImageRaw(),
sA_.getDepthRaw(),
sA_.getCx(), sA_.getCy(),
sA_.getFx(), sA_.getFy(),
decimation);
}
else
{
cloudA = util3d::cloudFromDepthRGB(
sA_.getImageRaw(),
sA_.getDepthRaw(),
sA_.getCx(), sA_.getCy(),
sA_.getFx(), sA_.getFy(),
decimation);
}
cloudA = util3d::removeNaNFromPointCloud(cloudA);
if(maxDepth>0.0)
{
cloudA = util3d::passThrough(cloudA, "z", 0, maxDepth);
}
if(samples>0 && (int)cloudA->size() > samples)
{
cloudA = util3d::sampling(cloudA, samples);
}
cloudA = util3d::transformPointCloud(cloudA, sA_.getLocalTransform());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
if(sB_.getDepthRaw().type() == CV_8UC1)
{
cloudB = util3d::cloudFromStereoImages(
sB_.getImageRaw(),
sB_.getDepthRaw(),
sB_.getCx(), sB_.getCy(),
sB_.getFx(), sB_.getFy(),
decimation);
}
else
{
cloudB = util3d::cloudFromDepthRGB(
sB_.getImageRaw(),
sB_.getDepthRaw(),
sB_.getCx(), sB_.getCy(),
sB_.getFx(), sB_.getFy(),
decimation);
}
cloudB = util3d::removeNaNFromPointCloud(cloudB);
if(maxDepth>0.0)
{
cloudB = util3d::passThrough(cloudB, "z", 0, maxDepth);
}
if(samples>0 && (int)cloudB->size() > samples)
{
cloudB = util3d::sampling(cloudB, samples);
}
//cloud 3d
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA, cloudB;
cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, 0.0f, samples);
cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, 0.0f, samples);
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = util3d::laserScanToPointCloud(sA_.getLaserScanRaw());
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = util3d::laserScanToPointCloud(sA_.sensorData().laserScanRaw());
scanB = util3d::laserScanToPointCloud(sB_.sensorData().laserScanRaw());
scanB = util3d::transformPointCloud(scanB, t);
@@ -184,6 +124,7 @@ void LoopClosureViewer::updateView(const Transform & transform)
ui_->cloudViewerTransform->addOrUpdateCloud("cloud0", cloudA);
}
if(cloudB->size())
{
cloudB = util3d::transformPointCloud(cloudB, t);
ui_->cloudViewerTransform->addOrUpdateCloud("cloud1", cloudB);
}