CObservation2DRangeScanWithUncertainty.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, 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::math::square(otherScan.stdError);
        const size_t N = rangesMean.size();

        const double max_var = mrpt::math::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::utils::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;
}