/// <summary>
/// Update with new data
/// </summary>
/// <param name="dataContainer">Scan data</param>
/// <param name="poseHintWorld">Pose hint</param>
/// <param name="mapWithoutMatching">Map without matching ?</param>
public void Update(DataContainer dataContainer, Vector3 poseHintWorld, bool mapWithoutMatching = false)
{
System.Diagnostics.Debug.WriteLine($"ph: {poseHintWorld}");
if (!mapWithoutMatching)
{
LastScanMatchPose = MapRep.MatchData(poseHintWorld, dataContainer, out Matrix4x4 lastScanMatchCov);
LastScanMatchCov = lastScanMatchCov;
}
else
{
LastScanMatchPose = poseHintWorld;
}
//std::cout << "\nt1:\n" << newPoseEstimateWorld << "\n";
//std::cout << "\n1";
//std::cout << "\n" << lastScanMatchPose << "\n";
if (Util.Util.PoseDifferenceLargerThan(LastScanMatchPose, LastMapUpdatePose, MinDistanceDiffForMapUpdate, MinAngleDiffForMapUpdate) || mapWithoutMatching)
{
MapRep.UpdateByScan(dataContainer, LastScanMatchPose);
MapRep.OnMapUpdated();
LastMapUpdatePose = LastScanMatchPose;
}
if (drawInterface != null)
{
GridMap gridMapRef = MapRep.GetGridMap(0);
drawInterface.SetColor(1.0, 0.0, 0.0);
drawInterface.SetScale(0.15);
drawInterface.DrawPoint(gridMapRef.GetWorldCoords(Vector2.Zero));
drawInterface.DrawPoint(gridMapRef.GetWorldCoords(gridMapRef.Dimensions.ToVector2()));
drawInterface.DrawPoint(new Vector2(1.0f, 1.0f));
drawInterface.SendAndResetData();
}
if (debugInterface != null)
{
debugInterface.SendAndResetData();
}
}
/// <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);
}