fixed broken slope calculation (again)
[rock-planning:vfh_star.git] / src / TraversabilityMapGenerator.cpp
1 #include "TraversabilityMapGenerator.h"
2 #include <Eigen/LU>
3 #include <iostream>
4 #include <envire/maps/MLSGrid.hpp>
5
6 using namespace Eigen;
7
8 namespace vfh_star {
9
10 TraversabilityMapGenerator::TraversabilityMapGenerator()
11 {
12     boundarySize = 0;
13     maxStepSize = 0.2;
14     maxSlope = 0.0;
15     lastBody2Odo = Affine3d::Identity();
16     lastLaser2Odo = Affine3d::Identity();
17     lastHeight = 0.0;
18     heightToGround = 0.0;
19 }
20
21 void TraversabilityMapGenerator::setHeightToGround(double value)
22 {
23     heightToGround = value;
24 }
25
26 double TraversabilityMapGenerator::getHeightToGround() const
27 {
28     return heightToGround;
29 }
30
31 void TraversabilityMapGenerator::setBoundarySize(double size)
32 {
33     boundarySize = size;
34 }
35
36 void TraversabilityMapGenerator::setMaxStepSize(double size)
37 {
38     maxStepSize = size;
39 }
40
41 void TraversabilityMapGenerator::setMaxSlope(double slope)
42 {
43     maxSlope = slope;
44 }
45
46 bool TraversabilityMapGenerator::getZCorrection(Eigen::Affine3d& body2Odo)
47 {
48     //correct body2Odo z measurement
49     Vector2i pInGrid;
50     if(!laserGrid.getGridPoint(body2Odo.translation(), pInGrid))
51     {
52         return false;
53     }
54
55     const ElevationEntry entry(laserGrid.getEntry(pInGrid));
56
57     double curHeight;
58     
59     if(entry.getMeasurementCount())
60         curHeight = entry.getMedian();
61     else
62         curHeight = lastHeight;
63     
64     Vector3d vecToGround = body2Odo.rotation() * Vector3d(0,0, heightToGround);
65     
66     body2Odo.translation().z() = curHeight + vecToGround.z();
67     
68     lastHeight = curHeight;
69
70     return true;
71 }
72
73 void TraversabilityMapGenerator::addPointVector(const std::vector< Vector3d >& rangePoints_odo)
74 {
75     laserGrid.addLaserScan(rangePoints_odo);
76 }
77
78
79 bool TraversabilityMapGenerator::addLaserScan(const base::samples::LaserScan& ls, const Eigen::Affine3d& body2Odo2, const Eigen::Affine3d& laser2Body)
80 {
81     Eigen::Affine3d body2Odo(body2Odo2);
82     //std::cout << "TraversabilityMapGenerator: Got laserScan" << std::endl;
83     //std::cout << "body2Odo " << std::endl << body2Odo.matrix() << " laser2Body " << std::endl << laser2Body.matrix() << std::endl;
84
85     moveGridIfRobotNearBoundary(laserGrid, body2Odo.translation());
86     
87     //correct body2Odo z measurement
88     Vector2i pInGrid;
89     if(!getZCorrection(body2Odo))
90     {
91         std::cout << "Odometry position not in Grid" <<std::endl;
92         throw std::runtime_error("Odometry position not in Grid");
93     }
94     
95     Affine3d laser2Odo(body2Odo * laser2Body);
96     Affine3d  body2LastBody(lastBody2Odo.inverse() * body2Odo);
97
98     double distanceBodyToLastBody = body2LastBody.translation().norm();
99     Vector3d Ylaser2Odo = laser2Odo * Vector3d::UnitY() - laser2Odo.translation();
100     Ylaser2Odo.normalize();
101     Vector3d YlastLaser2Odo = lastLaser2Odo * Vector3d::UnitY() - lastLaser2Odo.translation();
102     YlastLaser2Odo.normalize();
103     double laserChange = acos(Ylaser2Odo.dot(YlastLaser2Odo));
104
105     
106     std::vector<Vector3d> currentLaserPoints = ls.convertScanToPointCloud(laser2Odo);
107     
108     laser2Map = laser2Odo;
109     
110     laserGrid.addLaserScan(currentLaserPoints);
111     
112     //    std::cout << "GridMapSegmenter: Odometry distance : " << distanceBodyToLastBody << " Orientation change is " << orientationChange / M_PI * 180.0 << std::endl;
113     if(distanceBodyToLastBody < 0.05 && laserChange < M_PI / 36.0) {
114         return false;
115     }
116
117     
118     lastBody2Odo = body2Odo;
119     lastLaser2Odo = laser2Odo;
120     return true;
121 }
122
123 bool TraversabilityMapGenerator::moveMapIfRobotNearBoundary(const Eigen::Vector3d& robotPosition_world)
124 {
125     return moveGridIfRobotNearBoundary(laserGrid, robotPosition_world);
126 }
127
128 void TraversabilityMapGenerator::addKnowMap(envire::MLSGrid const *mls, const Affine3d &mls2LaserGrid)
129 {
130     const Eigen::Vector3d gridPos(laserGrid.getPosition());
131     const Affine3d laserGrid2mls(mls2LaserGrid.inverse());
132     
133     for(int x = 0; x < laserGrid.getWidth(); x++)
134     {
135         for(int y = 0; y < laserGrid.getHeight(); y++)
136         {
137             Eigen::Vector3d posLaserGrid;
138             Eigen::Vector2i posLG(x,y);
139             
140             if(!laserGrid.fromGridPoint(posLG, posLaserGrid))
141             {
142                 std::cout << "WARNING point that should be in the grid is not in grid" <<std::endl;
143                 continue;
144             }
145             const Eigen::Vector3d posMls = laserGrid2mls * posLaserGrid;
146
147             size_t mlsX;
148             size_t mlsY;
149             if(mls->toGrid(posMls, mlsX, mlsY))
150             {
151                 vfh_star::ElevationEntry &entry(laserGrid.getEntry(posLG));
152
153                 if(!entry.getMeasurementCount())
154                 {
155                     envire::MLSGrid::const_iterator cellIt = mls->beginCell(mlsX, mlsY);
156                     envire::MLSGrid::const_iterator cellEndIt = mls->endCell();
157                     
158                     for(; cellIt != cellEndIt; cellIt++)
159                     {
160                         entry.addHeightMeasurement(cellIt->mean + mls2LaserGrid.translation().z());
161                     }
162                 }
163             }
164         }
165     }
166 }
167
168 void TraversabilityMapGenerator::setGridEntriesWindowSize(int window_size)
169 {
170         laserGrid.setEntriesWindowSize(window_size);
171         interpolatedGrid.setEntriesWindowSize(window_size);
172 }
173
174 void TraversabilityMapGenerator::setHeightMeasureMethod(int entry_height_conf){
175         laserGrid.setHeightMeasureMethod(entry_height_conf);
176         interpolatedGrid.setHeightMeasureMethod(entry_height_conf);
177 }
178
179 void TraversabilityMapGenerator::computeNewMap()
180 {
181     //interpolate grid
182     smoothElevationGrid(laserGrid, interpolatedGrid);
183     
184     computeSmoothElevelationGrid(interpolatedGrid, smoothedGrid);
185     
186     updateTraversabilityGrid(interpolatedGrid, traversabilityGrid);    
187 }
188
189 void TraversabilityMapGenerator::clearMap()
190 {
191     lastBody2Odo = Affine3d::Identity();
192     lastLaser2Odo = Affine3d::Identity();
193     lastHeight = 0.0;
194     laserGrid.clear();
195     interpolatedGrid.clear();
196     traversabilityGrid.clear();
197     smoothedGrid.clear();
198 }
199
200 void TraversabilityMapGenerator::getGridDump(GridDump& gd) const
201 {
202     assert(laserGrid.getHeight() == interpolatedGrid.getHeight() &&
203     laserGrid.getHeight() == traversabilityGrid.getHeight() &&
204     laserGrid.getWidth() == interpolatedGrid.getWidth() &&
205     laserGrid.getWidth() == traversabilityGrid.getWidth() );
206     
207     for(int x = 0; x < laserGrid.getWidth(); x++) {
208         for(int y = 0; y < laserGrid.getHeight(); y++) {
209             if(interpolatedGrid.getEntry(x, y).getMeasurementCount()) {
210                 gd.height[x*laserGrid.getWidth()+y] = interpolatedGrid.getEntry(x, y).getMedian();
211             } else {
212                 gd.height[x*laserGrid.getWidth()+y] = std::numeric_limits<double>::infinity();          
213             }
214             
215             gd.max[x*laserGrid.getWidth()+y] = interpolatedGrid.getEntry(x, y).getMaximum();
216             
217             gd.interpolated[x*laserGrid.getWidth()+y] = interpolatedGrid.getEntry(x, y).isInterpolated();
218             
219             gd.traversability[x*laserGrid.getWidth()+y] = traversabilityGrid.getEntry(x, y);
220         }
221     }
222     
223     gd.gridPositionX = traversabilityGrid.getGridPosition().x();
224     gd.gridPositionY = traversabilityGrid.getGridPosition().y();
225     gd.gridPositionZ = traversabilityGrid.getGridPosition().z();
226 }
227
228 bool TraversabilityMapGenerator::moveGridIfRobotNearBoundary(ElevationGrid &grid, const Eigen::Vector3d& robotPosition_world)
229 {
230     Vector3d posInGrid = robotPosition_world - grid.getPosition();
231     posInGrid.z() = 0;
232     
233     double width = grid.getWidth() * grid.getGridResolution() / 2.0;
234     double height = grid.getHeight() * grid.getGridResolution() / 2.0;
235     
236     if(fabs(posInGrid.x()) > (width - boundarySize) || fabs(posInGrid.y()) > (height - boundarySize)) {
237     
238         if(fabs(posInGrid.x()) > width || fabs(posInGrid.y()) > height)
239         {
240             //inital case, robot might be out of grid
241             posInGrid = Vector3d(0, 0, 0);
242         }
243
244         //we assume the robot keeps moving into the same direction
245         grid.moveGrid(robotPosition_world + posInGrid * 2.0 / 3.0);
246
247         return true;
248     }
249     return false;
250 }
251
252 void TraversabilityMapGenerator::updateTraversabilityGrid(const ElevationGrid &elGrid, TraversabilityGrid &trGrid)
253 {
254     trGrid.setGridPosition(elGrid.getGridPosition());
255     
256     for(int x = 0;x < elGrid.getWidth(); x++) {
257         for(int y = 0;y < elGrid.getHeight(); y++) {
258             testNeighbourEntry(Eigen::Vector2i(x,y), elGrid, trGrid);
259         }
260     }
261 }
262
263 void TraversabilityMapGenerator::testNeighbourEntry(Eigen::Vector2i p, const ElevationGrid &elGrid, TraversabilityGrid &trGrid) {
264     if(!elGrid.inGrid(p))
265         return;
266     
267     const ElevationEntry &entry = elGrid.getEntry(p);
268     Traversability cl = TRAVERSABLE;
269     double curHeight;
270
271     if(!entry.getMeasurementCount() && entry.getMaximum() == -std::numeric_limits< double >::max()) 
272     {
273         trGrid.getEntry(p) = UNCLASSIFIED;
274         return;
275     }
276
277     if(!entry.getMeasurementCount())
278     {
279         curHeight = entry.getMaximum();
280         cl = UNKNOWN_OBSTACLE;
281     }
282     else
283     {
284         curHeight = entry.getMedian();
285     }
286     
287     double curHeightSmooth = smoothedGrid.getEntry(p);
288
289     double meanSlope = 0.0;
290     double maximumSlope = -std::numeric_limits< double >::max();
291     double minimumSlope = std::numeric_limits< double >::max();
292     int neighbourCnt = 0;
293     const double diagonalDist = sqrt(smoothedGrid.getGridResolution() * smoothedGrid.getGridResolution()  * 2);
294
295     for(int x = -1; x <= 1; x++) {
296         for(int y = -1; y <= 1; y++) {
297             //skip onw entry
298             if(x == 0 && y == 0)
299                 continue;
300
301             const int rx = p.x() + x;
302             const int ry = p.y() + y;
303             if(elGrid.inGrid(rx, ry)) {
304                 const ElevationEntry &neighbourEntry = elGrid.getEntry(rx, ry);
305                 
306                 double neighbourHeight;
307                 
308                 if(neighbourEntry.getMeasurementCount()) 
309                 {
310                     //use real measurement if available
311                     neighbourHeight = neighbourEntry.getMedian();
312                 }
313                 else
314                 {
315                     if(neighbourEntry.getMaximum() == -std::numeric_limits<double>::max())
316                         continue;
317                     
318                     neighbourHeight = neighbourEntry.getMinimum();
319                 }
320                 
321                 //only make the higher one an obstacle
322                 if(fabs(neighbourHeight - curHeight) > maxStepSize && curHeight > neighbourHeight) {
323                     cl = OBSTACLE;
324                     break;
325                 } 
326
327                 const double a = smoothedGrid.getEntry(rx, ry) - curHeightSmooth;
328                 double b = 0;
329                 if((x == 0) || (y==0))
330                 {
331                     b = smoothedGrid.getGridResolution();
332                 }
333                 else
334                 {
335                     b = diagonalDist;
336                 }
337                 
338                 
339                 const double slope = atan2(a, b);
340                 maximumSlope = std::max(slope, maximumSlope);
341                 minimumSlope = std::min(slope, minimumSlope);
342                 
343                 meanSlope += slope;
344                 neighbourCnt++;
345             }
346         }
347     }
348     
349     if((cl != OBSTACLE) && (neighbourCnt > 0))
350     {
351         /*
352          * A Perfect slope has actualy a mean slope of Zero.
353          * Therefor we filter by a low mean slope and a heigh 
354          * maximum slope.
355          * */
356         double angle = 15.0/180.0*M_PI;
357         meanSlope /= neighbourCnt;
358         if((maximumSlope > maxSlope) && (fabs(meanSlope) < angle)
359             && (fabs(minimumSlope + maximumSlope) < angle))
360         {
361             cl = OBSTACLE;
362         }
363     }
364     
365     trGrid.getEntry(p) = cl;
366 }
367
368 void TraversabilityMapGenerator::markUnknownInRadiusAs(const base::Pose& pose, double radius, Traversability type)
369 {
370     Eigen::Vector2i gridp;
371     if(!traversabilityGrid.getGridPoint(pose.position, gridp))
372     {
373         std::cout << "Pose " << pose.position.transpose() << " Gridpos " << traversabilityGrid.getPosition().transpose() << " Boundary Size " << boundarySize << std::endl;
374         throw std::runtime_error("markUnknownInRadiusAsObstacle: Pose out of grid");
375     }
376     
377     int posX = gridp.x();
378     int posY = gridp.y();
379     
380     int radiusGrid = radius / traversabilityGrid.getGridResolution();
381     for(int x = -radiusGrid; x < radiusGrid; x++)
382     {
383         for(int y = -radiusGrid; y < radiusGrid; y++)
384         {
385             const double xd = x * traversabilityGrid.getGridResolution();
386             const double yd = y * traversabilityGrid.getGridResolution();
387             double distToRobot = sqrt(xd*xd + yd*yd);
388
389             //check if outside circle
390             if(distToRobot > radius)
391                 continue;
392
393             const int rx = posX + x;
394             const int ry = posY + y;
395             
396             if(!traversabilityGrid.inGrid(rx, ry))
397             {
398                 std::cout << "x " << x << " y " << y << " posX " << posX << " posY " << posY << " radGrid " << radiusGrid << std::endl;
399                 throw std::runtime_error("markUnknownInRadiusAsObstacle: Access out of grid");
400             }
401             
402             Traversability &entry(traversabilityGrid.getEntry(rx, ry));
403             
404             if(entry == UNCLASSIFIED || entry == UNKNOWN_OBSTACLE) 
405             {
406                 entry = type;
407                 if(type == TRAVERSABLE)
408                 {
409                     laserGrid.getEntry(rx, ry).addHeightMeasurement(laserGrid.getEntry(rx, ry).getMedian());
410                 }
411             }
412         }
413     }
414 }
415
416 ConsistencyStats TraversabilityMapGenerator::checkMapConsistencyInArea(const base::Pose& pose, double width, double height)
417 {
418     double heading = pose.orientation.toRotationMatrix().eulerAngles(2,1,0)[0];
419     AngleAxisd rot = AngleAxisd(heading, Vector3d::UnitZ());
420     
421     ConsistencyStats stats;
422     
423     stats.cellCnt = 0;
424     stats.noMeasurementCnt = 0;
425     stats.measurementCnt = 0;
426     stats.averageCertainty = 0.0;
427     
428     for(double x = -width / 2.0; x <= (width / 2.0); x += 0.03)
429     {
430         for(double y = -height / 2.0; y <= (height / 2.0); y += 0.03)
431         {
432             Vector2i p_g;
433             Vector3d p_w = pose.position + rot * Vector3d(x, y, 0);
434             
435             if(laserGrid.getGridPoint(p_w, p_g))
436             {
437                 vfh_star::ElevationEntry &entry(laserGrid.getEntry(p_g));
438                 stats.cellCnt++;
439                 
440                 if(entry.getMeasurementCount())
441                 {
442                     stats.measurementCnt++;
443                     //FIXME hard coded
444                     stats.averageCertainty += entry.getMeasurementCount() / 50.0;
445                 }
446                 else 
447                 {
448                     stats.noMeasurementCnt++;
449                 }
450             }
451             else 
452             {
453                 std::cout << "Error point not in grid " << std::endl;
454             }
455         }
456     }
457     
458     stats.averageCertainty /= stats.measurementCnt;
459     
460 //     std::cout << "CellCnt " << stats.cellCnt << " Cells with Measurement " << stats.measurementCnt << " Cells without Measurement " << stats.noMeasurementCnt << " average certainty " << stats.averageCertainty << std::endl;    
461     return stats;
462 }
463
464
465 void TraversabilityMapGenerator::markUnknownInRectangeAs(const base::Pose& pose, double width, double height, double forwardOffset, Traversability type)
466 {
467     Vector3d vecToGround = pose.orientation * Vector3d(0,0, heightToGround);
468
469     for(double x = -width / 2.0; x <= (width / 2.0); x += 0.03)
470     {
471         for(double y = -height / 2.0; y <= (height / 2.0 + forwardOffset); y += 0.03)
472         {
473             Vector2i p_g;
474             Vector3d p_w = pose.position + pose.orientation * Vector3d(x, y, 0);
475             
476             if(laserGrid.getGridPoint(p_w, p_g))
477             {
478                 Traversability &entry(traversabilityGrid.getEntry(p_g));
479                 if(entry == UNCLASSIFIED || entry == UNKNOWN_OBSTACLE) 
480                 {
481                     entry = type;
482                     if(type == TRAVERSABLE)
483                     {           
484                         vfh_star::ElevationEntry &entry(laserGrid.getEntry(p_g));
485
486                         if(!entry.getMeasurementCount())
487                         {
488                             entry.addHeightMeasurement(p_w.z() - vecToGround.z());
489                         }
490                     }
491                 }
492             }
493             else 
494             {
495                 std::cout << "Error point not in grid " << std::endl;
496             }
497         }
498     }
499 }
500
501 void TraversabilityMapGenerator::markUnknownInRectangeAsObstacle(const base::Pose& pose, double width, double height, double forwardOffset)
502 {
503     markUnknownInRectangeAs(pose, width, height, forwardOffset, OBSTACLE);
504 }
505
506 void TraversabilityMapGenerator::markUnknownInRectangeAsTraversable(const base::Pose& pose, double width, double height, double forwardOffset)
507 {
508     markUnknownInRectangeAs(pose, width, height, forwardOffset, TRAVERSABLE);
509 }
510
511 void TraversabilityMapGenerator::markUnknownInRadiusAsTraversable(const base::Pose& pose, double radius)
512 {
513     markUnknownInRadiusAs(pose, radius, TRAVERSABLE);
514 }
515
516 void TraversabilityMapGenerator::markUnknownInRadiusAsObstacle(const base::Pose& pose, double radius)
517 {
518     markUnknownInRadiusAs(pose, radius, OBSTACLE);
519 }
520
521 void TraversabilityMapGenerator::doConservativeInterpolation(const vfh_star::ElevationGrid& source, vfh_star::ElevationGrid& target, Eigen::Vector2i p) const {
522     if(!source.inGrid(p))
523         return;
524
525     target.getEntry(p) = source.getEntry(p);
526
527     //point has a measurement
528     if(source.getEntry(p).getMeasurementCount()) {
529         return;
530     }
531
532     //two possible valid cases for interpolation:
533     // XXX    XOX
534     // OOO or XOX
535     // XXX    XOX
536
537     bool firstFound = false;
538     bool secondFound = false;
539
540     for(int x = -1; x <= 1; x++) {
541         int rx = p.x() + x;
542         int ry = p.y() -1;
543         if(source.inGrid(rx, ry)) {
544             firstFound |= source.getEntry(rx, ry).getMeasurementCount();
545         }
546
547         ry = p.y() + 1;
548         if(source.inGrid(rx, ry)) {
549             secondFound |= source.getEntry(rx, ry).getMeasurementCount();
550         }
551     }
552     
553     if(!firstFound || !secondFound) {
554         firstFound = false;
555         secondFound = false;
556         for(int y = -1; y <= 1; y++) {
557             int rx = p.x() - 1;
558             int ry = p.y() + y;
559             if(source.inGrid(rx, ry)) {
560                 firstFound |= source.getEntry(rx, ry).getMeasurementCount();
561             }
562
563             ry = p.x() + 1;
564             if(source.inGrid(rx, ry)) {
565                 secondFound |= source.getEntry(rx, ry).getMeasurementCount();
566             }   
567         }
568     }
569
570     if(firstFound && secondFound) {
571 //      std::cout << "Starting interpolation" << std::endl;
572         for(int x = -1; x <= 1; x++) {
573             for(int y = -1; y <= 1; y++) {
574                 int rx = p.x() + x;
575                 int ry = p.y() + y;
576                 if(source.inGrid(rx, ry)) {
577                     if(source.getEntry(rx, ry).getMeasurementCount()) {
578                         target.getEntry(p).addHeightMeasurement(source.getEntry(rx, ry).getMedian());
579 //                      std::cout << "Adding height " << source.getEntry(rx, ry).getMedian() << std::endl;
580                     }
581                 }
582             }
583         }
584         target.getEntry(p).setInterpolatedMeasurement(target.getEntry(p).getMedian());
585 //      std::cout << "Resulting height " << target.getEntry(p).getMedian() << std::endl;
586     }
587 }
588
589 bool TraversabilityMapGenerator::getMeanHeightOfNeighbourhood(const vfh_star::ElevationGrid& grid, Eigen::Vector2i p, double &meanHeight) const
590 {
591     double height = 0;
592     int neighbourCnt = 0;
593     bool gotValue = false;
594     
595     for(int x = -1; x <= 1; x++) {
596         for(int y = -1; y <= 1; y++) {
597             int rx = p.x() + x;
598             int ry = p.y() + y;
599             if(grid.inGrid(rx, ry)) {
600                 const ElevationEntry &neighbourEntry = grid.getEntry(rx, ry);
601                 
602                 double neighbourHeight;
603                 
604                 if(neighbourEntry.getMeasurementCount()) 
605                 {
606                     //use real measurement if available
607                     neighbourHeight = neighbourEntry.getMedian();
608                 }
609                 else
610                 {
611                     if(neighbourEntry.getMaximum() == -std::numeric_limits<double>::max())
612                         continue;
613                     
614                     neighbourHeight = neighbourEntry.getMinimum();
615                 }
616                 
617                 neighbourCnt++;
618                 height += neighbourHeight;
619                 
620                 gotValue = true;
621             }
622         }
623     }
624     
625     meanHeight = height / neighbourCnt;
626     
627     return gotValue;
628 }
629
630
631 void TraversabilityMapGenerator::computeSmoothElevelationGrid(const vfh_star::ElevationGrid& source, vfh_star::Grid< double, 600, 12 >& target) const
632 {
633     target.setGridPosition(source.getGridPosition());
634     
635     for(int x = 0;x < source.getWidth(); x++) {
636         for(int y = 0;y < source.getHeight(); y++) {
637             double mean = std::numeric_limits< double >::quiet_NaN();
638             getMeanHeightOfNeighbourhood(source, Eigen::Vector2i(x, y), mean);
639             target.getEntry(x, y) = mean;;
640         }
641     }
642 }
643
644
645 void TraversabilityMapGenerator::smoothElevationGrid(const ElevationGrid& source, ElevationGrid& target) const
646 {
647     target.setGridPosition(source.getGridPosition());
648     
649     for(int x = 0;x < source.getWidth(); x++) {
650         for(int y = 0;y < source.getHeight(); y++) {
651             doConservativeInterpolation(source, target, Eigen::Vector2i(x, y));
652         }
653     } 
654 }
655
656
657
658
659 }