Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 {
27  ASSERT_EQUAL_( (int)otherScan.scan.size(),(int)otherScan.validRange.size() );
28  ASSERT_EQUAL_( (int)otherScan.scan.size(), (int)this->rangesMean.size() );
29  ASSERT_EQUAL_( (int)otherScan.scan.size(), (int)this->rangesCovar.rows() );
30  ASSERT_EQUAL_( (int)otherScan.scan.size(), (int)this->rangesCovar.cols() );
31  ASSERT_(params.prob_outliers>=0.0 && params.prob_outliers<=1.0)
32  ASSERT_(otherScan.maxRange>0.0)
33 
34  const double sensorRangeVar = mrpt::math::square(otherScan.stdError);
35  const size_t N = rangesMean.size();
36 
37  const double max_var = mrpt::math::square(params.max_prediction_std_dev);
38  double lik_sum = .0;
39  size_t num_valid = 0;
40  for (size_t i=0;i<N;i++)
41  {
42  const double prediction_total_var = rangesCovar(i,i)+sensorRangeVar;
43 
44  if (prediction_total_var > max_var) {
45  continue;
46  }
47  num_valid++;
48 
49  const double otherScanRange = otherScan.validRange[i] ? otherScan.scan[i] : otherScan.maxRange;
50 
51  const double likGauss = std::exp(-0.5*mrpt::utils::square(otherScanRange - rangesMean[i]) / prediction_total_var);
52  double pi;
53  if (otherScan.scan[i] > rangesMean[i]) {
54  if (otherScan.validRange[i])
55  pi = likGauss;
56  else pi = std::max(likGauss, params.prob_lost_ray);
57  }
58  else {
59  pi = std::max( likGauss, std::min(1.0, params.prob_outliers) );
60  }
61 
62  double lpi = std::max(params.min_ray_log_lik, log(pi));
63  lik_sum += lpi;
64  }
65  if (num_valid) lik_sum/=num_valid;
66  lik_sum = exp(lik_sum);
67 
68  return lik_sum;
69 }
70 
#define ASSERT_EQUAL_(__A, __B)
#define min(a, b)
Eigen::VectorXd rangesMean
The same ranges than in rangeScan.scan[], for convenience as an Eigen container, and with double prec...
double prob_lost_ray
(Default: 0.3) Conditional probability: how many of the "no return" ranges come from a failure to det...
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
double evaluateScanLikelihood(const CObservation2DRangeScan &otherScan, const TEvalParams &params) const
Returns a measure of the likelihood of a given scan, compared to this scan variances.
double prob_outliers
(Default: 0.5) Probability of having an outlier (dynamic obstacles, not mapped) in each scan ray...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
float maxRange
The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< char > > validRange
It's false (=0) on no reflected rays, referenced to elements in scan.
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
double max_prediction_std_dev
(Default: 1.0m) Maximum std deviation of overall uncertainty for a range prediction to be considered ...
T square(const T x)
Inline function for the square of a number.
Eigen::MatrixXd rangesCovar
The covariance matrix for all the ranges in rangeScan.scan[].
#define ASSERT_(f)
GLfloat * params
Definition: glew.h:1436
double min_ray_log_lik
(Default: -20) Minimum log-likelihood of a single ray
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters. Must have same length than validRange.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018