/// <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;
}