/// <summary>
        /// Reset
        /// </summary>
        public void Reset()
        {
            LastMapUpdatePose = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
            LastScanMatchPose = new Vector3(0.0f, 0.0f, 0.0f);

            MapRep.Reset();
        }
        /// <summary>
        /// Reset
        /// </summary>
        public void Reset()
        {
            MapRep.Reset();

            // Set initial poses
            MatchPose         = startPose;
            LastMapUpdatePose = new Vector3(float.MinValue, float.MinValue, float.MinValue);
        }
        /// <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);
        }
 public void SetUpdateFactorOccupied(float occupied_factor)
 {
     MapRep.SetUpdateFactorOccupied(occupied_factor);
 }
 public void SetUpdateFactorFree(float free_factor)
 {
     MapRep.SetUpdateFactorFree(free_factor);
 }
 public IMapLocker GetMapMutex(int i)
 {
     return(MapRep.GetMapMutex(i));
 }
 public void AddMapMutex(int i, IMapLocker mapMutex)
 {
     MapRep.AddMapMutex(i, mapMutex);
 }
 public GridMap GetGridMap(int mapLevel = 0)
 {
     return(MapRep.GetGridMap(mapLevel));
 }