/*---------------------------------------------------------------
computeObservationLikelihood_Consensus
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_Consensus(
const CObservation *obs,
const CPose2D &takenFrom )
{
double likResult = 0;
// This function depends on the observation type:
// -----------------------------------------------------
if ( obs->GetRuntimeClass() != CLASS_ID(CObservation2DRangeScan) )
{
//THROW_EXCEPTION("This method is defined for 'CObservation2DRangeScan' classes only.");
return 1e-3;
}
// Observation is a laser range scan:
// -------------------------------------------
const CObservation2DRangeScan *o = static_cast<const CObservation2DRangeScan*>( obs );
// Insert only HORIZONTAL scans, since the grid is supposed to
// be a horizontal representation of space.
if ( ! o->isPlanarScan(insertionOptions.horizontalTolerance) ) return 0.5f; // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!
// Assure we have a 2D points-map representation of the points from the scan:
const CPointsMap *compareMap = o->buildAuxPointsMap<mrpt::maps::CPointsMap>();
// Observation is a points map:
// -------------------------------------------
size_t Denom=0;
// int Acells = 1;
TPoint2D pointGlobal,pointLocal;
// Get the points buffers:
// compareMap.getPointsBuffer( n, xs, ys, zs );
const size_t n = compareMap->size();
for (size_t i=0;i<n;i+=likelihoodOptions.consensus_takeEachRange)
{
// Get the point and pass it to global coordinates:
compareMap->getPoint(i,pointLocal);
takenFrom.composePoint(pointLocal, pointGlobal);
int cx0 = x2idx( pointGlobal.x );
int cy0 = y2idx( pointGlobal.y );
likResult += 1-getCell_nocheck(cx0,cy0);
Denom++;
}
if (Denom) likResult/=Denom;
likResult = pow(likResult, static_cast<double>( likelihoodOptions.consensus_pow ) );
return log(likResult);
}
/*---------------------------------------------------------------
computeObservationLikelihood_ConsensusOWA
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_ConsensusOWA(
const CObservation *obs,
const CPose2D &takenFrom )
{
double likResult = 0;
// This function depends on the observation type:
// -----------------------------------------------------
if ( obs->GetRuntimeClass() == CLASS_ID(CObservation2DRangeScan) )
{
//THROW_EXCEPTION("This method is defined for 'CObservation2DRangeScan' classes only.");
return 1e-3;
}
// Observation is a laser range scan:
// -------------------------------------------
const CObservation2DRangeScan *o = static_cast<const CObservation2DRangeScan*>( obs );
// Insert only HORIZONTAL scans, since the grid is supposed to
// be a horizontal representation of space.
if ( ! o->isPlanarScan(insertionOptions.horizontalTolerance) ) return 0.5; // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!
// Assure we have a 2D points-map representation of the points from the scan:
CPointsMap::TInsertionOptions insOpt;
insOpt.minDistBetweenLaserPoints = -1; // ALL the laser points
const CPointsMap *compareMap = o->buildAuxPointsMap<mrpt::maps::CPointsMap>( &insOpt );
// Observation is a points map:
// -------------------------------------------
int Acells = 1;
TPoint2D pointGlobal,pointLocal;
// Get the points buffers:
const size_t n = compareMap->size();
// Store the likelihood values in this vector:
likelihoodOutputs.OWA_pairList.clear();
for (size_t i=0;i<n;i++)
{
// Get the point and pass it to global coordinates:
compareMap->getPoint(i,pointLocal);
takenFrom.composePoint(pointLocal, pointGlobal);
int cx0 = x2idx( pointGlobal.x );
int cy0 = y2idx( pointGlobal.y );
int cxMin = max(0,cx0 - Acells);
int cxMax = min(static_cast<int>(size_x)-1,cx0 + Acells);
int cyMin = max(0,cy0 - Acells);
int cyMax = min(static_cast<int>(size_y)-1,cy0 + Acells);
double lik = 0;
for (int cx=cxMin;cx<=cxMax;cx++)
for (int cy=cyMin;cy<=cyMax;cy++)
lik += 1-getCell_nocheck(cx,cy);
int nCells = (cxMax-cxMin+1)*(cyMax-cyMin+1);
ASSERT_(nCells>0);
lik/=nCells;
TPairLikelihoodIndex element;
element.first = lik;
element.second = pointGlobal;
likelihoodOutputs.OWA_pairList.push_back( element );
} // for each range point
// Sort the list of likelihood values, in descending order:
// ------------------------------------------------------------
std::sort(likelihoodOutputs.OWA_pairList.begin(),likelihoodOutputs.OWA_pairList.end());
// Cut the vector to the highest "likelihoodOutputs.OWA_length" elements:
size_t M = likelihoodOptions.OWA_weights.size();
ASSERT_( likelihoodOutputs.OWA_pairList.size()>=M );
likelihoodOutputs.OWA_pairList.resize(M);
likelihoodOutputs.OWA_individualLikValues.resize( M );
likResult = 0;
for (size_t k=0;k<M;k++)
{
likelihoodOutputs.OWA_individualLikValues[k] = likelihoodOutputs.OWA_pairList[k].first;
likResult+= likelihoodOptions.OWA_weights[k] * likelihoodOutputs.OWA_individualLikValues[k];
}
return log(likResult);
}
