From 4cd870571081f7aab9ad66dd4330ae128ce0c3d6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 8 Jul 2016 12:47:23 -0400 Subject: [PATCH] Fixed cloudFromDepth() method when depth image is not the same size as the calibration file. That fixes projection map created from Tango databases (where depth size != rgb size) --- corelib/include/rtabmap/core/util3d.h | 19 +- .../rtabmap/core/util3d_registration.h | 12 -- corelib/src/util3d.cpp | 95 +++++++-- corelib/src/util3d_registration.cpp | 54 ----- guilib/include/rtabmap/gui/MainWindow.h | 8 +- guilib/src/DatabaseViewer.cpp | 27 ++- guilib/src/MainWindow.cpp | 192 +++++++++++------- tools/CameraRGBD/main.cpp | 10 +- 8 files changed, 239 insertions(+), 178 deletions(-) diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 4536ca85..b77d3caf 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -74,16 +74,23 @@ pcl::PointXYZ RTABMAP_EXP projectDepthTo3D( bool smoothing, float maxZError = 0.02f); -pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDepth( +RTABMAP_DEPRECATED (pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDepth( const cv::Mat & imageDepth, float cx, float cy, float fx, float fy, int decimation = 1, float maxDepth = 0.0f, float minDepth = 0.0f, + std::vector * validIndices = 0), "Use cloudFromDepth with CameraModel interface."); +pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDepth( + const cv::Mat & imageDepth, + const CameraModel & model, + int decimation = 1, + float maxDepth = 0.0f, + float minDepth = 0.0f, std::vector * validIndices = 0); -pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDepthRGB( +RTABMAP_DEPRECATED (pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDepthRGB( const cv::Mat & imageRgb, const cv::Mat & imageDepth, float cx, float cy, @@ -91,6 +98,14 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDepthRGB( int decimation = 1, float maxDepth = 0.0f, float minDepth = 0.0f, + std::vector * validIndices = 0), "Use cloudFromDepthRGB with CameraModel interface."); +pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDepthRGB( + const cv::Mat & imageRgb, + const cv::Mat & imageDepth, + const CameraModel & model, + int decimation = 1, + float maxDepth = 0.0f, + float minDepth = 0.0f, std::vector * validIndices = 0); pcl::PointCloud::Ptr RTABMAP_EXP cloudFromDisparity( diff --git a/corelib/include/rtabmap/core/util3d_registration.h b/corelib/include/rtabmap/core/util3d_registration.h index 264ef3e3..93c5ef84 100644 --- a/corelib/include/rtabmap/core/util3d_registration.h +++ b/corelib/include/rtabmap/core/util3d_registration.h @@ -92,18 +92,6 @@ Transform RTABMAP_EXP icpPointToPlane( float epsilon = 0.0f, bool icp2D = false); -pcl::PointCloud::Ptr RTABMAP_EXP getICPReadyCloud( - const cv::Mat & depth, - float fx, - float fy, - float cx, - float cy, - int decimation, - double maxDepth, - float voxel, - int samples, - const Transform & transform = Transform::getIdentity()); - } // namespace util3d } // namespace rtabmap diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index 026e9c7e..c1e6f277 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -248,9 +248,42 @@ pcl::PointCloud::Ptr cloudFromDepth( float minDepth, std::vector * validIndices) { + CameraModel model(fx, fy, cx, cy); + return cloudFromDepth(imageDepth, model, decimation, maxDepth, minDepth, validIndices); +} + +pcl::PointCloud::Ptr cloudFromDepth( + const cv::Mat & imageDepth, + const CameraModel & model, + int decimation, + float maxDepth, + float minDepth, + std::vector * validIndices) +{ + float rgbToDepthFactorX = 1.0f; + float rgbToDepthFactorY = 1.0f; + + UASSERT(model.isValidForProjection()); UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1)); - UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("rows=%d decimation=%d", imageDepth.rows, decimation).c_str()); - UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("cols=%d decimation=%d", imageDepth.cols, decimation).c_str()); + + int imageRows = imageDepth.rows; + int imageCols = imageDepth.cols; + + if(model.imageHeight()>0 && model.imageWidth()>0) + { + UASSERT(model.imageHeight() % imageDepth.rows == 0 && model.imageWidth() % imageDepth.cols == 0); + UASSERT_MSG(model.imageHeight() % decimation == 0, uFormat("model.imageHeight()=%d decimation=%d", model.imageHeight(), decimation).c_str()); + UASSERT_MSG(model.imageWidth() % decimation == 0, uFormat("model.imageWidth()=%d decimation=%d", model.imageWidth(), decimation).c_str()); + rgbToDepthFactorX = 1.0f/float((model.imageWidth() / imageDepth.cols)); + rgbToDepthFactorY = 1.0f/float((model.imageHeight() / imageDepth.rows)); + imageRows = model.imageHeight(); + imageCols = model.imageWidth(); + } + else + { + UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("rows=%d decimation=%d", imageDepth.rows, decimation).c_str()); + UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("cols=%d decimation=%d", imageDepth.cols, decimation).c_str()); + } pcl::PointCloud::Ptr cloud(new pcl::PointCloud); if(decimation < 1) @@ -259,8 +292,8 @@ pcl::PointCloud::Ptr cloudFromDepth( } //cloud.header = cameraInfo.header; - cloud->height = imageDepth.rows/decimation; - cloud->width = imageDepth.cols/decimation; + cloud->height = imageRows/decimation; + cloud->width = imageCols/decimation; cloud->is_dense = false; cloud->resize(cloud->height * cloud->width); if(validIndices) @@ -268,14 +301,27 @@ pcl::PointCloud::Ptr cloudFromDepth( validIndices->resize(cloud->size()); } + float depthFx = model.fx() * rgbToDepthFactorX; + float depthFy = model.fy() * rgbToDepthFactorY; + float depthCx = model.cx() * rgbToDepthFactorX; + float depthCy = model.cy() * rgbToDepthFactorY; + + UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d", + imageCols, imageRows, + imageDepth.cols, imageDepth.rows, + model.fx(), model.fy(), model.cx(), model.cy(), + rgbToDepthFactorX, + rgbToDepthFactorY, + decimation); + int oi = 0; - for(int h = 0; h < imageDepth.rows; h+=decimation) + for(int h = 0; h < imageRows && h/decimation < (int)cloud->height; h+=decimation) { - for(int w = 0; w < imageDepth.cols; w+=decimation) + for(int w = 0; w < imageCols && w/decimation < (int)cloud->width; w+=decimation) { pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation)); - pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false); + pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w*rgbToDepthFactorX, h*rgbToDepthFactorY, depthCx, depthCy, depthFx, depthFy, false); if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth)) { pt.x = ptXYZ.x; @@ -310,8 +356,23 @@ pcl::PointCloud::Ptr cloudFromDepthRGB( float maxDepth, float minDepth, std::vector * validIndices) +{ + CameraModel model(fx, fy, cx, cy); + return cloudFromDepthRGB(imageRgb, imageDepth, model, decimation, maxDepth, minDepth, validIndices); +} + +pcl::PointCloud::Ptr cloudFromDepthRGB( + const cv::Mat & imageRgb, + const cv::Mat & imageDepth, + const CameraModel & model, + int decimation, + float maxDepth, + float minDepth, + std::vector * validIndices) { UDEBUG(""); + UASSERT(model.isValidForProjection()); + UASSERT((model.imageHeight() == 0 && model.imageWidth() == 0) || (model.imageHeight() == imageRgb.rows && model.imageWidth() == imageRgb.cols)); UASSERT(imageRgb.rows % imageDepth.rows == 0 && imageRgb.cols % imageDepth.cols == 0); UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1)); UASSERT_MSG(imageRgb.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageRgb.rows, decimation).c_str()); @@ -349,15 +410,15 @@ pcl::PointCloud::Ptr cloudFromDepthRGB( float rgbToDepthFactorX = 1.0f/float((imageRgb.cols / imageDepth.cols)); float rgbToDepthFactorY = 1.0f/float((imageRgb.rows / imageDepth.rows)); - float depthFx = fx * rgbToDepthFactorX; - float depthFy = fy * rgbToDepthFactorY; - float depthCx = cx * rgbToDepthFactorX; - float depthCy = cy * rgbToDepthFactorY; + float depthFx = model.fx() * rgbToDepthFactorX; + float depthFy = model.fy() * rgbToDepthFactorY; + float depthCx = model.cx() * rgbToDepthFactorX; + float depthCy = model.cy() * rgbToDepthFactorY; UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d", imageRgb.cols, imageRgb.rows, imageDepth.cols, imageDepth.rows, - fx, fy, cx, cy, + model.fx(), model.fy(), model.cx(), model.cy(), rgbToDepthFactorX, rgbToDepthFactorY, decimation); @@ -653,10 +714,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( { pcl::PointCloud::Ptr tmp = util3d::cloudFromDepth( cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)), - sensorData.cameraModels()[i].cx(), - sensorData.cameraModels()[i].cy(), - sensorData.cameraModels()[i].fx(), - sensorData.cameraModels()[i].fy(), + sensorData.cameraModels()[i], decimation, maxDepth, minDepth, @@ -767,10 +825,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( pcl::PointCloud::Ptr tmp = util3d::cloudFromDepthRGB( cv::Mat(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows)), cv::Mat(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows)), - sensorData.cameraModels()[i].cx(), - sensorData.cameraModels()[i].cy(), - sensorData.cameraModels()[i].fx(), - sensorData.cameraModels()[i].fy(), + sensorData.cameraModels()[i], decimation, maxDepth, minDepth, diff --git a/corelib/src/util3d_registration.cpp b/corelib/src/util3d_registration.cpp index 16a7f5db..d168b9e7 100644 --- a/corelib/src/util3d_registration.cpp +++ b/corelib/src/util3d_registration.cpp @@ -380,60 +380,6 @@ Transform icpPointToPlane( return Transform::fromEigen4f(icp.getFinalTransformation()); } -// If "voxel" > 0, "samples" is ignored -pcl::PointCloud::Ptr getICPReadyCloud( - const cv::Mat & depth, - float fx, - float fy, - float cx, - float cy, - int decimation, - double maxDepth, - float voxel, - int samples, - const Transform & transform) -{ - UASSERT(!depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1)); - pcl::PointCloud::Ptr cloud; - cloud = cloudFromDepth( - depth, - cx, - cy, - fx, - fy, - decimation); - - if(cloud->size()) - { - if(maxDepth>0.0) - { - cloud = passThrough(cloud, "z", 0, maxDepth); - } - - if(cloud->size()) - { - if(voxel>0) - { - cloud = voxelize(cloud, voxel); - } - else if(samples>0 && (int)cloud->size() > samples) - { - cloud = randomSampling(cloud, samples); - } - - if(cloud->size()) - { - if(!transform.isNull() && !transform.isIdentity()) - { - cloud = transformPointCloud(cloud, transform); - } - } - } - } - - return cloud; -} - } } diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 8285bdca..b2296ce9 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -204,6 +204,7 @@ private slots: void dataRecorder(); void dataRecorderDestroyed(); void updateNodeVisibility(int, bool); + void updateGraphView(); signals: void statsReceived(const rtabmap::Statistics &); @@ -233,7 +234,12 @@ private: const std::map & labels, const std::map & groundTruths, bool verboseProgress = false); - void createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId); + std::pair::Ptr, pcl::IndicesPtr> createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId); + void createAndAddProjectionMap( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + int nodeId, + const Transform & pose); void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId); void createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId); Transform alignPosesToGroundTruth(std::map & poses, const std::map & groundTruth); diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index c88ed080..977a72a2 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -2328,7 +2328,10 @@ void DatabaseViewer::update(int value, { depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f); } - cloud = util3d::cloudFromDepthRGB(data.imageRaw(), depth, data.cameraModels()[0].cx(), data.cameraModels()[0].cy(), data.cameraModels()[0].fx(), data.cameraModels()[0].fy()); + cloud = util3d::cloudFromDepthRGB( + data.imageRaw(), + depth, + data.cameraModels()[0]); } else { @@ -2388,15 +2391,6 @@ void DatabaseViewer::update(int value, } } - if(!imgDepth.isNull()) - { - view->setImageDepth(imgDepth); - rect = imgDepth.rect(); - } - else - { - ULOGGER_DEBUG("Image depth is empty"); - } if(!img.isNull()) { view->setImage(img); @@ -2407,6 +2401,19 @@ void DatabaseViewer::update(int value, ULOGGER_DEBUG("Image is empty"); } + if(!imgDepth.isNull()) + { + view->setImageDepth(imgDepth); + if(!img.isNull()) + { + rect = imgDepth.rect(); + } + } + else + { + ULOGGER_DEBUG("Image depth is empty"); + } + // loops std::map links; dbDriver_->loadLinks(id, links); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 2bca0e0c..39798a2b 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -453,6 +453,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configGUIModified())); connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified())); } + connect(_ui->dockWidget_graphViewer->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView())); // catch resize events _ui->dockWidget_posterior->installEventFilter(this); _ui->dockWidget_likelihood->installEventFilter(this); @@ -1912,9 +1913,16 @@ void MainWindow::updateMapCloud( std::string cloudName = uFormat("cloud%d", iter->first); // 3d point cloud - if((_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)) || - (_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible() && _preferencesDialog->isGridMapFrom3DCloud())) + bool update3dCloud = _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0); + bool updateProjMap = + _ui->graphicsView_graphView->isVisible() && + _ui->graphicsView_graphView->isGridMapVisible() && + _preferencesDialog->isGridMapFrom3DCloud() && + _projectionLocalMaps.find(iter->first) == _projectionLocalMaps.end(); + if(update3dCloud || updateProjMap) { + // update cloud + std::pair::Ptr, pcl::IndicesPtr> createdCloud; if(viewerClouds.contains(cloudName)) { // Update only if the pose has changed @@ -1933,14 +1941,24 @@ void MainWindow::updateMapCloud( } else if(_cachedClouds.find(iter->first) == _cachedClouds.end() && _cachedSignatures.contains(iter->first)) { - if((_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)) || - _projectionLocalMaps.find(iter->first) == _projectionLocalMaps.end()) + createdCloud = this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1)); + if(viewerClouds.contains(cloudName)) { - this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1)); - if(viewerClouds.contains(cloudName)) - { - _cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)); - } + _cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)); + } + } + + //Update projection map + if(updateProjMap) + { + std::map::Ptr, pcl::IndicesPtr> >::iterator cloudIter = _cachedClouds.find(iter->first); + if(cloudIter != _cachedClouds.end()) + { + createAndAddProjectionMap(cloudIter->second.first, cloudIter->second.second, iter->first, iter->second); + } + else if(createdCloud.first->size() && createdCloud.second->size()) + { + createAndAddProjectionMap(createdCloud.first, createdCloud.second, iter->first, iter->second); } } } @@ -2247,22 +2265,23 @@ void MainWindow::updateMapCloud( UDEBUG(""); } -void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId) +std::pair::Ptr, pcl::IndicesPtr> MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId) { UDEBUG(""); UASSERT(!pose.isNull()); std::string cloudName = uFormat("cloud%d", nodeId); + std::pair::Ptr, pcl::IndicesPtr> outputPair; if(_cloudViewer->getAddedClouds().contains(cloudName)) { UERROR("Cloud %d already added to map.", nodeId); - return; + return outputPair; } QMap::iterator iter = _cachedSignatures.find(nodeId); if(iter == _cachedSignatures.end()) { UERROR("Node %d is not in the cache.", nodeId); - return; + return outputPair; } UASSERT(_cachedClouds.find(nodeId) == _cachedClouds.end()); @@ -2286,7 +2305,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int _preferencesDialog->getCloudDecimation(0), image.cols, image.rows); - return; + return outputPair; } // Create organized cloud @@ -2336,49 +2355,6 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int _preferencesDialog->getMapNoiseMinNeighbors()); } - if(indices->size() && - _preferencesDialog->isGridMapFrom3DCloud() && - _projectionLocalMaps.find(nodeId) == _projectionLocalMaps.end()) - { - UTimer timer; - cv::Mat ground, obstacles; - pcl::PointCloud::Ptr voxelCloud = cloud; - - // voxelize to grid cell size - if(_preferencesDialog->getMapVoxel() < _preferencesDialog->getGridMapResolution()) - { - voxelCloud = util3d::voxelize(voxelCloud, indices, _preferencesDialog->getGridMapResolution()); - } - - // add pose rotation without yaw - if(_preferencesDialog->projMapFrame()) - { - float roll, pitch, yaw; - pose.getEulerAngles(roll, pitch, yaw); - voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0, pose.z(), roll, pitch, 0)); - } - - if(_preferencesDialog->projMaxObstaclesHeight()) - { - voxelCloud = util3d::passThrough(voxelCloud, "z", std::numeric_limits::min(), _preferencesDialog->projMaxObstaclesHeight()); - } - - util3d::occupancy2DFromCloud3D( - voxelCloud, - ground, - obstacles, - _preferencesDialog->getGridMapResolution(), - _preferencesDialog->projMaxGroundAngle(), - _preferencesDialog->projMinClusterSize(), - _preferencesDialog->projFlatObstaclesDetected(), - _preferencesDialog->projMaxGroundHeight()); - if(!ground.empty() || !obstacles.empty()) - { - _projectionLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles))); - } - UDEBUG("time gridMapFrom3DCloud = %f s", timer.ticks()); - } - pcl::PointCloud::Ptr cloudWithNormals(new pcl::PointCloud); if(_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)) { @@ -2443,7 +2419,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int } - UWARN("Time subtract filtering %d from %d -> %d (%fs)", + UINFO("Time subtract filtering %d from %d -> %d (%fs)", (int)_previousCloud.second.second->size(), (int)beforeFiltering->size(), (int)indices->size(), @@ -2458,10 +2434,11 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int if(indices->size()) { + pcl::PointCloud::Ptr output; + bool added = false; if(_preferencesDialog->isCloudMeshing() && cloud->isOrganized()) { // Fast organized mesh - pcl::PointCloud::Ptr output; // we need to extract indices as pcl::OrganizedFastMesh doesn't take indices output = util3d::extractIndices(cloud, indices, false, true); std::vector polygons = util3d::organizedFastMesh( @@ -2480,10 +2457,9 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int { UERROR("Adding mesh cloud %d to viewer failed!", nodeId); } - else if(_preferencesDialog->isCloudsKept()) + else { - _cachedClouds.insert(std::make_pair(nodeId, std::make_pair(output, indices))); - _createdCloudsMemoryUsage += output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int); + added = true; } } } @@ -2508,7 +2484,6 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int color = (Qt::GlobalColor)(mapId+3 % 12 + 7 ); } - pcl::PointCloud::Ptr output; output = util3d::extractIndices(cloud, indices, false, true); if(cloudWithNormals->size()) @@ -2520,37 +2495,95 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int { UERROR("Adding cloud %d to viewer failed!", nodeId); } - else if(_preferencesDialog->isCloudsKept()) + else { - _cachedClouds.insert(std::make_pair(nodeId, std::make_pair(output, indices))); - _createdCloudsMemoryUsage += output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int); + added = true; } } else { - if(!_cloudViewer->addCloud(cloudName, output, pose, color)) { UERROR("Adding cloud %d to viewer failed!", nodeId); } - else if(_preferencesDialog->isCloudsKept()) + else { - _cachedClouds.insert(std::make_pair(nodeId, std::make_pair(output, indices))); - _createdCloudsMemoryUsage += output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int); + added = true; } } } + if(added) + { + outputPair.first = output; + outputPair.second = indices; + if(_preferencesDialog->isCloudsKept()) + { + _cachedClouds.insert(std::make_pair(nodeId, outputPair)); + _createdCloudsMemoryUsage += output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int); + } + } } _cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0)); _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0)); } } - else + + return outputPair; + UDEBUG(""); +} + +void MainWindow::createAndAddProjectionMap( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + int nodeId, + const Transform & pose) +{ + UASSERT(!pose.isNull()); + + if(_projectionLocalMaps.find(nodeId) != _projectionLocalMaps.end()) { + UERROR("Projection map %d already added.", nodeId); return; } - UDEBUG(""); + if(indices->size()) + { + UTimer timer; + cv::Mat ground, obstacles; + pcl::PointCloud::Ptr voxelCloud = cloud; + + // voxelize to grid cell size + if(_preferencesDialog->getMapVoxel() < _preferencesDialog->getGridMapResolution()) + { + voxelCloud = util3d::voxelize(voxelCloud, indices, _preferencesDialog->getGridMapResolution()); + } + + // add pose rotation without yaw + if(_preferencesDialog->projMapFrame()) + { + float roll, pitch, yaw; + pose.getEulerAngles(roll, pitch, yaw); + voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0, pose.z(), roll, pitch, 0)); + } + + if(_preferencesDialog->projMaxObstaclesHeight()) + { + voxelCloud = util3d::passThrough(voxelCloud, "z", std::numeric_limits::min(), _preferencesDialog->projMaxObstaclesHeight()); + } + + util3d::occupancy2DFromCloud3D( + voxelCloud, + ground, + obstacles, + _preferencesDialog->getGridMapResolution(), + _preferencesDialog->projMaxGroundAngle(), + _preferencesDialog->projMinClusterSize(), + _preferencesDialog->projFlatObstaclesDetected(), + _preferencesDialog->projMaxGroundHeight()); + + _projectionLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles))); + UDEBUG("time gridMapFrom3DCloud = %f s", timer.ticks()); + } } void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int mapId) @@ -2838,6 +2871,23 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible) _cloudViewer->update(); } +void MainWindow::updateGraphView() +{ + if(_ui->dockWidget_graphViewer->isVisible()) + { + UDEBUG("Graph visible!"); + if(_currentPosesMap.size()) + { + this->updateMapCloud( + std::map(_currentPosesMap), + std::multimap(_currentLinksMap), + std::map(_currentMapIds), + std::map(_currentLabels), + std::map(_currentGTPosesMap)); + } + } +} + void MainWindow::processRtabmapEventInit(int status, const QString & info) { if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitializing) diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index 64beec8d..07a2349c 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -342,10 +342,7 @@ int main(int argc, char * argv[]) { pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB( rgb, depth, - data.cameraModels()[0].cx(), - data.cameraModels()[0].cy(), - data.cameraModels()[0].fx(), - data.cameraModels()[0].fy()); + data.cameraModels()[0]); cloud = rtabmap::util3d::transformPointCloud(cloud, t); if(viewer) viewer->showCloud(cloud, "cloud"); @@ -356,10 +353,7 @@ int main(int argc, char * argv[]) { pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepth( depth, - data.cameraModels()[0].cx(), - data.cameraModels()[0].cy(), - data.cameraModels()[0].fx(), - data.cameraModels()[0].fy()); + data.cameraModels()[0]); cloud = rtabmap::util3d::transformPointCloud(cloud, t); viewer->showCloud(cloud, "cloud"); }