1031 lines
42 KiB
C#
1031 lines
42 KiB
C#
using Microsoft.Extensions.Options;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
|
using RobotNet10.Shared.Geometry;
|
|
using RobotNet10.Shared.Localization;
|
|
using RobotNet10.Shared.Numbers;
|
|
|
|
namespace RobotNet10.RobotApp.SLAM.Cartographer.Mcl;
|
|
|
|
/// <summary>
|
|
/// MCL (Monte Carlo Localization) service. Port of xloc MCL for SetInitialPoseAsync flow.
|
|
/// Uses likelihood field measurement model (type 0), differential drive motion model, resampling.
|
|
/// </summary>
|
|
public class MclService(IOptions<CartographerConfiguration> config)
|
|
{
|
|
#region Fields and Properties
|
|
|
|
private readonly MclConfiguration _options = config.Value.Mcl;
|
|
private readonly Random _rnd = new();
|
|
|
|
// Map
|
|
private double[,]? _distMap; // distance to nearest occupied (meters)
|
|
private double _mapResolution;
|
|
private double _mapOriginX, _mapOriginY, _mapOriginYaw;
|
|
private int _mapWidth, _mapHeight;
|
|
private bool _gotMap;
|
|
|
|
// Scan
|
|
private double _angleMin, _angleMax, _angleIncrement, _rangeMin, _rangeMax;
|
|
private double[]? _ranges;
|
|
private bool _gotScan;
|
|
|
|
// Pose and particles
|
|
private MclPose2d _mclPose;
|
|
private MclPose2d _odomPose;
|
|
private List<MclParticle> _particles = [];
|
|
private double _deltaX, _deltaY, _deltaDist, _deltaYaw;
|
|
private double _deltaXSum, _deltaYSum, _deltaDistSum, _deltaYawSum, _deltaTimeSum;
|
|
|
|
// Measurement model constants (set when map is set)
|
|
private double _normConstHit, _denomHit, _pRand, _measurementModelRandom, _measurementModelInvalidScan;
|
|
|
|
// Base link to laser (for sensor pose). Simplified: use (0,0,0) if not set.
|
|
private double _baseLink2LaserX, _baseLink2LaserY, _baseLink2LaserYaw;
|
|
|
|
private bool _isInitialized = true;
|
|
/// <summary>When true, ResetParticlesDistribution uses InitialNoiseWhenPoseGiven* so particles stay near user-provided initial pose.</summary>
|
|
private bool _useTightInitialNoise;
|
|
/// <summary>When true, force resample even when motion is zero (for global localization when robot is stationary).</summary>
|
|
private bool _forceResampleForGlobalLocalization;
|
|
|
|
// Likelihood and augmented MCL (xloc: totalLikelihood_, averageLikelihood_, omegaSlow_, omegaFast_, amclRandomParticlesRate_)
|
|
private double _totalLikelihood, _averageLikelihood;
|
|
private int _maxLikelihoodParticleIdx;
|
|
private double _omegaSlow, _omegaFast, _amclRandomParticlesRate;
|
|
|
|
// Reliability / decision model (xloc: estimateReliability_, reliabilities_, maes_, reliability_)
|
|
private double[]? _reliabilities;
|
|
private double[]? _maes;
|
|
private double _reliability = 0.5;
|
|
private const double MaxResidualError = 1.0;
|
|
|
|
// GL pose sampler (xloc: glParticles_, glSampledPoses_, canUseGLSampledPoses_)
|
|
private List<MclParticle> _glParticles = [];
|
|
private List<MclPose2d> _glSampledPoses = [];
|
|
private double _glSampledPosesStamp;
|
|
private bool _canUseGLSampledPoses;
|
|
private bool _isGLSampledPosesUpdated;
|
|
private readonly List<bool> _likelihoodShiftedSteps = [];
|
|
|
|
public bool IsReady => _gotMap && _gotScan;
|
|
|
|
/// <summary>Current reliability [0,1] when EstimateReliability is true (xloc: reliability_).</summary>
|
|
public double Reliability => _reliability;
|
|
|
|
/// <summary>Total likelihood after measurement update (xloc: totalLikelihood_). Higher = better scan-map match.</summary>
|
|
public double TotalLikelihood => _totalLikelihood;
|
|
|
|
/// <summary>MAE in meters for the best particle when EstimateReliability is true (xloc: maes_[maxLikelihoodParticleIdx_]). Lower = better fit. Null if not available.</summary>
|
|
public double? MaeForBestParticle
|
|
{
|
|
get
|
|
{
|
|
if (!_options.EstimateReliability || _maes == null || _maxLikelihoodParticleIdx < 0 || _maxLikelihoodParticleIdx >= _maes.Length)
|
|
return null;
|
|
return _maes[_maxLikelihoodParticleIdx];
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Map Setup
|
|
|
|
public void SetMap(OccupancyGrid grid)
|
|
{
|
|
_mapWidth = grid.Width;
|
|
_mapHeight = grid.Height;
|
|
_mapResolution = grid.Resolution;
|
|
_mapOriginX = grid.Origin.Position.X;
|
|
_mapOriginY = grid.Origin.Position.Y;
|
|
_mapOriginYaw = grid.Origin.Orientation.ToYawRadian();
|
|
|
|
// Binary map: 0 = occupied (100), 1 = free
|
|
var binMap = new byte[_mapHeight, _mapWidth];
|
|
int occupiedCount = 0;
|
|
for (int v = 0; v < _mapHeight; v++)
|
|
{
|
|
for (int u = 0; u < _mapWidth; u++)
|
|
{
|
|
int node = v * _mapWidth + u;
|
|
int val = grid.Data[node];
|
|
binMap[v, u] = (byte)(val == 100 ? 0 : 1);
|
|
if (val == 100) occupiedCount++;
|
|
}
|
|
}
|
|
|
|
// Euclidean distance transform (Felzenszwalb-Huttenlocher)
|
|
_distMap = DistanceTransformHelper.ComputeEuclidean(binMap, _mapWidth, _mapHeight, _mapResolution);
|
|
|
|
_gotMap = true;
|
|
|
|
// Fixed parameters for measurement model (xloc MCL.cc). _rangeMax set when scan arrives.
|
|
_normConstHit = 1.0 / Math.Sqrt(2.0 * _options.VarHit * Math.PI);
|
|
_denomHit = 1.0 / (2.0 * _options.VarHit);
|
|
double rangeMaxForRand = _rangeMax > 0 ? _rangeMax : 20.0;
|
|
_pRand = 1.0 / (rangeMaxForRand / _mapResolution);
|
|
_measurementModelRandom = _options.ZRand * _pRand;
|
|
_measurementModelInvalidScan = _options.ZMax + _options.ZRand * _pRand;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Sensor Input
|
|
|
|
/// <summary>Set base_link to laser transform (xloc: baseLink2Laser_). Call before RunOneIteration when sensor is offset.</summary>
|
|
public void SetBaseLinkToLaser(double x, double y, double yawRad)
|
|
{
|
|
_baseLink2LaserX = x;
|
|
_baseLink2LaserY = y;
|
|
_baseLink2LaserYaw = yawRad;
|
|
}
|
|
|
|
/// <summary>Set global-localization sampled poses for this scan (xloc: glSampledPosesCB). Call when UseGLPoseSampler and poses available.</summary>
|
|
public void SetGLSampledPoses(IReadOnlyList<MclPose2d> poses, double stampSec)
|
|
{
|
|
_glSampledPoses = [.. poses];
|
|
_glSampledPosesStamp = stampSec;
|
|
_isGLSampledPosesUpdated = true;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Public API
|
|
|
|
public void SetInitialPose(Pose pose, bool useTightNoise = false)
|
|
{
|
|
double yaw = pose.Orientation.ToYawRadian();
|
|
_mclPose = new MclPose2d(pose.Position.X, pose.Position.Y, yaw);
|
|
_useTightInitialNoise = useTightNoise;
|
|
// Enable forced resampling for global localization (when robot is stationary and searching for initial pose)
|
|
// This ensures particles converge even without motion
|
|
_forceResampleForGlobalLocalization = !useTightNoise;
|
|
ResetParticlesDistribution();
|
|
_odomPose = new MclPose2d(0, 0, 0);
|
|
_deltaX = _deltaY = _deltaDist = _deltaYaw = 0;
|
|
_isInitialized = true;
|
|
if (_options.EstimateReliability)
|
|
ResetReliabilities();
|
|
}
|
|
|
|
public void OnScan(double angleMin, double angleMax, double angleIncrement, double rangeMin, double rangeMax, IReadOnlyList<double> ranges)
|
|
{
|
|
_angleMin = angleMin;
|
|
_angleMax = angleMax;
|
|
_angleIncrement = angleIncrement;
|
|
_rangeMin = rangeMin;
|
|
_rangeMax = rangeMax;
|
|
if (_ranges == null || _ranges.Length != ranges.Count)
|
|
_ranges = new double[ranges.Count];
|
|
for (int i = 0; i < ranges.Count; i++)
|
|
_ranges[i] = ranges[i];
|
|
_gotScan = true;
|
|
}
|
|
|
|
public void OnOdom(double deltaTimeSec, double linearX, double linearY, double angularZ)
|
|
{
|
|
if (_isInitialized) { _isInitialized = false; return; }
|
|
if (deltaTimeSec <= 0) return;
|
|
_deltaX += linearX * deltaTimeSec;
|
|
_deltaY += linearY * deltaTimeSec;
|
|
_deltaYaw += angularZ * deltaTimeSec;
|
|
double dx = linearX * deltaTimeSec;
|
|
double dy = linearY * deltaTimeSec;
|
|
_deltaDist += Math.Sqrt(dx * dx + dy * dy);
|
|
_deltaTimeSum += deltaTimeSec;
|
|
}
|
|
|
|
public Pose RunOneIteration()
|
|
{
|
|
if (!IsReady || _distMap == null || _ranges == null || _particles.Count == 0)
|
|
return GetPose();
|
|
|
|
UpdateParticlesByMotionModel();
|
|
CalculateLikelihoodsByMeasurementModel();
|
|
if (_options.EstimateReliability)
|
|
CalculateLikelihoodsByDecisionModel();
|
|
if (_options.UseGLPoseSampler && _glSampledPoses.Count > 0)
|
|
CalculateGLSampledPosesLikelihood();
|
|
if (_options.UseAugmentedMcl)
|
|
CalculateAMCLRandomParticlesRate();
|
|
EstimatePose();
|
|
ResampleParticles();
|
|
return GetPose();
|
|
}
|
|
|
|
public Pose GetPose()
|
|
{
|
|
return new Pose
|
|
{
|
|
Position = new Vector3(_mclPose.X, _mclPose.Y, 0),
|
|
Orientation = Quaternion.FromYawRadian(_mclPose.Yaw)
|
|
};
|
|
}
|
|
|
|
public void ResetRunning() { }
|
|
|
|
/// <summary>
|
|
/// Disable forced resampling for global localization mode.
|
|
/// Call this after MCL converges to switch to normal tracking mode.
|
|
/// </summary>
|
|
public void DisableGlobalLocalizationMode()
|
|
{
|
|
_forceResampleForGlobalLocalization = false;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Coordinate Helpers
|
|
|
|
private void Xy2Uv(double x, double y, out int u, out int v)
|
|
{
|
|
double dx = x - _mapOriginX;
|
|
double dy = y - _mapOriginY;
|
|
double yaw = -_mapOriginYaw;
|
|
double xx = dx * Math.Cos(yaw) - dy * Math.Sin(yaw);
|
|
double yy = dx * Math.Sin(yaw) + dy * Math.Cos(yaw);
|
|
u = (int)(xx / _mapResolution);
|
|
v = (int)(yy / _mapResolution);
|
|
}
|
|
|
|
private bool OnMap(int u, int v) => u >= 0 && u < _mapWidth && v >= 0 && v < _mapHeight;
|
|
|
|
#endregion
|
|
|
|
#region Particle Distribution
|
|
|
|
private double NRand(double sigma)
|
|
{
|
|
double u1 = _rnd.NextDouble();
|
|
if (u1 < 1e-10) u1 = 1e-10;
|
|
double u2 = _rnd.NextDouble();
|
|
return sigma * Math.Sqrt(-2.0 * Math.Log(u1)) * Math.Cos(2.0 * Math.PI * u2);
|
|
}
|
|
|
|
private void ResetParticlesDistribution()
|
|
{
|
|
int n = _options.ParticlesNum;
|
|
_particles = new List<MclParticle>(n);
|
|
double xo = _mclPose.X, yo = _mclPose.Y, yawo = _mclPose.Yaw;
|
|
double sigmaX = _useTightInitialNoise && _options.InitialNoiseWhenPoseGivenX > 0 ? _options.InitialNoiseWhenPoseGivenX : _options.InitialNoiseX;
|
|
double sigmaY = _useTightInitialNoise && _options.InitialNoiseWhenPoseGivenY > 0 ? _options.InitialNoiseWhenPoseGivenY : _options.InitialNoiseY;
|
|
double sigmaYaw = _useTightInitialNoise && _options.InitialNoiseWhenPoseGivenYaw > 0 ? _options.InitialNoiseWhenPoseGivenYaw : _options.InitialNoiseYaw;
|
|
double wo = 1.0 / n;
|
|
for (int i = 0; i < n; i++)
|
|
{
|
|
double x = xo + NRand(sigmaX);
|
|
double y = yo + NRand(sigmaY);
|
|
double yaw = yawo + NRand(sigmaYaw);
|
|
_particles.Add(new MclParticle(x, y, yaw, wo));
|
|
}
|
|
if (_options.EstimateReliability)
|
|
ResetReliabilities();
|
|
}
|
|
|
|
private void ResetReliabilities()
|
|
{
|
|
int n = _particles.Count;
|
|
_reliabilities = new double[n];
|
|
for (int i = 0; i < n; i++)
|
|
_reliabilities[i] = 0.5;
|
|
_maes = new double[n];
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Motion Model
|
|
|
|
private void UpdateParticlesByMotionModel()
|
|
{
|
|
double deltaX = _deltaX, deltaY = _deltaY, deltaDist = _deltaDist, deltaYaw = _deltaYaw;
|
|
_deltaX = _deltaY = _deltaDist = _deltaYaw = 0;
|
|
_deltaXSum += Math.Abs(deltaX);
|
|
_deltaYSum += Math.Abs(deltaY);
|
|
_deltaDistSum += Math.Abs(deltaDist);
|
|
_deltaYawSum += Math.Abs(deltaYaw);
|
|
|
|
if (!_options.UseOmniDirectionalModel)
|
|
{
|
|
// Differential drive model (xloc MCL.cc:341-368)
|
|
double yaw = _mclPose.Yaw;
|
|
double t = yaw + deltaYaw / 2.0;
|
|
double x = _mclPose.X + deltaDist * Math.Cos(t);
|
|
double y = _mclPose.Y + deltaDist * Math.Sin(t);
|
|
yaw += deltaYaw;
|
|
_mclPose = new MclPose2d(x, y, yaw);
|
|
|
|
double dist2 = deltaDist * deltaDist;
|
|
double yaw2 = deltaYaw * deltaYaw;
|
|
double distRandVal = dist2 * _options.OdomNoiseDdm[0] + yaw2 * _options.OdomNoiseDdm[1];
|
|
double yawRandVal = dist2 * _options.OdomNoiseDdm[2] + yaw2 * _options.OdomNoiseDdm[3];
|
|
|
|
double[]? relTransDdm = _options.RelTransDdm;
|
|
bool estimateReliability = _options.EstimateReliability && _reliabilities != null;
|
|
|
|
for (int i = 0; i < _particles.Count; i++)
|
|
{
|
|
var p = _particles[i];
|
|
double ddist = deltaDist + NRand(distRandVal);
|
|
double dyaw = deltaYaw + NRand(yawRandVal);
|
|
yaw = p.Pose.Yaw;
|
|
t = yaw + dyaw / 2.0;
|
|
x = p.Pose.X + ddist * Math.Cos(t);
|
|
y = p.Pose.Y + ddist * Math.Sin(t);
|
|
yaw += dyaw;
|
|
p.Pose = new MclPose2d(x, y, yaw);
|
|
p.W = 1.0 / _particles.Count;
|
|
|
|
if (estimateReliability && relTransDdm != null && relTransDdm.Length >= 2)
|
|
{
|
|
double decayRate = 1.0 - (relTransDdm[0] * ddist * ddist + relTransDdm[1] * dyaw * dyaw);
|
|
if (decayRate <= 0.0) decayRate = 1e-6;
|
|
_reliabilities![i] *= decayRate;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Omni-directional model (xloc MCL.cc:369-400)
|
|
double yaw = _mclPose.Yaw;
|
|
double t = yaw + deltaYaw / 2.0;
|
|
double x = _mclPose.X + deltaX * Math.Cos(t) + deltaY * Math.Cos(t + Math.PI / 2.0);
|
|
double y = _mclPose.Y + deltaX * Math.Sin(t) + deltaY * Math.Sin(t + Math.PI / 2.0);
|
|
yaw += deltaYaw;
|
|
_mclPose = new MclPose2d(x, y, yaw);
|
|
|
|
double x2 = deltaX * deltaX;
|
|
double y2 = deltaY * deltaY;
|
|
double yaw2 = deltaYaw * deltaYaw;
|
|
double xRandVal = x2 * _options.OdomNoiseOdm[0] + y2 * _options.OdomNoiseOdm[1] + yaw2 * _options.OdomNoiseOdm[2];
|
|
double yRandVal = x2 * _options.OdomNoiseOdm[3] + y2 * _options.OdomNoiseOdm[4] + yaw2 * _options.OdomNoiseOdm[5];
|
|
double yawRandVal = x2 * _options.OdomNoiseOdm[6] + y2 * _options.OdomNoiseOdm[7] + yaw2 * _options.OdomNoiseOdm[8];
|
|
|
|
double[]? relTransOdm = _options.RelTransOdm;
|
|
bool estimateReliability = _options.EstimateReliability && _reliabilities != null;
|
|
|
|
for (int i = 0; i < _particles.Count; i++)
|
|
{
|
|
var p = _particles[i];
|
|
double dx = deltaX + NRand(xRandVal);
|
|
double dy = deltaY + NRand(yRandVal);
|
|
double dyaw = deltaYaw + NRand(yawRandVal);
|
|
yaw = p.Pose.Yaw;
|
|
t = yaw + dyaw / 2.0;
|
|
x = p.Pose.X + dx * Math.Cos(t) + dy * Math.Cos(t + Math.PI / 2.0);
|
|
y = p.Pose.Y + dx * Math.Sin(t) + dy * Math.Sin(t + Math.PI / 2.0);
|
|
yaw += dyaw;
|
|
p.Pose = new MclPose2d(x, y, yaw);
|
|
p.W = 1.0 / _particles.Count;
|
|
|
|
if (estimateReliability && relTransOdm != null && relTransOdm.Length >= 3)
|
|
{
|
|
double decayRate = 1.0 - (relTransOdm[0] * dx * dx + relTransOdm[1] * dy * dy + relTransOdm[2] * dyaw * dyaw);
|
|
if (decayRate <= 0.0) decayRate = 1e-6;
|
|
_reliabilities![i] *= decayRate;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Unknown Scan Rejection
|
|
|
|
/// <summary>Reject beams likely "unknown" (xloc rejectUnknownScan). Minimal port: ray-cast from MCL pose, zero ranges beyond expected.</summary>
|
|
private void RejectUnknownScan()
|
|
{
|
|
if (_ranges == null || _distMap == null) return;
|
|
// _ranges is already binned (numBins elements from ConvertToScan); use every beam (step 1).
|
|
double xo = _baseLink2LaserX, yo = _baseLink2LaserY, yawo = _baseLink2LaserYaw;
|
|
double yaw = _mclPose.Yaw;
|
|
double sensorX = xo * Math.Cos(yaw) - yo * Math.Sin(yaw) + _mclPose.X;
|
|
double sensorY = xo * Math.Sin(yaw) + yo * Math.Cos(yaw) + _mclPose.Y;
|
|
double sensorYaw = yawo + yaw;
|
|
double hitThreshold = 0.5 * _mapResolution;
|
|
for (int i = 0; i < _ranges.Length; i++)
|
|
{
|
|
double r = _ranges[i];
|
|
if (r <= _rangeMin || r >= _rangeMax) continue;
|
|
double angle = i * _angleIncrement + _angleMin + sensorYaw;
|
|
double dx = _mapResolution * Math.Cos(angle);
|
|
double dy = _mapResolution * Math.Sin(angle);
|
|
double x = sensorX, y = sensorY;
|
|
double expectedRange = -1;
|
|
for (double rangeStep = 0; rangeStep <= _rangeMax; rangeStep += _mapResolution)
|
|
{
|
|
Xy2Uv(x, y, out int u, out int v);
|
|
if (!OnMap(u, v)) break;
|
|
double dist = _distMap[v, u];
|
|
if (dist < hitThreshold) { expectedRange = rangeStep; break; }
|
|
x += dx; y += dy;
|
|
}
|
|
if (expectedRange >= 0 && r > expectedRange + hitThreshold)
|
|
{
|
|
double pShort = _options.LambdaShort * Math.Exp(-_options.LambdaShort * r) / (1.0 - Math.Exp(-_options.LambdaShort * _rangeMax)) * _mapResolution;
|
|
double pBeam = _measurementModelRandom;
|
|
if (pShort / (pShort + pBeam) >= _options.UnknownScanProbThreshold)
|
|
_ranges[i] = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>Estimate unknown scan using class-conditional measurement model (xloc MCL.cc:1599-1630). For measurement model type 2.</summary>
|
|
private double[] EstimateUnknownScanWithClassConditionalMeasurementModel(MclPose2d pose)
|
|
{
|
|
if (_ranges == null || _distMap == null) return _ranges ?? [];
|
|
|
|
double[] unknownScanRanges = new double[_ranges.Length];
|
|
Array.Copy(_ranges, unknownScanRanges, _ranges.Length);
|
|
|
|
double xo = _baseLink2LaserX, yo = _baseLink2LaserY, yawo = _baseLink2LaserYaw;
|
|
double yaw = pose.Yaw;
|
|
double sensorX = xo * Math.Cos(yaw) - yo * Math.Sin(yaw) + pose.X;
|
|
double sensorY = xo * Math.Sin(yaw) + yo * Math.Cos(yaw) + pose.Y;
|
|
double sensorYaw = yawo + yaw;
|
|
|
|
for (int i = 0; i < unknownScanRanges.Length; i++)
|
|
{
|
|
double r = unknownScanRanges[i];
|
|
if (r <= _rangeMin || r >= _rangeMax)
|
|
{
|
|
unknownScanRanges[i] = 0;
|
|
continue;
|
|
}
|
|
|
|
double t = sensorYaw + i * _angleIncrement + _angleMin;
|
|
double x = r * Math.Cos(t) + sensorX;
|
|
double y = r * Math.Sin(t) + sensorY;
|
|
|
|
Xy2Uv(x, y, out int u, out int v);
|
|
double pKnown;
|
|
double pUnknown = _options.LambdaUnknown * Math.Exp(-_options.LambdaUnknown * r) / (1.0 - Math.Exp(-_options.LambdaUnknown * _rangeMax)) * _mapResolution * _options.PUnknownPrior;
|
|
|
|
if (OnMap(u, v))
|
|
{
|
|
double dist = _distMap[v, u];
|
|
double pHit = _normConstHit * Math.Exp(-(dist * dist) * _denomHit) * _mapResolution;
|
|
pKnown = (_options.ZHit * pHit + _measurementModelRandom) * _options.PKnownPrior;
|
|
}
|
|
else
|
|
{
|
|
pKnown = _measurementModelRandom * _options.PKnownPrior;
|
|
}
|
|
|
|
double sum = pKnown + pUnknown;
|
|
if (sum > 0)
|
|
pUnknown /= sum;
|
|
|
|
if (pUnknown < _options.UnknownScanProbThreshold)
|
|
unknownScanRanges[i] = 0;
|
|
}
|
|
|
|
return unknownScanRanges;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Measurement Models
|
|
|
|
private double CalculateLikelihoodFieldModel(MclPose2d pose, double range, double rangeAngle)
|
|
{
|
|
if (range <= _rangeMin || range >= _rangeMax) return _measurementModelInvalidScan;
|
|
double t = pose.Yaw + rangeAngle;
|
|
double x = range * Math.Cos(t) + pose.X;
|
|
double y = range * Math.Sin(t) + pose.Y;
|
|
Xy2Uv(x, y, out int u, out int v);
|
|
if (!OnMap(u, v)) return _measurementModelRandom;
|
|
double dist = _distMap![v, u];
|
|
double pHit = _normConstHit * Math.Exp(-(dist * dist) * _denomHit) * _mapResolution;
|
|
double p = _options.ZHit * pHit + _measurementModelRandom;
|
|
return Math.Min(p, 1.0);
|
|
}
|
|
|
|
/// <summary>Beam model: ray-cast to find expected range, compare with observed (xloc MCL.cc:1535-1574).</summary>
|
|
private double CalculateBeamModel(MclPose2d pose, double range, double rangeAngle)
|
|
{
|
|
if (range <= _rangeMin || range >= _rangeMax) return _measurementModelInvalidScan;
|
|
|
|
double t = pose.Yaw + rangeAngle;
|
|
double x = pose.X;
|
|
double y = pose.Y;
|
|
double dx = _mapResolution * Math.Cos(t);
|
|
double dy = _mapResolution * Math.Sin(t);
|
|
double expectedRange = -1.0;
|
|
double hitThreshold = 0.5 * _mapResolution;
|
|
|
|
// Ray-cast to find expected range (distance to first occupied cell)
|
|
for (double r = 0.0; r < _rangeMax; r += _mapResolution)
|
|
{
|
|
Xy2Uv(x, y, out int u, out int v);
|
|
if (!OnMap(u, v)) break;
|
|
double dist = _distMap![v, u];
|
|
if (dist < hitThreshold)
|
|
{
|
|
expectedRange = r;
|
|
break;
|
|
}
|
|
x += dx;
|
|
y += dy;
|
|
}
|
|
|
|
double p;
|
|
if (range <= expectedRange)
|
|
{
|
|
// Observed range is shorter than expected: hit or short
|
|
double error = expectedRange - range;
|
|
double pHit = _normConstHit * Math.Exp(-(error * error) * _denomHit) * _mapResolution;
|
|
double pShort = _options.LambdaShort * Math.Exp(-_options.LambdaShort * range) / (1.0 - Math.Exp(-_options.LambdaShort * _rangeMax)) * _mapResolution;
|
|
p = _options.ZHit * pHit + _options.ZShort * pShort + _measurementModelRandom;
|
|
}
|
|
else
|
|
{
|
|
// Observed range is longer than expected: random
|
|
p = _measurementModelRandom;
|
|
}
|
|
return Math.Min(p, 1.0);
|
|
}
|
|
|
|
/// <summary>Class-conditional measurement model: probabilistic model for known vs unknown obstacles (xloc MCL.cc:1576-1597).</summary>
|
|
private double CalculateClassConditionalMeasurementModel(MclPose2d pose, double range, double rangeAngle)
|
|
{
|
|
if (range <= _rangeMin || range >= _rangeMax) return _measurementModelInvalidScan;
|
|
|
|
double t = pose.Yaw + rangeAngle;
|
|
double x = range * Math.Cos(t) + pose.X;
|
|
double y = range * Math.Sin(t) + pose.Y;
|
|
|
|
// Unknown class: exponential decay with unknown prior
|
|
double pUnknown = _options.LambdaUnknown * Math.Exp(-_options.LambdaUnknown * range) / (1.0 - Math.Exp(-_options.LambdaUnknown * _rangeMax)) * _mapResolution * _options.PUnknownPrior;
|
|
|
|
Xy2Uv(x, y, out int u, out int v);
|
|
double p = pUnknown;
|
|
if (OnMap(u, v))
|
|
{
|
|
// Known class: likelihood field + random
|
|
double dist = _distMap![v, u];
|
|
double pHit = _normConstHit * Math.Exp(-(dist * dist) * _denomHit) * _mapResolution;
|
|
p += (_options.ZHit * pHit + _measurementModelRandom) * _options.PKnownPrior;
|
|
}
|
|
else
|
|
{
|
|
// Off map: known random
|
|
p += _measurementModelRandom * _options.PKnownPrior;
|
|
}
|
|
return Math.Min(p, 1.0);
|
|
}
|
|
|
|
private void CalculateLikelihoodsByMeasurementModel()
|
|
{
|
|
if (_distMap == null || _ranges == null)
|
|
{
|
|
return;
|
|
}
|
|
|
|
int scanStep = _options.ScanStep > 0 ? _options.ScanStep : 10;
|
|
|
|
// Count valid ranges
|
|
int validRanges = 0;
|
|
for (int i = 0; i < _ranges.Length; i++)
|
|
{
|
|
if (_ranges[i] > _rangeMin && _ranges[i] < _rangeMax)
|
|
validRanges++;
|
|
}
|
|
|
|
if (_options.RejectUnknownScan && (_options.MeasurementModelType == 0 || _options.MeasurementModelType == 1))
|
|
RejectUnknownScan();
|
|
|
|
// Use scanStep like xloc (xloc uses scanStep=10 by default to avoid underflow with many beams)
|
|
double xo = _baseLink2LaserX, yo = _baseLink2LaserY, yawo = _baseLink2LaserYaw;
|
|
|
|
_likelihoodShiftedSteps.Clear();
|
|
for (int i = 0; i < _particles.Count; i++)
|
|
_particles[i].W = 0;
|
|
|
|
for (int i = 0; i < _ranges.Length; i += scanStep)
|
|
{
|
|
double r = _ranges[i];
|
|
double rangeAngle = i * _angleIncrement + _angleMin; // beam angle in scan frame (relative to sensor)
|
|
double stepMax = double.MinValue;
|
|
for (int j = 0; j < _particles.Count; j++)
|
|
{
|
|
double yaw = _particles[j].Pose.Yaw;
|
|
double sensorX = xo * Math.Cos(yaw) - yo * Math.Sin(yaw) + _particles[j].Pose.X;
|
|
double sensorY = xo * Math.Sin(yaw) + yo * Math.Cos(yaw) + _particles[j].Pose.Y;
|
|
double sensorYaw = yawo + yaw;
|
|
var sensorPose = new MclPose2d(sensorX, sensorY, sensorYaw);
|
|
double p = _options.MeasurementModelType switch
|
|
{
|
|
0 => CalculateLikelihoodFieldModel(sensorPose, r, rangeAngle),
|
|
1 => CalculateBeamModel(sensorPose, r, rangeAngle),
|
|
2 => CalculateClassConditionalMeasurementModel(sensorPose, r, rangeAngle),
|
|
_ => CalculateLikelihoodFieldModel(sensorPose, r, rangeAngle)
|
|
};
|
|
|
|
double w = _particles[j].W + Math.Log(p);
|
|
_particles[j].W = w;
|
|
if (j == 0 || w > stepMax) stepMax = w;
|
|
}
|
|
// xloc: shift log weights if max < -300 to avoid underflow when converting to exp
|
|
bool shifted = stepMax < -300.0;
|
|
_likelihoodShiftedSteps.Add(shifted);
|
|
if (shifted)
|
|
{
|
|
for (int j = 0; j < _particles.Count; j++)
|
|
_particles[j].W += 300.0;
|
|
}
|
|
}
|
|
|
|
double sum = 0, max = double.MinValue;
|
|
int maxIdx = 0;
|
|
for (int i = 0; i < _particles.Count; i++)
|
|
{
|
|
double w = Math.Exp(_particles[i].W);
|
|
_particles[i].W = w;
|
|
sum += w;
|
|
if (w > max) { max = w; maxIdx = i; }
|
|
}
|
|
_totalLikelihood = sum;
|
|
_averageLikelihood = _particles.Count > 0 ? sum / _particles.Count : 0;
|
|
_maxLikelihoodParticleIdx = maxIdx;
|
|
}
|
|
|
|
/// <summary>Residual errors: distance from each beam endpoint to nearest occupied (xloc getResidualErrors). -1 if invalid.</summary>
|
|
private List<double> GetResidualErrors(MclPose2d pose, int scanStep = 1)
|
|
{
|
|
if (_distMap == null || _ranges == null) return [];
|
|
double xo = _baseLink2LaserX, yo = _baseLink2LaserY, yawo = _baseLink2LaserYaw;
|
|
double yaw = pose.Yaw;
|
|
double sensorX = xo * Math.Cos(yaw) - yo * Math.Sin(yaw) + pose.X;
|
|
double sensorY = xo * Math.Sin(yaw) + yo * Math.Cos(yaw) + pose.Y;
|
|
double sensorYaw = yawo + yaw;
|
|
int estimatedCount = (_ranges.Length + scanStep - 1) / scanStep;
|
|
var errors = new List<double>(estimatedCount);
|
|
for (int i = 0; i < _ranges.Length; i += scanStep)
|
|
{
|
|
double r = _ranges[i];
|
|
if (r <= _rangeMin || r >= _rangeMax) { errors.Add(-1.0); continue; }
|
|
double t = i * _angleIncrement + _angleMin + sensorYaw;
|
|
double x = r * Math.Cos(t) + sensorX;
|
|
double y = r * Math.Sin(t) + sensorY;
|
|
Xy2Uv(x, y, out int u, out int v);
|
|
if (!OnMap(u, v)) { errors.Add(-1.0); continue; }
|
|
errors.Add(_distMap[v, u]);
|
|
}
|
|
return errors;
|
|
}
|
|
|
|
/// <summary>MAE over valid residual errors in [0, MaxResidualError] (xloc MAEClassifier::getMAE).</summary>
|
|
private static double GetMAE(List<double> residualErrors, double maxResidualError = MaxResidualError)
|
|
{
|
|
double sum = 0;
|
|
int num = 0;
|
|
foreach (double e in residualErrors)
|
|
{
|
|
if (e >= 0 && e <= maxResidualError) { sum += e; num++; }
|
|
}
|
|
return num == 0 ? 0.0 : sum / num;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Decision Model and Reliability
|
|
|
|
/// <summary>Simple decision model: decay by MAE vs failure threshold; update reliability (xloc calculateDecisionModel when no histogram files).</summary>
|
|
private double CalculateDecisionModelSimple(double mae, ref double reliability)
|
|
{
|
|
double th = _options.FailureThreshold;
|
|
if (th <= 0) th = 0.12;
|
|
double decisionLikelihood = Math.Exp(-mae / th);
|
|
double rel = decisionLikelihood * reliability;
|
|
double relInv = (1.0 - decisionLikelihood) * (1.0 - reliability);
|
|
double p = rel + relInv;
|
|
if (p < 1e-9) p = 1e-9;
|
|
reliability = Math.Clamp(rel / p, 0.0001, 0.9999);
|
|
return Math.Min(p, 1.0);
|
|
}
|
|
|
|
private void CalculateLikelihoodsByDecisionModel()
|
|
{
|
|
if (_reliabilities == null || _maes == null || _totalLikelihood <= 0) return;
|
|
|
|
// Estimate unknown scan if using class-conditional model (xloc MCL.cc:491-494)
|
|
if (_options.MeasurementModelType == 2 && _maxLikelihoodParticleIdx >= 0 && _maxLikelihoodParticleIdx < _particles.Count)
|
|
{
|
|
var mlPose = _particles[_maxLikelihoodParticleIdx].Pose;
|
|
_ = EstimateUnknownScanWithClassConditionalMeasurementModel(mlPose);
|
|
}
|
|
|
|
int scanStep = _options.ScanStep > 0 ? _options.ScanStep : 10;
|
|
int n = _particles.Count;
|
|
double sum = 0;
|
|
double max = 0;
|
|
int maxIdx = 0;
|
|
for (int i = 0; i < n; i++)
|
|
{
|
|
var residualErrors = GetResidualErrors(_particles[i].Pose, scanStep);
|
|
double mae = GetMAE(residualErrors);
|
|
_maes[i] = mae;
|
|
double rel = _reliabilities[i];
|
|
double decisionLikelihood = CalculateDecisionModelSimple(mae, ref rel);
|
|
_reliabilities[i] = rel;
|
|
double w = _particles[i].W * decisionLikelihood;
|
|
_particles[i].W = w;
|
|
sum += w;
|
|
if (w > max) { max = w; maxIdx = i; }
|
|
}
|
|
_totalLikelihood = sum;
|
|
_averageLikelihood = n > 0 ? sum / n : 0;
|
|
_maxLikelihoodParticleIdx = maxIdx;
|
|
_reliability = _reliabilities[maxIdx];
|
|
}
|
|
|
|
private void CalculateAMCLRandomParticlesRate()
|
|
{
|
|
_omegaSlow += _options.AlphaSlow * (_averageLikelihood - _omegaSlow);
|
|
_omegaFast += _options.AlphaFast * (_averageLikelihood - _omegaFast);
|
|
_amclRandomParticlesRate = _omegaSlow > 1e-10 ? Math.Max(0, 1.0 - _omegaFast / _omegaSlow) : 0;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region GL Sampled Poses
|
|
|
|
private void CalculateGLSampledPosesLikelihood()
|
|
{
|
|
if (_glSampledPoses.Count == 0 || !_isGLSampledPosesUpdated) return;
|
|
double stampSec = 0; // we don't have scan stamp in C#; use 0 so time check is relaxed if needed
|
|
if (Math.Abs(stampSec - _glSampledPosesStamp) > _options.GLSampledPoseTimeTH) return;
|
|
|
|
_isGLSampledPosesUpdated = false;
|
|
int glNum = _glSampledPoses.Count;
|
|
_glParticles = new List<MclParticle>(glNum);
|
|
double xo = _baseLink2LaserX, yo = _baseLink2LaserY, yawo = _baseLink2LaserYaw;
|
|
int scanStep = _options.ScanStep > 0 ? _options.ScanStep : 10;
|
|
|
|
for (int i = 0; i < glNum; i++)
|
|
{
|
|
var pose = _glSampledPoses[i];
|
|
_glParticles.Add(new MclParticle(pose.X, pose.Y, pose.Yaw, 0));
|
|
}
|
|
|
|
for (int i = 0; i < _ranges!.Length; i += scanStep)
|
|
{
|
|
double r = _ranges[i];
|
|
double rangeAngle = i * _angleIncrement + _angleMin;
|
|
for (int j = 0; j < glNum; j++)
|
|
{
|
|
var p = _glParticles[j];
|
|
double yaw = p.Pose.Yaw;
|
|
double sensorX = xo * Math.Cos(yaw) - yo * Math.Sin(yaw) + p.Pose.X;
|
|
double sensorY = xo * Math.Sin(yaw) + yo * Math.Cos(yaw) + p.Pose.Y;
|
|
double sensorYaw = yawo + yaw;
|
|
var sensorPose = new MclPose2d(sensorX, sensorY, sensorYaw);
|
|
double prob = CalculateLikelihoodFieldModel(sensorPose, r, rangeAngle);
|
|
_glParticles[j] = new MclParticle(p.Pose, p.W + Math.Log(prob));
|
|
}
|
|
if (i < _likelihoodShiftedSteps.Count && _likelihoodShiftedSteps[i])
|
|
{
|
|
for (int j = 0; j < glNum; j++)
|
|
{
|
|
var p = _glParticles[j];
|
|
_glParticles[j] = new MclParticle(p.Pose, p.W + 300.0);
|
|
}
|
|
}
|
|
}
|
|
|
|
double gmmPosVar = _options.GmmPositionalVariance;
|
|
double gmmAngVar = _options.GmmAngularVariance;
|
|
double normConst = 1.0 / Math.Sqrt(2.0 * Math.PI * (gmmPosVar + gmmPosVar + gmmAngVar));
|
|
double angleRes = Math.PI / 180.0;
|
|
double gmmRate = 1.0 - _options.PredDistUnifRate;
|
|
int mainN = _particles.Count;
|
|
double sum = _totalLikelihood;
|
|
double max = 0;
|
|
int maxIdx = -1;
|
|
|
|
for (int i = 0; i < glNum; i++)
|
|
{
|
|
double w = Math.Exp(_glParticles[i].W);
|
|
double gmmVal = 0;
|
|
var glPose = _glParticles[i].Pose;
|
|
for (int j = 0; j < mainN; j++)
|
|
{
|
|
double dx = glPose.X - _particles[j].Pose.X;
|
|
double dy = glPose.Y - _particles[j].Pose.Y;
|
|
double dyaw = MclPose2d.NormalizeYaw(glPose.Yaw - _particles[j].Pose.Yaw);
|
|
gmmVal += normConst * Math.Exp(-((dx * dx) / (2 * gmmPosVar) + (dy * dy) / (2 * gmmPosVar) + (dyaw * dyaw) / (2 * gmmAngVar)));
|
|
}
|
|
double pGmm = glNum * gmmVal * _mapResolution * _mapResolution * angleRes / mainN;
|
|
double predLikelihood = gmmRate * pGmm + _options.PredDistUnifRate * 1e-8;
|
|
w *= predLikelihood;
|
|
w = Math.Min(w, 1.0);
|
|
_glParticles[i] = new MclParticle(_glParticles[i].Pose, w);
|
|
sum += w;
|
|
if (w > max) { max = w; maxIdx = i; }
|
|
}
|
|
|
|
if (double.IsNaN(sum))
|
|
{
|
|
_canUseGLSampledPoses = false;
|
|
return;
|
|
}
|
|
_totalLikelihood = sum;
|
|
_averageLikelihood = (mainN + glNum) > 0 ? sum / (mainN + glNum) : 0;
|
|
if (maxIdx >= 0)
|
|
{
|
|
_maxLikelihoodParticleIdx = mainN + maxIdx;
|
|
}
|
|
_canUseGLSampledPoses = true;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Pose Estimation
|
|
|
|
/// <summary>
|
|
/// Weighted mean pose (xloc estimatePose). Weights must be used with sum for correct average when not normalized.
|
|
/// When sum is too small (underflow) or result is non-finite, keep previous pose or use max-likelihood particle to avoid (0,0) collapse.
|
|
/// </summary>
|
|
private void EstimatePose()
|
|
{
|
|
double tmpYaw = _mclPose.Yaw;
|
|
double x = 0, y = 0, yaw = 0, sum = 0;
|
|
for (int i = 0; i < _particles.Count; i++)
|
|
{
|
|
double w = _particles[i].W;
|
|
x += _particles[i].Pose.X * w;
|
|
y += _particles[i].Pose.Y * w;
|
|
double dyaw = tmpYaw - _particles[i].Pose.Yaw;
|
|
dyaw = MclPose2d.NormalizeYaw(dyaw);
|
|
yaw += dyaw * w;
|
|
sum += w;
|
|
}
|
|
double x2 = x, y2 = y, yaw2 = yaw;
|
|
if (_canUseGLSampledPoses && _glParticles.Count > 0)
|
|
{
|
|
for (int i = 0; i < _glParticles.Count; i++)
|
|
{
|
|
double w = _glParticles[i].W;
|
|
x += _glParticles[i].Pose.X * w;
|
|
y += _glParticles[i].Pose.Y * w;
|
|
double dyaw = tmpYaw - _glParticles[i].Pose.Yaw;
|
|
dyaw = MclPose2d.NormalizeYaw(dyaw);
|
|
yaw += dyaw * w;
|
|
sum += w;
|
|
}
|
|
if (sum > 1.0)
|
|
{
|
|
x = x2; y = y2; yaw = yaw2;
|
|
}
|
|
}
|
|
if (sum > 1e-10 && double.IsFinite(sum))
|
|
{
|
|
x /= sum;
|
|
y /= sum;
|
|
yaw = tmpYaw - (yaw / sum);
|
|
if (double.IsFinite(x) && double.IsFinite(y) && double.IsFinite(yaw))
|
|
{
|
|
// Reject estimate if it jumps too far (avoids garbage from degenerate likelihood / wrong frame)
|
|
const double maxPoseJumpMeters = 50.0;
|
|
double dx = x - _mclPose.X;
|
|
double dy = y - _mclPose.Y;
|
|
double dist = Math.Sqrt(dx * dx + dy * dy);
|
|
if (dx * dx + dy * dy <= maxPoseJumpMeters * maxPoseJumpMeters)
|
|
{
|
|
_mclPose = new MclPose2d(x, y, yaw);
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
// Underflow or non-finite: use max-likelihood particle to avoid (0,0) or garbage
|
|
if (_maxLikelihoodParticleIdx >= 0)
|
|
{
|
|
if (_maxLikelihoodParticleIdx < _particles.Count)
|
|
{
|
|
var best = _particles[_maxLikelihoodParticleIdx].Pose;
|
|
_mclPose = new MclPose2d(best.X, best.Y, best.Yaw);
|
|
}
|
|
else if (_canUseGLSampledPoses && _glParticles.Count > 0)
|
|
{
|
|
int glIdx = _maxLikelihoodParticleIdx - _particles.Count;
|
|
if (glIdx >= 0 && glIdx < _glParticles.Count)
|
|
{
|
|
var best = _glParticles[glIdx].Pose;
|
|
_mclPose = new MclPose2d(best.X, best.Y, best.Yaw);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Resampling
|
|
|
|
/// <summary>
|
|
/// Resample particles (xloc resampleParticles). Skip if ESS above threshold or motion below thresholds.
|
|
/// Supports GL particles and random particles (addRandomParticlesInResampling / augmented MCL).
|
|
/// </summary>
|
|
private void ResampleParticles()
|
|
{
|
|
int mainN = _particles.Count;
|
|
int totalN = mainN;
|
|
if (_canUseGLSampledPoses && _glParticles.Count > 0)
|
|
totalN = mainN + _glParticles.Count;
|
|
|
|
double threshold = totalN * _options.ResampleThresholdEss;
|
|
double sumW = _totalLikelihood;
|
|
if (sumW > 1e-10)
|
|
{
|
|
double sumW2 = 0;
|
|
for (int i = 0; i < mainN; i++) sumW2 += _particles[i].W * _particles[i].W;
|
|
if (_canUseGLSampledPoses && _glParticles.Count > 0)
|
|
for (int i = 0; i < _glParticles.Count; i++) sumW2 += _glParticles[i].W * _glParticles[i].W;
|
|
double effectiveSampleSize = (sumW * sumW) / sumW2;
|
|
if (effectiveSampleSize > threshold)
|
|
return;
|
|
}
|
|
// Skip motion threshold check when in global localization mode (robot stationary, searching for pose)
|
|
// This allows particles to converge even without motion
|
|
if (!_forceResampleForGlobalLocalization)
|
|
{
|
|
if (_deltaXSum < _options.ResampleThresholds[0] && _deltaYSum < _options.ResampleThresholds[1] &&
|
|
_deltaDistSum < _options.ResampleThresholds[2] && _deltaYawSum < _options.ResampleThresholds[3] &&
|
|
_deltaTimeSum < _options.ResampleThresholds[4])
|
|
{
|
|
return;
|
|
}
|
|
}
|
|
|
|
_deltaXSum = _deltaYSum = _deltaDistSum = _deltaYawSum = _deltaTimeSum = 0;
|
|
|
|
if (sumW <= 0) return;
|
|
|
|
var wBuffer = new double[totalN];
|
|
wBuffer[0] = _particles[0].W / sumW;
|
|
for (int i = 1; i < mainN; i++)
|
|
wBuffer[i] = wBuffer[i - 1] + _particles[i].W / sumW;
|
|
if (_canUseGLSampledPoses && _glParticles.Count > 0)
|
|
{
|
|
for (int i = 0; i < _glParticles.Count; i++)
|
|
wBuffer[mainN + i] = (mainN + i == 0 ? 0 : wBuffer[mainN + i - 1]) + _glParticles[i].W / sumW;
|
|
}
|
|
|
|
var tmpMain = _particles.Select(p => new MclParticle(p.Pose, p.W)).ToList();
|
|
double[]? tmpRel = _reliabilities != null ? (double[])_reliabilities.Clone() : null;
|
|
double wo = 1.0 / mainN;
|
|
|
|
bool useRandomParticles = _options.AddRandomParticlesInResampling || _options.UseAugmentedMcl;
|
|
double randomRate = _options.AddRandomParticlesInResampling ? _options.RandomParticlesRate : 0;
|
|
if (_options.UseAugmentedMcl && _amclRandomParticlesRate > 0)
|
|
{
|
|
randomRate = _amclRandomParticlesRate;
|
|
_omegaSlow = _omegaFast = 0;
|
|
}
|
|
else if (!_options.AddRandomParticlesInResampling)
|
|
randomRate = 0;
|
|
|
|
int resampledNum = (int)(useRandomParticles && randomRate > 0
|
|
? Math.Max(0, (int)((1.0 - randomRate) * mainN))
|
|
: mainN);
|
|
int randomNum = mainN - resampledNum;
|
|
|
|
double xo = _mclPose.X, yo = _mclPose.Y, yawo = _mclPose.Yaw;
|
|
var noise = _options.RandomParticlesNoise ?? [0.05, 0.05, 0.1];
|
|
if (noise.Length < 3) noise = [0.05, 0.05, 0.1];
|
|
|
|
for (int i = 0; i < resampledNum; i++)
|
|
{
|
|
double darts = _rnd.NextSingle();
|
|
int j = Array.BinarySearch(wBuffer, 0, totalN, darts);
|
|
if (j < 0) j = ~j; // BinarySearch returns bitwise complement of next larger index
|
|
if (j >= totalN) j = totalN - 1;
|
|
|
|
if (j < mainN)
|
|
{
|
|
_particles[i].Pose = tmpMain[j].Pose;
|
|
if (tmpRel != null && i < _reliabilities!.Length && j < tmpRel.Length)
|
|
_reliabilities[i] = tmpRel[j];
|
|
}
|
|
else if (_canUseGLSampledPoses && j - mainN < _glParticles.Count)
|
|
{
|
|
_particles[i].Pose = _glParticles[j - mainN].Pose;
|
|
}
|
|
_particles[i].W = wo;
|
|
}
|
|
for (int i = resampledNum; i < resampledNum + randomNum; i++)
|
|
{
|
|
double x = xo + NRand(noise[0]);
|
|
double y = yo + NRand(noise[1]);
|
|
double yaw = yawo + NRand(noise[2]);
|
|
_particles[i].Pose = new MclPose2d(x, y, yaw);
|
|
_particles[i].W = wo;
|
|
if (_options.EstimateReliability && _reliabilities != null && i < _reliabilities.Length)
|
|
_reliabilities[i] = _reliability;
|
|
}
|
|
|
|
_canUseGLSampledPoses = false;
|
|
}
|
|
|
|
#endregion
|
|
}
|