diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 12730093..d11ce79f 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -490,7 +490,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Grid, MinGroundHeight, float, 0.0, "Minimum ground height (0=disabled)."); RTABMAP_PARAM(Grid, MaxGroundHeight, float, 0.0, uFormat("Maximum ground height (0=disabled). Should be set if \"%s\" is true.", kGridNormalsSegmentation().c_str())); RTABMAP_PARAM(Grid, MaxGroundAngle, float, 45, uFormat("[%s=true] Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.", kGridNormalsSegmentation().c_str())); - RTABMAP_PARAM(Grid, NormalK, int, 10, uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str())); + RTABMAP_PARAM(Grid, NormalK, int, 20, uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str())); RTABMAP_PARAM(Grid, ClusterRadius, float, 0.1, uFormat("[%s=true] Cluster maximum radius.", kGridNormalsSegmentation().c_str())); RTABMAP_PARAM(Grid, MinClusterSize, int, 10, uFormat("[%s=true] Minimum cluster size to project the points.", kGridNormalsSegmentation().c_str())); RTABMAP_PARAM(Grid, FlatObstacleDetected, bool, true, uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str())); @@ -504,7 +504,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int, 5, "Noise filtering minimum neighbors."); RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled, bool, false, "Unknown space filled. Only used with 2D laser scans."); RTABMAP_PARAM(Grid, Scan2dMaxFilledRange, float, 4.0, "Unknown space filled maximum range. If 0, the laser scan maximum range is used."); - RTABMAP_PARAM(Grid, ProjRayTracing, bool, false, uFormat("[%s=false] 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.", kGrid3D().c_str())); + RTABMAP_PARAM(Grid, ProjRayTracing, bool, true, uFormat("[%s=false] 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.", kGrid3D().c_str())); public: virtual ~Parameters(); diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 95a3d2ac..65745d41 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -213,6 +213,8 @@ pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const cv::Mat & laserScan, i // For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters. pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r = 255, unsigned char g = 255, unsigned char b = 255); +void RTABMAP_EXP getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max); +void RTABMAP_EXP getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max); cv::Point3f RTABMAP_EXP projectDisparityTo3D( const cv::Point2f & pt, diff --git a/corelib/include/rtabmap/core/util3d_mapping.h b/corelib/include/rtabmap/core/util3d_mapping.h index 1d9cf9b0..6c137eb4 100644 --- a/corelib/include/rtabmap/core/util3d_mapping.h +++ b/corelib/include/rtabmap/core/util3d_mapping.h @@ -51,13 +51,23 @@ RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan( bool unknownSpaceFilled = false, float scanMaxRange = 0.0f), "Use interface with \"viewpoint\" parameter to make sure the ray tracing origin is from the sensor and not the base."); -void RTABMAP_EXP occupancy2DFromLaserScan( +RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan( const cv::Mat & scan, // in /base_link frame const cv::Point3f & viewpoint, // /base_link -> /base_scan cv::Mat & ground, cv::Mat & obstacles, float cellSize, bool unknownSpaceFilled = false, + float scanMaxRange = 0.0f), "Use interface with scanHit/scanNoHit parameters: scanNoHit set to null matrix has the same functionality than this method."); + +void RTABMAP_EXP occupancy2DFromLaserScan( + const cv::Mat & scanHit, // in /base_link frame + const cv::Mat & scanNoHit, // in /base_link frame + const cv::Point3f & viewpoint, // /base_link -> /base_scan + cv::Mat & ground, + cv::Mat & obstacles, + float cellSize, + bool unknownSpaceFilled = false, float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps( @@ -79,7 +89,7 @@ RTABMAP_DEPRECATED(cv::Mat RTABMAP_EXP create2DMap(const std::map & poses, +RTABMAP_DEPRECATED(cv::Mat RTABMAP_EXP create2DMap(const std::map & poses, const std::map::Ptr > & scans, // in /base_link frame const std::map & viewpoints, // /base_link -> /base_scan float cellSize, @@ -87,6 +97,16 @@ cv::Mat RTABMAP_EXP create2DMap(const std::map & poses, float & xMin, float & yMin, float minMapSize = 0.0f, + float scanMaxRange = 0.0f), "Use interface with cv::Mat scans."); + +cv::Mat RTABMAP_EXP create2DMap(const std::map & poses, + const std::map > & scans, // >, in /base_link frame + const std::map & viewpoints, // /base_link -> /base_scan + float cellSize, + bool unknownSpaceFilled, + float & xMin, + float & yMin, + float minMapSize = 0.0f, float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true void RTABMAP_EXP rayTrace(const cv::Point2i & start, diff --git a/corelib/src/OccupancyGrid.cpp b/corelib/src/OccupancyGrid.cpp index e5789313..4b92af93 100644 --- a/corelib/src/OccupancyGrid.cpp +++ b/corelib/src/OccupancyGrid.cpp @@ -208,6 +208,7 @@ void OccupancyGrid::createLocalMap( util3d::occupancy2DFromLaserScan( util3d::transformLaserScan(node.sensorData().laserScanRaw(), node.sensorData().laserScanInfo().localTransform()), + cv::Mat(), viewPoint, ground, obstacles, @@ -345,9 +346,12 @@ void OccupancyGrid::createLocalMap( if(projRayTracing_) { cv::Mat laserScan = obstacles; + cv::Mat laserScanNoHit = ground; obstacles = cv::Mat(); + ground = cv::Mat(); util3d::occupancy2DFromLaserScan( laserScan, + laserScanNoHit, viewPoint, ground, obstacles, diff --git a/corelib/src/OctoMap.cpp b/corelib/src/OctoMap.cpp index a7cd57e1..56cfd7c1 100644 --- a/corelib/src/OctoMap.cpp +++ b/corelib/src/OctoMap.cpp @@ -107,7 +107,7 @@ void OctoMap::update(const std::map & poses) } else { - UWARN("Updated pose for node %d is not found, some points may not be copied.", jter->first); + UWARN("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first); } } if(graphChanged) diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index 82b8cff3..b870cdcb 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -1424,6 +1424,44 @@ pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsig return output; } +void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max) +{ + UASSERT(!laserScan.empty()); + UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6)); + + const float * ptr = laserScan.ptr(0, 0); + min.x = max.x = ptr[0]; + min.y = max.y = ptr[1]; + min.z = max.z = laserScan.channels() >= 3?ptr[2]:0.0f; + for(int i=1; i(0, i); + + if(ptr[0] < min.x) min.x = ptr[0]; + else if(ptr[0] > max.x) max.x = ptr[0]; + + if(ptr[1] < min.y) min.y = ptr[1]; + else if(ptr[1] > max.y) max.y = ptr[1]; + + if(laserScan.channels() >= 3) + { + if(ptr[2] < min.z) min.z = ptr[2]; + else if(ptr[2] > max.z) max.z = ptr[2]; + } + } +} +void getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max) +{ + cv::Point3f minCV, maxCV; + getMinMax3D(laserScan, minCV, maxCV); + min.x = minCV.x; + min.y = minCV.y; + min.z = minCV.z; + max.x = maxCV.x; + max.y = maxCV.y; + max.z = maxCV.z; +} + // inspired from ROS image_geometry/src/stereo_camera_model.cpp cv::Point3f projectDisparityTo3D( const cv::Point2f & pt, diff --git a/corelib/src/util3d_mapping.cpp b/corelib/src/util3d_mapping.cpp index 5a63aced..3d482c42 100644 --- a/corelib/src/util3d_mapping.cpp +++ b/corelib/src/util3d_mapping.cpp @@ -74,7 +74,20 @@ void occupancy2DFromLaserScan( bool unknownSpaceFilled, float scanMaxRange) { - if(scan.empty()) + occupancy2DFromLaserScan(scan, cv::Mat(), viewpoint, ground, obstacles, cellSize, unknownSpaceFilled, scanMaxRange); +} + +void occupancy2DFromLaserScan( + const cv::Mat & scanHit, + const cv::Mat & scanNoHit, + const cv::Point3f & viewpoint, + cv::Mat & ground, + cv::Mat & obstacles, + float cellSize, + bool unknownSpaceFilled, + float scanMaxRange) +{ + if(scanHit.empty() && scanNoHit.empty()) { return; } @@ -82,11 +95,8 @@ void occupancy2DFromLaserScan( std::map poses; poses.insert(std::make_pair(1, Transform::getIdentity())); - pcl::PointCloud::Ptr obstaclesCloud = util3d::laserScanToPointCloud(scan); - //obstaclesCloud = util3d::voxelize(obstaclesCloud, cellSize); - - std::map::Ptr> scans; - scans.insert(std::make_pair(1, obstaclesCloud)); + std::map > scans; + scans.insert(std::make_pair(1, std::make_pair(scanHit, scanNoHit))); std::map viewpoints; viewpoints.insert(std::make_pair(1, viewpoint)); @@ -94,23 +104,6 @@ void occupancy2DFromLaserScan( float xMin, yMin; cv::Mat map8S = create2DMap(poses, scans, viewpoints, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange); - // If input ground has already values, add them to map - if(ground.rows == 1 && ground.cols>0 && ground.type() == CV_32FC2) - { - for(int i=0; i(); - - // cell - cv::Point2i cell((ptr[i][0]-xMin)/cellSize, (ptr[i][1]-yMin)/cellSize); - - if(cell.x>=0 && cell.x= 0 && cell.y < map8S.rows && map8S.at(cell.y, cell.x) == -1) - { - map8S.at(cell.y, cell.x) = 0; - } - } - } - // find ground cells std::list groundIndices; for(unsigned int i=0; i< map8S.total(); ++i) @@ -139,17 +132,7 @@ void occupancy2DFromLaserScan( } // copy directly obstacles precise positions - obstacles = cv::Mat(); - if(obstaclesCloud->size()) - { - obstacles = cv::Mat(1, (int)obstaclesCloud->size(), CV_32FC2); - for(unsigned int i=0;isize(); ++i) - { - cv::Vec2f * ptr = obstacles.ptr(); - ptr[i][0] = obstaclesCloud->at(i).x; - ptr[i][1] = obstaclesCloud->at(i).y; - } - } + obstacles = scanHit.clone(); } /** @@ -510,8 +493,39 @@ cv::Mat create2DMap(const std::map & poses, float scanMaxRange) { std::map viewpoints; + std::map > scansCv; + for(std::map::Ptr >::const_iterator iter = scans.begin(); iter!=scans.end(); ++iter) + { + scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second), cv::Mat()))); + } return create2DMap(poses, - scans, + scansCv, + viewpoints, + cellSize, + unknownSpaceFilled, + xMin, + yMin, + minMapSize, + scanMaxRange); +} + +cv::Mat create2DMap(const std::map & poses, + const std::map::Ptr > & scans, + const std::map & viewpoints, + float cellSize, + bool unknownSpaceFilled, + float & xMin, + float & yMin, + float minMapSize, + float scanMaxRange) +{ + std::map > scansCv; + for(std::map::Ptr >::const_iterator iter = scans.begin(); iter!=scans.end(); ++iter) + { + scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second), cv::Mat()))); + } + return create2DMap(poses, + scansCv, viewpoints, cellSize, unknownSpaceFilled, @@ -537,7 +551,7 @@ cv::Mat create2DMap(const std::map & poses, * @param scanMaxRange laser scan maximum range, would be set if unknownSpaceFilled=true */ cv::Mat create2DMap(const std::map & poses, - const std::map::Ptr > & scans, + const std::map > & scans, // > const std::map & viewpoints, float cellSize, bool unknownSpaceFilled, @@ -547,7 +561,7 @@ cv::Mat create2DMap(const std::map & poses, float scanMaxRange) { UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange); - std::map::Ptr > localScans; + std::map > localScans; pcl::PointCloud minMax; if(minMapSize > 0.0f) @@ -557,15 +571,25 @@ cv::Mat create2DMap(const std::map & poses, } for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { - std::map::Ptr >::const_iterator jter=scans.find(iter->first); - if(jter!=scans.end() && jter->second->size()) + std::map >::const_iterator jter=scans.find(iter->first); + if(jter!=scans.end() && (jter->second.first.cols || jter->second.second.cols)) { UASSERT(!iter->second.isNull()); - pcl::PointCloud::Ptr cloud = util3d::transformPointCloud(jter->second, iter->second); + cv::Mat hit = util3d::transformLaserScan(jter->second.first, iter->second); + cv::Mat noHit = util3d::transformLaserScan(jter->second.second, iter->second); pcl::PointXYZ min, max; - pcl::getMinMax3D(*cloud, min, max); - minMax.push_back(min); - minMax.push_back(max); + if(!hit.empty()) + { + util3d::getMinMax3D(hit, min, max); + minMax.push_back(min); + minMax.push_back(max); + } + if(!noHit.empty()) + { + util3d::getMinMax3D(noHit, min, max); + minMax.push_back(min); + minMax.push_back(max); + } minMax.push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z())); std::map::const_iterator kter=viewpoints.find(iter->first); @@ -574,7 +598,7 @@ cv::Mat create2DMap(const std::map & poses, minMax.push_back(pcl::PointXYZ(iter->second.x()+kter->second.x, iter->second.y()+kter->second.y, iter->second.z()+kter->second.z)); } - localScans.insert(std::make_pair(iter->first, cloud)); + localScans.insert(std::make_pair(iter->first, std::make_pair(hit, noHit))); } } @@ -599,7 +623,7 @@ cv::Mat create2DMap(const std::map & poses, map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1; int j=0; - for(std::map::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter) + for(std::map >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter) { const Transform & pose = poses.at(iter->first); cv::Point3f viewpoint(0,0,0); @@ -609,15 +633,48 @@ cv::Mat create2DMap(const std::map & poses, viewpoint = kter->second; } cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize); - for(unsigned int i=0; isecond->size(); ++i) + + // Set obstacles first + for(unsigned int i=0; isecond.first.cols; ++i) { - cv::Point2i end((iter->second->points[i].x-xMin)/cellSize, (iter->second->points[i].y-yMin)/cellSize); + const float * ptr = iter->second.first.ptr(0, i); + cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize); if(end!=start) { - rayTrace(start, end, map, true); // trace free space map.at(end.y, end.x) = 100; // obstacle } } + + // ray tracing for hits + for(unsigned int i=0; isecond.first.cols; ++i) + { + const float * ptr = iter->second.first.ptr(0, i); + cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize); + if(end!=start) + { + if(localScans.size() > 1 || map.at(end.y, end.x) != 0) + { + rayTrace(start, end, map, true); // trace free space + } + } + } + // ray tracing for no hits + for(unsigned int i=0; isecond.second.cols; ++i) + { + const float * ptr = iter->second.second.ptr(0, i); + cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize); + if(end!=start) + { + if(localScans.size() > 1 || map.at(end.y, end.x) != 0) + { + rayTrace(start, end, map, true); // trace free space + if(map.at(end.y, end.x) == -1) + { + map.at(end.y, end.x) = 0; // empty + } + } + } + } ++j; } UDEBUG("Ray trace known space=%fs", timer.ticks()); @@ -627,9 +684,9 @@ cv::Mat create2DMap(const std::map & poses, { j=0; float a = CV_PI/256.0f; // angle increment - for(std::map::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter) + for(std::map >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter) { - if(iter->second->size() > 1) + if(iter->second.first.cols > 1) { if(scanMaxRange > cellSize) { @@ -650,19 +707,10 @@ cv::Mat create2DMap(const std::map & poses, cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F); origin.at(0) = pose.x()+viewpoint.x; origin.at(1) = pose.y()+viewpoint.y; - pcl::PointXYZ ptFirst = iter->second->points[0]; - pcl::PointXYZ ptLast = iter->second->points[iter->second->points.size()-1]; - //if(ptFirst.y > ptLast.y) - //{ - // swap to iterate counterclockwise - // pcl::PointXYZ tmp = ptLast; - // ptLast = ptFirst; - // ptFirst = tmp; - //} - endFirst.at(0) = ptFirst.x; - endFirst.at(1) = ptFirst.y; - endLast.at(0) = ptLast.x; - endLast.at(1) = ptLast.y; + endFirst.at(0) = iter->second.first.ptr(0,0)[0]; + endFirst.at(1) = iter->second.first.ptr(0,0)[1]; + endLast.at(0) = iter->second.first.ptr(0,iter->second.first.cols-1)[0]; + endLast.at(1) = iter->second.first.ptr(0,iter->second.first.cols-1)[1]; //UWARN("origin = %f %f", origin.at(0), origin.at(1)); //UWARN("endFirst = %f %f", endFirst.at(0), endFirst.at(1)); //UWARN("endLast = %f %f", endLast.at(0), endLast.at(1)); diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 31b0ee3a..3b45c7a0 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -241,7 +241,8 @@ private: const std::map & mapIds, const std::map & labels, const std::map & groundTruths, - bool verboseProgress = false); + bool verboseProgress = false, + std::map * stats = 0); std::pair::Ptr, pcl::IndicesPtr> createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId); void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId); void createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 04242885..9e599160 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -171,6 +171,7 @@ public: int getOctomapTreeDepth() const; bool isOctomapGroundAnObstacle() const; double getOctomapOccupancyThr() const; + int getOctomapPointSize() const; int getCloudDecimation(int index) const; // 0=map, 1=odom double getCloudMaxDepth(int index) const; // 0=map, 1=odom double getCloudMinDepth(int index) const; // 0=map, 1=odom diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 125be51b..3ccdc792 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -1721,6 +1721,7 @@ void DatabaseViewer::regenerateLocalMaps() rtabmap::ProgressDialog progressDialog(this); progressDialog.setMaximumSteps(ids_.size()); progressDialog.show(); + progressDialog.setCancelButtonVisible(true); UPlot * plot = new UPlot(this); plot->setWindowFlags(Qt::Window); @@ -1730,10 +1731,19 @@ void DatabaseViewer::regenerateLocalMaps() UPlotCurve * gridCreationCurve = plot->addCurve("Grid Creation"); plot->show(); + UPlot * plotCells = new UPlot(this); + plotCells->setWindowFlags(Qt::Window); + plotCells->setWindowTitle("Occupancy Cells"); + plotCells->setAttribute(Qt::WA_DeleteOnClose); + UPlotCurve * totalCurve = plotCells->addCurve("Total"); + UPlotCurve * groundCurve = plotCells->addCurve("Empty"); + UPlotCurve * obstaclesCurve = plotCells->addCurve("Occupied"); + plotCells->show(); + double decompressionTime = 0; double gridCreationTime = 0; - for(int i =0; iaddValue(ids_.at(i), obstacles.cols+ground.cols); + groundCurve->addValue(ids_.at(i), ground.cols); + obstaclesCurve->addValue(ids_.at(i), obstacles.cols); } progressDialog.appendText(msg); @@ -1772,7 +1786,16 @@ void DatabaseViewer::regenerateLocalMaps() } } progressDialog.setValue(progressDialog.maximumSteps()); - updateGrid(); + + if(graphes_.size()) + { + update3dView(); + sliderIterationsValueChanged((int)graphes_.size()-1); + } + else + { + updateGrid(); + } } void DatabaseViewer::regenerateCurrentLocalMaps() @@ -1826,7 +1849,16 @@ void DatabaseViewer::regenerateCurrentLocalMaps() QApplication::processEvents(); } progressDialog.setValue(progressDialog.maximumSteps()); - updateGrid(); + + if(graphes_.size()) + { + update3dView(); + sliderIterationsValueChanged((int)graphes_.size()-1); + } + else + { + updateGrid(); + } } void DatabaseViewer::view3DMap() @@ -3967,6 +3999,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) Qt::red); occupancyGridViewer_->setCloudPointSize("obstaclesXYZ", 5); } + occupancyGridViewer_->update(); } } } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index a0128063..55c7a080 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -586,6 +586,13 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", false); _ui->statsToolBox->updateStat("GUI/RGB-D cloud/ms", false); + _ui->statsToolBox->updateStat("GUI/Graph Update/ms", false); +#ifdef RTABMAP_OCTOMAP + _ui->statsToolBox->updateStat("GUI/Octomap Update/ms", false); + _ui->statsToolBox->updateStat("GUI/Octomap Rendering/ms", false); +#endif + _ui->statsToolBox->updateStat("GUI/Grid Update/ms", false); + _ui->statsToolBox->updateStat("GUI/Grid Rendering/ms", false); _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", false); _ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", false); _ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", false); @@ -1429,12 +1436,18 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) signature = stat.getSignatures().at(stat.refImageId()); signature.sensorData().uncompressData(); // make sure data are uncompressed - if(!smallMovement && - uStr2Bool(_preferencesDialog->getParameter(Parameters::kMemIncrementalMemory())) && + if( uStr2Bool(_preferencesDialog->getParameter(Parameters::kMemIncrementalMemory())) && signature.getWeight()>=0) // ignore intermediate nodes for the cache { - _cachedSignatures.insert(signature.id(), signature); - _cachedMemoryUsage += signature.sensorData().getMemoryUsed(); + if(smallMovement) + { + _cachedSignatures.insert(-1, signature); // negative means temporary + } + else + { + _cachedSignatures.insert(signature.id(), signature); + _cachedMemoryUsage += signature.sensorData().getMemoryUsed(); + } } } @@ -1675,8 +1688,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) //====================== // RGB-D Mapping stuff //====================== - UTimer timerVis; - // update clouds if(stat.poses().size()) { @@ -1725,19 +1736,40 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) } } + if(_cachedSignatures.contains(-1)) + { + if(poses.find(stat.refImageId())!=poses.end()) + { + poses.insert(std::make_pair(-1, poses.at(stat.refImageId()))); + poses.erase(stat.refImageId()); + } + if(groundTruth.find(stat.refImageId())!=groundTruth.end()) + { + groundTruth.insert(std::make_pair(-1, groundTruth.at(stat.refImageId()))); + groundTruth.erase(stat.refImageId()); + } + } + + std::map updateCloudSats; updateMapCloud( poses, stat.constraints(), mapIds, labels, - groundTruth); + groundTruth, + false, + &updateCloudSats); _odometryReceived = false; _odometryCorrection = groundTruthOffset * stat.mapCorrection(); UDEBUG("time= %d ms", time.restart()); - _ui->statsToolBox->updateStat("GUI/RGB-D cloud/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(timerVis.elapsed()*1000.0f), _preferencesDialog->isCacheSavedInFigures()); + + for(std::map::iterator iter=updateCloudSats.begin(); iter!=updateCloudSats.end(); ++iter) + { + _ui->statsToolBox->updateStat(iter->first.c_str(), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(iter->second), _preferencesDialog->isCacheSavedInFigures()); + } // loop closure view if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0) && @@ -1783,6 +1815,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) } UDEBUG(""); + _cachedSignatures.remove(-1); // remove tmp negative ids + // keep only compressed data in cache if(_cachedSignatures.contains(stat.refImageId())) { @@ -1796,7 +1830,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) _cachedMemoryUsage += s.sensorData().getMemoryUsed(); } - UDEBUG(""); } else if(!stat.extended() && stat.loopClosureId()>0) @@ -1843,17 +1876,21 @@ void MainWindow::updateMapCloud( const std::map & mapIdsIn, const std::map & labels, const std::map & groundTruths, // ground truth should contain only valid transforms - bool verboseProgress) + bool verboseProgress, + std::map * stats) { + UTimer timer; UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d labelsIn=%d", (int)posesIn.size(), (int)constraints.size(), (int)mapIdsIn.size(), (int)labels.size()); if(posesIn.size()) { _currentPosesMap = posesIn; + _currentPosesMap.erase(-1); // don't keep -1 if it is there _currentLinksMap = constraints; _currentMapIds = mapIdsIn; _currentLabels = labels; _currentGTPosesMap = groundTruths; + _currentGTPosesMap.erase(-1); if(_state != kMonitoring && _state != kDetecting) { _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1); @@ -1902,12 +1939,12 @@ void MainWindow::updateMapCloud( } _ui->widget_mapVisibility->setMap(posesIn, posesMask); - if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked()) + if(groundTruths.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked()) { for(std::map::iterator iter = poses.begin(); iter!=poses.end(); ++iter) { - std::map::iterator gtIter = _currentGTPosesMap.find(iter->first); - if(gtIter!=_currentGTPosesMap.end()) + std::map::const_iterator gtIter = groundTruths.find(iter->first); + if(gtIter!=groundTruths.end()) { iter->second = gtIter->second; } @@ -1932,6 +1969,12 @@ void MainWindow::updateMapCloud( { std::string cloudName = uFormat("cloud%d", iter->first); + if(iter->first < 0) + { + viewerClouds.remove(cloudName); + _cloudViewer->removeCloud(cloudName); + } + // 3d point cloud bool update3dCloud = _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0); if(update3dCloud) @@ -1971,6 +2014,11 @@ void MainWindow::updateMapCloud( // 2d point cloud std::string scanName = uFormat("scan%d", iter->first); + if(iter->first < 0) + { + viewerClouds.remove(scanName); + _cloudViewer->removeCloud(scanName); + } if(_cloudViewer->isVisible() && _preferencesDialog->isScansShown(0)) { if(viewerClouds.contains(scanName)) @@ -2004,6 +2052,12 @@ void MainWindow::updateMapCloud( } // occupancy grids + if(iter->first < 0) + { + _gridLocalMaps.erase(iter->first); + _gridViewPoints.erase(iter->first); + } + bool updateGridMap = ((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) || (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) && @@ -2061,6 +2115,11 @@ void MainWindow::updateMapCloud( // 3d features std::string featuresName = uFormat("features%d", iter->first); + if(iter->first < 0) + { + viewerClouds.remove(featuresName); + _cloudViewer->removeCloud(featuresName); + } if(_cloudViewer->isVisible() && _preferencesDialog->isFeaturesShown(0)) { if(viewerClouds.contains(featuresName)) @@ -2129,6 +2188,10 @@ void MainWindow::updateMapCloud( } UDEBUG(""); + if(stats) + { + stats->insert(std::make_pair("GUI/RGB-D cloud/ms", (float)timer.restart()*1000.0f)); + } // update 3D graphes (show all poses) _cloudViewer->removeAllGraphs(); @@ -2268,6 +2331,10 @@ void MainWindow::updateMapCloud( } UDEBUG(""); + if(stats) + { + stats->insert(std::make_pair("GUI/Graph Update/ms", (float)timer.restart()*1000.0f)); + } #ifdef RTABMAP_OCTOMAP _cloudViewer->removeOctomap(); @@ -2279,6 +2346,10 @@ void MainWindow::updateMapCloud( _octomap->update(poses); UINFO("Octomap update time = %fs", time.ticks()); } + if(stats) + { + stats->insert(std::make_pair("GUI/Octomap Update/ms", (float)timer.restart()*1000.0f)); + } if(_preferencesDialog->isOctomapShown()) { UDEBUG(""); @@ -2294,11 +2365,16 @@ void MainWindow::updateMapCloud( if(obstacles->size()) { _cloudViewer->addCloud("octomap_cloud", cloud); + _cloudViewer->setCloudPointSize("octomap_cloud", _preferencesDialog->getOctomapPointSize()); } } UINFO("Octomap show 3d map time = %fs", time.ticks()); } UDEBUG(""); + if(stats) + { + stats->insert(std::make_pair("GUI/Octomap Rendering/ms", (float)timer.restart()*1000.0f)); + } #endif // Update occupancy grid map in 3D map view and graph view @@ -2326,6 +2402,10 @@ void MainWindow::updateMapCloud( if(_preferencesDialog->isGridMapIncremental()) { _occupancyGrid->update(poses, 0, _preferencesDialog->getGridMapFootprintRadius()); + if(stats) + { + stats->insert(std::make_pair("GUI/Grid Update/ms", (float)timer.restart()*1000.0f)); + } map8S = _occupancyGrid->getMap(xMin, yMin); } else @@ -2359,6 +2439,10 @@ void MainWindow::updateMapCloud( _ui->graphicsView_graphView->update(); UDEBUG(""); + if(stats) + { + stats->insert(std::make_pair("GUI/Grid Rendering/ms", (float)timer.restart()*1000.0f)); + } if(!_preferencesDialog->getGridMapShown()) { diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index b1cfd6db..cb7462c7 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -412,6 +412,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkBox_octomap_2dgrid, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_octomap_show3dMap, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_octomap_cubeRendering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->spinBox_octomap_pointSize, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->doubleSpinBox_octomap_occupancyThr, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->groupBox_organized, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_mesh_angleTolerance, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); @@ -1313,6 +1315,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkBox_octomap_2dgrid->setChecked(true); _ui->checkBox_octomap_show3dMap->setChecked(true); _ui->checkBox_octomap_cubeRendering->setChecked(true); + _ui->spinBox_octomap_pointSize->setValue(5); _ui->doubleSpinBox_octomap_occupancyThr->setValue(0.5); } else if(groupBox->objectName() == _ui->groupBox_logging1->objectName()) @@ -1692,6 +1695,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath) _ui->checkBox_octomap_show3dMap->setChecked(settings.value("octomap_3dmap", _ui->checkBox_octomap_show3dMap->isChecked()).toBool()); _ui->checkBox_octomap_cubeRendering->setChecked(settings.value("octomap_cube", _ui->checkBox_octomap_cubeRendering->isChecked()).toBool()); _ui->doubleSpinBox_octomap_occupancyThr->setValue(settings.value("octomap_occupancy_thr", _ui->doubleSpinBox_octomap_occupancyThr->value()).toDouble()); + _ui->spinBox_octomap_pointSize->setValue(settings.value("octomap_point_size", _ui->spinBox_octomap_pointSize->value()).toInt()); _ui->groupBox_organized->setChecked(settings.value("meshing", _ui->groupBox_organized->isChecked()).toBool()); _ui->doubleSpinBox_mesh_angleTolerance->setValue(settings.value("meshing_angle", _ui->doubleSpinBox_mesh_angleTolerance->value()).toDouble()); @@ -2077,6 +2081,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const settings.setValue("octomap_3dmap", _ui->checkBox_octomap_show3dMap->isChecked()); settings.setValue("octomap_cube", _ui->checkBox_octomap_cubeRendering->isChecked()); settings.setValue("octomap_occupancy_thr", _ui->doubleSpinBox_octomap_occupancyThr->value()); + settings.setValue("octomap_point_size", _ui->spinBox_octomap_pointSize->value()); settings.setValue("meshing", _ui->groupBox_organized->isChecked()); @@ -4067,6 +4072,10 @@ double PreferencesDialog::getOctomapOccupancyThr() const { return _ui->doubleSpinBox_octomap_occupancyThr->value(); } +int PreferencesDialog::getOctomapPointSize() const +{ + return _ui->spinBox_octomap_pointSize->value(); +} double PreferencesDialog::getVoxel() const { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 293c89cf..d703c936 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -86,7 +86,7 @@ QFrame::Raised - 15 + 3 @@ -1958,7 +1958,33 @@ Show a yellow background when the number of odometry inliers goes under this thr true - + + + + Occupancy threshold. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Cube rendering. Disable to show as a point cloud (a lot less GPU power required). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + Octomap maximum tree depth (max 16). The highest depth means the smallest resolution of the map (cell size). At smallest resolution the octomap shows RGB colors. Other resolutions produce z-axis gradient colored octomap. @@ -1971,7 +1997,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + 1 @@ -1984,7 +2010,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + @@ -1994,7 +2020,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + Show 2D occupancy grid map from OctoMap projection. @@ -2030,42 +2056,19 @@ Show a yellow background when the number of odometry inliers goes under this thr - - - - Occupancy threshold. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + 1.000000000000000 + + 0.050000000000000 + 0.500000000000000 - - - - Cube rendering. Disable to show as a point cloud (a lot less GPU power required). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -2076,6 +2079,32 @@ Show a yellow background when the number of odometry inliers goes under this thr + + + + Point size. When cube rendering is disabled. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 99 + + + 5 + + +