MRPT  1.9.9
CObservation2DRangeScanWithUncertainty.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
14 
15 using namespace mrpt::obs;
16 
18 
19  = default;
20 
22  const CObservation2DRangeScan& otherScan, const TEvalParams& params) const
23 {
25  otherScan.getScanSize(), static_cast<size_t>(rangesMean.size()));
27  otherScan.getScanSize(), static_cast<size_t>(rangesCovar.rows()));
29  ASSERT_(params.prob_outliers >= 0.0 && params.prob_outliers <= 1.0);
30  ASSERT_(otherScan.maxRange > 0.0f);
31 
32  const double sensorRangeVar = mrpt::square(otherScan.stdError);
33  const size_t N = rangesMean.size();
34 
35  const double max_var = mrpt::square(params.max_prediction_std_dev);
36  double lik_sum = .0;
37  size_t num_valid = 0;
38  for (size_t i = 0; i < N; i++)
39  {
40  const double prediction_total_var = rangesCovar(i, i) + sensorRangeVar;
41 
42  if (prediction_total_var > max_var)
43  {
44  continue;
45  }
46  num_valid++;
47 
48  const double otherScanRange = otherScan.getScanRangeValidity(i)
49  ? otherScan.getScanRange(i)
50  : otherScan.maxRange;
51 
52  const double likGauss = std::exp(
53  -0.5 * mrpt::square(otherScanRange - rangesMean[i]) /
54  prediction_total_var);
55  double pi;
56  if (otherScan.getScanRange(i) > rangesMean[i])
57  {
58  if (otherScan.getScanRangeValidity(i))
59  pi = likGauss;
60  else
61  pi = std::max(likGauss, params.prob_lost_ray);
62  }
63  else
64  pi = std::max(likGauss, std::min(1.0, params.prob_outliers));
65 
66  double lpi = std::max(params.min_ray_log_lik, log(pi));
67  lik_sum += lpi;
68  }
69  if (num_valid) lik_sum /= num_valid;
70  lik_sum = exp(lik_sum);
71 
72  return lik_sum;
73 }
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
size_t getScanSize() const
Get number of scan rays.
float maxRange
The maximum range allowed by the device, in meters (e.g.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::math::CVectorDouble rangesMean
The same ranges than in rangeScan.getScanRange(), for convenience as a math vector container...
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
T square(const T x)
Inline function for the square of a number.
This namespace contains representation of robot actions and observations.
double evaluateScanLikelihood(const CObservation2DRangeScan &otherScan, const TEvalParams &params) const
Returns a measure of the likelihood of a given scan, compared to this scan variances.
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
mrpt::math::CMatrixDouble rangesCovar
The covariance matrix for all the ranges in rangeScan.getScanRange()
GLenum const GLfloat * params
Definition: glext.h:3538
bool getScanRangeValidity(const size_t i) const
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 479715d5b Tue Nov 12 07:26:21 2019 +0100 at mar nov 12 07:30:12 CET 2019