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