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