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/util3d_mapping.h"
00029
00030 #include <rtabmap/core/util3d_transforms.h>
00031 #include <rtabmap/core/util3d_filtering.h>
00032 #include <rtabmap/core/util3d.h>
00033
00034 #include <rtabmap/utilite/ULogger.h>
00035 #include <rtabmap/utilite/UTimer.h>
00036 #include <rtabmap/utilite/UStl.h>
00037 #include <rtabmap/utilite/UConversion.h>
00038 #include <rtabmap/utilite/UMath.h>
00039
00040 #include <pcl/common/common.h>
00041 #include <pcl/common/centroid.h>
00042 #include <pcl/common/io.h>
00043
00044 namespace rtabmap
00045 {
00046
00047 namespace util3d
00048 {
00049
00050 void occupancy2DFromLaserScan(
00051 const cv::Mat & scan,
00052 cv::Mat & empty,
00053 cv::Mat & occupied,
00054 float cellSize,
00055 bool unknownSpaceFilled,
00056 float scanMaxRange)
00057 {
00058 cv::Point3f viewpoint(0,0,0);
00059 occupancy2DFromLaserScan(
00060 scan,
00061 cv::Mat(),
00062 viewpoint,
00063 empty,
00064 occupied,
00065 cellSize,
00066 unknownSpaceFilled,
00067 scanMaxRange);
00068 }
00069
00070 void occupancy2DFromLaserScan(
00071 const cv::Mat & scan,
00072 const cv::Point3f & viewpoint,
00073 cv::Mat & empty,
00074 cv::Mat & occupied,
00075 float cellSize,
00076 bool unknownSpaceFilled,
00077 float scanMaxRange)
00078 {
00079 occupancy2DFromLaserScan(scan, cv::Mat(), viewpoint, empty, occupied, cellSize, unknownSpaceFilled, scanMaxRange);
00080 }
00081
00082 void occupancy2DFromLaserScan(
00083 const cv::Mat & scanHit,
00084 const cv::Mat & scanNoHit,
00085 const cv::Point3f & viewpoint,
00086 cv::Mat & empty,
00087 cv::Mat & occupied,
00088 float cellSize,
00089 bool unknownSpaceFilled,
00090 float scanMaxRange)
00091 {
00092 if(scanHit.empty() && scanNoHit.empty())
00093 {
00094 return;
00095 }
00096
00097 std::map<int, Transform> poses;
00098 poses.insert(std::make_pair(1, Transform::getIdentity()));
00099
00100 std::map<int, std::pair<cv::Mat, cv::Mat> > scans;
00101 scans.insert(std::make_pair(1, std::make_pair(scanHit, scanNoHit)));
00102
00103 std::map<int, cv::Point3f> viewpoints;
00104 viewpoints.insert(std::make_pair(1, viewpoint));
00105
00106 float xMin, yMin;
00107 cv::Mat map8S = create2DMap(poses, scans, viewpoints, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange);
00108
00109
00110 std::list<int> emptyIndices;
00111 for(unsigned int i=0; i< map8S.total(); ++i)
00112 {
00113 if(map8S.data[i] == 0)
00114 {
00115 emptyIndices.push_back(i);
00116 }
00117 }
00118
00119
00120 empty = cv::Mat();
00121 if(emptyIndices.size())
00122 {
00123 empty = cv::Mat(1, (int)emptyIndices.size(), CV_32FC2);
00124 int i=0;
00125 for(std::list<int>::iterator iter=emptyIndices.begin();iter!=emptyIndices.end(); ++iter)
00126 {
00127 int y = *iter / map8S.cols;
00128 int x = *iter - y*map8S.cols;
00129 cv::Vec2f * ptr = empty.ptr<cv::Vec2f>();
00130 ptr[i][0] = (float(x))*cellSize + xMin;
00131 ptr[i][1] = (float(y))*cellSize + yMin;
00132 ++i;
00133 }
00134 }
00135
00136
00137 if(scanMaxRange > cellSize)
00138 {
00139 occupied = util3d::rangeFiltering(LaserScan::backwardCompatibility(scanHit), 0.0f, scanMaxRange).data().clone();
00140 }
00141 else
00142 {
00143 occupied = scanHit.clone();
00144 }
00145 }
00146
00160 cv::Mat create2DMapFromOccupancyLocalMaps(
00161 const std::map<int, Transform> & posesIn,
00162 const std::map<int, std::pair<cv::Mat, cv::Mat> > & occupancy,
00163 float cellSize,
00164 float & xMin,
00165 float & yMin,
00166 float minMapSize,
00167 bool erode,
00168 float footprintRadius)
00169 {
00170 UASSERT(minMapSize >= 0.0f);
00171 UDEBUG("cellSize=%f m, minMapSize=%f m, erode=%d", cellSize, minMapSize, erode?1:0);
00172 UTimer timer;
00173
00174 std::map<int, cv::Mat> emptyLocalMaps;
00175 std::map<int, cv::Mat> occupiedLocalMaps;
00176
00177 std::list<std::pair<int, Transform> > poses;
00178
00179 for(std::map<int, Transform>::const_reverse_iterator iter = posesIn.rbegin(); iter!=posesIn.rend(); ++iter)
00180 {
00181 if(iter->first>0)
00182 {
00183 poses.push_front(*iter);
00184 }
00185 else
00186 {
00187 poses.push_back(*iter);
00188 }
00189 }
00190
00191 float minX=-minMapSize/2.0, minY=-minMapSize/2.0, maxX=minMapSize/2.0, maxY=minMapSize/2.0;
00192 bool undefinedSize = minMapSize == 0.0f;
00193 for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00194 {
00195 UASSERT(!iter->second.isNull());
00196
00197 float x = iter->second.x();
00198 float y =iter->second.y();
00199 if(undefinedSize)
00200 {
00201 minX = maxX = x;
00202 minY = maxY = y;
00203 undefinedSize = false;
00204 }
00205 else
00206 {
00207 if(minX > x)
00208 minX = x;
00209 else if(maxX < x)
00210 maxX = x;
00211
00212 if(minY > y)
00213 minY = y;
00214 else if(maxY < y)
00215 maxY = y;
00216 }
00217
00218 if(uContains(occupancy, iter->first))
00219 {
00220 const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
00221
00222
00223 if(pair.first.cols)
00224 {
00225 if(pair.first.rows > 1 && pair.first.cols == 1)
00226 {
00227 UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.rows, pair.first.cols);
00228 }
00229 cv::Mat ground(1, pair.first.cols, CV_32FC2);
00230 for(int i=0; i<ground.cols; ++i)
00231 {
00232 const float * vi = pair.first.ptr<float>(0,i);
00233 float * vo = ground.ptr<float>(0,i);
00234 cv::Point3f vt;
00235 if(pair.first.channels() != 2 && pair.first.channels() != 5)
00236 {
00237 vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
00238 }
00239 else
00240 {
00241 vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
00242 }
00243 vo[0] = vt.x;
00244 vo[1] = vt.y;
00245 if(minX > vo[0])
00246 minX = vo[0];
00247 else if(maxX < vo[0])
00248 maxX = vo[0];
00249
00250 if(minY > vo[1])
00251 minY = vo[1];
00252 else if(maxY < vo[1])
00253 maxY = vo[1];
00254 }
00255 emptyLocalMaps.insert(std::make_pair(iter->first, ground));
00256 }
00257
00258
00259 if(pair.second.cols)
00260 {
00261 if(pair.second.rows > 1 && pair.second.cols == 1)
00262 {
00263 UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
00264 }
00265 cv::Mat obstacles(1, pair.second.cols, CV_32FC2);
00266 for(int i=0; i<obstacles.cols; ++i)
00267 {
00268 const float * vi = pair.second.ptr<float>(0,i);
00269 float * vo = obstacles.ptr<float>(0,i);
00270 cv::Point3f vt;
00271 if(pair.second.channels() != 2 && pair.second.channels() != 5)
00272 {
00273 vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
00274 }
00275 else
00276 {
00277 vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
00278 }
00279 vo[0] = vt.x;
00280 vo[1] = vt.y;
00281 if(minX > vo[0])
00282 minX = vo[0];
00283 else if(maxX < vo[0])
00284 maxX = vo[0];
00285
00286 if(minY > vo[1])
00287 minY = vo[1];
00288 else if(maxY < vo[1])
00289 maxY = vo[1];
00290 }
00291 occupiedLocalMaps.insert(std::make_pair(iter->first, obstacles));
00292 }
00293 }
00294 }
00295 UDEBUG("timer=%fs", timer.ticks());
00296
00297 cv::Mat map;
00298 if(minX != maxX && minY != maxY)
00299 {
00300
00301 float margin = cellSize*10.0f;
00302 xMin = minX-margin;
00303 xMin -= cellSize/2.0f;
00304 yMin = minY-margin;
00305 yMin += cellSize/2.0f;
00306 float xMax = maxX+margin;
00307 float yMax = maxY+margin;
00308 if(fabs((yMax - yMin) / cellSize) > 30000 ||
00309 fabs((xMax - xMin) / cellSize) > 30000)
00310 {
00311 UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
00312 "There's maybe an error with the poses provided! The map will not be created!",
00313 xMin, yMin, xMax, yMax);
00314 }
00315 else
00316 {
00317 UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
00318
00319
00320 map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
00321 for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
00322 {
00323 std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
00324 std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
00325 if(iter!=emptyLocalMaps.end())
00326 {
00327 for(int i=0; i<iter->second.cols; ++i)
00328 {
00329 float * ptf = iter->second.ptr<float>(0, i);
00330 cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
00331 UASSERT_MSG(pt.y>0 && pt.y<map.rows && pt.x>0 && pt.x<map.cols,
00332 uFormat("id=%d, map min=(%f, %f) max=(%f,%f) map=%dx%d pt=(%d,%d)", kter->first, xMin, yMin, xMax, yMax, map.cols, map.rows, pt.x, pt.y).c_str());
00333 char & value = map.at<char>(pt.y, pt.x);
00334 if(value != -2)
00335 {
00336 value = 0;
00337 }
00338 }
00339 }
00340
00341 if(footprintRadius >= cellSize*1.5f)
00342 {
00343
00344 cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize, (kter->second.y()-footprintRadius-yMin)/cellSize);
00345 cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize, (kter->second.y()+footprintRadius-yMin)/cellSize);
00346 if(ptBegin.x < 0)
00347 ptBegin.x = 0;
00348 if(ptEnd.x >= map.cols)
00349 ptEnd.x = map.cols-1;
00350
00351 if(ptBegin.y < 0)
00352 ptBegin.y = 0;
00353 if(ptEnd.y >= map.rows)
00354 ptEnd.y = map.rows-1;
00355 for(int i=ptBegin.x; i<ptEnd.x; ++i)
00356 {
00357 for(int j=ptBegin.y; j<ptEnd.y; ++j)
00358 {
00359 map.at<char>(j, i) = -2;
00360 }
00361 }
00362 }
00363
00364 if(jter!=occupiedLocalMaps.end())
00365 {
00366 for(int i=0; i<jter->second.cols; ++i)
00367 {
00368 float * ptf = jter->second.ptr<float>(0, i);
00369 cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
00370 UASSERT_MSG(pt.y>0 && pt.y<map.rows && pt.x>0 && pt.x<map.cols,
00371 uFormat("id=%d: map min=(%f, %f) max=(%f,%f) map=%dx%d pt=(%d,%d)", kter->first, xMin, yMin, xMax, yMax, map.cols, map.rows, pt.x, pt.y).c_str());
00372 char & value = map.at<char>(pt.y, pt.x);
00373 if(value != -2)
00374 {
00375 value = 100;
00376 }
00377 }
00378 }
00379
00380
00381 }
00382
00383
00384 cv::Mat updatedMap = map.clone();
00385 std::list<std::pair<int, int> > obstacleIndices;
00386 for(int i=0; i<map.rows; ++i)
00387 {
00388 for(int j=0; j<map.cols; ++j)
00389 {
00390 if(map.at<char>(i, j) == -2)
00391 {
00392 updatedMap.at<char>(i, j) = 0;
00393 }
00394
00395 if(i >=2 && i<map.rows-2 && j>=2 && j<map.cols-2)
00396 {
00397 if(map.at<char>(i, j) == -1 &&
00398 map.at<char>(i+1, j) != -1 &&
00399 map.at<char>(i-1, j) != -1 &&
00400 map.at<char>(i, j+1) != -1 &&
00401 map.at<char>(i, j-1) != -1)
00402 {
00403 updatedMap.at<char>(i, j) = 0;
00404 }
00405 else if(map.at<char>(i, j) == 100)
00406 {
00407
00408
00409 if((map.at<char>(i-1, j) == 0 || map.at<char>(i-1, j) == -2) &&
00410 map.at<char>(i-2, j) == -1)
00411 {
00412 updatedMap.at<char>(i-1, j) = -1;
00413 }
00414 else if((map.at<char>(i+1, j) == 0 || map.at<char>(i+1, j) == -2) &&
00415 map.at<char>(i+2, j) == -1)
00416 {
00417 updatedMap.at<char>(i+1, j) = -1;
00418 }
00419 if((map.at<char>(i, j-1) == 0 || map.at<char>(i, j-1) == -2) &&
00420 map.at<char>(i, j-2) == -1)
00421 {
00422 updatedMap.at<char>(i, j-1) = -1;
00423 }
00424 else if((map.at<char>(i, j+1) == 0 || map.at<char>(i, j+1) == -2) &&
00425 map.at<char>(i, j+2) == -1)
00426 {
00427 updatedMap.at<char>(i, j+1) = -1;
00428 }
00429
00430 if(erode)
00431 {
00432 obstacleIndices.push_back(std::make_pair(i, j));
00433 }
00434 }
00435 else if(map.at<char>(i, j) == 0)
00436 {
00437
00438 if(map.at<char>(i-1, j) == 100 &&
00439 map.at<char>(i+1, j) == 100)
00440 {
00441 updatedMap.at<char>(i, j) = -1;
00442 }
00443 else if(map.at<char>(i, j-1) == 100 &&
00444 map.at<char>(i, j+1) == 100)
00445 {
00446 updatedMap.at<char>(i, j) = -1;
00447 }
00448 }
00449 }
00450 }
00451 }
00452 map = updatedMap;
00453
00454 if(erode)
00455 {
00456
00457 cv::Mat erodedMap = map.clone();
00458 for(std::list<std::pair<int,int> >::iterator iter = obstacleIndices.begin();
00459 iter!= obstacleIndices.end();
00460 ++iter)
00461 {
00462 int i = iter->first;
00463 int j = iter->second;
00464 int touchEmpty = (map.at<char>(i+1, j) == 0?1:0) +
00465 (map.at<char>(i-1, j) == 0?1:0) +
00466 (map.at<char>(i, j+1) == 0?1:0) +
00467 (map.at<char>(i, j-1) == 0?1:0);
00468 if(touchEmpty>=3 && map.at<char>(i+1, j) != -1 &&
00469 map.at<char>(i-1, j) != -1 &&
00470 map.at<char>(i, j+1) != -1 &&
00471 map.at<char>(i, j-1) != -1)
00472 {
00473 erodedMap.at<char>(i, j) = 0;
00474 }
00475 }
00476 map = erodedMap;
00477 }
00478 }
00479 }
00480
00481 UDEBUG("timer=%fs", timer.ticks());
00482 return map;
00483 }
00484
00499 cv::Mat create2DMap(const std::map<int, Transform> & poses,
00500 const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
00501 float cellSize,
00502 bool unknownSpaceFilled,
00503 float & xMin,
00504 float & yMin,
00505 float minMapSize,
00506 float scanMaxRange)
00507 {
00508 std::map<int, cv::Point3f > viewpoints;
00509 std::map<int, std::pair<cv::Mat, cv::Mat> > scansCv;
00510 for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator iter = scans.begin(); iter!=scans.end(); ++iter)
00511 {
00512 scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second), cv::Mat())));
00513 }
00514 return create2DMap(poses,
00515 scansCv,
00516 viewpoints,
00517 cellSize,
00518 unknownSpaceFilled,
00519 xMin,
00520 yMin,
00521 minMapSize,
00522 scanMaxRange);
00523 }
00524
00525 cv::Mat create2DMap(const std::map<int, Transform> & poses,
00526 const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
00527 const std::map<int, cv::Point3f > & viewpoints,
00528 float cellSize,
00529 bool unknownSpaceFilled,
00530 float & xMin,
00531 float & yMin,
00532 float minMapSize,
00533 float scanMaxRange)
00534 {
00535 std::map<int, std::pair<cv::Mat, cv::Mat> > scansCv;
00536 for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator iter = scans.begin(); iter!=scans.end(); ++iter)
00537 {
00538 scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second), cv::Mat())));
00539 }
00540 return create2DMap(poses,
00541 scansCv,
00542 viewpoints,
00543 cellSize,
00544 unknownSpaceFilled,
00545 xMin,
00546 yMin,
00547 minMapSize,
00548 scanMaxRange);
00549 }
00550
00566 cv::Mat create2DMap(const std::map<int, Transform> & poses,
00567 const std::map<int, std::pair<cv::Mat, cv::Mat> > & scans,
00568 const std::map<int, cv::Point3f > & viewpoints,
00569 float cellSize,
00570 bool unknownSpaceFilled,
00571 float & xMin,
00572 float & yMin,
00573 float minMapSize,
00574 float scanMaxRange)
00575 {
00576 UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange);
00577
00578
00579 std::map<int, std::pair<cv::Mat, cv::Mat> > localScans;
00580
00581 pcl::PointCloud<pcl::PointXYZ> minMax;
00582 if(minMapSize > 0.0f)
00583 {
00584 minMax.push_back(pcl::PointXYZ(-minMapSize/2.0, -minMapSize/2.0, 0));
00585 minMax.push_back(pcl::PointXYZ(minMapSize/2.0, minMapSize/2.0, 0));
00586 }
00587 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00588 {
00589 std::map<int, std::pair<cv::Mat, cv::Mat> >::const_iterator jter=scans.find(iter->first);
00590 if(jter!=scans.end() && (jter->second.first.cols || jter->second.second.cols))
00591 {
00592 UASSERT(!iter->second.isNull());
00593 cv::Mat hit = util3d::transformLaserScan(LaserScan::backwardCompatibility(jter->second.first), iter->second).data();
00594 cv::Mat noHit = util3d::transformLaserScan(LaserScan::backwardCompatibility(jter->second.second), iter->second).data();
00595 pcl::PointXYZ min, max;
00596 if(!hit.empty())
00597 {
00598 util3d::getMinMax3D(hit, min, max);
00599 minMax.push_back(min);
00600 minMax.push_back(max);
00601 }
00602 if(!noHit.empty())
00603 {
00604 util3d::getMinMax3D(noHit, min, max);
00605 minMax.push_back(min);
00606 minMax.push_back(max);
00607 }
00608 minMax.push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
00609
00610 std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
00611 if(kter!=viewpoints.end())
00612 {
00613 minMax.push_back(pcl::PointXYZ(iter->second.x()+kter->second.x, iter->second.y()+kter->second.y, iter->second.z()+kter->second.z));
00614 }
00615
00616 localScans.insert(std::make_pair(iter->first, std::make_pair(hit, noHit)));
00617 }
00618 }
00619
00620 cv::Mat map;
00621 if(minMax.size())
00622 {
00623
00624 pcl::PointXYZ min, max;
00625 pcl::getMinMax3D(minMax, min, max);
00626
00627
00628 float margin = cellSize*10.0f;
00629 xMin = (unknownSpaceFilled && scanMaxRange > 0 && -scanMaxRange < min.x?-scanMaxRange:min.x) - margin;
00630 yMin = (unknownSpaceFilled && scanMaxRange > 0 && -scanMaxRange < min.y?-scanMaxRange:min.y) - margin;
00631 float xMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.x?scanMaxRange:max.x) + margin;
00632 float yMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.y?scanMaxRange:max.y) + margin;
00633
00634 UDEBUG("map min=(%fm, %fm) max=(%fm,%fm) (margin=%fm, cellSize=%fm, scan range=%f, min=[%fm,%fm] max=[%fm,%fm])",
00635 xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y);
00636
00637 UTimer timer;
00638
00639 map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
00640 UDEBUG("map size = %dx%d", map.cols, map.rows);
00641
00642 int j=0;
00643 float scanMaxRangeSqr = scanMaxRange * scanMaxRange;
00644 for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
00645 {
00646 const Transform & pose = poses.at(iter->first);
00647 cv::Point3f viewpoint(0,0,0);
00648 std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
00649 if(kter!=viewpoints.end())
00650 {
00651 viewpoint = kter->second;
00652 }
00653 cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
00654
00655
00656 for(int i=0; i<iter->second.first.cols; ++i)
00657 {
00658 const float * ptr = iter->second.first.ptr<float>(0, i);
00659 bool ignore = scanMaxRange>cellSize && uNormSquared(ptr[0]-(pose.x()+viewpoint.x)+cellSize, ptr[1]-(pose.y()+viewpoint.y)+cellSize) > scanMaxRangeSqr;
00660 if(!ignore)
00661 {
00662 cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
00663 if(end!=start)
00664 {
00665 map.at<char>(end.y, end.x) = 100;
00666 }
00667 }
00668 }
00669
00670
00671 for(int i=0; i<iter->second.first.cols; ++i)
00672 {
00673 const float * ptr = iter->second.first.ptr<float>(0, i);
00674
00675 cv::Vec2f pt(ptr[0], ptr[1]);
00676 if(scanMaxRange>cellSize)
00677 {
00678 cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
00679 float n = cv::norm(v);
00680 if(n > scanMaxRange+cellSize)
00681 {
00682 v = (v/n) * scanMaxRange;
00683 pt[0] = pose.x()+viewpoint.x + v[0];
00684 pt[1] = pose.y()+viewpoint.y + v[1];
00685 }
00686 }
00687
00688 cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
00689 if(end!=start)
00690 {
00691 if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
00692 {
00693 rayTrace(start, end, map, true);
00694 }
00695 }
00696 }
00697
00698 for(int i=0; i<iter->second.second.cols; ++i)
00699 {
00700 const float * ptr = iter->second.second.ptr<float>(0, i);
00701
00702 cv::Vec2f pt(ptr[0], ptr[1]);
00703 if(scanMaxRange>cellSize)
00704 {
00705 cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
00706 float n = cv::norm(v);
00707 if(n > scanMaxRange+cellSize)
00708 {
00709 v = (v/n) * scanMaxRange;
00710 pt[0] = pose.x()+viewpoint.x + v[0];
00711 pt[1] = pose.y()+viewpoint.y + v[1];
00712 }
00713 }
00714
00715 cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
00716 if(end!=start)
00717 {
00718 if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
00719 {
00720 rayTrace(start, end, map, true);
00721 if(map.at<char>(end.y, end.x) == -1)
00722 {
00723 map.at<char>(end.y, end.x) = 0;
00724 }
00725 }
00726 }
00727 }
00728 ++j;
00729 }
00730 UDEBUG("Ray trace known space=%fs", timer.ticks());
00731
00732
00733 if(unknownSpaceFilled && scanMaxRange > 0)
00734 {
00735 j=0;
00736 float a = CV_PI/256.0f;
00737 for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
00738 {
00739 if(iter->second.first.cols > 1)
00740 {
00741 if(scanMaxRange > cellSize)
00742 {
00743 const Transform & pose = poses.at(iter->first);
00744 cv::Point3f viewpoint(0,0,0);
00745 std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
00746 if(kter!=viewpoints.end())
00747 {
00748 viewpoint = kter->second;
00749 }
00750 cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
00751
00752
00753
00754
00755 cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(a), -sin(a),
00756 sin(a), cos(a));
00757 cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F);
00758 origin.at<float>(0) = pose.x()+viewpoint.x;
00759 origin.at<float>(1) = pose.y()+viewpoint.y;
00760 endFirst.at<float>(0) = iter->second.first.ptr<float>(0,0)[0];
00761 endFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1];
00762 endLast.at<float>(0) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[0];
00763 endLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[1];
00764
00765
00766
00767 cv::Mat tmp = (endFirst - origin);
00768 cv::Mat endRotated = rotation*((tmp/cv::norm(tmp))*scanMaxRange) + origin;
00769 cv::Mat endLastVector(3,1,CV_32F), endRotatedVector(3,1,CV_32F);
00770 endLastVector.at<float>(0) = endLast.at<float>(0) - origin.at<float>(0);
00771 endLastVector.at<float>(1) = endLast.at<float>(1) - origin.at<float>(1);
00772 endLastVector.at<float>(2) = 0.0f;
00773 endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);
00774 endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
00775 endRotatedVector.at<float>(2) = 0.0f;
00776
00777 float normEndRotatedVector = cv::norm(endRotatedVector);
00778 endLastVector = endLastVector / cv::norm(endLastVector);
00779 float angle = (endRotatedVector/normEndRotatedVector).dot(endLastVector);
00780 angle = angle<-1.0f?-1.0f:angle>1.0f?1.0f:angle;
00781 while(acos(angle) > M_PI_4 || endRotatedVector.cross(endLastVector).at<float>(2) > 0.0f)
00782 {
00783 cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize, (endRotated.at<float>(1)-yMin)/cellSize);
00784
00785 end.x = end.x < 0?0:end.x;
00786 end.x = end.x >= map.cols?map.cols-1:end.x;
00787 end.y = end.y < 0?0:end.y;
00788 end.y = end.y >= map.rows?map.rows-1:end.y;
00789 rayTrace(start, end, map, true);
00790
00791 endRotated = rotation*(endRotated - origin) + origin;
00792 endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);
00793 endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
00794 angle = (endRotatedVector/normEndRotatedVector).dot(endLastVector);
00795 angle = angle<-1.0f?-1.0f:angle>1.0f?1.0f:angle;
00796
00797
00798
00799
00800
00801
00802 }
00803 }
00804 }
00805 ++j;
00806 }
00807 UDEBUG("Fill empty space=%fs", timer.ticks());
00808
00809
00810 }
00811 }
00812 return map;
00813 }
00814
00815 void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid, bool stopOnObstacle)
00816 {
00817 UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
00818 UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
00819 UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
00820 UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
00821
00822 cv::Point2i ptA, ptB;
00823 ptA = start;
00824 ptB = end;
00825
00826 float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
00827
00828 bool swapped = false;
00829 if(slope<-1.0f || slope>1.0f)
00830 {
00831
00832 slope = 1.0f/slope;
00833
00834 int tmp = ptA.x;
00835 ptA.x = ptA.y;
00836 ptA.y = tmp;
00837
00838 tmp = ptB.x;
00839 ptB.x = ptB.y;
00840 ptB.y = tmp;
00841
00842 swapped = true;
00843 }
00844
00845 float b = ptA.y - slope*ptA.x;
00846 for(int x=ptA.x; ptA.x<ptB.x?x<ptB.x:x>ptB.x; ptA.x<ptB.x?++x:--x)
00847 {
00848 int upperbound = float(x)*slope + b;
00849 int lowerbound = upperbound;
00850 if(x != ptA.x)
00851 {
00852 lowerbound = (ptA.x<ptB.x?x+1:x-1)*slope + b;
00853 }
00854
00855 if(lowerbound > upperbound)
00856 {
00857 int tmp = upperbound;
00858 upperbound = lowerbound;
00859 lowerbound = tmp;
00860 }
00861
00862 if(!swapped)
00863 {
00864 UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%f grid.rows=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.rows, x, slope, b, x).c_str());
00865 UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.rows=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.rows, x+1, slope, b, x).c_str());
00866 }
00867 else
00868 {
00869 UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.cols, uFormat("lowerbound=%f grid.cols=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.cols, x, slope, b, x).c_str());
00870 UASSERT_MSG(upperbound >= 0 && upperbound < grid.cols, uFormat("upperbound=%f grid.cols=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.cols, x+1, slope, b, x).c_str());
00871 }
00872
00873 for(int y = lowerbound; y<=(int)upperbound; ++y)
00874 {
00875 char * v;
00876 if(swapped)
00877 {
00878 v = &grid.at<char>(x, y);
00879 }
00880 else
00881 {
00882 v = &grid.at<char>(y, x);
00883 }
00884 if(*v == 100 && stopOnObstacle)
00885 {
00886 return;
00887 }
00888 else
00889 {
00890 *v = 0;
00891 }
00892 }
00893 }
00894 }
00895
00896
00897 cv::Mat convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat)
00898 {
00899 UASSERT(map8S.channels() == 1 && map8S.type() == CV_8S);
00900 cv::Mat map8U = cv::Mat(map8S.rows, map8S.cols, CV_8U);
00901 for (int i = 0; i < map8S.rows; ++i)
00902 {
00903 for (int j = 0; j < map8S.cols; ++j)
00904 {
00905 char v = pgmFormat?map8S.at<char>((map8S.rows-1)-i, j):map8S.at<char>(i, j);
00906 unsigned char gray;
00907 if(v == 0)
00908 {
00909 gray = pgmFormat?254:178;
00910 }
00911 else if(v == 100)
00912 {
00913 gray = 0;
00914 }
00915 else if(v == -2)
00916 {
00917 gray = pgmFormat?254:200;
00918 }
00919 else
00920 {
00921 gray = pgmFormat?205:89;
00922 }
00923 map8U.at<unsigned char>(i, j) = gray;
00924 }
00925 }
00926 return map8U;
00927 }
00928
00929
00930 cv::Mat convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat)
00931 {
00932 UASSERT_MSG(map8U.channels() == 1 && map8U.type() == CV_8U, uFormat("map8U.channels()=%d map8U.type()=%d", map8U.channels(), map8U.type()).c_str());
00933 cv::Mat map8S = cv::Mat(map8U.rows, map8U.cols, CV_8S);
00934 for (int i = 0; i < map8U.rows; ++i)
00935 {
00936 for (int j = 0; j < map8U.cols; ++j)
00937 {
00938 unsigned char v = pgmFormat?map8U.at<char>((map8U.rows-1)-i, j):map8U.at<char>(i, j);
00939 char occupancy;
00940 if(pgmFormat)
00941 {
00942 if(v >= 254)
00943 {
00944 occupancy = 0;
00945 }
00946 else if(v == 0)
00947 {
00948 occupancy = 100;
00949 }
00950 else
00951 {
00952 occupancy = -1;
00953 }
00954 }
00955 else
00956 {
00957 if(v == 178)
00958 {
00959 occupancy = 0;
00960 }
00961 else if(v == 0)
00962 {
00963 occupancy = 100;
00964 }
00965 else if(v == 200)
00966 {
00967 occupancy = -2;
00968 }
00969 else
00970 {
00971 occupancy = -1;
00972 }
00973 }
00974
00975 map8S.at<char>(i, j) = occupancy;
00976 }
00977 }
00978 return map8S;
00979 }
00980
00981 cv::Mat erodeMap(const cv::Mat & map)
00982 {
00983 UASSERT(map.type() == CV_8SC1);
00984 cv::Mat erodedMap = map.clone();
00985 for(int i=0; i<map.rows; ++i)
00986 {
00987 for(int j=0; j<map.cols; ++j)
00988 {
00989 if(map.at<char>(i, j) == 100)
00990 {
00991
00992 int touchEmpty = (map.at<char>(i+1, j) == 0?1:0) +
00993 (map.at<char>(i-1, j) == 0?1:0) +
00994 (map.at<char>(i, j+1) == 0?1:0) +
00995 (map.at<char>(i, j-1) == 0?1:0);
00996
00997 if(touchEmpty>=3 && map.at<char>(i+1, j) != -1 &&
00998 map.at<char>(i-1, j) != -1 &&
00999 map.at<char>(i, j+1) != -1 &&
01000 map.at<char>(i, j-1) != -1)
01001 {
01002 erodedMap.at<char>(i, j) = 0;
01003 }
01004 }
01005 }
01006 }
01007 return erodedMap;
01008 }
01009
01010 }
01011
01012 }