37 for (
size_t i = 0; i < N; i++)
39 const double prediction_total_var =
rangesCovar(i, i) + sensorRangeVar;
41 if (prediction_total_var > max_var)
47 const double otherScanRange =
50 const double likGauss = std::exp(
52 prediction_total_var);
59 pi = std::max(likGauss,
params.prob_lost_ray);
64 double lpi = std::max(
params.min_ray_log_lik, log(pi));
67 if (num_valid) lik_sum /= num_valid;
68 lik_sum = exp(lik_sum);
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'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.
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.
This namespace contains representation of robot actions and observations.
double evaluateScanLikelihood(const CObservation2DRangeScan &otherScan, const TEvalParams ¶ms) 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