/// <summary>
/// Search for best robot pose in monto-carlo method
/// </summary>
/// <param name="cloud">Scan points cloud</param>
/// <param name="startPose">Search start position</param>
/// <param name="randomXY">Queue of random XY standard deviation numbers</param>
/// <param name="randomTheta">Queue of random Theta standard deviation numbers</param>
/// <param name="iterations">Number of search iterations</param>
/// <param name="distance">Best pose distance value (the lower the better)</param>
/// <returns>Best found pose</returns>
public Vector3 MonteCarloSearch(ScanCloud cloud, Vector3 startPose, Queue <float> randomXY, Queue <float> randomTheta, int iterations, out int distance)
{
Vector3 bestPose = startPose;
int currentDistance = CalculateDistance(cloud, startPose);
int bestDistance = currentDistance;
for (int counter = 0; counter < iterations; counter++)
{
// Create new random position
Vector3 currentpose = new Vector3()
{
X = startPose.X + randomXY.Dequeue(),
Y = startPose.Y + randomXY.Dequeue(),
Z = startPose.Z + randomTheta.Dequeue()
};
// Calculate distance at that position
currentDistance = CalculateDistance(cloud, currentpose);
// Is it the best ?
if (currentDistance < bestDistance)
{
bestDistance = currentDistance;
bestPose = currentpose;
}
}
distance = bestDistance;
return(bestPose);
}
/// <summary>
/// Search for best robot pose in monto-carlo method
/// </summary>
/// <param name="cloud">Scan points cloud</param>
/// <param name="startPose">Search start position</param>
/// <param name="sigmaXY">XY coordinates standard deviation</param>
/// <param name="sigmaTheta">Theta standard deviation</param>
/// <param name="iterations">Number of search iterations</param>
/// <param name="distance">Best pose distance value (the lower the better)</param>
/// <returns>Best found pose</returns>
public Vector3 MonteCarloSearch(ScanCloud cloud, Vector3 startPose, float sigmaXY, float sigmaTheta, int iterations, out int distance)
{
// Use 3rd party library for fast normal distribution random number generator
var samplerXY = new ZigguratGaussianSampler(0.0f, sigmaXY);
var samplerTheta = new ZigguratGaussianSampler(0.0f, sigmaTheta);
Vector3 bestPose = startPose;
int currentDistance = CalculateDistance(cloud, startPose);
int bestDistance = currentDistance;
for (int counter = 0; counter < iterations; counter++)
{
// Create new random position
Vector3 currentpose = new Vector3()
{
X = startPose.X + samplerXY.Sample(),
Y = startPose.Y + samplerXY.Sample(),
Z = startPose.Z + samplerTheta.Sample()
};
// Calculate distance at that position
currentDistance = CalculateDistance(cloud, currentpose);
// Is it the best ?
if (currentDistance < bestDistance)
{
bestDistance = currentDistance;
bestPose = currentpose;
}
}
distance = bestDistance;
return(bestPose);
}
/// <summary>
/// Estimate pose
/// </summary>
/// <param name="gridMap">Map</param>
/// <param name="scan">Scanned points</param>
/// <param name="estimate">Estimate position</param>
/// <returns>true if estimation was madde, false if failed</returns>
protected bool EstimateTransformationLogLh(OccGridMap gridMap, ScanCloud scan, ref Vector3 estimate)
{
GetCompleteHessianDerivs(gridMap, scan, estimate, out Matrix4x4 H, out Vector3 dTr);
if ((H.M11 != 0.0f) && (H.M22 != 0.0f))
{
if (!Matrix4x4.Invert(H, out Matrix4x4 iH))
{
logger?.LogError($"Failed to calculate inverse matrix from {H}");
return(false);
}
Vector3 searchDir = Vector3.Transform(dTr, iH);
if (searchDir.Z > 0.2f)
{
logger?.LogWarning($"SearchDir angle change ({MathEx.RadToDeg(searchDir.Z)}°) too large");
searchDir.Z = 0.2f;
}
else if (searchDir.Z < -0.2f)
{
logger?.LogWarning($"SearchDir angle change ({MathEx.RadToDeg(searchDir.Z)}°) too large");
searchDir.Z = -0.2f;
}
estimate += searchDir;
return(true);
}
return(false);
}
/// <summary>
/// Calculate "distance" of cloud at determined pose
/// </summary>
/// <param name="cloud">Cloud of points</param>
/// <param name="pose">Pose of cloud</param>
/// <returns></returns>
private int CalculateDistanceSISD(ScanCloud cloud, Vector3 pose)
{
int nb_points = 0;
long sum = 0;
// Pre-calculate centerpoint with additional 0.5 to get rounding effect
float px = pose.X * HoleMap.Scale + 0.5f;
float py = pose.Y * HoleMap.Scale + 0.5f;
float c = MathF.Cos(pose.Z) * HoleMap.Scale;
float s = MathF.Sin(pose.Z) * HoleMap.Scale;
// Translate and rotate scan to robot position and compute the "distance"
for (int i = 0; i < cloud.Points.Count; i++)
{
int x = (int)(px + c * cloud.Points[i].X - s * cloud.Points[i].Y);
int y = (int)(py + s * cloud.Points[i].X + c * cloud.Points[i].Y);
// Check boundaries
if ((x >= 0) && (x < HoleMap.Size) && (y >= 0) && (y < HoleMap.Size))
{
sum += HoleMap.Pixels[y * HoleMap.Size + x];
nb_points++;
}
}
if (nb_points > 0)
{
return((int)((sum * 1024) / cloud.Points.Count));
}
else
{
return(int.MaxValue);
}
}
/// <summary>
/// Update obstacle map with lidar scan points cloud
/// </summary>
/// <param name="cloud">Scan points cloud</param>
private void UpdateObstacleMap(ScanCloud cloud)
{
ArrayEx.Fill(noHitMap, false);
// Pre-calculate centerpoint with additional 0.5 to get rounding effect
float px = Pose.X * ObstacleMap.Scale + 0.5f;
float py = Pose.Y * ObstacleMap.Scale + 0.5f;
float c = MathF.Cos(Pose.Z) * ObstacleMap.Scale;
float s = MathF.Sin(Pose.Z) * ObstacleMap.Scale;
// TODO Use cloud lines to draw map ?
// Line start
int x1 = (int)px;
int y1 = (int)py;
// Boundary check
if (x1 < 0 || x1 >= ObstacleMap.Size || y1 < 0 || y1 >= ObstacleMap.Size)
{
return; // Robot is out of map
}
// Draw rays
foreach (Vector2 p in cloud.Points)
{
// Translate and rotate scan to robot position
int x2 = (int)(px + c * p.X - s * p.Y);
int y2 = (int)(py + s * p.X + c * p.Y);
// Draw obstacle map
DrawLaserRayOnObstacleMap(noHitMap, x1, y1, x2, y2);
}
// TODO Far small objects disappear too easily... do some tricks
// Deal with no hit pixels
for (int y = 0; y < ObstacleMap.Size; y++)
{
for (int x = 0; x < ObstacleMap.Size; x++)
{
if (noHitMap[y, x])
{
if (ObstacleMap.Pixels[y, x] < 0)
{
ObstacleMap.Pixels[y, x]++;
}
else if (ObstacleMap.Pixels[y, x] > 0)
{
ObstacleMap.Pixels[y, x]--;
}
}
}
}
}
/// <summary>
/// Match scan data and get best position estimate.
/// </summary>
/// <param name="multiMap">Multi-level map</param>
/// <param name="scan">Scanned points</param>
/// <param name="hintPose">Estimation hint pose in world coordinates (meters)</param>
/// <returns>Best position estimate in world coordinates (meters)</returns>
public Vector3 MatchData(MapRepMultiMap multiMap, ScanCloud scan, Vector3 hintPose)
{
Vector3 estimate = hintPose;
logger?.LogInformation($"Hint pose {hintPose.ToPoseString()}");
// Start matching from coarsest map
for (int index = multiMap.Maps.Length - 1; index >= 0; index--)
{
estimate = MatchData(multiMap.Maps[index], scan, estimate);
logger?.LogInformation($"Layer {index} estimate: {estimate.ToPoseString()}");
}
return(estimate);
}
/// <summary>
/// Calculate "distance" of cloud at determined pose
/// TODO - It's actually slower than SISD. Need more parallelism.
/// </summary>
/// <param name="cloud">Cloud of points</param>
/// <param name="pose">Pose of cloud</param>
/// <returns></returns>
private int CalculateDistanceSSE41(ScanCloud cloud, Vector3 pose)
{
int nb_points = 0;
long sum = 0;
float px = pose.X * HoleMap.Scale;
float py = pose.Y * HoleMap.Scale;
float c = MathF.Cos(pose.Z) * HoleMap.Scale;
float s = MathF.Sin(pose.Z) * HoleMap.Scale;
Vector128 <float> sincos = Vector128.Create(c, -s, s, c);
Vector128 <float> posxy = Vector128.Create(px, py, px, py);
// Translate and rotate scan to robot position and compute the "distance"
for (int i = 0; i < cloud.Points.Count; i++)
{
Vector128 <float> xy = Vector128.Create(cloud.Points[i].X, cloud.Points[i].Y, cloud.Points[i].X, cloud.Points[i].Y);
xy = Sse41.Multiply(sincos, xy);
xy = Sse41.HorizontalAdd(xy, xy);
xy = Sse41.Add(xy, posxy);
xy = Sse41.RoundToNearestInteger(xy);
int x = (int)xy.GetElement(0);
int y = (int)xy.GetElement(1);
// Check boundaries
if ((x >= 0) && (x < HoleMap.Size) && (y >= 0) && (y < HoleMap.Size))
{
sum += HoleMap.Pixels[y * HoleMap.Size + x];
nb_points++;
}
}
if (nb_points > 0)
{
return((int)((sum * 1024) / cloud.Points.Count));
}
else
{
return(int.MaxValue);
}
}
/// <summary>
/// Update map with new scan data and search for the best pose estimate.
/// </summary>
/// <param name="scan">Scanned cloud points</param>
/// <param name="poseHintWorld">Pose hint</param>
/// <param name="mapWithoutMatching">Map without matching ?</param>
/// <returns>true if map was updated, false if not</returns>
public bool Update(ScanCloud scan, Vector3 poseHintWorld, bool mapWithoutMatching = false)
{
// Do position matching or not ?
if (!mapWithoutMatching)
{
// Match and measure the performance
var watch = Stopwatch.StartNew();
MatchPose = scanMatcher.MatchData(MapRep, scan, poseHintWorld);
// Calculate average timing
MatchTiming = (3.0f * MatchTiming + (float)watch.Elapsed.TotalMilliseconds) / 4.0f;
}
else
{
MatchPose = poseHintWorld;
}
// Update map(s) when:
// Map hasn't been updated yet
// Position or rotation has changed significantly.
// Mapping is requested.
if (Vector2.DistanceSquared(MatchPose.ToVector2(), LastMapUpdatePose.ToVector2()) > MinDistanceDiffForMapUpdate.Sqr() ||
(MathEx.DegDiff(MatchPose.Z, LastMapUpdatePose.Z) > MinAngleDiffForMapUpdate) ||
mapWithoutMatching)
{
var watch = Stopwatch.StartNew();
MapRep.UpdateByScan(scan, MatchPose);
// Calculate average timing
UpdateTiming = (3.0f * UpdateTiming + (float)watch.Elapsed.TotalMilliseconds) / 4.0f;
// Remember update pose
LastMapUpdatePose = MatchPose;
// Notify about update
logger?.LogInformation($"Map update at {MatchPose.ToPoseString()}");
return(true);
}
return(false);
}
/// <summary>
/// Search for best robot position in monto-carlo method
/// </summary>
/// <param name="cloud">Scan points cloud</param>
/// <param name="startPose">Search start position</param>
/// <param name="distance">Best pose distance value (the lower the better)</param>
/// <returns>Best found pose</returns>
public Vector3 ParallelMonteCarloSearch(ScanCloud cloud, Vector3 startPose, out int distance)
{
// Feed input to jobs
for (int i = 0; i < searchContexts.Length; i++)
{
// Just in case reset output
searchContexts[i].OutputQueue.Clear();
searchContexts[i].OutputSignal.Reset();
// Add input data to jobs
searchContexts[i].InputQueue.Enqueue(new MonteCarloSearchInput()
{
cloud = cloud,
startPose = startPose
});
searchContexts[i].InputSignal.Set();
}
// Wait until all jobs are finished
WaitHandle.WaitAll(searchResultSignals);
// Find best pose of out all results
int bestDistance = int.MaxValue;
Vector3 bestPose = startPose;
for (int i = 0; i < searchContexts.Length; i++)
{
if (searchContexts[i].OutputQueue.TryDequeue(out MonteCarloSearchResult result))
{
if (result.distance < bestDistance)
{
bestDistance = result.distance;
bestPose = result.pose;
}
}
}
// Return best pose and "distance"
distance = bestDistance;
return(bestPose);
}
/// <summary>
/// Match scan data
/// </summary>
/// <param name="gridMap">Map</param>
/// <param name="scan">Scanned points</param>
/// <param name="hintPose">Estimation hint pose in world coordinates (meters)</param>
/// <param name="numIterations">Number of estimate iterations to do</param>
/// <returns>Best position estimate in world coordinates (meters)</returns>
public Vector3 MatchData(OccGridMap gridMap, ScanCloud scan, Vector3 hintPose)
{
if (scan.Points.Count > 0)
{
Vector3 estimate = gridMap.GetMapCoordsPose(hintPose);
for (int i = 0; i < gridMap.EstimateIterations; i++)
{
EstimateTransformationLogLh(gridMap, scan, ref estimate);
}
// Normalize Z rotation
estimate.Z = MathEx.NormalizeAngle(estimate.Z);
// Return world coordinates
return(gridMap.GetWorldCoordsPose(estimate));
}
// When no scan points provided, return hint pose
return(hintPose);
}
/// <summary>
/// Create scan points cloud from scan segments
/// Each segment is taken from different pose - it happens when robot is moving.
/// That means there's no single center point.
///
/// Segments poses should be relative to the latest position of robot
/// </summary>
/// <param name="segments">Scan segments</param>
/// <param name="odometryPose">Last odometry pose</param>
/// <param name="cloud">Scan cloud</param>
private void ScanSegmentsToCloud(IEnumerable <ScanSegment> segments, Vector3 odometryPose, out ScanCloud cloud)
{
cloud = new ScanCloud();
foreach (ScanSegment segment in segments)
{
// Calculate segment relative position against latest odometry-measured position
Vector3 pose = segment.Pose - odometryPose;
foreach (Ray r in segment.Rays)
{
Vector2 hitPoint = new Vector2()
{
X = pose.X + r.Radius * MathF.Cos(r.Angle + pose.Z),
Y = pose.Y + r.Radius * MathF.Sin(r.Angle + pose.Z),
};
cloud.Points.Add(hitPoint);
}
}
}
/// <summary>
/// Update hole map with lidar scan points cloud
/// </summary>
/// <param name="cloud">Scan points cloud</param>
private void UpdateHoleMap(ScanCloud cloud)
{
// Pre-calculate centerpoint with additional 0.5 to get rounding effect
float px = Pose.X * HoleMap.Scale + 0.5f;
float py = Pose.Y * HoleMap.Scale + 0.5f;
float c = MathF.Cos(Pose.Z) * HoleMap.Scale;
float s = MathF.Sin(Pose.Z) * HoleMap.Scale;
// Line start
int x1 = (int)px;
int y1 = (int)py;
// Boundary check
if (x1 < 0 || x1 >= HoleMap.Size || y1 < 0 || y1 >= HoleMap.Size)
{
return; // Robot is out of map
}
// TODO Use cloud lines to draw map ?
// Translate and rotate scan to robot position
foreach (Vector2 p in cloud.Points)
{
float x2p = c * p.X - s * p.Y;
float y2p = s * p.X + c * p.Y;
int xp = (int)(px + x2p);
int yp = (int)(py + y2p);
float dist = MathF.Sqrt(x2p * x2p + y2p * y2p);
float add = HoleWidth * HoleMap.Scale / 2.0f / dist;
x2p *= (1.0f + add);
y2p *= (1.0f + add);
int x2 = (int)(px + x2p);
int y2 = (int)(py + y2p);
DrawLaserRayOnHoleMap(x1, y1, x2, y2, xp, yp, TS_OBSTACLE, Quality);
}
}
/// <summary>
/// Search for best robot position in monto-carlo method
/// </summary>
/// <param name="cloud">Scan points cloud</param>
/// <param name="searchPose">Search start position</param>
/// <param name="distance">Best pose distance value (the lower the better)</param>
/// <returns>Best found pose</returns>
private Vector3 ParallelMonteCarloSearch(ScanCloud cloud, Vector3 searchPose, out int distance)
{
int[] distances = new int[NumSearchThreads];
Vector3[] poses = new Vector3[NumSearchThreads];
// Let all worker threads find best position
worker.Work(index =>
{
poses[index] = MonteCarloSearch(
cloud,
searchPose,
() => randomQueuesXY[index].Dequeue(),
() => randomQueuesTheta[index].Dequeue(),
SearchIterationsPerThread,
out distances[index]);
}, true);
// Queue a task to fill random number queues for next search loop
worker.Work(FillRandomQueues, false);
// Find best pose of out all worker results
int bestDistance = int.MaxValue;
Vector3 bestPose = searchPose;
for (int i = 0; i < NumSearchThreads; i++)
{
if (distances[i] < bestDistance)
{
bestDistance = distances[i];
bestPose = poses[i];
}
}
// Return best pose and "distance"
distance = bestDistance;
return(bestPose);
}
/// <summary>
/// Get complete hessian matrix derivatives
/// </summary>
/// <param name="gridMap">Map</param>
/// <param name="scan">Scanned points</param>
/// <param name="pose">Pose at which to calculate</param>
/// <param name="H"></param>
/// <param name="dTr"></param>
protected void GetCompleteHessianDerivs(OccGridMap gridMap, ScanCloud scan, Vector3 pose, out Matrix4x4 H, out Vector3 dTr)
{
// Transformation of lidar measurements.
// Translation is in pixels, need to convert it meters first.
Matrix3x2 transform =
Matrix3x2.CreateRotation(pose.Z) *
Matrix3x2.CreateTranslation(pose.X * gridMap.Properties.CellLength, pose.Y * gridMap.Properties.CellLength) *
Matrix3x2.CreateScale(gridMap.Properties.ScaleToMap);
// Do the measurements scaling here, rather than wasting time in the rotDeriv calculation
float sinRot = MathF.Sin(pose.Z) * gridMap.Properties.ScaleToMap;
float cosRot = MathF.Cos(pose.Z) * gridMap.Properties.ScaleToMap;
// Divide work to chunks
int chunkSize = (scan.Points.Count + worker.NumThreads - 1) / worker.NumThreads;
Vector3[] vectors = new Vector3[worker.NumThreads];
Matrix4x4[] matrixes = new Matrix4x4[worker.NumThreads];
// Do work in parallel threads
worker.Work(index =>
{
var localH = new Matrix4x4();
var localTr = Vector3.Zero;
foreach (Vector2 scanPoint in scan.Points.Skip(index * chunkSize).Take(chunkSize))
{
Vector2 scanPointMap = Vector2.Transform(scanPoint, transform);
Vector3 transformedPointData = InterpMapValueWithDerivatives(gridMap, scanPointMap);
float funVal = 1.0f - transformedPointData.X;
localTr.X += transformedPointData.Y * funVal;
localTr.Y += transformedPointData.Z * funVal;
float rotDeriv = ((-sinRot * scanPoint.X - cosRot * scanPoint.Y) * transformedPointData.Y +
(cosRot * scanPoint.X - sinRot * scanPoint.Y) * transformedPointData.Z);
localTr.Z += rotDeriv * funVal;
localH.M11 += transformedPointData.Y.Sqr();
localH.M22 += transformedPointData.Z.Sqr();
localH.M33 += rotDeriv.Sqr();
localH.M12 += transformedPointData.Y * transformedPointData.Z;
localH.M13 += transformedPointData.Y * rotDeriv;
localH.M23 += transformedPointData.Z * rotDeriv;
}
matrixes[index] = localH;
vectors[index] = localTr;
});
// Aggragate threaded results
H = new Matrix4x4();
dTr = Vector3.Zero;
for (int i = 0; i < worker.NumThreads; i++)
{
H += matrixes[i];
dTr += vectors[i];
}
// Symmetry for inversion
H.M21 = H.M12;
H.M31 = H.M13;
H.M32 = H.M23;
// Make 4x4 matrix inversible
H.M44 = 1.0f;
}
/// <summary>
/// Search for best robot pose in monto-carlo method
/// </summary>
/// <param name="cloud">Scan points cloud</param>
/// <param name="searchPose">Search start position</param>
/// <param name="distance">Best pose distance value (the lower the better)</param>
/// <returns>Best found pose</returns>
private Vector3 SingleMonteCarloSearch(ScanCloud cloud, Vector3 searchPose, out int distance)
{
return(MonteCarloSearch(cloud, searchPose, () => samplerXY.Sample(), () => samplerTheta.Sample(), SearchIterationsPerThread, out distance));
}
/// <summary>
/// Update map layers
/// </summary>
/// <param name="scan">Scanned points</param>
/// <param name="pose">Lidar pose in world coordinates (meters)</param>
public void UpdateByScan(ScanCloud scan, Vector3 pose)
{
// Run update in parallel tasks. It's about twice as fast on 6 core CPU as doing sequentially
Parallel.ForEach(Maps, parallelOptions, m => m.UpdateByScan(scan, pose));
}
/// <summary>
/// Scan
/// </summary>
private void Scan()
{
int loops = 0;
bool hsWrong = false;
while (isRunning) //boucle principale ?
{
if (doReset)
{
coreSlam.Reset();
hectorSlam.Reset();
lidarPose = startPose;
loops = 0;
doReset = false;
hsWrong = false;
}
var sw = Stopwatch.StartNew();
var snapPose = lidarPose;
bufferedLogger.LogInformation($"Real pose {snapPose.ToPoseString()}");
ScanSegments(snapPose, coreSlam.Pose, out List <ScanSegment> scanSegments); //Ici simulation d'un lidar, je dois donc remplacer ça
coreSlam.Update(scanSegments);
ScanCloud scanCloud = new ScanCloud()
{
Pose = Vector3.Zero
};
foreach (ScanSegment seg in scanSegments)
{
foreach (Ray ray in seg.Rays)
{
scanCloud.Points.Add(new Vector2()
{
X = ray.Radius * MathF.Cos(ray.Angle),
Y = ray.Radius * MathF.Sin(ray.Angle),
});
}
}
hectorSlam.Update(scanCloud, hectorSlam.MatchPose, loops < 10);
// Detect first large difference in hector SLAM
if (!hsWrong)
{
Vector2 linDiff = hectorSlam.MatchPose.ToVector2() - snapPose.ToVector2();
float distDiff = linDiff.Length();
float angDiff = MathEx.RadToDeg(MathEx.RadDiff(hectorSlam.MatchPose.Z, snapPose.Z));
if ((distDiff > 1.0f) || (Math.Abs(angDiff) > 10.0f))
{
Debug.WriteLine(string.Empty);
Debug.WriteLine($"{DateTime.Now:mm:ss:ffff} - HectorSLAM large difference");
Debug.WriteLine($" Distance {distDiff:f2}m, angular: {angDiff:f2}°, linear: {linDiff.X:f2}x{linDiff.Y:f2}m");
bufferedLogger.Items.TakeLast(30).ForEach(i => Debug.WriteLine($" {i}"));
Debug.WriteLine(string.Empty);
hsWrong = true;
}
}
// Clear log buffer once in a while
if (bufferedLogger.Items.Count > 130)
{
bufferedLogger.Items.RemoveRange(0, 100);
}
// Ensure periodicity
Thread.Sleep((int)Math.Max(0, (long)scanPeriod - sw.ElapsedMilliseconds));
// Count loops
loops++;
}
}
/// <summary>
/// Calculate "distance" of cloud at determined pose
/// </summary>
/// <param name="cloud">Cloud of points</param>
/// <param name="pose">Pose of cloud</param>
/// <returns></returns>
private int CalculateDistance(ScanCloud cloud, Vector3 pose)
{
return(CalculateDistanceSISD(cloud, pose));
}
