Esempio n. 1
0
        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;
        }
Esempio n. 2
0
        public EkfSlam(double degreesPerScan)
        {
            this.degreesPerScan = degreesPerScan;

            for (int i = 0; i < landmarkDB.Length; i++)
            {
                landmarkDB [i] = new Landmark ();
            }
        }
Esempio n. 3
0
        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);
        }
Esempio n. 4
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);
        }
Esempio n. 5
0
        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);
        }
Esempio n. 6
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);
        }
Esempio n. 7
0
        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);
        }
Esempio n. 8
0
 /// <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 ();
 }
Esempio n. 9
0
 /// <summary>
 /// Updates the landmarks.
 /// </summary>
 /// <param name="landmarkArray">Landmark array.</param>
 public void UpdateLandmarks(Landmark[] landmarkArray)
 {
     landmarks.Clear ();
     landmarks.AddRange (landmarkArray);
     RaiseMapUpdate ();
 }
Esempio n. 10
0
        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;
        }
Esempio n. 11
0
 /// <summary>
 /// Adds a single landmark to the map.
 /// </summary>
 /// <param name="landmark">Landmark.</param>
 public void AddLandmark(Landmark landmark)
 {
     landmarks.Add (landmark);
     RaiseMapUpdate ();
 }
Esempio n. 12
0
        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);
        }
Esempio n. 13
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;
        }
Esempio n. 14
0
        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;
        }
Esempio n. 15
0
        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);
        }
Esempio n. 16
0
        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;
        }
Esempio n. 17
0
        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;
        }
Esempio n. 18
0
        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;
            }
        }
Esempio n. 19
0
        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;
        }
Esempio n. 20
0
 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));
 }
Esempio n. 21
0
        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;
        }
Esempio n. 22
0
 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)));
 }
Esempio n. 23
0
        public Landmark[] GetDB()
        {
            Landmark[] temp = new Landmark[DBSize];

            for (int i = 0; i < DBSize; i++)
            {
                temp [i] = landmarkDB [i];
            }

            return temp;
        }
Esempio n. 24
0
        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;
        }
Esempio n. 25
0
        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);
        }
Esempio n. 26
0
        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;
        }
Esempio n. 27
0
        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;
        }
Esempio n. 28
0
        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;
        }
Esempio n. 29
0
        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;
        }
Esempio n. 30
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;
        }
Esempio n. 31
0
 /// <summary>
 /// Adds a single landmark to the map.
 /// </summary>
 /// <param name="landmark">Landmark.</param>
 public void AddLandmark(Landmark landmark)
 {
     landmarks.Add(landmark);
     RaiseMapUpdate();
 }
Esempio n. 32
0
        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;
        }