CObservation2DRangeScanWithUncertainty.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#include "obs-precomp.h"  // Precompiled headers
 
#include <mrpt/obs/CObservation2DRangeScanWithUncertainty.h>
#include <mrpt/math/distributions.h>
 
using namespace mrpt::obs;
 
CObservation2DRangeScanWithUncertainty::TEvalParams::TEvalParams()
    : prob_outliers(0.5),
      prob_lost_ray(0.3),
      max_prediction_std_dev(1.0),
      min_ray_log_lik(-20.0)
{
}
 
double CObservation2DRangeScanWithUncertainty::evaluateScanLikelihood(
    const CObservation2DRangeScan& otherScan, const TEvalParams& params) const
{
    ASSERT_EQUAL_((int)otherScan.scan.size(), (int)otherScan.validRange.size());
    ASSERT_EQUAL_((int)otherScan.scan.size(), (int)this->rangesMean.size());
    ASSERT_EQUAL_((int)otherScan.scan.size(), (int)this->rangesCovar.rows());
    ASSERT_EQUAL_((int)otherScan.scan.size(), (int)this->rangesCovar.cols());
    ASSERT_(params.prob_outliers >= 0.0 && params.prob_outliers <= 1.0);
    ASSERT_(otherScan.maxRange > 0.0);
 
    const double sensorRangeVar = mrpt::square(otherScan.stdError);
    const size_t N = rangesMean.size();
 
    const double max_var = mrpt::square(params.max_prediction_std_dev);
    double lik_sum = .0;
    size_t num_valid = 0;
    for (size_t i = 0; i < N; i++)
    {
        const double prediction_total_var = rangesCovar(i, i) + sensorRangeVar;
 
        if (prediction_total_var > max_var)
        {
            continue;
        }
        num_valid++;
 
        const double otherScanRange =
            otherScan.validRange[i] ? otherScan.scan[i] : otherScan.maxRange;
 
        const double likGauss = std::exp(-0.5*mrpt::square(
            otherScanRange - rangesMean[i]) / prediction_total_var);
        double pi;
        if (otherScan.scan[i] > rangesMean[i])
        {
            if (otherScan.validRange[i])
                pi = likGauss;
            else
                pi = std::max(likGauss, params.prob_lost_ray);
        }
        else pi = std::max( likGauss, std::min(1.0, params.prob_outliers) );
 
        double lpi = std::max(params.min_ray_log_lik, log(pi));
        lik_sum += lpi;
    }
    if (num_valid) lik_sum /= num_valid;
    lik_sum = exp(lik_sum);
 
    return lik_sum;
}