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