/// <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);
}
public Vector3 MatchData(Vector3 beginEstimateWorld, DataContainer dataContainer, out Matrix4x4 covMatrix)
{
return(scanMatcher.MatchData(beginEstimateWorld, gridMapUtil, dataContainer, out covMatrix, 20));
}
public Vector3 MatchData(Vector3 beginEstimateWorld, DataContainer dataContainer, out Matrix4x4 covMatrix, int maxIterations)
{
return(ScanMatcher.MatchData(beginEstimateWorld, GridMapUtil, dataContainer, out covMatrix, maxIterations));
}