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 }
mrpt::obs::CObservation2DRangeScan::maxRange
float maxRange
The maximum range allowed by the device, in meters (e.g.
Definition: CObservation2DRangeScan.h:130
ASSERT_EQUAL_
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:56
CObservation2DRangeScanWithUncertainty.h
obs-precomp.h
mrpt::obs::CObservation2DRangeScanWithUncertainty::rangesCovar
Eigen::MatrixXd rangesCovar
The covariance matrix for all the ranges in rangeScan.scan[].
Definition: CObservation2DRangeScanWithUncertainty.h:31
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::obs::CObservation2DRangeScanWithUncertainty::TEvalParams
Definition: CObservation2DRangeScanWithUncertainty.h:33
mrpt::square
T square(const T x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:18
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::obs::CObservation2DRangeScanWithUncertainty::rangesMean
Eigen::VectorXd rangesMean
The same ranges than in rangeScan.scan[], for convenience as an Eigen container, and with double prec...
Definition: CObservation2DRangeScanWithUncertainty.h:29
mrpt::obs::CObservation2DRangeScanWithUncertainty::evaluateScanLikelihood
double evaluateScanLikelihood(const CObservation2DRangeScan &otherScan, const TEvalParams &params) const
Returns a measure of the likelihood of a given scan, compared to this scan variances.
Definition: CObservation2DRangeScanWithUncertainty.cpp:25
mrpt::containers::ContainerReadOnlyProxyAccessor::size
size_t size() const
Definition: ContainerReadOnlyProxyAccessor.h:42
mrpt::obs::CObservation2DRangeScanWithUncertainty::TEvalParams::TEvalParams
TEvalParams()
Definition: CObservation2DRangeScanWithUncertainty.cpp:17
distributions.h
mrpt::obs::CObservation2DRangeScan::stdError
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
Definition: CObservation2DRangeScan.h:136
mrpt::obs::CObservation2DRangeScan::scan
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
Definition: CObservation2DRangeScan.h:103
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
mrpt::obs::CObservation2DRangeScan::validRange
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< char > > validRange
It's false (=0) on no reflected rays, referenced to elements in scan.
Definition: CObservation2DRangeScan.h:119
params
GLenum const GLfloat * params
Definition: glext.h:3534



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST