public int AddToDB(Landmark lm)
{
if (DBSize + 1 < landmarkDB.Length)
{
((Landmark)landmarkDB [DBSize]).pos [0] = lm.pos [0]; // Set landmark coordinates.
((Landmark)landmarkDB [DBSize]).pos [1] = lm.pos [1]; // Set landmark coordinates.
((Landmark)landmarkDB [DBSize]).life = LIFE; // Set landmark life counter.
((Landmark)landmarkDB [DBSize]).id = DBSize; // Set landmark id.
((Landmark)landmarkDB [DBSize]).totalTimesObserved = 1; // Initialise number of times
// we've seen the landmark.
((Landmark)landmarkDB [DBSize]).bearing = lm.bearing; // Set last bearing was seen at.
((Landmark)landmarkDB [DBSize]).range = lm.range; // Set last range was seen at.
((Landmark)landmarkDB [DBSize]).a = lm.a; // Store landmarks wall equation.
((Landmark)landmarkDB [DBSize]).b = lm.b; // Store landmarks wall equation.
((Landmark)landmarkDB [DBSize]).x1y1[0] = lm.x1y1[0];
((Landmark)landmarkDB [DBSize]).x1y1[1] = lm.x1y1[1];
((Landmark)landmarkDB [DBSize]).x2y2[0] = lm.x2y2[0];
((Landmark)landmarkDB [DBSize]).x2y2[1] = lm.x2y2[1];
DBSize++;
return (DBSize - 1);
}
return -1;
}
public EkfSlam(double degreesPerScan)
{
this.degreesPerScan = degreesPerScan;
for (int i = 0; i < landmarkDB.Length; i++)
{
landmarkDB [i] = new Landmark ();
}
}
public Landmark[] UpdateAndAddLandmarksUsingEKFResults(bool[] matched, int[] id, double[] ranges,
double[] bearings, double[] robotPosition)
{
Landmark[] foundLandmarks = new Landmark[matched.Length];
for (int i = 0; i < matched.Length; i++)
{
foundLandmarks [i] = UpdateLandmark(matched [i], id [i], ranges [i], bearings [i],
robotPosition);
}
return(foundLandmarks);
}
public int UpdateLineLandmark(Landmark lm)
{
// Try to do data-association on landmark.
int id = GetAssociation(lm);
// If we failed to associate landmark, then add it to DB.
if (id == -1)
{
id = AddToDB(lm);
}
return(id);
}
private Landmark UpdateLandmark(Landmark lm)
{
// Try to do data-association on landmark.
int id = GetAssociation(lm);
// If we failed to associate landmark, then add it to DB.
if (id == -1)
{
id = AddToDB(lm);
}
lm.id = id;
// Return landmarks.
return(lm);
}
private Landmark GetOrigin()
{
Landmark lm = new Landmark();
// Convert landmark to map coordinate.
lm.pos [0] = 0;
lm.pos [1] = 0;
lm.range = -1;
lm.bearing = -1;
// Associate landmark to closest landmark.
int id = -1;
int totalTimesObserved = 0;
GetClosestAssociation(lm, ref id, ref totalTimesObserved);
lm.id = id;
// Return landmarks.
return(lm);
}
private Landmark UpdateLandmark(bool matched, int id, double distance, double readingNo,
double[] robotPosition)
{
Landmark lm;
if (matched)
{
// EKF matched landmark so increase times landmark has been observed.
landmarkDB [id].totalTimesObserved++;
lm = landmarkDB [id];
}
else
{
// EKF failed to match landmark so add it to DB as new landmark.
lm = new Landmark();
// Convert landmark to map coordinate.
lm.pos [0] = Math.Cos((readingNo * degreesPerScan * conv) + (robotPosition [2] *
Math.PI / 180)
) * distance;
lm.pos [1] = Math.Sin((readingNo * degreesPerScan * conv) + (robotPosition [2] *
Math.PI / 180)
) * distance;
lm.pos [0] += robotPosition [0]; // Add robot position.
lm.pos [1] += robotPosition [1]; // Add robot position.
lm.bearing = readingNo;
lm.range = distance;
id = AddToDB(lm);
lm.id = id;
}
// Return landmarks.
return(lm);
}
/// <summary>
/// Adds a range of landmarks to the map.
/// </summary>
/// <param name="landmarkArray">Landmark array.</param>
public void AddLandmarks(Landmark[] landmarkArray)
{
landmarks.AddRange (landmarkArray);
RaiseMapUpdate ();
}
/// <summary>
/// Updates the landmarks.
/// </summary>
/// <param name="landmarkArray">Landmark array.</param>
public void UpdateLandmarks(Landmark[] landmarkArray)
{
landmarks.Clear ();
landmarks.AddRange (landmarkArray);
RaiseMapUpdate ();
}
private Landmark UpdateLandmark(Landmark lm)
{
// Try to do data-association on landmark.
int id = GetAssociation (lm);
// If we failed to associate landmark, then add it to DB.
if (id == -1)
{
id = AddToDB (lm);
}
lm.id = id;
// Return landmarks.
return lm;
}
/// <summary>
/// Adds a single landmark to the map.
/// </summary>
/// <param name="landmark">Landmark.</param>
public void AddLandmark(Landmark landmark)
{
landmarks.Add (landmark);
RaiseMapUpdate ();
}
public int AlignLandmarkData(Landmark[] extractedLandmarks, ref bool[] matched, ref int[] id,
ref double[] ranges, ref double[] bearings, ref double[,] lmrks, ref double[,] exlmrks)
{
int uniquelmrks = 0;
double leastDistance = 99999;
double temp;
Landmark[] uniqueLandmarks = new Landmark[100];
for (int i = 0; i < extractedLandmarks.Length; i++)
{
if (extractedLandmarks [i].id != -1)
{
leastDistance = 99999;
// Remove doubles in extractedLandmarks
// if two observations match same landmark, take closest landmark.
for (int j = 0; j < extractedLandmarks.Length; j++)
{
if (extractedLandmarks [i].id == extractedLandmarks [j].id)
{
if (j < i)
{
break;
}
temp = Distance(extractedLandmarks [j],
landmarkDB [extractedLandmarks [j].id]);
if (temp < leastDistance)
{
leastDistance = temp;
uniqueLandmarks [uniquelmrks] = extractedLandmarks [j];
}
}
}
}
if (leastDistance != 99999)
{
uniquelmrks++;
}
}
matched = new bool[uniquelmrks];
id = new int[uniquelmrks];
ranges = new double[uniquelmrks];
bearings = new double[uniquelmrks];
lmrks = new double[uniquelmrks, 2];
exlmrks = new double[uniquelmrks, 2];
for (int i = 0; i < uniquelmrks; i++)
{
matched [i] = true;
id [i] = uniqueLandmarks [i].id;
ranges [i] = uniqueLandmarks [i].range;
bearings [i] = uniqueLandmarks [i].bearing;
lmrks [i, 0] = landmarkDB [uniqueLandmarks [i].id].pos [0];
lmrks [i, 1] = landmarkDB [uniqueLandmarks [i].id].pos [1];
exlmrks [i, 0] = uniqueLandmarks [i].pos [0];
exlmrks [i, 1] = uniqueLandmarks [i].pos [1];
}
return(0);
}
public int UpdateLineLandmark(Landmark lm)
{
// Try to do data-association on landmark.
int id = GetAssociation (lm);
// If we failed to associate landmark, then add it to DB.
if (id == -1)
{
id = AddToDB (lm);
}
return id;
}
public Landmark[] UpdateAndAddLandmarksUsingEKFResults(bool[] matched, int[] id, double[] ranges,
double[] bearings, double[] robotPosition)
{
Landmark[] foundLandmarks = new Landmark[matched.Length];
for (int i = 0; i < matched.Length; i++)
{
foundLandmarks [i] = UpdateLandmark (matched [i], id [i], ranges [i], bearings [i],
robotPosition);
}
return foundLandmarks;
}
public Landmark[] ExtractLineLandmarks(double[] laserdata, double[] robotPosition)
{
// Two arrays corresponding to found lines.
double[] la = new double[100];
double[] lb = new double[100];
double[] lx1 = new double[100];
double[] ly1 = new double[100];
double[] lx2 = new double[100];
double[] ly2 = new double[100];
int totalLines = 0;
// Array of laser data points corresponding to the seen lines.
int[] linepoints = new int[laserdata.Length];
int totalLinepoints = 0;
// Have a large array to keep track of found landmarks.
Landmark[] tempLandmarks = new Landmark[400];
for (int i = 0; i < tempLandmarks.Length; i++)
{
tempLandmarks [i] = new Landmark();
}
// FIXME - OR RATHER REMOVE ME SOMEHOW...
for (int i = 0; i < laserdata.Length - 1; i++)
{
linepoints [totalLinepoints] = i;
totalLinepoints++;
}
#region RANSAC
// RANSAC ALGORITHM
int numberOfTrials = 0;
Random random = new Random();
while (numberOfTrials < MAXTRIALS && totalLinepoints > MINLINEPOINTS)
{
int[] randomSelectedPoints = new int[MAXSAMPLE];
int temp = 0;
bool newpoint;
// Randomly select a subset S1 of n data points and
// compute the model M1.
//
// Initial version chooses entirely randomly. Now choose
// one point randomly and then sample from neighbours within some defined
// radius.
int centerPoint = random.Next(MAXSAMPLE, totalLinepoints - 1);
randomSelectedPoints [0] = centerPoint;
for (int i = 1; i < MAXSAMPLE; i++)
{
newpoint = false;
while (!newpoint)
{
temp = centerPoint + (random.Next(2) - 1) * random.Next(0, MAXSAMPLE);
for (int j = 0; j < i; j++)
{
if (randomSelectedPoints [j] == temp)
{
break;
}
// Point has already been selected.
if (j >= i - 1)
{
newpoint = true;
}
// Point has not already been selected.
}
}
randomSelectedPoints [i] = temp;
}
// Compute model M1.
// y = ax + b same as y = mx + c
double a = 0;
double b = 0;
double x1 = 0;
double y1 = 0;
double x2 = 0;
double y2 = 0;
LeastSquaresLineEstimate(laserdata, robotPosition, randomSelectedPoints, MAXSAMPLE,
ref a, ref b, ref x1, ref y1, ref x2, ref y2);
// Determine the consensus set S1* of points is P
// compatible with M1 (within some error tolerance).
int[] consensusPoints = new int[laserdata.Length];
int totalConsensusPoints = 0;
int[] newLinePoints = new int[laserdata.Length];
int totalNewLinePoints = 0;
double x = 0;
double y = 0;
double d = 0;
for (int i = 0; i < totalLinepoints; i++)
{
// Convert ranges and bearing to coordinates.
x = (Math.Cos((linepoints [i] * degreesPerScan * conv) + robotPosition [2] * conv) *
laserdata [linepoints [i]]) + robotPosition [0];
y = (Math.Sin((linepoints [i] * degreesPerScan * conv) + robotPosition [2] * conv) *
laserdata [linepoints [i]]) + robotPosition [1];
d = DistanceToLine(x, y, a, b);
if (d < RANSAC_TOLERANCE)
{
// Add points which are close to line.
consensusPoints [totalConsensusPoints] = linepoints [i];
totalConsensusPoints++;
}
else
{
// Add points which are not close to line.
newLinePoints [totalNewLinePoints] = linepoints [i];
totalNewLinePoints++;
}
}
// If #(S1*) > t, use S1* to compute (maybe using least
// squares) a new model M1*.
if (totalConsensusPoints > RANSAC_CONSENSUS)
{
// Calculate updated line equation based on consensus points.
LeastSquaresLineEstimate(laserdata, robotPosition, consensusPoints,
totalConsensusPoints, ref a, ref b, ref x1, ref y1, ref x2, ref y2);
// For now add points associated to line as landmarks to see results.
for (int i = 0; i < totalConsensusPoints; i++)
{
// Remove points that have now been associated to this line.
newLinePoints.CopyTo(linepoints, 0);
totalLinepoints = totalNewLinePoints;
}
// Add line to found lines.
la [totalLines] = a;
lb [totalLines] = b;
lx1 [totalLines] = x1;
ly1 [totalLines] = y1;
lx2 [totalLines] = x2;
ly2 [totalLines] = y2;
totalLines++;
// Restart search since we found a line.
numberOfTrials = 0;
}
else
{
numberOfTrials++;
}
}
#endregion
// For each line we found:
// calculate the point on line closest to origin (0,0)
// add this point as a landmark.
for (int i = 0; i < totalLines; i++)
{
tempLandmarks [i] = GetLineLandmark(la [i], lb [i], robotPosition, lx1[i], ly1[i], lx2[i], ly2[i]);
}
// Now return found landmarks in an array of correct dimensions.
Landmark[] foundLandmarks = new Landmark[totalLines];
// Copy landmarks into array of correct dimensions.
for (int i = 0; i < foundLandmarks.Length; i++)
{
foundLandmarks [i] = (Landmark)tempLandmarks [i];
}
return(foundLandmarks);
}
private Landmark GetLandmark(double range, int readingNo, double[] robotPosition)
{
Landmark lm = new Landmark ();
// Convert landmark to map coordinate.
lm.pos [0] = Math.Cos ((readingNo * degreesPerScan * conv) +
(robotPosition [2] * Math.PI / 180)
) * range;
lm.pos [1] = Math.Sin ((readingNo * degreesPerScan * conv) +
(robotPosition [2] * Math.PI / 180)
) * range;
lm.pos [0] += robotPosition [0]; // Add robot position.
lm.pos [1] += robotPosition [1]; // Add robot position.
lm.range = range;
lm.bearing = readingNo;
// Associate landmark to closest landmark.
int id = -1;
int totalTimesObserved = 0;
GetClosestAssociation (lm, ref id, ref totalTimesObserved);
lm.id = id;
// Return landmarks.
return lm;
}
private Landmark GetLine(double a, double b)
{
// Our goal is to calculate point on line closest to origin (0, 0)
// calculate line perpendicular to input line. a * ao = -1
double ao = -1.0 / a;
// Get intersection between y = ax + b and y = aox
// so: aox = ax + b => aox - ax = b => x = b/(ao - a), y = ao*b/(ao - a)
double x = b / (ao - a);
double y = (ao * b) / (ao - a);
Landmark lm = new Landmark ();
// Convert landmark to map coordinate.
lm.pos [0] = x;
lm.pos [1] = y;
lm.range = -1;
lm.bearing = -1;
lm.a = a;
lm.b = b;
// Associate landmark to closest landmark.
int id = -1;
int totalTimesObserved = 0;
GetClosestAssociation (lm, ref id, ref totalTimesObserved);
lm.id = id;
// Return landmarks.
return lm;
}
private void GetClosestAssociation(Landmark lm, ref int id, ref int totalTimesObserved)
{
// Given a landmark we find the closest landmark in DB.
int closestLandmark = 0;
double temp;
double leastDistance = 99999; //99999m is least initial distance, its big.
for (int i = 0; i < DBSize; i++)
{
// Only associate to landmarks we have seen more than MINOBSERVATIONS times.
if (landmarkDB [i].totalTimesObserved > MINOBSERVATIONS)
{
temp = Distance (lm, landmarkDB [i]);
if (temp < leastDistance)
{
leastDistance = temp;
closestLandmark = landmarkDB [i].id;
}
}
}
if (leastDistance == 99999)
{
id = -1;
}
else
{
id = landmarkDB [closestLandmark].id;
totalTimesObserved = landmarkDB [closestLandmark].totalTimesObserved;
}
}
private int GetAssociation(Landmark lm)
{
// This method needs to be improved so we use innovation as a validation gate
// currently we just check if a landmark is within some predetermined distance
// of a landmark in DB.
for (int i = 0; i < DBSize; i++)
{
if (Distance (lm, landmarkDB [i]) < MAXERROR && ((Landmark)landmarkDB [i]).id != -1)
{
((Landmark)landmarkDB [i]).life = LIFE;
// Landmark seen so reset its life counter.
((Landmark)landmarkDB [i]).totalTimesObserved++; // Increase number of times we seen landmark.
((Landmark)landmarkDB [i]).bearing = lm.bearing; // Set last bearing seen at.
((Landmark)landmarkDB [i]).range = lm.range; // Set last range seen at.
//Test code in attempt to improve accuracy
//((Landmark)landmarkDB [i]).pos[0] = lm.pos[0];
//((Landmark)landmarkDB [i]).pos[1] = lm.pos[1];
//((Landmark)landmarkDB [i]).a = (((Landmark)landmarkDB [i]).a + lm.a)/2;
//((Landmark)landmarkDB [i]).b = (((Landmark)landmarkDB [i]).a+lm.b)/2;
//Console.Write ("\n\nentered\n\n");
return ((Landmark)landmarkDB [i]).id;
}
}
return -1;
}
private double Distance(Landmark lm1, Landmark lm2)
{
return Math.Sqrt (Math.Pow (lm1.pos [0] - lm2.pos [0], 2) + Math.Pow (lm1.pos [1] - lm2.pos [1], 2));
}
public Landmark[] RemoveDoubles(Landmark[] extractedLandmarks)
{
int uniquelmrks = 0;
double leastDistance = 99999;
double temp;
Landmark[] uniqueLandmarks = new Landmark[100];
for (int i = 0; i < extractedLandmarks.Length; i++)
{
// Remove landmarks that didn't get associated and also pass
// landmarks through our temporary landmark validation gate.
if (extractedLandmarks [i].id != -1 && GetAssociation (extractedLandmarks [i]) != -1)
{
leastDistance = 99999;
// Remove doubles in extractedLandmarks
// if two observations match same landmark, take closest landmark.
for (int j = 0; j < extractedLandmarks.Length; j++)
{
if (extractedLandmarks [i].id == extractedLandmarks [j].id)
{
if (j < i)
{
break;
}
temp = Distance (extractedLandmarks [j],
landmarkDB [extractedLandmarks [j].id]);
if (temp < leastDistance)
{
leastDistance = temp;
uniqueLandmarks [uniquelmrks] = extractedLandmarks [j];
}
}
}
}
if (leastDistance != 99999)
{
uniquelmrks++;
}
}
// Copy landmarks over into an array of correct dimensions.
extractedLandmarks = new Landmark[uniquelmrks];
for (int i = 0; i < uniquelmrks; i++)
{
extractedLandmarks [i] = uniqueLandmarks [i];
}
return extractedLandmarks;
}
private double Distance(Landmark lm1, Landmark lm2)
{
return(Math.Sqrt(Math.Pow(lm1.pos [0] - lm2.pos [0], 2) + Math.Pow(lm1.pos [1] - lm2.pos [1], 2)));
}
public Landmark[] GetDB()
{
Landmark[] temp = new Landmark[DBSize];
for (int i = 0; i < DBSize; i++)
{
temp [i] = landmarkDB [i];
}
return temp;
}
public Landmark[] ExtractSpikeLandmarks(double[] laserdata, double[] robotPosition)
{
// Have a large array to keep track of found landmarks.
Landmark[] tempLandmarks = new Landmark[400];
for (int i = 0; i < tempLandmarks.Length; i++)
{
tempLandmarks [i] = new Landmark ();
}
Console.WriteLine ();
int totalFound = 0;
for (int i = 1; i < laserdata.Length - 1; i++)
{
// Check for error measurement in laser data.
if (laserdata [i - 1] < 3)
{
if (laserdata [i + 1] < 3)
{
if ((laserdata [i - 1] - laserdata [i]) + (laserdata [i + 1] - laserdata [i]) > 0.5)
{
tempLandmarks [i] = GetLandmark (laserdata [i], i, robotPosition);
}
else
{
if ((laserdata [i - 1] - laserdata [i]) > 0.3)
{
tempLandmarks [i] = GetLandmark (laserdata [i], i, robotPosition);
}
else if (laserdata [i + 1] < 3)
{
if ((laserdata [i + 1] - laserdata [i]) > 0.3)
{
tempLandmarks [i] = GetLandmark (laserdata [i], i, robotPosition);
}
}
}
}
}
}
// Get total found landmarks so you can return array of correct dimensions.
for (int i = 0; i < tempLandmarks.Length; i++)
{
if (((int)tempLandmarks [i].id) != -1)
{
totalFound++;
}
}
// Now return found landmarks in an array of correct dimensions.
Landmark[] foundLandmarks = new Landmark[totalFound];
// Copy landmarks into array of correct dimensions.
int j = 0;
for (int i = 0; i < ((Landmark[])tempLandmarks).Length; i++)
{
if (((Landmark)tempLandmarks [i]).id != -1)
{
foundLandmarks [j] = (Landmark)tempLandmarks [i];
j++;
}
}
return foundLandmarks;
}
public Landmark[] ExtractSpikeLandmarks(double[] laserdata, double[] robotPosition)
{
// Have a large array to keep track of found landmarks.
Landmark[] tempLandmarks = new Landmark[400];
for (int i = 0; i < tempLandmarks.Length; i++)
{
tempLandmarks [i] = new Landmark();
}
Console.WriteLine();
int totalFound = 0;
for (int i = 1; i < laserdata.Length - 1; i++)
{
// Check for error measurement in laser data.
if (laserdata [i - 1] < 3)
{
if (laserdata [i + 1] < 3)
{
if ((laserdata [i - 1] - laserdata [i]) + (laserdata [i + 1] - laserdata [i]) > 0.5)
{
tempLandmarks [i] = GetLandmark(laserdata [i], i, robotPosition);
}
else
{
if ((laserdata [i - 1] - laserdata [i]) > 0.3)
{
tempLandmarks [i] = GetLandmark(laserdata [i], i, robotPosition);
}
else if (laserdata [i + 1] < 3)
{
if ((laserdata [i + 1] - laserdata [i]) > 0.3)
{
tempLandmarks [i] = GetLandmark(laserdata [i], i, robotPosition);
}
}
}
}
}
}
// Get total found landmarks so you can return array of correct dimensions.
for (int i = 0; i < tempLandmarks.Length; i++)
{
if (((int)tempLandmarks [i].id) != -1)
{
totalFound++;
}
}
// Now return found landmarks in an array of correct dimensions.
Landmark[] foundLandmarks = new Landmark[totalFound];
// Copy landmarks into array of correct dimensions.
int j = 0;
for (int i = 0; i < ((Landmark[])tempLandmarks).Length; i++)
{
if (((Landmark)tempLandmarks [i]).id != -1)
{
foundLandmarks [j] = (Landmark)tempLandmarks [i];
j++;
}
}
return(foundLandmarks);
}
private Landmark UpdateLandmark(bool matched, int id, double distance, double readingNo,
double[] robotPosition)
{
Landmark lm;
if (matched)
{
// EKF matched landmark so increase times landmark has been observed.
landmarkDB [id].totalTimesObserved++;
lm = landmarkDB [id];
}
else
{
// EKF failed to match landmark so add it to DB as new landmark.
lm = new Landmark ();
// Convert landmark to map coordinate.
lm.pos [0] = Math.Cos ((readingNo * degreesPerScan * conv) + (robotPosition [2] *
Math.PI / 180)
) * distance;
lm.pos [1] = Math.Sin ((readingNo * degreesPerScan * conv) + (robotPosition [2] *
Math.PI / 180)
) * distance;
lm.pos [0] += robotPosition [0]; // Add robot position.
lm.pos [1] += robotPosition [1]; // Add robot position.
lm.bearing = readingNo;
lm.range = distance;
id = AddToDB (lm);
lm.id = id;
}
// Return landmarks.
return lm;
}
private Landmark GetLineLandmark(double a, double b, double[] robotPosition,
double x1, double y1, double x2, double y2)
{
// our goal is to calculate point on line closest to origin (0,0)
// calculate line perpendicular to input line. a * ao = -1.
double ao = -1.0 / a;
// Landmark position.
double x = b / (ao - a);
double y = (ao * b) / (ao - a);
double range = Math.Sqrt (Math.Pow (x - robotPosition [0], 2) + Math.Pow (y - robotPosition [1], 2));
double bearing = Math.Atan ((y - robotPosition [1]) / (x - robotPosition [0])) - robotPosition [2];
// Now do same calculation but get point on wall closest to robot instead:
// y = aox + bo => bo = y - aox
double bo = robotPosition [1] - ao * robotPosition [0];
// Get intersection between y = ax + b and y = aox + bo
// so: aox + bo = ax + b => aox - ax = b - bo => x = (b - bo)/(ao - a),
// y = ao*(b - bo)/(ao - a) +bo
double px = (b - bo) / (ao - a);
double py = ((ao * (b - bo)) / (ao - a)) + bo;
double rangeError = Distance (robotPosition [0], robotPosition [1], px, py);
double bearingError = Math.Atan ((py - robotPosition [1]) /
(px - robotPosition [0])
) - robotPosition [2]; // Do you subtract or add robot bearing? I am not sure!
Landmark lm = new Landmark ();
// Convert landmark to map coordinate.
lm.pos [0] = x;
lm.pos [1] = y;
lm.x1y1 [0] = x1;
lm.x1y1 [1] = y1;
lm.x2y2 [0] = x2;
lm.x2y2 [1] = y2;
lm.range = range;
lm.bearing = bearing;
lm.a = a;
lm.b = b;
lm.rangeError = rangeError;
lm.bearingError = bearingError;
// Associate landmark to closest landmark.
int id = 0;
int totalTimesObserved = 0;
GetClosestAssociation (lm, ref id, ref totalTimesObserved);
lm.id = id;
lm.totalTimesObserved = totalTimesObserved;
// Return landmarks
return lm;
}
public Landmark[] UpdateAndAddLineLandmarks(Landmark[] extractedLandmarks)
{
// Returns the found landmarks.
Landmark[] tempLandmarks = new Landmark[extractedLandmarks.Length];
for (int i = 0; i < extractedLandmarks.Length; i++)
{
tempLandmarks [i] = UpdateLandmark (extractedLandmarks [i]);
}
return tempLandmarks;
}
public int AlignLandmarkData(Landmark[] extractedLandmarks, ref bool[] matched, ref int[] id,
ref double[] ranges, ref double[] bearings, ref double[,] lmrks, ref double[,] exlmrks)
{
int uniquelmrks = 0;
double leastDistance = 99999;
double temp;
Landmark[] uniqueLandmarks = new Landmark[100];
for (int i = 0; i < extractedLandmarks.Length; i++)
{
if (extractedLandmarks [i].id != -1)
{
leastDistance = 99999;
// Remove doubles in extractedLandmarks
// if two observations match same landmark, take closest landmark.
for (int j = 0; j < extractedLandmarks.Length; j++)
{
if (extractedLandmarks [i].id == extractedLandmarks [j].id)
{
if (j < i)
{
break;
}
temp = Distance (extractedLandmarks [j],
landmarkDB [extractedLandmarks [j].id]);
if (temp < leastDistance)
{
leastDistance = temp;
uniqueLandmarks [uniquelmrks] = extractedLandmarks [j];
}
}
}
}
if (leastDistance != 99999)
{
uniquelmrks++;
}
}
matched = new bool[uniquelmrks];
id = new int[uniquelmrks];
ranges = new double[uniquelmrks];
bearings = new double[uniquelmrks];
lmrks = new double[uniquelmrks, 2];
exlmrks = new double[uniquelmrks, 2];
for (int i = 0; i < uniquelmrks; i++)
{
matched [i] = true;
id [i] = uniqueLandmarks [i].id;
ranges [i] = uniqueLandmarks [i].range;
bearings [i] = uniqueLandmarks [i].bearing;
lmrks [i, 0] = landmarkDB [uniqueLandmarks [i].id].pos [0];
lmrks [i, 1] = landmarkDB [uniqueLandmarks [i].id].pos [1];
exlmrks [i, 0] = uniqueLandmarks [i].pos [0];
exlmrks [i, 1] = uniqueLandmarks [i].pos [1];
}
return 0;
}
private Landmark GetOrigin()
{
Landmark lm = new Landmark ();
// Convert landmark to map coordinate.
lm.pos [0] = 0;
lm.pos [1] = 0;
lm.range = -1;
lm.bearing = -1;
// Associate landmark to closest landmark.
int id = -1;
int totalTimesObserved = 0;
GetClosestAssociation (lm, ref id, ref totalTimesObserved);
lm.id = id;
// Return landmarks.
return lm;
}
/// <summary>
/// Adds a single landmark to the map.
/// </summary>
/// <param name="landmark">Landmark.</param>
public void AddLandmark(Landmark landmark)
{
landmarks.Add(landmark);
RaiseMapUpdate();
}
public Landmark[] ExtractLineLandmarks(double[] laserdata, double[] robotPosition)
{
// Two arrays corresponding to found lines.
double[] la = new double[100];
double[] lb = new double[100];
double[] lx1 = new double[100];
double[] ly1 = new double[100];
double[] lx2 = new double[100];
double[] ly2 = new double[100];
int totalLines = 0;
// Array of laser data points corresponding to the seen lines.
int[] linepoints = new int[laserdata.Length];
int totalLinepoints = 0;
// Have a large array to keep track of found landmarks.
Landmark[] tempLandmarks = new Landmark[400];
for (int i = 0; i < tempLandmarks.Length; i++)
{
tempLandmarks [i] = new Landmark ();
}
// FIXME - OR RATHER REMOVE ME SOMEHOW...
for (int i = 0; i < laserdata.Length - 1; i++)
{
linepoints [totalLinepoints] = i;
totalLinepoints++;
}
#region RANSAC
// RANSAC ALGORITHM
int numberOfTrials = 0;
Random random = new Random ();
while (numberOfTrials < MAXTRIALS && totalLinepoints > MINLINEPOINTS)
{
int[] randomSelectedPoints = new int[MAXSAMPLE];
int temp = 0;
bool newpoint;
// Randomly select a subset S1 of n data points and
// compute the model M1.
//
// Initial version chooses entirely randomly. Now choose
// one point randomly and then sample from neighbours within some defined
// radius.
int centerPoint = random.Next (MAXSAMPLE, totalLinepoints - 1);
randomSelectedPoints [0] = centerPoint;
for (int i = 1; i < MAXSAMPLE; i++)
{
newpoint = false;
while (!newpoint)
{
temp = centerPoint + (random.Next (2) - 1) * random.Next (0, MAXSAMPLE);
for (int j = 0; j < i; j++)
{
if (randomSelectedPoints [j] == temp)
{
break;
}
// Point has already been selected.
if (j >= i - 1)
{
newpoint = true;
}
// Point has not already been selected.
}
}
randomSelectedPoints [i] = temp;
}
// Compute model M1.
// y = ax + b same as y = mx + c
double a = 0;
double b = 0;
double x1 = 0;
double y1 = 0;
double x2 = 0;
double y2 = 0;
LeastSquaresLineEstimate (laserdata, robotPosition, randomSelectedPoints, MAXSAMPLE,
ref a, ref b, ref x1, ref y1, ref x2, ref y2);
// Determine the consensus set S1* of points is P
// compatible with M1 (within some error tolerance).
int[] consensusPoints = new int[laserdata.Length];
int totalConsensusPoints = 0;
int[] newLinePoints = new int[laserdata.Length];
int totalNewLinePoints = 0;
double x = 0;
double y = 0;
double d = 0;
for (int i = 0; i < totalLinepoints; i++)
{
// Convert ranges and bearing to coordinates.
x = (Math.Cos ((linepoints [i] * degreesPerScan * conv) + robotPosition [2] * conv) *
laserdata [linepoints [i]]) + robotPosition [0];
y = (Math.Sin ((linepoints [i] * degreesPerScan * conv) + robotPosition [2] * conv) *
laserdata [linepoints [i]]) + robotPosition [1];
d = DistanceToLine (x, y, a, b);
if (d < RANSAC_TOLERANCE)
{
// Add points which are close to line.
consensusPoints [totalConsensusPoints] = linepoints [i];
totalConsensusPoints++;
}
else
{
// Add points which are not close to line.
newLinePoints [totalNewLinePoints] = linepoints [i];
totalNewLinePoints++;
}
}
// If #(S1*) > t, use S1* to compute (maybe using least
// squares) a new model M1*.
if (totalConsensusPoints > RANSAC_CONSENSUS)
{
// Calculate updated line equation based on consensus points.
LeastSquaresLineEstimate (laserdata, robotPosition, consensusPoints,
totalConsensusPoints, ref a, ref b, ref x1, ref y1, ref x2, ref y2);
// For now add points associated to line as landmarks to see results.
for (int i = 0; i < totalConsensusPoints; i++)
{
// Remove points that have now been associated to this line.
newLinePoints.CopyTo (linepoints, 0);
totalLinepoints = totalNewLinePoints;
}
// Add line to found lines.
la [totalLines] = a;
lb [totalLines] = b;
lx1 [totalLines] = x1;
ly1 [totalLines] = y1;
lx2 [totalLines] = x2;
ly2 [totalLines] = y2;
totalLines++;
// Restart search since we found a line.
numberOfTrials = 0;
}
else
{
numberOfTrials++;
}
}
#endregion
// For each line we found:
// calculate the point on line closest to origin (0,0)
// add this point as a landmark.
for (int i = 0; i < totalLines; i++)
{
tempLandmarks [i] = GetLineLandmark (la [i], lb [i], robotPosition, lx1[i], ly1[i], lx2[i], ly2[i]);
}
// Now return found landmarks in an array of correct dimensions.
Landmark[] foundLandmarks = new Landmark[totalLines];
// Copy landmarks into array of correct dimensions.
for (int i = 0; i < foundLandmarks.Length; i++)
{
foundLandmarks [i] = (Landmark)tempLandmarks [i];
}
return foundLandmarks;
}