public void updateValues(Laser l, Pose2D curPose) { //RobotPos = new Point3D(pos); //RobotRot = new Point3D(rot); ////Add For KF //KRobotPos = new Point3D(pos); //KRobotRot = new Point3D(rot); RobotCURState = new Pose2D(curPose.Position, curPose.Rotation); ReservedValidData = (l != null); LaserData = l; currentMatch = null; timeCycle++; }
private bool retryIfSlamFailed() { retryMode = true; for (retryCycle = 0; retryCycle < MAX_RETRY_CYCLE; retryCycle++) { LineMatchResult lm = GetMovementCorrection(MapEdges, NewEdges); Pose2D Changes; if (lm != null) { Changes = lm.changes; } else { Changes = null; } hasValidOdometryForEvaluation = false; if (Changes != null) { if (NewEdges.Count > 1) { if (isChangeValid(Changes)) { currentMatch = lm; RobotCURState.Position += Changes.Position; RobotCURState.Rotation += Changes.Rotation; hasValidOdometryForEvaluation = true; getNewEdgesRegardingNewEstimatedChanges(); UpdateEdges(NewEdges, lm.matchedEdges); // Age BEkhaym Taghiri Roosh BEdim In Update Edges Kharab Mishe. continuousSlamFailureCycles = 0; break; } } } } retryMode = false; return(hasValidOdometryForEvaluation); }
private LineMatchResult GetMovementCorrection(List <Edge> MapEdges, List <Edge> NewEdges) { List <Edge> possibleMatches; Pose2D conv; float best = float.MaxValue; List <LineMatchResult> matches = new List <LineMatchResult>(); LineMatchResult lmt; float maxDisttoMatch; foreach (Edge edge in NewEdges) { if (EDGE_IN_NEIGHBOUR_FINDING_NEW) { maxDisttoMatch = getMaxDistanceToMatchNeighbourNewEdges(edge); possibleMatches = getEdgesInNeighbourNew(edge, maxDisttoMatch); } else { possibleMatches = getEdgesInNeighbourOld(edge, MAX_DISTANCE_TO_MATCH); } foreach (Edge ted in possibleMatches) { conv = getConversions(ted, edge); if (conv == null) { continue; } lmt = edgeMatcher.getCorrelation_NEW(conv, NewEdges, MapEdges, RobotCURState.Position); if (lmt.corrolation == float.NaN) { continue; } matches.Add(lmt); } } if (matches.Count == 0) { return(null); } float curCount = NewEdges.Count; List <LineMatchResult> tt = new List <LineMatchResult>(); do { foreach (LineMatchResult ch in matches) { if (ch.count == curCount && ch.corrolation < MAX_CORROLATION_TO_CONSIDER_MATCH) // { if (isChangeValid(ch.changes)) { tt.Add(ch); } } } curCount--; } while (curCount > 0 && tt.Count == 0); // FAQAT Ta Vaghty Ke Ba Maximum Te'dad Javab Begire. curCount++; if (tt.Count == 0) { return(null); } best = float.MaxValue; LineMatchResult reslm = null; foreach (LineMatchResult mt in tt) { if (mt.corrolation < best) { best = mt.corrolation; reslm = mt; } } return(reslm); }
public int DoSLAM() { if (!ReservedValidData) { return(-4); } //featureExtractor.updateValues(RobotPos, RobotRot, LaserData, timeCycle); featureExtractor.updateValues(RobotCURState, LaserData, timeCycle); NewLines = featureExtractor.ExtractParts(); NewEdges = featureExtractor.FindEdge(); // Calculate Odometry if (!isInit) { MapEdges.Clear(); float d; foreach (Edge e in NewEdges) { d = MathHelper.GetDistance(e.point, RobotCURState.Position); //e.point.getDistance2D(RobotPos); if (d < MAX_DIST_EDGE_ROBOT_TO_ADD_NEW_EDGE) { MapEdges.Add(e); } } isInit = !isInit; return(1); } else { if (timeCycle % TRIMING_PERIOD == 0) { edgeTrimmer.trimMapEdges(MapEdges, timeCycle); } Debug.DrawPoints(GetEdgeVector(MapEdges), 1); Debug.DrawPoints(GetEdgeVector(NewEdges), 2); currentMatch = GetMovementCorrection(MapEdges, NewEdges); if (currentMatch != null) { DeltaPose = currentMatch.changes; } else { DeltaPose = null; } hasValidOdometryForEvaluation = false; continuousSlamFailureCycles++; if (DeltaPose != null) { if (NewEdges.Count > 1) { if (isChangeValid(DeltaPose)) { RobotCURState.Position += DeltaPose.Position; RobotCURState.Rotation += DeltaPose.Rotation; hasValidOdometryForEvaluation = true; getNewEdgesRegardingNewEstimatedChanges(); UpdateEdges(NewEdges, currentMatch.matchedEdges);// Age BEkhaym Taghiri Roosh BEdim In Update Edges Kharab Mishe. continuousSlamFailureCycles = 0; return(0); } else { if (RETRY_IF_SLAM_FAILED && RETRY_IF_NON_VALID_TRANS && retryIfSlamFailed()) { hasValidOdometryForEvaluation = true; return(0); } else { return(-1); } } } else { return(-2); } } else { return(-1); } } }