Main MRPT website > C++ reference for MRPT 1.5.7
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...
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...
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,...)
This namespace contains representation of robot actions and observations.
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< char > > validRange
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.
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...
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)
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters. Must have same length than validRange.
GLenum const GLfloat * params
Definition: glext.h:3514



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019