/*---------------------------------------------------------------
auxiliarComputeObservationLikelihood
---------------------------------------------------------------*/
double CPosePDFParticlesExtended::auxiliarComputeObservationLikelihood(
const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
const CParticleFilterCapable *obj,
size_t particleIndexForMap,
const CSensoryFrame *observation,
const TExtendedCPose2D *x )
{
double ret = 1;
CMetricMap *map; // The map:
const CPosePDFParticlesExtended *pdf = static_cast<const CPosePDFParticlesExtended*> (obj);
if (pdf->options.metricMap)
map = pdf->options.metricMap;
else
{
ASSERT_( pdf->options.metricMaps.size()>particleIndexForMap );
map = pdf->options.metricMaps[particleIndexForMap]; //->m_gridMaps[0];
}
// For each observation:
for (CSensoryFrame::const_iterator it=observation->begin();it!=observation->end();++it)
{
const CObservation *obser = it->pointer();
CObservationBeaconRanges obserDumm;
// JLBC: 20/ABR/2007 -> UWB offset from extended state vector
if (obser->GetRuntimeClass() == CLASS_ID(CObservationBeaconRanges) )
{
CObservationBeaconRanges *obs = (CObservationBeaconRanges*) obser;
obserDumm = *obs;
// Introduce bias:
ASSERT_( obserDumm.sensedData.size() == x->state.size() );
for (size_t k=0;k<size_t(obserDumm.sensedData.size());k++)
obserDumm.sensedData[k].sensedDistance -= x->state[k];
// Substitute:
obser = &obserDumm;
}
ret += map->computeObservationLikelihood( obser, x->pose ); // Compute the likelihood:
}
//cout << x->pose << format(": lik = %f",ret) << endl;
// Done!
return ret;
}
/*---------------------------------------------------------------
PF_SLAM_computeObservationLikelihoodForParticle
---------------------------------------------------------------*/
double CMonteCarloLocalization2D::PF_SLAM_computeObservationLikelihoodForParticle(
const CParticleFilter::TParticleFilterOptions &PF_options,
const size_t particleIndexForMap,
const CSensoryFrame &observation,
const CPose3D &x ) const
{
ASSERT_( options.metricMap || particleIndexForMap<options.metricMaps.size() )
CMetricMap *map = (options.metricMap) ?
options.metricMap : // All particles, one map
options.metricMaps[particleIndexForMap]; // One map per particle
// For each observation:
double ret = 1;
for (CSensoryFrame::const_iterator it=observation.begin();it!=observation.end();++it)
ret += map->computeObservationLikelihood( it->pointer(), x ); // Compute the likelihood:
// Done!
return ret;
}
CSetOfObjects::Ptr CMetricMap_getAs3DObject(CMetricMap& self)
{
CSetOfObjects::Ptr outObj = mrpt::make_aligned_shared<CSetOfObjects>();
self.getAs3DObject(outObj);
return outObj;
}
bool CMetricMap_insertObservationPtr(
CMetricMap& self, const CObservation::Ptr& obs,
const CPose3D& robotPose = CPose3D())
{
return self.insertObservationPtr(obs, &robotPose);
}
