diff --git a/corelib/include/rtabmap/core/util3d_mapping.h b/corelib/include/rtabmap/core/util3d_mapping.h index 8d256c1a..1d9cf9b0 100644 --- a/corelib/include/rtabmap/core/util3d_mapping.h +++ b/corelib/include/rtabmap/core/util3d_mapping.h @@ -43,8 +43,17 @@ namespace rtabmap namespace util3d { +RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan( + const cv::Mat & scan, // in /base_link frame + cv::Mat & ground, + cv::Mat & obstacles, + float cellSize, + 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( - const cv::Mat & scan, + const cv::Mat & scan, // in /base_link frame + const cv::Point3f & viewpoint, // /base_link -> /base_scan cv::Mat & ground, cv::Mat & obstacles, float cellSize, @@ -61,8 +70,18 @@ cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps( bool erode = false, float footprintRadius = 0.0f); +RTABMAP_DEPRECATED(cv::Mat RTABMAP_EXP create2DMap(const std::map & poses, + const std::map::Ptr > & scans, // in /base_link frame + float cellSize, + bool unknownSpaceFilled, + float & xMin, + float & yMin, + float minMapSize = 0.0f, + float scanMaxRange = 0.0f), "Use interface with \"viewpoints\" parameter to make sure the ray tracing origin is from the sensor and not the base."); + cv::Mat RTABMAP_EXP create2DMap(const std::map & poses, - const std::map::Ptr > & scans, + const std::map::Ptr > & scans, // in /base_link frame + const std::map & viewpoints, // /base_link -> /base_scan float cellSize, bool unknownSpaceFilled, float & xMin, diff --git a/corelib/src/OccupancyGrid.cpp b/corelib/src/OccupancyGrid.cpp index e2e90ce9..99956fcf 100644 --- a/corelib/src/OccupancyGrid.cpp +++ b/corelib/src/OccupancyGrid.cpp @@ -201,8 +201,14 @@ void OccupancyGrid::createLocalMap( { UDEBUG("2D laser scan"); //2D + viewPoint = cv::Point3f( + node.sensorData().laserScanInfo().localTransform().x(), + node.sensorData().laserScanInfo().localTransform().y(), + node.sensorData().laserScanInfo().localTransform().z()); + util3d::occupancy2DFromLaserScan( util3d::transformLaserScan(node.sensorData().laserScanRaw(), node.sensorData().laserScanInfo().localTransform()), + viewPoint, ground, obstacles, cellSize_, @@ -331,6 +337,7 @@ void OccupancyGrid::createLocalMap( obstacles = cv::Mat(); util3d::occupancy2DFromLaserScan( laserScan, + viewPoint, ground, obstacles, cellSize_, diff --git a/corelib/src/util3d_mapping.cpp b/corelib/src/util3d_mapping.cpp index a675c76d..e7ff50f7 100644 --- a/corelib/src/util3d_mapping.cpp +++ b/corelib/src/util3d_mapping.cpp @@ -45,6 +45,7 @@ namespace rtabmap namespace util3d { + void occupancy2DFromLaserScan( const cv::Mat & scan, cv::Mat & ground, @@ -52,6 +53,26 @@ void occupancy2DFromLaserScan( float cellSize, bool unknownSpaceFilled, float scanMaxRange) +{ + cv::Point3f viewpoint(0,0,0); + occupancy2DFromLaserScan( + scan, + viewpoint, + ground, + obstacles, + cellSize, + unknownSpaceFilled, + scanMaxRange); +} + +void occupancy2DFromLaserScan( + const cv::Mat & scan, + const cv::Point3f & viewpoint, + cv::Mat & ground, + cv::Mat & obstacles, + float cellSize, + bool unknownSpaceFilled, + float scanMaxRange) { if(scan.empty()) { @@ -67,8 +88,11 @@ void occupancy2DFromLaserScan( std::map::Ptr> scans; scans.insert(std::make_pair(1, obstaclesCloud)); + std::map viewpoints; + viewpoints.insert(std::make_pair(1, viewpoint)); + float xMin, yMin; - cv::Mat map8S = create2DMap(poses, scans, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange); + 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) @@ -482,6 +506,43 @@ cv::Mat create2DMap(const std::map & poses, float & yMin, float minMapSize, float scanMaxRange) +{ + std::map viewpoints; + return create2DMap(poses, + scans, + viewpoints, + cellSize, + unknownSpaceFilled, + xMin, + yMin, + minMapSize, + scanMaxRange); +} + +/** + * Create 2d Occupancy grid (CV_8S) + * -1 = unknown + * 0 = empty space + * 100 = obstacle + * @param poses + * @param scans + * @param viewpoints + * @param cellSize m + * @param unknownSpaceFilled if false no fill, otherwise a virtual laser sweeps the unknown space from each pose (stopping on detected obstacle) + * @param xMin + * @param yMin + * @param minMapSize minimum map size in meters + * @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 & viewpoints, + float cellSize, + bool unknownSpaceFilled, + float & xMin, + float & yMin, + float minMapSize, + float scanMaxRange) { UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange); std::map::Ptr > localScans; @@ -494,15 +555,23 @@ cv::Mat create2DMap(const std::map & poses, } for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { - if(uContains(scans, iter->first) && scans.at(iter->first)->size()) + std::map::Ptr >::const_iterator jter=scans.find(iter->first); + if(jter!=scans.end() && jter->second->size()) { UASSERT(!iter->second.isNull()); - pcl::PointCloud::Ptr cloud = util3d::transformPointCloud(scans.at(iter->first), iter->second); + pcl::PointCloud::Ptr cloud = util3d::transformPointCloud(jter->second, iter->second); pcl::PointXYZ min, max; pcl::getMinMax3D(*cloud, 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); + if(kter!=viewpoints.end()) + { + 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)); } } @@ -531,7 +600,13 @@ cv::Mat create2DMap(const std::map & poses, for(std::map::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter) { const Transform & pose = poses.at(iter->first); - cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f); + cv::Point3f viewpoint(0,0,0); + std::map::const_iterator kter=viewpoints.find(iter->first); + if(kter!=viewpoints.end()) + { + viewpoint = kter->second; + } + cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize + 0.5f, ((pose.y()+viewpoint.y)-yMin)/cellSize + 0.5f); for(unsigned int i=0; isecond->size(); ++i) { cv::Point2i end((iter->second->points[i].x-xMin)/cellSize, (iter->second->points[i].y-yMin)/cellSize); @@ -557,7 +632,13 @@ cv::Mat create2DMap(const std::map & poses, if(scanMaxRange > cellSize) { const Transform & pose = poses.at(iter->first); - cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f); + cv::Point3f viewpoint(0,0,0); + std::map::const_iterator kter=viewpoints.find(iter->first); + if(kter!=viewpoints.end()) + { + viewpoint = kter->second; + } + cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize + 0.5f, ((pose.y()+viewpoint.y)-yMin)/cellSize + 0.5f); //UWARN("maxLength = %f", maxLength); //rotate counterclockwise from the first point until we pass the last point @@ -565,8 +646,8 @@ cv::Mat create2DMap(const std::map & poses, cv::Mat rotation = (cv::Mat_(2,2) << cos(a), -sin(a), sin(a), cos(a)); cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F); - origin.at(0) = pose.x(); - origin.at(1) = pose.y(); + 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) diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index fb776e9c..04242885 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -299,6 +299,7 @@ private slots: void updateKpROI(); void updateStereoDisparityVisibility(); void useOdomFeatures(); + void useGridProjRayTracing(); void changeWorkingDirectory(); void changeDictionaryPath(); void readSettingsEnd(); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index df764463..cf875032 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -832,6 +832,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->doubleSpinBox_grid_minDepth->setObjectName(Parameters::kGridDepthMin().c_str()); _ui->lineEdit_grid_roi->setObjectName(Parameters::kGridDepthRoiRatios().c_str()); _ui->checkBox_grid_projRayTracing->setObjectName(Parameters::kGridProjRayTracing().c_str()); + connect(_ui->checkBox_grid_projRayTracing, SIGNAL(stateChanged(int)), this, SLOT(useGridProjRayTracing())); _ui->doubleSpinBox_grid_footprintLength->setObjectName(Parameters::kGridFootprintLength().c_str()); _ui->doubleSpinBox_grid_footprintWidth->setObjectName(Parameters::kGridFootprintWidth().c_str()); _ui->doubleSpinBox_grid_footprintHeight->setObjectName(Parameters::kGridFootprintHeight().c_str()); @@ -3750,6 +3751,22 @@ void PreferencesDialog::useOdomFeatures() } } + +void PreferencesDialog::useGridProjRayTracing() +{ + if(this->isVisible() && _ui->checkBox_grid_projRayTracing->isChecked() && _ui->groupBox_grid_3d->isChecked()) + { + int r = QMessageBox::question(this, tr("Using ray tracing for 2D projection..."), + tr("Currently the 3D occupancy grid parameter is checked, but 2D ray tracing " + "only works with 2D occupancy grids. Do you want to uncheck 3D occupancy grid?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes); + + if(r == QMessageBox::Yes) + { + _ui->groupBox_grid_3d->setChecked(false); + } + } +} + void PreferencesDialog::changeWorkingDirectory() { QString directory = QFileDialog::getExistingDirectory(this, tr("Working directory"), _ui->lineEdit_workingDirectory->text());