00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <rtabmap/core/OccupancyGrid.h>
00029 #include <rtabmap/core/util3d.h>
00030 #include <rtabmap/utilite/ULogger.h>
00031 #include <rtabmap/utilite/UConversion.h>
00032 #include <rtabmap/utilite/UStl.h>
00033 #include <rtabmap/utilite/UTimer.h>
00034
00035 #ifdef RTABMAP_OCTOMAP
00036 #include <rtabmap/core/OctoMap.h>
00037 #endif
00038
00039 #include <pcl/io/pcd_io.h>
00040
00041 namespace rtabmap {
00042
00043 OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
00044 parameters_(parameters),
00045 cloudDecimation_(Parameters::defaultGridDepthDecimation()),
00046 cloudMaxDepth_(Parameters::defaultGridRangeMax()),
00047 cloudMinDepth_(Parameters::defaultGridRangeMin()),
00048
00049 footprintLength_(Parameters::defaultGridFootprintLength()),
00050 footprintWidth_(Parameters::defaultGridFootprintWidth()),
00051 footprintHeight_(Parameters::defaultGridFootprintHeight()),
00052 scanDecimation_(Parameters::defaultGridScanDecimation()),
00053 cellSize_(Parameters::defaultGridCellSize()),
00054 preVoxelFiltering_(Parameters::defaultGridPreVoxelFiltering()),
00055 occupancyFromDepth_(Parameters::defaultGridFromDepth()),
00056 projMapFrame_(Parameters::defaultGridMapFrameProjection()),
00057 maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
00058 normalKSearch_(Parameters::defaultGridNormalK()),
00059 maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f),
00060 clusterRadius_(Parameters::defaultGridClusterRadius()),
00061 minClusterSize_(Parameters::defaultGridMinClusterSize()),
00062 flatObstaclesDetected_(Parameters::defaultGridFlatObstacleDetected()),
00063 minGroundHeight_(Parameters::defaultGridMinGroundHeight()),
00064 maxGroundHeight_(Parameters::defaultGridMaxGroundHeight()),
00065 normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()),
00066 grid3D_(Parameters::defaultGrid3D()),
00067 groundIsObstacle_(Parameters::defaultGridGroundIsObstacle()),
00068 noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
00069 noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
00070 scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
00071 rayTracing_(Parameters::defaultGridRayTracing()),
00072 fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
00073 minMapSize_(Parameters::defaultGridGlobalMinSize()),
00074 erode_(Parameters::defaultGridGlobalEroded()),
00075 footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()),
00076 updateError_(Parameters::defaultGridGlobalUpdateError()),
00077 occupancyThr_(Parameters::defaultGridGlobalOccupancyThr()),
00078 probHit_(logodds(Parameters::defaultGridGlobalProbHit())),
00079 probMiss_(logodds(Parameters::defaultGridGlobalProbMiss())),
00080 probClampingMin_(logodds(Parameters::defaultGridGlobalProbClampingMin())),
00081 probClampingMax_(logodds(Parameters::defaultGridGlobalProbClampingMax())),
00082 xMin_(0.0f),
00083 yMin_(0.0f),
00084 cloudAssembling_(false),
00085 assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
00086 assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
00087 assembledEmptyCells_(new pcl::PointCloud<pcl::PointXYZRGB>)
00088 {
00089 this->parseParameters(parameters);
00090 }
00091
00092 void OccupancyGrid::parseParameters(const ParametersMap & parameters)
00093 {
00094 Parameters::parse(parameters, Parameters::kGridFromDepth(), occupancyFromDepth_);
00095 Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
00096 if(cloudDecimation_ == 0)
00097 {
00098 cloudDecimation_ = 1;
00099 }
00100 Parameters::parse(parameters, Parameters::kGridRangeMin(), cloudMinDepth_);
00101 Parameters::parse(parameters, Parameters::kGridRangeMax(), cloudMaxDepth_);
00102 Parameters::parse(parameters, Parameters::kGridFootprintLength(), footprintLength_);
00103 Parameters::parse(parameters, Parameters::kGridFootprintWidth(), footprintWidth_);
00104 Parameters::parse(parameters, Parameters::kGridFootprintHeight(), footprintHeight_);
00105 Parameters::parse(parameters, Parameters::kGridScanDecimation(), scanDecimation_);
00106 float cellSize = cellSize_;
00107 if(Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize))
00108 {
00109 this->setCellSize(cellSize);
00110 }
00111
00112 Parameters::parse(parameters, Parameters::kGridPreVoxelFiltering(), preVoxelFiltering_);
00113 Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_);
00114 Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
00115 Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
00116 Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
00117 Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
00118 if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
00119 {
00120 maxGroundAngle_ *= M_PI/180.0f;
00121 }
00122 Parameters::parse(parameters, Parameters::kGridClusterRadius(), clusterRadius_);
00123 UASSERT_MSG(clusterRadius_ > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridClusterRadius().c_str()).c_str());
00124 Parameters::parse(parameters, Parameters::kGridMinClusterSize(), minClusterSize_);
00125 Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_);
00126 Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_);
00127 Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
00128 Parameters::parse(parameters, Parameters::kGridGroundIsObstacle(), groundIsObstacle_);
00129 Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
00130 Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
00131 Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
00132 Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
00133 Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
00134 Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
00135 Parameters::parse(parameters, Parameters::kGridGlobalEroded(), erode_);
00136 Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_);
00137 Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
00138
00139 Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr_);
00140 if(Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit_))
00141 {
00142 probHit_ = logodds(probHit_);
00143 UASSERT_MSG(probHit_ >= 0.0f, uFormat("probHit_=%f",probHit_).c_str());
00144 }
00145 if(Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss_))
00146 {
00147 probMiss_ = logodds(probMiss_);
00148 UASSERT_MSG(probMiss_ <= 0.0f, uFormat("probMiss_=%f",probMiss_).c_str());
00149 }
00150 if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), probClampingMin_))
00151 {
00152 probClampingMin_ = logodds(probClampingMin_);
00153 }
00154 if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), probClampingMax_))
00155 {
00156 probClampingMax_ = logodds(probClampingMax_);
00157 }
00158 UASSERT(probClampingMax_ > probClampingMin_);
00159
00160 UASSERT(minMapSize_ >= 0.0f);
00161
00162
00163 ParametersMap::const_iterator iter;
00164 if((iter=parameters.find(Parameters::kGridDepthRoiRatios())) != parameters.end())
00165 {
00166 std::list<std::string> strValues = uSplit(iter->second, ' ');
00167 if(strValues.size() != 4)
00168 {
00169 ULOGGER_ERROR("The number of values must be 4 (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
00170 }
00171 else
00172 {
00173 std::vector<float> tmpValues(4);
00174 unsigned int i=0;
00175 for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
00176 {
00177 tmpValues[i] = uStr2Float(*jter);
00178 ++i;
00179 }
00180
00181 if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
00182 tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
00183 tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
00184 tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
00185 {
00186 roiRatios_ = tmpValues;
00187 }
00188 else
00189 {
00190 ULOGGER_ERROR("The roi ratios are not valid (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
00191 }
00192 }
00193 }
00194
00195 if(maxGroundHeight_ == 0.0f && !normalsSegmentation_)
00196 {
00197 UWARN("\"%s\" should be not equal to 0 if not using normals "
00198 "segmentation approach. Setting it to cell size (%f).",
00199 Parameters::kGridMaxGroundHeight().c_str(), cellSize_);
00200 maxGroundHeight_ = cellSize_;
00201 }
00202 if(maxGroundHeight_ != 0.0f &&
00203 maxObstacleHeight_ != 0.0f &&
00204 maxObstacleHeight_ < maxGroundHeight_)
00205 {
00206 UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
00207 Parameters::kGridMaxGroundHeight().c_str(),
00208 Parameters::kGridMaxObstacleHeight().c_str(),
00209 Parameters::kGridMaxObstacleHeight().c_str());
00210 maxObstacleHeight_ = 0;
00211 }
00212 if(maxGroundHeight_ != 0.0f &&
00213 minGroundHeight_ != 0.0f &&
00214 maxGroundHeight_ < minGroundHeight_)
00215 {
00216 UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
00217 Parameters::kGridMinGroundHeight().c_str(),
00218 Parameters::kGridMaxGroundHeight().c_str(),
00219 Parameters::kGridMinGroundHeight().c_str());
00220 minGroundHeight_ = 0;
00221 }
00222 }
00223
00224 void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses)
00225 {
00226 UDEBUG("map=%d/%d xMin=%f yMin=%f cellSize=%f poses=%d",
00227 map.cols, map.rows, xMin, yMin, cellSize, (int)poses.size());
00228 this->clear();
00229 if(!poses.empty() && !map.empty())
00230 {
00231 UASSERT(cellSize > 0.0f);
00232 UASSERT(map.type() == CV_8SC1);
00233 map_ = map.clone();
00234 mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC4);
00235 for(int i=0; i<map_.rows; ++i)
00236 {
00237 for(int j=0; j<map_.cols; ++j)
00238 {
00239 const char value = map_.at<char>(i,j);
00240 float * info = mapInfo_.ptr<float>(i,j);
00241 if(value == 0)
00242 {
00243 info[3] = probClampingMin_;
00244 }
00245 else if(value == 100)
00246 {
00247 info[3] = probClampingMax_;
00248 }
00249 }
00250 }
00251 xMin_ = xMin;
00252 yMin_ = yMin;
00253 cellSize_ = cellSize;
00254 addedNodes_ = poses;
00255 }
00256 }
00257
00258 void OccupancyGrid::setCellSize(float cellSize)
00259 {
00260 UASSERT_MSG(cellSize > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridCellSize().c_str()).c_str());
00261 if(cellSize_ != cellSize)
00262 {
00263 if(!map_.empty())
00264 {
00265 UWARN("Grid cell size has changed, the map is cleared!");
00266 }
00267 this->clear();
00268 cellSize_ = cellSize;
00269 }
00270 }
00271
00272 void OccupancyGrid::setCloudAssembling(bool enabled)
00273 {
00274 cloudAssembling_ = enabled;
00275 if(!cloudAssembling_)
00276 {
00277 assembledGround_->clear();
00278 assembledObstacles_->clear();
00279 }
00280 }
00281
00282 void OccupancyGrid::createLocalMap(
00283 const Signature & node,
00284 cv::Mat & groundCells,
00285 cv::Mat & obstacleCells,
00286 cv::Mat & emptyCells,
00287 cv::Point3f & viewPoint) const
00288 {
00289 UDEBUG("scan format=%d, occupancyFromDepth_=%d normalsSegmentation_=%d grid3D_=%d",
00290 node.sensorData().laserScanRaw().isEmpty()?0:node.sensorData().laserScanRaw().format(), occupancyFromDepth_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
00291
00292 if((node.sensorData().laserScanRaw().is2d()) && !occupancyFromDepth_)
00293 {
00294 UDEBUG("2D laser scan");
00295
00296 viewPoint = cv::Point3f(
00297 node.sensorData().laserScanRaw().localTransform().x(),
00298 node.sensorData().laserScanRaw().localTransform().y(),
00299 node.sensorData().laserScanRaw().localTransform().z());
00300
00301 LaserScan scan = node.sensorData().laserScanRaw();
00302 if(cloudMinDepth_ > 0.0f)
00303 {
00304 scan = util3d::rangeFiltering(scan, cloudMinDepth_, 0.0f);
00305 }
00306
00307 float maxRange = cloudMaxDepth_;
00308 if(cloudMaxDepth_>0.0f && node.sensorData().laserScanRaw().maxRange()>0.0f)
00309 {
00310 maxRange = cloudMaxDepth_ < node.sensorData().laserScanRaw().maxRange()?cloudMaxDepth_:node.sensorData().laserScanRaw().maxRange();
00311 }
00312 else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().maxRange()>0.0f)
00313 {
00314 maxRange = node.sensorData().laserScanRaw().maxRange();
00315 }
00316 util3d::occupancy2DFromLaserScan(
00317 util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(),
00318 cv::Mat(),
00319 viewPoint,
00320 emptyCells,
00321 obstacleCells,
00322 cellSize_,
00323 scan2dUnknownSpaceFilled_,
00324 maxRange);
00325
00326 UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
00327 }
00328 else
00329 {
00330
00331 if(!occupancyFromDepth_)
00332 {
00333 if(!node.sensorData().laserScanRaw().isEmpty())
00334 {
00335 UDEBUG("3D laser scan");
00336 const Transform & t = node.sensorData().laserScanRaw().localTransform();
00337 LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
00338 #ifdef RTABMAP_OCTOMAP
00339
00340 float maxRange = grid3D_&&rayTracing_?0.0f:cloudMaxDepth_;
00341 #else
00342 float maxRange = cloudMaxDepth_;
00343 #endif
00344 if(cloudMinDepth_ > 0.0f || maxRange > 0.0f)
00345 {
00346 scan = util3d::rangeFiltering(scan, cloudMinDepth_, maxRange);
00347 }
00348
00349
00350 viewPoint = cv::Point3f(t.x(), t.y(), t.z());
00351
00352 UDEBUG("scan format=%d", scan.format());
00353 createLocalMap(scan, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
00354 }
00355 else
00356 {
00357 UWARN("Cannot create local map, scan is empty (node=%d).", node.id());
00358 }
00359 }
00360 else
00361 {
00362 pcl::IndicesPtr indices(new std::vector<int>);
00363 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
00364 UDEBUG("Depth image : decimation=%d max=%f min=%f",
00365 cloudDecimation_,
00366 cloudMaxDepth_,
00367 cloudMinDepth_);
00368 cloud = util3d::cloudRGBFromSensorData(
00369 node.sensorData(),
00370 cloudDecimation_,
00371 #ifdef RTABMAP_OCTOMAP
00372
00373 grid3D_&&rayTracing_?0.0f:cloudMaxDepth_,
00374 #else
00375 cloudMaxDepth_,
00376 #endif
00377 cloudMinDepth_,
00378 indices.get(),
00379 parameters_,
00380 roiRatios_);
00381
00382
00383 if(node.sensorData().cameraModels().size())
00384 {
00385
00386 float sum = 0;
00387 for(unsigned int i=0; i<node.sensorData().cameraModels().size(); ++i)
00388 {
00389 const Transform & t = node.sensorData().cameraModels()[i].localTransform();
00390 if(!t.isNull())
00391 {
00392 viewPoint.x += t.x();
00393 viewPoint.y += t.y();
00394 viewPoint.z += t.z();
00395 sum += 1.0f;
00396 }
00397 }
00398 if(sum > 0.0f)
00399 {
00400 viewPoint.x /= sum;
00401 viewPoint.y /= sum;
00402 viewPoint.z /= sum;
00403 }
00404 }
00405 else
00406 {
00407 const Transform & t = node.sensorData().stereoCameraModel().localTransform();
00408 viewPoint = cv::Point3f(t.x(), t.y(), t.z());
00409 }
00410 createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f, LaserScan::kXYZRGB), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
00411 }
00412 }
00413 }
00414
00415 void OccupancyGrid::createLocalMap(
00416 const LaserScan & scan,
00417 const Transform & pose,
00418 cv::Mat & groundCells,
00419 cv::Mat & obstacleCells,
00420 cv::Mat & emptyCells,
00421 cv::Point3f & viewPointInOut) const
00422 {
00423 if(projMapFrame_)
00424 {
00425
00426 float roll, pitch, yaw;
00427 pose.getEulerAngles(roll, pitch, yaw);
00428 Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z, 0,0,0);
00429 viewPointInOut.x = viewpointRotated.x();
00430 viewPointInOut.y = viewpointRotated.y();
00431 viewPointInOut.z = viewpointRotated.z();
00432 }
00433
00434 if(scan.size())
00435 {
00436 pcl::IndicesPtr groundIndices(new std::vector<int>);
00437 pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
00438 cv::Mat groundCloud;
00439 cv::Mat obstaclesCloud;
00440
00441 if(scan.hasRGB() && scan.hasNormals())
00442 {
00443 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform());
00444 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGBNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
00445 UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
00446 if(grid3D_)
00447 {
00448 groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
00449 obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
00450 }
00451 else
00452 {
00453 util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGBNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
00454 }
00455 }
00456 else if(scan.hasRGB())
00457 {
00458 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
00459 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGB>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
00460 UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
00461 if(grid3D_)
00462 {
00463 groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
00464 obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
00465 }
00466 else
00467 {
00468 util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
00469 }
00470 }
00471 else if(scan.hasNormals())
00472 {
00473 pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(scan, scan.localTransform());
00474 pcl::PointCloud<pcl::PointNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
00475 UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
00476 if(grid3D_)
00477 {
00478 groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
00479 obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
00480 }
00481 else
00482 {
00483 util3d::occupancy2DFromGroundObstacles<pcl::PointNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
00484 }
00485 }
00486 else
00487 {
00488 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
00489 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZ>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
00490 UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
00491 if(grid3D_)
00492 {
00493 groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
00494 obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
00495 }
00496 else
00497 {
00498 util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
00499 }
00500 }
00501
00502 if(grid3D_ && (!obstaclesCloud.empty() || !groundCloud.empty()))
00503 {
00504 UDEBUG("ground=%d obstacles=%d", groundCloud.cols, obstaclesCloud.cols);
00505 if(groundIsObstacle_ && !groundCloud.empty())
00506 {
00507 if(obstaclesCloud.empty())
00508 {
00509 obstaclesCloud = groundCloud;
00510 groundCloud = cv::Mat();
00511 }
00512 else
00513 {
00514 UASSERT(obstaclesCloud.type() == groundCloud.type());
00515 cv::Mat merged(1,obstaclesCloud.cols+groundCloud.cols, obstaclesCloud.type());
00516 obstaclesCloud.copyTo(merged(cv::Range::all(), cv::Range(0, obstaclesCloud.cols)));
00517 groundCloud.copyTo(merged(cv::Range::all(), cv::Range(obstaclesCloud.cols, obstaclesCloud.cols+groundCloud.cols)));
00518 }
00519 }
00520
00521
00522 float roll, pitch, yaw;
00523 pose.getEulerAngles(roll, pitch, yaw);
00524 Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse();
00525
00526 if(rayTracing_)
00527 {
00528 #ifdef RTABMAP_OCTOMAP
00529 if(!groundCloud.empty() || !obstaclesCloud.empty())
00530 {
00531
00532 OctoMap octomap(cellSize_);
00533 octomap.setMaxRange(cloudMaxDepth_);
00534 octomap.addToCache(1, groundCloud, obstaclesCloud, cv::Mat(), cv::Point3f(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
00535 std::map<int, Transform> poses;
00536 poses.insert(std::make_pair(1, Transform::getIdentity()));
00537 octomap.update(poses);
00538
00539 pcl::IndicesPtr groundIndices(new std::vector<int>);
00540 pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
00541 pcl::IndicesPtr emptyIndices(new std::vector<int>);
00542 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get());
00543 UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
00544 if(scan.hasRGB())
00545 {
00546 groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv);
00547 obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv);
00548 emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv);
00549 }
00550 else
00551 {
00552 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithRayTracing2(new pcl::PointCloud<pcl::PointXYZ>);
00553 pcl::copyPointCloud(*cloudWithRayTracing, *cloudWithRayTracing2);
00554 groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, groundIndices, tinv);
00555 obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, obstaclesIndices, tinv);
00556 emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, emptyIndices, tinv);
00557 }
00558 }
00559 }
00560 else
00561 #else
00562 UWARN("RTAB-Map is not built with OctoMap dependency, 3D ray tracing is ignored. Set \"%s\" to false to avoid this warning.", Parameters::kGridRayTracing().c_str());
00563 }
00564 #endif
00565 {
00566 groundCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(groundCloud), tinv).data();
00567 obstacleCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(obstaclesCloud), tinv).data();
00568 }
00569
00570 }
00571 else if(!grid3D_ && rayTracing_ && (!obstacleCells.empty() || !groundCells.empty()))
00572 {
00573 cv::Mat laserScan = obstacleCells;
00574 cv::Mat laserScanNoHit = groundCells;
00575 obstacleCells = cv::Mat();
00576 groundCells = cv::Mat();
00577 util3d::occupancy2DFromLaserScan(
00578 laserScan,
00579 laserScanNoHit,
00580 viewPointInOut,
00581 emptyCells,
00582 obstacleCells,
00583 cellSize_,
00584 false,
00585 cloudMaxDepth_);
00586 }
00587 }
00588 UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
00589 }
00590
00591 void OccupancyGrid::clear()
00592 {
00593 cache_.clear();
00594 map_ = cv::Mat();
00595 mapInfo_ = cv::Mat();
00596 cellCount_.clear();
00597 xMin_ = 0.0f;
00598 yMin_ = 0.0f;
00599 addedNodes_.clear();
00600 assembledGround_->clear();
00601 assembledObstacles_->clear();
00602 }
00603
00604 cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
00605 {
00606 xMin = xMin_;
00607 yMin = yMin_;
00608
00609 cv::Mat map = map_;
00610
00611 UTimer t;
00612 if(occupancyThr_ != 0.0f && !map.empty())
00613 {
00614 float occThr = logodds(occupancyThr_);
00615 map = cv::Mat(map.size(), map.type());
00616 UASSERT(mapInfo_.cols == map.cols && mapInfo_.rows == map.rows);
00617 for(int i=0; i<map.rows; ++i)
00618 {
00619 for(int j=0; j<map.cols; ++j)
00620 {
00621 const float * info = mapInfo_.ptr<float>(i, j);
00622 if(info[3] == 0.0f)
00623 {
00624 map.at<char>(i, j) = -1;
00625 }
00626 else if(info[3] >= occThr)
00627 {
00628 map.at<char>(i, j) = 100;
00629 }
00630 else
00631 {
00632 map.at<char>(i, j) = 0;
00633 }
00634 }
00635 }
00636 UDEBUG("Converting map from probabilities (thr=%f) = %fs", occupancyThr_, t.ticks());
00637 }
00638
00639 if(erode_ && !map.empty())
00640 {
00641 map = util3d::erodeMap(map);
00642 UDEBUG("Eroding map = %fs", t.ticks());
00643 }
00644 return map;
00645 }
00646
00647 cv::Mat OccupancyGrid::getProbMap(float & xMin, float & yMin) const
00648 {
00649 xMin = xMin_;
00650 yMin = yMin_;
00651
00652 cv::Mat map;
00653 if(!mapInfo_.empty())
00654 {
00655 map = cv::Mat(mapInfo_.size(), map_.type());
00656 for(int i=0; i<map.rows; ++i)
00657 {
00658 for(int j=0; j<map.cols; ++j)
00659 {
00660 const float * info = mapInfo_.ptr<float>(i, j);
00661 if(info[3] == 0.0f)
00662 {
00663 map.at<char>(i, j) = -1;
00664 }
00665 else
00666 {
00667 map.at<char>(i, j) = char(probability(info[3])*100.0f);
00668 }
00669 }
00670 }
00671 }
00672 return map;
00673 }
00674
00675 void OccupancyGrid::addToCache(
00676 int nodeId,
00677 const cv::Mat & ground,
00678 const cv::Mat & obstacles,
00679 const cv::Mat & empty)
00680 {
00681 UDEBUG("nodeId=%d", nodeId);
00682 uInsert(cache_, std::make_pair(nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
00683 }
00684
00685 void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
00686 {
00687 UTimer timer;
00688 UDEBUG("Update (poses=%d addedNodes_=%d)", (int)posesIn.size(), (int)addedNodes_.size());
00689
00690 float margin = cellSize_*10.0f+(footprintRadius_>cellSize_*1.5f?float(int(footprintRadius_/cellSize_)+1):0.0f)*cellSize_;
00691
00692 float minX=-minMapSize_/2.0f;
00693 float minY=-minMapSize_/2.0f;
00694 float maxX=minMapSize_/2.0f;
00695 float maxY=minMapSize_/2.0f;
00696 bool undefinedSize = minMapSize_ == 0.0f;
00697 std::map<int, cv::Mat> emptyLocalMaps;
00698 std::map<int, cv::Mat> occupiedLocalMaps;
00699
00700
00701 bool graphOptimized = false;
00702 bool graphChanged = addedNodes_.size()>0;
00703 std::map<int, Transform> transforms;
00704 float updateErrorSqrd = updateError_*updateError_;
00705 for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
00706 {
00707 std::map<int, Transform>::const_iterator jter = posesIn.find(iter->first);
00708 if(jter != posesIn.end())
00709 {
00710 graphChanged = false;
00711
00712 UASSERT(!iter->second.isNull() && !jter->second.isNull());
00713 Transform t = Transform::getIdentity();
00714 if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
00715 {
00716 t = jter->second * iter->second.inverse();
00717 graphOptimized = true;
00718 }
00719 transforms.insert(std::make_pair(jter->first, t));
00720
00721 float x = jter->second.x();
00722 float y =jter->second.y();
00723 if(undefinedSize)
00724 {
00725 minX = maxX = x;
00726 minY = maxY = y;
00727 undefinedSize = false;
00728 }
00729 else
00730 {
00731 if(minX > x)
00732 minX = x;
00733 else if(maxX < x)
00734 maxX = x;
00735
00736 if(minY > y)
00737 minY = y;
00738 else if(maxY < y)
00739 maxY = y;
00740 }
00741 }
00742 else
00743 {
00744 UDEBUG("Updated pose for node %d is not found, some points may not be copied if graph has changed.", iter->first);
00745 }
00746 }
00747
00748 bool assembledGroundUpdated = false;
00749 bool assembledObstaclesUpdated = false;
00750 bool assembledEmptyCellsUpdated = false;
00751
00752 if(graphOptimized || graphChanged)
00753 {
00754 if(graphChanged)
00755 {
00756 UWARN("Graph has changed! The whole map should be rebuilt.");
00757 }
00758 else
00759 {
00760 UINFO("Graph optimized!");
00761 }
00762
00763 if(cloudAssembling_)
00764 {
00765 assembledGround_->clear();
00766 assembledObstacles_->clear();
00767 }
00768
00769 if(!fullUpdate_ && !graphChanged && !map_.empty())
00770 {
00771
00772 UASSERT(map_.cols == mapInfo_.cols &&
00773 map_.rows == mapInfo_.rows);
00774 std::map<int, std::pair<int, int> > tmpIndices;
00775 for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
00776 {
00777 if(!uContains(cache_, iter->first) && transforms.find(iter->first) != transforms.end())
00778 {
00779 if(iter->second.first)
00780 {
00781 emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
00782 }
00783 if(iter->second.second)
00784 {
00785 occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
00786 }
00787 tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
00788 }
00789 }
00790 for(int y=0; y<map_.rows; ++y)
00791 {
00792 for(int x=0; x<map_.cols; ++x)
00793 {
00794 float * info = mapInfo_.ptr<float>(y,x);
00795 int nodeId = (int)info[0];
00796 if(nodeId > 0 && map_.at<char>(y,x) >= 0)
00797 {
00798 if(tmpIndices.find(nodeId)!=tmpIndices.end())
00799 {
00800 std::map<int, Transform>::iterator tter = transforms.find(nodeId);
00801 UASSERT(tter != transforms.end());
00802
00803 cv::Point3f pt(info[1], info[2], 0.0f);
00804 pt = util3d::transformPoint(pt, tter->second);
00805
00806 if(minX > pt.x)
00807 minX = pt.x;
00808 else if(maxX < pt.x)
00809 maxX = pt.x;
00810
00811 if(minY > pt.y)
00812 minY = pt.y;
00813 else if(maxY < pt.y)
00814 maxY = pt.y;
00815
00816 std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
00817 if(map_.at<char>(y, x) == 0)
00818 {
00819
00820 std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
00821 UASSERT(iter != emptyLocalMaps.end());
00822 UASSERT(jter->second.first < iter->second.cols);
00823 float * ptf = iter->second.ptr<float>(0,jter->second.first++);
00824 ptf[0] = pt.x;
00825 ptf[1] = pt.y;
00826 }
00827 else
00828 {
00829
00830 std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
00831 UASSERT(iter != occupiedLocalMaps.end());
00832 UASSERT(iter!=occupiedLocalMaps.end());
00833 UASSERT(jter->second.second < iter->second.cols);
00834 float * ptf = iter->second.ptr<float>(0,jter->second.second++);
00835 ptf[0] = pt.x;
00836 ptf[1] = pt.y;
00837 }
00838 }
00839 }
00840 else if(nodeId > 0)
00841 {
00842 UERROR("Cell referred b node %d is unknown!?", nodeId);
00843 }
00844 }
00845 }
00846
00847
00848 for(std::map<int, std::pair<int, int> >::iterator iter=tmpIndices.begin(); iter!=tmpIndices.end(); ++iter)
00849 {
00850 std::map<int, cv::Mat>::iterator jter = emptyLocalMaps.find(iter->first);
00851 UASSERT_MSG((iter->second.first == 0 && (jter==emptyLocalMaps.end() || jter->second.empty())) ||
00852 (iter->second.first != 0 && jter!=emptyLocalMaps.end() && jter->second.cols == iter->second.first),
00853 uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str());
00854 jter = occupiedLocalMaps.find(iter->first);
00855 UASSERT_MSG((iter->second.second == 0 && (jter==occupiedLocalMaps.end() || jter->second.empty())) ||
00856 (iter->second.second != 0 && jter!=occupiedLocalMaps.end() && jter->second.cols == iter->second.second),
00857 uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str());
00858 }
00859
00860 UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
00861 }
00862
00863 addedNodes_.clear();
00864 map_ = cv::Mat();
00865 mapInfo_ = cv::Mat();
00866 cellCount_.clear();
00867 xMin_ = 0.0f;
00868 yMin_ = 0.0f;
00869 }
00870 else if(!map_.empty())
00871 {
00872
00873 minX=xMin_+margin+cellSize_/2.0f;
00874 minY=yMin_+margin+cellSize_/2.0f;
00875 maxX=xMin_+float(map_.cols)*cellSize_ - margin;
00876 maxY=yMin_+float(map_.rows)*cellSize_ - margin;
00877 undefinedSize = false;
00878 }
00879
00880 bool incrementalGraphUpdate = graphOptimized && !fullUpdate_ && !graphChanged;
00881
00882 std::list<std::pair<int, Transform> > poses;
00883 int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
00884 UDEBUG("Last id = %d", lastId);
00885
00886
00887 for(std::map<int, Transform>::const_iterator iter=posesIn.upper_bound(0); iter!=posesIn.end(); ++iter)
00888 {
00889 if(addedNodes_.find(iter->first) == addedNodes_.end())
00890 {
00891 UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first);
00892 poses.push_back(*iter);
00893 }
00894 }
00895
00896
00897 for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
00898 {
00899 if(iter->first < 0)
00900 {
00901 poses.push_back(*iter);
00902 }
00903 else
00904 {
00905 break;
00906 }
00907 }
00908
00909 for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00910 {
00911 UASSERT(!iter->second.isNull());
00912
00913 float x = iter->second.x();
00914 float y =iter->second.y();
00915 if(undefinedSize)
00916 {
00917 minX = maxX = x;
00918 minY = maxY = y;
00919 undefinedSize = false;
00920 }
00921 else
00922 {
00923 if(minX > x)
00924 minX = x;
00925 else if(maxX < x)
00926 maxX = x;
00927
00928 if(minY > y)
00929 minY = y;
00930 else if(maxY < y)
00931 maxY = y;
00932 }
00933 }
00934
00935 if(!cache_.empty())
00936 {
00937 for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00938 {
00939 if(uContains(cache_, iter->first))
00940 {
00941 const std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> & pair = cache_.at(iter->first);
00942
00943 UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, pair.first.first.cols, pair.first.second.cols, pair.second.cols);
00944
00945
00946 if(pair.first.first.cols)
00947 {
00948 if(pair.first.first.rows > 1 && pair.first.first.cols == 1)
00949 {
00950 UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.first.rows, pair.first.first.cols);
00951 }
00952 cv::Mat ground(1, pair.first.first.cols, CV_32FC2);
00953 for(int i=0; i<ground.cols; ++i)
00954 {
00955 const float * vi = pair.first.first.ptr<float>(0,i);
00956 float * vo = ground.ptr<float>(0,i);
00957 cv::Point3f vt;
00958 if(pair.first.first.channels() != 2 && pair.first.first.channels() != 5)
00959 {
00960 vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
00961 }
00962 else
00963 {
00964 vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
00965 }
00966 vo[0] = vt.x;
00967 vo[1] = vt.y;
00968 if(minX > vo[0])
00969 minX = vo[0];
00970 else if(maxX < vo[0])
00971 maxX = vo[0];
00972
00973 if(minY > vo[1])
00974 minY = vo[1];
00975 else if(maxY < vo[1])
00976 maxY = vo[1];
00977 }
00978 uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
00979
00980 if(cloudAssembling_)
00981 {
00982 *assembledGround_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.first.first), iter->second, 0, 255, 0);
00983 assembledGroundUpdated = true;
00984 }
00985 }
00986
00987
00988 if(pair.second.cols)
00989 {
00990 if(pair.second.rows > 1 && pair.second.cols == 1)
00991 {
00992 UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
00993 }
00994 cv::Mat ground(1, pair.second.cols, CV_32FC2);
00995 for(int i=0; i<ground.cols; ++i)
00996 {
00997 const float * vi = pair.second.ptr<float>(0,i);
00998 float * vo = ground.ptr<float>(0,i);
00999 cv::Point3f vt;
01000 if(pair.second.channels() != 2 && pair.second.channels() != 5)
01001 {
01002 vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
01003 }
01004 else
01005 {
01006 vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
01007 }
01008 vo[0] = vt.x;
01009 vo[1] = vt.y;
01010 if(minX > vo[0])
01011 minX = vo[0];
01012 else if(maxX < vo[0])
01013 maxX = vo[0];
01014
01015 if(minY > vo[1])
01016 minY = vo[1];
01017 else if(maxY < vo[1])
01018 maxY = vo[1];
01019 }
01020 uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
01021
01022 if(cloudAssembling_)
01023 {
01024 *assembledEmptyCells_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.second), iter->second, 0, 255, 0);
01025 assembledEmptyCellsUpdated = true;
01026 }
01027 }
01028
01029
01030 if(pair.first.second.cols)
01031 {
01032 if(pair.first.second.rows > 1 && pair.first.second.cols == 1)
01033 {
01034 UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.second.rows, pair.first.second.cols);
01035 }
01036 cv::Mat obstacles(1, pair.first.second.cols, CV_32FC2);
01037 for(int i=0; i<obstacles.cols; ++i)
01038 {
01039 const float * vi = pair.first.second.ptr<float>(0,i);
01040 float * vo = obstacles.ptr<float>(0,i);
01041 cv::Point3f vt;
01042 if(pair.first.second.channels() != 2 && pair.first.second.channels() != 5)
01043 {
01044 vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
01045 }
01046 else
01047 {
01048 vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
01049 }
01050 vo[0] = vt.x;
01051 vo[1] = vt.y;
01052 if(minX > vo[0])
01053 minX = vo[0];
01054 else if(maxX < vo[0])
01055 maxX = vo[0];
01056
01057 if(minY > vo[1])
01058 minY = vo[1];
01059 else if(maxY < vo[1])
01060 maxY = vo[1];
01061 }
01062 uInsert(occupiedLocalMaps, std::make_pair(iter->first, obstacles));
01063
01064 if(cloudAssembling_)
01065 {
01066 *assembledObstacles_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.first.second), iter->second, 255, 0, 0);
01067 assembledObstaclesUpdated = true;
01068 }
01069 }
01070 }
01071 }
01072 }
01073
01074 cv::Mat map;
01075 cv::Mat mapInfo;
01076 if(minX != maxX && minY != maxY)
01077 {
01078
01079 float xMin = minX-margin;
01080 xMin -= cellSize_/2.0f;
01081 float yMin = minY-margin;
01082 yMin -= cellSize_/2.0f;
01083 float xMax = maxX+margin;
01084 float yMax = maxY+margin;
01085
01086 if(fabs((yMax - yMin) / cellSize_) > 99999 ||
01087 fabs((xMax - xMin) / cellSize_) > 99999)
01088 {
01089 UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
01090 "There's maybe an error with the poses provided! The map will not be created!",
01091 xMin, yMin, xMax, yMax);
01092 }
01093 else
01094 {
01095 UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, xMin_, yMin_, xMax, yMax);
01096 cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
01097 if(map_.empty())
01098 {
01099 UDEBUG("Map empty!");
01100 map = cv::Mat::ones(newMapSize, CV_8S)*-1;
01101 mapInfo = cv::Mat::zeros(newMapSize, CV_32FC4);
01102 }
01103 else
01104 {
01105 if(xMin == xMin_ && yMin == yMin_ &&
01106 newMapSize.width == map_.cols &&
01107 newMapSize.height == map_.rows)
01108 {
01109
01110 UDEBUG("Map same size!");
01111 map = map_;
01112 mapInfo = mapInfo_;
01113 }
01114 else
01115 {
01116 UASSERT_MSG(xMin <= xMin_+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, xMin_, cellSize_).c_str());
01117 UASSERT_MSG(yMin <= yMin_+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, yMin_, cellSize_).c_str());
01118 UASSERT_MSG(xMax >= xMin_+float(map_.cols)*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, xMin_, map_.cols, cellSize_).c_str());
01119 UASSERT_MSG(yMax >= yMin_+float(map_.rows)*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, yMin_, map_.rows, cellSize_).c_str());
01120
01121 UDEBUG("Copy map");
01122
01123
01124 int deltaX = 0;
01125 if(xMin < xMin_)
01126 {
01127 deltaX = (xMin_ - xMin) / cellSize_ + 1.0f;
01128 xMin = xMin_-float(deltaX)*cellSize_;
01129 }
01130 int deltaY = 0;
01131 if(yMin < yMin_)
01132 {
01133 deltaY = (yMin_ - yMin) / cellSize_ + 1.0f;
01134 yMin = yMin_-float(deltaY)*cellSize_;
01135 }
01136 UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
01137 newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
01138 newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
01139 UDEBUG("%d/%d -> %d/%d", map_.cols, map_.rows, newMapSize.width, newMapSize.height);
01140 UASSERT(newMapSize.width >= map_.cols && newMapSize.height >= map_.rows);
01141 UASSERT(newMapSize.width >= map_.cols+deltaX && newMapSize.height >= map_.rows+deltaY);
01142 UASSERT(deltaX>=0 && deltaY>=0);
01143 map = cv::Mat::ones(newMapSize, CV_8S)*-1;
01144 mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
01145 map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
01146 mapInfo_.copyTo(mapInfo(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
01147 }
01148 }
01149 UASSERT(map.cols == mapInfo.cols && map.rows == mapInfo.rows);
01150 UDEBUG("map %d %d", map.cols, map.rows);
01151 if(poses.size())
01152 {
01153 UDEBUG("first pose= %d last pose=%d", poses.begin()->first, poses.rbegin()->first);
01154 }
01155 for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
01156 {
01157 if(kter->first > 0)
01158 {
01159 uInsert(addedNodes_, *kter);
01160 }
01161 std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
01162 std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
01163 std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
01164 if(cter == cellCount_.end() && kter->first > 0)
01165 {
01166 cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
01167 }
01168 if(iter!=emptyLocalMaps.end())
01169 {
01170 for(int i=0; i<iter->second.cols; ++i)
01171 {
01172 float * ptf = iter->second.ptr<float>(0,i);
01173 cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
01174 UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
01175 uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
01176 kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
01177 char & value = map.at<char>(pt.y, pt.x);
01178 if(value != -2 && (!incrementalGraphUpdate || value==-1))
01179 {
01180 float * info = mapInfo.ptr<float>(pt.y, pt.x);
01181 int nodeId = (int)info[0];
01182 if(value != -1)
01183 {
01184 if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
01185 {
01186
01187 continue;
01188 }
01189 if(nodeId > 0)
01190 {
01191 std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
01192 UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
01193 if(value == 0)
01194 {
01195 eter->second.first -= 1;
01196 }
01197 else if(value == 100)
01198 {
01199 eter->second.second -= 1;
01200 }
01201 if(kter->first < 0)
01202 {
01203 eter->second.first += 1;
01204 }
01205 }
01206 }
01207 if(kter->first > 0)
01208 {
01209 info[0] = (float)kter->first;
01210 info[1] = ptf[0];
01211 info[2] = ptf[1];
01212 cter->second.first+=1;
01213 }
01214 value = 0;
01215
01216
01217 if(nodeId != kter->first)
01218 {
01219 info[3] += probMiss_;
01220 if (info[3] < probClampingMin_)
01221 {
01222 info[3] = probClampingMin_;
01223 }
01224 if (info[3] > probClampingMax_)
01225 {
01226 info[3] = probClampingMax_;
01227 }
01228 }
01229 }
01230 }
01231 }
01232
01233 if(footprintRadius_ >= cellSize_*1.5f)
01234 {
01235
01236 cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_);
01237 cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_);
01238 if(ptBegin.x < 0)
01239 ptBegin.x = 0;
01240 if(ptEnd.x >= map.cols)
01241 ptEnd.x = map.cols-1;
01242
01243 if(ptBegin.y < 0)
01244 ptBegin.y = 0;
01245 if(ptEnd.y >= map.rows)
01246 ptEnd.y = map.rows-1;
01247 for(int i=ptBegin.x; i<ptEnd.x; ++i)
01248 {
01249 for(int j=ptBegin.y; j<ptEnd.y; ++j)
01250 {
01251 UASSERT(j < map.rows && i < map.cols);
01252 char & value = map.at<char>(j, i);
01253 float * info = mapInfo.ptr<float>(j, i);
01254 int nodeId = (int)info[0];
01255 if(value != -1)
01256 {
01257 if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
01258 {
01259
01260 continue;
01261 }
01262 if(nodeId>0)
01263 {
01264 std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
01265 UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
01266 if(value == 0)
01267 {
01268 eter->second.first -= 1;
01269 }
01270 else if(value == 100)
01271 {
01272 eter->second.second -= 1;
01273 }
01274 if(kter->first < 0)
01275 {
01276 eter->second.first += 1;
01277 }
01278 }
01279 }
01280 if(kter->first > 0)
01281 {
01282 info[0] = (float)kter->first;
01283 info[1] = float(i) * cellSize_ + xMin;
01284 info[2] = float(j) * cellSize_ + yMin;
01285 cter->second.first+=1;
01286 }
01287 value = -2;
01288 }
01289 }
01290 }
01291
01292 if(jter!=occupiedLocalMaps.end())
01293 {
01294 for(int i=0; i<jter->second.cols; ++i)
01295 {
01296 float * ptf = jter->second.ptr<float>(0,i);
01297 cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
01298 UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
01299 uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
01300 kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
01301 char & value = map.at<char>(pt.y, pt.x);
01302 if(value != -2)
01303 {
01304 float * info = mapInfo.ptr<float>(pt.y, pt.x);
01305 int nodeId = (int)info[0];
01306 if(value != -1)
01307 {
01308 if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
01309 {
01310
01311 continue;
01312 }
01313 if(nodeId>0)
01314 {
01315 std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
01316 UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
01317 if(value == 0)
01318 {
01319 eter->second.first -= 1;
01320 }
01321 else if(value == 100)
01322 {
01323 eter->second.second -= 1;
01324 }
01325 if(kter->first < 0)
01326 {
01327 eter->second.second += 1;
01328 }
01329 }
01330 }
01331 if(kter->first > 0)
01332 {
01333 info[0] = (float)kter->first;
01334 info[1] = ptf[0];
01335 info[2] = ptf[1];
01336 cter->second.second+=1;
01337 }
01338 value = 100;
01339
01340
01341 if(nodeId != kter->first)
01342 {
01343 info[3] += probHit_;
01344 if (info[3] < probClampingMin_)
01345 {
01346 info[3] = probClampingMin_;
01347 }
01348 if (info[3] > probClampingMax_)
01349 {
01350 info[3] = probClampingMax_;
01351 }
01352 }
01353 }
01354 }
01355 }
01356 }
01357
01358 if(footprintRadius_ >= cellSize_*1.5f || incrementalGraphUpdate)
01359 {
01360 for(int i=1; i<map.rows-1; ++i)
01361 {
01362 for(int j=1; j<map.cols-1; ++j)
01363 {
01364 char & value = map.at<char>(i, j);
01365 if(value == -2)
01366 {
01367 value = 0;
01368 }
01369
01370 if(incrementalGraphUpdate && value == -1)
01371 {
01372 float * info = mapInfo.ptr<float>(i, j);
01373
01374
01375 if(map.at<char>(i+1, j) == 100 && map.at<char>(i-1, j) == 100)
01376 {
01377 value = 100;
01378
01379 if(mapInfo.ptr<float>(i+1, j)[0]>0.0f)
01380 {
01381 info[0] = mapInfo.ptr<float>(i+1, j)[0];
01382 info[1] = float(j) * cellSize_ + xMin;
01383 info[2] = float(i) * cellSize_ + yMin;
01384 std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
01385 UASSERT(cter!=cellCount_.end());
01386 cter->second.second+=1;
01387 }
01388 else if(mapInfo.ptr<float>(i-1, j)[0]>0.0f)
01389 {
01390 info[0] = mapInfo.ptr<float>(i-1, j)[0];
01391 info[1] = float(j) * cellSize_ + xMin;
01392 info[2] = float(i) * cellSize_ + yMin;
01393 std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
01394 UASSERT(cter!=cellCount_.end());
01395 cter->second.second+=1;
01396 }
01397 }
01398 else if(map.at<char>(i, j+1) == 100 && map.at<char>(i, j-1) == 100)
01399 {
01400 value = 100;
01401
01402 if(mapInfo.ptr<float>(i, j+1)[0]>0.0f)
01403 {
01404 info[0] = mapInfo.ptr<float>(i, j+1)[0];
01405 info[1] = float(j) * cellSize_ + xMin;
01406 info[2] = float(i) * cellSize_ + yMin;
01407 std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
01408 UASSERT(cter!=cellCount_.end());
01409 cter->second.second+=1;
01410 }
01411 else if(mapInfo.ptr<float>(i, j-1)[0]>0.0f)
01412 {
01413 info[0] = mapInfo.ptr<float>(i, j-1)[0];
01414 info[1] = float(j) * cellSize_ + xMin;
01415 info[2] = float(i) * cellSize_ + yMin;
01416 std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
01417 UASSERT(cter!=cellCount_.end());
01418 cter->second.second+=1;
01419 }
01420 }
01421 else
01422 {
01423
01424 char sum = (map.at<char>(i+1, j) == 0?1:0) +
01425 (map.at<char>(i-1, j) == 0?1:0) +
01426 (map.at<char>(i, j+1) == 0?1:0) +
01427 (map.at<char>(i, j-1) == 0?1:0);
01428 if(sum >=3)
01429 {
01430 value = 0;
01431
01432 if(map.at<char>(i+1, j) != -1 && mapInfo.ptr<float>(i+1, j)[0]>0.0f)
01433 {
01434 info[0] = mapInfo.ptr<float>(i+1, j)[0];
01435 info[1] = float(j) * cellSize_ + xMin;
01436 info[2] = float(i) * cellSize_ + yMin;
01437 std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
01438 UASSERT(cter!=cellCount_.end());
01439 cter->second.first+=1;
01440 }
01441 else if(map.at<char>(i-1, j) != -1 && mapInfo.ptr<float>(i-1, j)[0]>0.0f)
01442 {
01443 info[0] = mapInfo.ptr<float>(i-1, j)[0];
01444 info[1] = float(j) * cellSize_ + xMin;
01445 info[2] = float(i) * cellSize_ + yMin;
01446 std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
01447 UASSERT(cter!=cellCount_.end());
01448 cter->second.first+=1;
01449 }
01450 }
01451 }
01452 }
01453
01454
01455
01456
01457
01458
01459
01460
01461 }
01462 }
01463 }
01464
01465
01466
01467
01468
01469
01470
01471 map_ = map;
01472 mapInfo_ = mapInfo;
01473 xMin_ = xMin;
01474 yMin_ = yMin;
01475
01476
01477 for(std::map<int, std::pair<int, int> >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();)
01478 {
01479 UASSERT(iter->second.first >= 0 && iter->second.second >= 0);
01480 if(iter->second.first == 0 && iter->second.second == 0)
01481 {
01482 cellCount_.erase(iter++);
01483 }
01484 else
01485 {
01486 ++iter;
01487 }
01488 }
01489 }
01490 }
01491
01492 if(cloudAssembling_)
01493 {
01494 if(assembledGroundUpdated && assembledGround_->size() > 1)
01495 {
01496 assembledGround_ = util3d::voxelize(assembledGround_, cellSize_);
01497 }
01498 if(assembledObstaclesUpdated && assembledGround_->size() > 1)
01499 {
01500 assembledObstacles_ = util3d::voxelize(assembledObstacles_, cellSize_);
01501 }
01502 if(assembledEmptyCellsUpdated && assembledEmptyCells_->size() > 1)
01503 {
01504 assembledEmptyCells_ = util3d::voxelize(assembledEmptyCells_, cellSize_);
01505 }
01506 }
01507
01508 if(!fullUpdate_ && !cloudAssembling_)
01509 {
01510 cache_.clear();
01511 }
01512 else
01513 {
01514
01515 for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=cache_.begin(); iter!=cache_.end();)
01516 {
01517 if(iter->first < 0)
01518 {
01519 cache_.erase(iter++);
01520 }
01521 else
01522 {
01523 break;
01524 }
01525 }
01526 }
01527
01528 UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
01529 }
01530
01531 }