iterator getPatchByZ(const SurfacePatch& patch, double sigma_threshold = 3.0, bool ignore_negative = true)
{
iterator it = begin();
for (;it != end(); ++it)
{
SurfacePatch &p(*it);
const double interval = sqrt(sq(patch.getStdev()) + sq(p.getStdev())) * sigma_threshold;
if( p.distance( patch ) < interval && (!ignore_negative || !p.isNegative()) )
{
return it;
}
}
return it;
}
It getPatchByZ(It it, It end, const SurfacePatch& patch, double sigma_threshold = 3.0, bool ignore_negative = true) const
{
It found_patch = end;
float nearest_distance = std::numeric_limits<float>::max();
// TODO take advantage of the fact that patches are sorted
for (;it != end; ++it)
{
const SurfacePatch &p(*it);
const double interval = sqrt(sq(patch.getStdev()) + sq(p.getStdev())) * sigma_threshold;
const float distance = p.distance( patch );
if( distance < interval && distance < nearest_distance && (!ignore_negative || !p.isNegative()) )
{
nearest_distance = distance;
found_patch = it;
}
}
return found_patch;
}
void TraversabilityGrassfire::setTraversability(size_t x, size_t y)
{
bool debug = false;
totalCnt++;
SurfacePatch *currentPatch = bestPatchMap[y][x];
if(!currentPatch)
{
(*trData)[y][x] = UNKNOWN;
return;
}
numeric::PlaneFitting<double> fitter;
int count = 0;
double thisHeight = currentPatch->getMean() + currentPatch->getStdev();
const double scaleX =mlsGrid->getScaleX();
const double scaleY = mlsGrid->getScaleY();
if(debug)
{
std::cout << "x " << x << " y " << y << " height " << thisHeight << " mean " << currentPatch->getMean() << " stdev " << currentPatch->getStdev() << std::endl;
}
for(int yi = -1; yi <= 1; yi++)
{
for(int xi = -1; xi <= 1; xi++)
{
if(yi == 0 && xi == 0)
continue;
size_t newX = x + xi;
size_t newY = y + yi;
if(newX < mlsGrid->getCellSizeX() && newY < mlsGrid->getCellSizeY())
{
SurfacePatch *neighbourPatch = bestPatchMap[newY][newX];
if(neighbourPatch)
{
count++;
double neighbourHeight = neighbourPatch->getMean() + neighbourPatch->getStdev();
if(debug)
{
std::cout << "Nx " << newX << " Ny " << newY << " height " << neighbourHeight << " mean " << neighbourPatch->getMean() << " stdev " << neighbourPatch->getStdev() << std::endl;
}
if(fabs(neighbourHeight - thisHeight) > config.maxStepHeight)
{
if(debug)
{
std::cout << "Step do hight" << std::endl;
}
stepTooHigh++;
(*trData)[y][x] = OBSTACLE;
return;
}
Eigen::Vector3d input(xi * scaleX, yi * scaleY, thisHeight - neighbourHeight);
if(debug)
{
std::cout << "Input to plane fitter " << input.transpose() << std::endl;
}
fitter.update(input);
}
else
{
if(debug)
{
std::cout << "Nx " << newX << " Ny " << newY << " is unknown " << std::endl;
}
}
}
}
}
fitter.update(Eigen::Vector3d(0,0,0));
if (count < 5)
{
if(debug)
{
std::cout << "count to small " << count << "Setting patch to unknown " << std::endl;
}
(*trData)[y][x] = UNKNOWN;
return;
}
Eigen::Vector3d fit(fitter.getCoeffs());
const double divider = sqrt(fit.x() * fit.x() + fit.y() * fit.y() + 1);
double slope = acos(1 / divider);
if(debug)
{
std::cout << "slope is " << slope << std::endl;
}
if(slope > config.maxSlope)
{
if(debug)
{
std::cout << "slope is to hight " << std::endl;
}
slopeTooHigh++;
(*trData)[y][x] = OBSTACLE;
return;
}
double drivability = 1.0 - (slope / config.maxSlope);
//-0.00001 to get rid of precision problems...
(*trData)[y][x] = OBSTACLE + ceil((drivability - 0.00001) * config.numTraversabilityClasses);
if(debug)
{
std::cout << "Setting tr class " << ceil((drivability - 0.00001) * config.numTraversabilityClasses) << std::endl;
}
drivable++;
}
SurfacePatch* TraversabilityGrassfire::getNearestPatchWhereRobotFits(size_t x, size_t y, double height, bool &isObstacle)
{
SurfacePatch *bestMatchingPatch = NULL;
double minDistance = std::numeric_limits< double >::max();
isObstacle = true;
MLSGrid::iterator it = mlsGrid->beginCell(x, y);
MLSGrid::iterator itEnd = mlsGrid->endCell();
for(; it != itEnd; it++)
{
//HACK filter outliers
if(it->getMeasurementCount() < config.outliertFilterMinMeasurements && it->getStdev() > config.outliertFilterMaxStdDev)
{
continue;
}
double curFloorHeight = it->getMean() + it->getStdev();
//we need to check for patches blocking our way from the current height to this coorindate
double curPatchTop = it->getMean() + it->getStdev();
double curPatchBottom = it->getMean() - it->getStdev();
if(curPatchBottom > (height + config.robotHeight + config.maxStepHeight))
{
//robot can never reach this patch, so it is not part of the drive plane
//discard it
continue;
}
//check if patch blocks the robot 'at the top'
if(curPatchBottom < height + config.robotHeight && curPatchTop > height + config.robotHeight)
{
//found a patch that is blocking the robot
bestMatchingPatch = &(*it);
return bestMatchingPatch;
}
//check if patch blocks the robot 'at the ground'
if(curPatchBottom < height + config.maxStepHeight && curPatchTop > height + config.maxStepHeight)
{
//found a patch that is blocking the robot
bestMatchingPatch = &(*it);
return bestMatchingPatch;
}
double curDist = fabs(curFloorHeight - height);
if(curDist < minDistance)
{
minDistance = curDist;
bestMatchingPatch =&(*it);
}
}
if(!bestMatchingPatch)
return bestMatchingPatch;
double curFloorHeight;
bool gapTooSmall = true;
while(gapTooSmall)
{
//now we need to check if there is a blocking patch above this matching patch
MLSGrid::iterator hcIt= mlsGrid->beginCell(x, y);
MLSGrid::iterator hcItEnd = mlsGrid->endCell();
gapTooSmall = false;
curFloorHeight = bestMatchingPatch->getMean() + bestMatchingPatch->getStdev();
//check if selected patch is allready an obstacle
if(curFloorHeight - height > config.maxStepHeight)
return bestMatchingPatch;
for(; hcIt != hcItEnd; hcIt++)
{
//HACK filter outliers
if(hcIt->getMeasurementCount() < config.outliertFilterMinMeasurements && hcIt->getStdev() > config.outliertFilterMaxStdDev)
{
continue;
}
//check if the robot can pass between this and the other patches
double otherCeilingHeight = hcIt->getMean() - hcIt->getStdev();
if((curFloorHeight < otherCeilingHeight) && otherCeilingHeight - curFloorHeight < config.robotHeight)
{
bestMatchingPatch = &(*hcIt);
gapTooSmall = true;
break;
}
}
}
isObstacle = false;
return bestMatchingPatch;
}
bool TraversabilityGrassfire::determineDrivePlane(base::Vector3d startPos, bool searchSourunding)
{
size_t startX;
size_t startY;
if(!mlsGrid->toGrid(startPos, startX, startY, mlsGrid->getEnvironment()->getRootNode()))
return false;
//make shure temp maps have correct size
bestPatchMap.resize(boost::extents[mlsGrid->getCellSizeY()][mlsGrid->getCellSizeX()]);
visited.resize(boost::extents[mlsGrid->getCellSizeY()][mlsGrid->getCellSizeX()]);
trData->resize(boost::extents[mlsGrid->getCellSizeY()][mlsGrid->getCellSizeX()]);
//fill them with defautl values
SurfacePatch *emptyPatch = NULL;
//Note passing directly NULL to fill makes the compiler cry....
std::fill(bestPatchMap.data(), bestPatchMap.data() + bestPatchMap.num_elements(), emptyPatch);
std::fill(visited.data(), visited.data() + visited.num_elements(), false);
std::fill(trData->data(), trData->data() + trData->num_elements(), UNKNOWN);
//init probability with zero
TraversabilityGrid::ArrayType *probabilityArray = &(trGrid->getGridData(TraversabilityGrid::PROBABILITY));
std::fill(probabilityArray->data(), probabilityArray->data() + probabilityArray->num_elements(), 0);
double bestHeightDiff = std::numeric_limits< double >::max();
SurfacePatch *bestMatchingPatch = NULL;
size_t correctedStartX = startX;
size_t correctedStartY = startY;
//search the sourounding of the start pos for a start patch
for(int i = 0; i < 10; i++)
{
for(int yi = -i; yi <= i; yi++)
{
for(int xi = -i; xi <= i; xi++)
{
//only check the 'outer rim'
if(abs(yi) != i && abs(xi) != i)
continue;
size_t newX = startX + xi;
size_t newY = startY + yi;
if(newX < mlsGrid->getCellSizeX() && newY < mlsGrid->getCellSizeY())
{
bool isObstacle;
//look for patch with best height
SurfacePatch *curPatch = getNearestPatchWhereRobotFits(newX, newY, startPos.z(), isObstacle);
if(curPatch)
{
double curHeightDiff = fabs(startPos.z() - curPatch->getMean() + curPatch->getStdev());
if(curHeightDiff < bestHeightDiff)
{
bestMatchingPatch = curPatch;
bestHeightDiff = curHeightDiff;
correctedStartX = newX;
correctedStartY = newY;
}
}
}
}
}
if(bestMatchingPatch)
break;
}
if(!bestMatchingPatch)
{
//we are screwed, can't start the grassfire
return false;
}
//recuse to surounding patches
addNeightboursToSearchList(correctedStartX, correctedStartY, bestMatchingPatch);
return true;
}