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