Main MRPT website > C++ reference for MRPT 1.9.9
CObservation2DRangeScanWithUncertainty.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
14 
15 using namespace mrpt::obs;
16 
18  : prob_outliers(0.5),
19  prob_lost_ray(0.3),
20  max_prediction_std_dev(1.0),
21  min_ray_log_lik(-20.0)
22 {
23 }
24 
26  const CObservation2DRangeScan& otherScan, const TEvalParams& params) const
27 {
28  ASSERT_EQUAL_((int)otherScan.scan.size(), (int)otherScan.validRange.size());
29  ASSERT_EQUAL_((int)otherScan.scan.size(), (int)this->rangesMean.size());
30  ASSERT_EQUAL_((int)otherScan.scan.size(), (int)this->rangesCovar.rows());
31  ASSERT_EQUAL_((int)otherScan.scan.size(), (int)this->rangesCovar.cols());
32  ASSERT_(params.prob_outliers >= 0.0 && params.prob_outliers <= 1.0);
33  ASSERT_(otherScan.maxRange > 0.0);
34 
35  const double sensorRangeVar = mrpt::square(otherScan.stdError);
36  const size_t N = rangesMean.size();
37 
38  const double max_var = mrpt::square(params.max_prediction_std_dev);
39  double lik_sum = .0;
40  size_t num_valid = 0;
41  for (size_t i = 0; i < N; i++)
42  {
43  const double prediction_total_var = rangesCovar(i, i) + sensorRangeVar;
44 
45  if (prediction_total_var > max_var)
46  {
47  continue;
48  }
49  num_valid++;
50 
51  const double otherScanRange =
52  otherScan.validRange[i] ? otherScan.scan[i] : otherScan.maxRange;
53 
54  const double likGauss = std::exp(-0.5*mrpt::square(
55  otherScanRange - rangesMean[i]) / prediction_total_var);
56  double pi;
57  if (otherScan.scan[i] > rangesMean[i])
58  {
59  if (otherScan.validRange[i])
60  pi = likGauss;
61  else
62  pi = std::max(likGauss, params.prob_lost_ray);
63  }
64  else 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 }
#define min(a, b)
Eigen::VectorXd rangesMean
The same ranges than in rangeScan.scan[], for convenience as an Eigen container, and with double prec...
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:113
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:153
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.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Eigen::MatrixXd rangesCovar
The covariance matrix for all the ranges in rangeScan.scan[].
GLenum const GLfloat * params
Definition: glext.h:3534



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019