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 {
24  ASSERT_EQUAL_((int)otherScan.scan.size(), (int)otherScan.validRange.size());
25  ASSERT_EQUAL_((int)otherScan.scan.size(), (int)this->rangesMean.size());
26  ASSERT_EQUAL_((int)otherScan.scan.size(), (int)this->rangesCovar.rows());
27  ASSERT_EQUAL_((int)otherScan.scan.size(), (int)this->rangesCovar.cols());
28  ASSERT_(params.prob_outliers >= 0.0 && params.prob_outliers <= 1.0);
29  ASSERT_(otherScan.maxRange > 0.0);
30 
31  const double sensorRangeVar = mrpt::square(otherScan.stdError);
32  const size_t N = rangesMean.size();
33 
34  const double max_var = mrpt::square(params.max_prediction_std_dev);
35  double lik_sum = .0;
36  size_t num_valid = 0;
37  for (size_t i = 0; i < N; i++)
38  {
39  const double prediction_total_var = rangesCovar(i, i) + sensorRangeVar;
40 
41  if (prediction_total_var > max_var)
42  {
43  continue;
44  }
45  num_valid++;
46 
47  const double otherScanRange =
48  otherScan.validRange[i] ? otherScan.scan[i] : otherScan.maxRange;
49 
50  const double likGauss = std::exp(
51  -0.5 * mrpt::square(otherScanRange - rangesMean[i]) /
52  prediction_total_var);
53  double pi;
54  if (otherScan.scan[i] > rangesMean[i])
55  {
56  if (otherScan.validRange[i])
57  pi = likGauss;
58  else
59  pi = std::max(likGauss, params.prob_lost_ray);
60  }
61  else
62  pi = std::max(likGauss, std::min(1.0, params.prob_outliers));
63 
64  double lpi = std::max(params.min_ray_log_lik, log(pi));
65  lik_sum += lpi;
66  }
67  if (num_valid) lik_sum /= num_valid;
68  lik_sum = exp(lik_sum);
69 
70  return lik_sum;
71 }
#define min(a, b)
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...
double prob_outliers
(Default: 0.5) Probability of having an outlier (dynamic obstacles, not mapped) in each scan ray...
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< char > > validRange
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.
float maxRange
The maximum range allowed by the device, in meters (e.g.
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::math::CVectorDouble rangesMean
The same ranges than in rangeScan.scan[], for convenience as a math vector container, and with double precision.
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
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...
mrpt::math::CMatrixDouble rangesCovar
The covariance matrix for all the ranges in rangeScan.scan[].
GLenum const GLfloat * params
Definition: glext.h:3538



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019