示例#1
0
文件: Landmarks.cs 项目: MoIdriez/SM
        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);
        }
示例#2
0
文件: Landmarks.cs 项目: MoIdriez/SM
        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;
            }
        }
示例#3
0
文件: Landmarks.cs 项目: MoIdriez/SM
        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);
        }
示例#4
0
文件: Landmarks.cs 项目: MoIdriez/SM
        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();
            }
            int    totalFound = 0;
            double val        = laserdata[0];

            for (int i = 1; i < laserdata.Length - 1; i++)
            {
                // Check for error measurement in laser data
                if (laserdata[i - 1] < 8.1)
                {
                    if (laserdata[i + 1] < 8.1)
                    {
                        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] < 8.1)
                        {
                            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);
        }
示例#5
0
 public agent(string _name, int _id, GameObject _go, landmark loc)
 {
     id       = _id;
     name     = _name;
     isBusy   = false;
     go       = _go;
     location = loc;
 }
示例#6
0
文件: Landmarks.cs 项目: MoIdriez/SM
        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);
        }
示例#7
0
文件: Landmarks.cs 项目: MoIdriez/SM
        /*
         * public int GetSlamID(int id)
         * {
         *  for (int i = 0; i < EKFLandmarks; i++)
         *  {
         *      if (IDtoID[i, 0] == id)
         *          return IDtoID[i, 1];
         *  }
         *  return -1;
         * }
         *
         * public int AddSlamID(int landmarkID, int slamID)
         * {
         *  IDtoID[EKFLandmarks, 0] = landmarkID;
         *  IDtoID[EKFLandmarks, 1] = slamID;
         *  EKFLandmarks++;
         *  return 0;
         * }
         */
        public Landmarks(double degreesPerScan)
        {
            this.degreesPerScan = degreesPerScan;

            for (int i = 0; i < landmarkDB.Length; i++)
            {
                landmarkDB[i] = new landmark();
            }
        }
示例#8
0
文件: Landmarks.cs 项目: MoIdriez/SM
 public landmark[] GetDB()
 {
     landmark[] temp = new landmark[DBSize];
     for (int i = 0; i < DBSize; i++)
     {
         temp[i] = landmarkDB[i];
     }
     return(temp);
 }
示例#9
0
文件: Landmarks.cs 项目: MoIdriez/SM
        /*
         * public landmark[] UpdateAndAddLandmarks(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();
         *  int totalFound = 0;
         *  double val = laserdata[0];
         *  for (int i = 1; i < laserdata.Length - 1; i++)
         *  {
         *      // Check for error measurement in laser data
         *      if (laserdata[i - 1] < 8.1)
         *          if (laserdata[i + 1] < 8.1)
         *              if ((laserdata[i - 1] - laserdata[i]) + (laserdata[i + 1] - laserdata[i]) > 0.5)
         *                  tempLandmarks[i] = UpdateLandmark(laserdata[i], i, robotPosition);
         *              else
         *                  if ((laserdata[i - 1] - laserdata[i]) > 0.3)
         *                      tempLandmarks[i] = UpdateLandmark(laserdata[i], i, robotPosition);
         *                  else if (laserdata[i + 1] < 8.1)
         *                      if ((laserdata[i + 1] - laserdata[i]) > 0.3)
         *                      tempLandmarks[i] = UpdateLandmark(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[] 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);
        }
示例#10
0
文件: Landmarks.cs 项目: MoIdriez/SM
        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);
        }
示例#11
0
文件: Landmarks.cs 项目: MoIdriez/SM
        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);
        }
示例#12
0
文件: Landmarks.cs 项目: MoIdriez/SM
        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);
        }
示例#13
0
文件: Landmarks.cs 项目: MoIdriez/SM
        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);
        }
示例#14
0
文件: Landmarks.cs 项目: MoIdriez/SM
 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
             return(((landmark)landmarkDB[i]).id);
         }
     }
     return(-1);
 }
示例#15
0
文件: Landmarks.cs 项目: MoIdriez/SM
        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);
        }
示例#16
0
文件: Landmarks.cs 项目: MoIdriez/SM
        private landmark GetLineLandmark(double a, double b, double[] robotPosition)
        {
            //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.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);
        }
示例#17
0
文件: Landmarks.cs 项目: MoIdriez/SM
        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);
        }
示例#18
0
文件: Landmarks.cs 项目: MoIdriez/SM
        public int AddToDB(landmark lm)
        {
            if (DBSize + 1 < landmarkDB.Length)
            {
                //for(int i=0; i<DBSize+1; i++)
                //{
                //if(((landmark)landmarkDB[i]).id != i)//if(((landmark)landmarkDB[i]).id == -1 || ((landmark)landmarkDB[i]).life <= 0)
                //{
                ((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 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
                DBSize++;

                return(DBSize - 1);
            }
            return(-1);
        }
示例#19
0
文件: Landmarks.cs 项目: MoIdriez/SM
 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)));
 }
示例#20
0
文件: Landmarks.cs 项目: MoIdriez/SM
        public landmark[] ExtractLineLandmarks(double[] laserdata, double[] robotPosition)
        {
            //two arrays corresponding to found lines
            double[] la         = new double[100];
            double[] lb         = 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();
            }

            int totalFound = 0;

            double val             = laserdata[0];
            double lastreading     = laserdata[2];
            double lastlastreading = laserdata[2];

            /*
             * //removes worst outliers (points which for sure aren't on any lines)
             * for (int i = 2; i < laserdata.Length - 1; i++)
             * {
             *  // Check for error measurement in laser data
             *  if (laserdata[i] < 8.1)
             *      if(Math.Abs(laserdata[i]-lastreading)+Math.Abs(lastreading-lastlastreading) < 0.2)
             *      {
             *          linepoints[totalLinepoints] = i;
             *           totalLinepoints++;
             *           //tempLandmarks[i] = GetLandmark(laserdata[i], i, robotPosition);
             *           lastreading = laserdata[i];
             *           lastreading = laserdata[i-1];
             *      }
             *      else
             *      {
             *          lastreading = laserdata[i];
             *          lastlastreading = laserdata[i-1];
             *      }
             * }
             */
            //FIXME - OR RATHER REMOVE ME SOMEHOW...
            for (int i = 0; i < laserdata.Length - 1; i++)
            {
                linepoints[totalLinepoints] = i;
                totalLinepoints++;
            }

            #region RANSAC
            //RANSAC ALGORITHM
            int    noTrials = 0;
            Random rnd      = new Random();
            while (noTrials < MAXTRIALS && totalLinepoints > MINLINEPOINTS)
            {
                int[] rndSelectedPoints = 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 = rnd.Next(MAXSAMPLE, totalLinepoints - 1);
                rndSelectedPoints[0] = centerPoint;
                for (int i = 1; i < MAXSAMPLE; i++)
                {
                    newpoint = false;
                    while (!newpoint)
                    {
                        temp = centerPoint + (rnd.Next(2) - 1) * rnd.Next(0, MAXSAMPLE);
                        for (int j = 0; j < i; j++)
                        {
                            if (rndSelectedPoints[j] == temp)
                            {
                                break; //point has already been selected
                            }
                            if (j >= i - 1)
                            {
                                newpoint = true; //point has not already been selected
                            }
                        }
                    }
                    rndSelectedPoints[i] = temp;
                }

                //compute model M1
                double a = 0;
                double b = 0;

                //y = a+ bx
                LeastSquaresLineEstimate(laserdata, robotPosition, rndSelectedPoints, MAXSAMPLE, ref a, ref b);

                //– 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, 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];
                    //x
                    //y =(Math.Sin((linepoints[i] * degreesPerScan * 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);
                    //for now add points associated to line as landmarks to see results
                    for (int i = 0; i < totalConsensusPoints; i++)
                    {
                        //tempLandmarks[consensusPoints[i]] = GetLandmark(laserdata[consensusPoints[i]], consensusPoints[i], robotPosition);
                        //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;
                    totalLines++;
                    //restart search since we found a line
                    //noTrials = MAXTRIALS; //when maxtrials = debugging
                    noTrials = 0;
                }
                else
                {
                    //DEBUG add point that we chose as middle value
                    //tempLandmarks[centerPoint] = GetLandmark(laserdata[centerPoint], centerPoint, robotPosition);
                    //– If #(S1*) < t, randomly select another subset S2 and
                    //repeat
                    //– If, after some predetermined number of trials there is
                    //no consensus set with t points, return with failure
                    noTrials++;
                }
            }
            #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);
                //tempLandmarks[i+1] = GetLine(la[i], lb[i]);
            }
            //for debug add origin as landmark
            //tempLandmarks[totalLines+1] = GetOrigin();
            //tempLandmarks[i] = GetLandmark(laserdata[i], i, robotPosition);
            //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);
        }
示例#21
0
        public bool AddProperty()
        {
            try
            {
                property      prop    = new property();
                List <Marker> markers = GetMarkers();
                RealEntities  db      = new RealEntities();
                prop.name      = Name;
                prop.area      = Area;
                prop.latitude  = Latitude;
                prop.longitude = Longitude;
                prop.seller_id = SellerId;
                prop.category  = Category;
                prop.status    = 1;
                db.properties.Add(prop);
                db.SaveChanges();
                min_price price = new min_price();
                switch (prop.category)
                {
                case 1:
                {
                    price.plot_price      = LandPrice;
                    price.apartment_price = 0;
                    break;
                }

                case 2:
                {
                    price.plot_price      = LandPrice;
                    price.apartment_price = HousePrice;
                    break;
                }

                case 3:
                {
                    price.plot_price      = 0;
                    price.apartment_price = HousePrice;
                    break;
                }
                }
                price.property_id = prop.property_id;
                db.min_price.Add(price);
                db.SaveChanges();
                db.properties.Attach(prop);
                foreach (var marker in markers)
                {
                    RealEntities ldb = new RealEntities();
                    landmark     lm  = ldb.landmarks.Where(l => l.latitude == marker.Lat && l.longitude == marker.Lng && l.landmarktype == marker.Type).FirstOrDefault();
                    if (null == lm)
                    {
                        lm = new landmark();
                        lm.landmarktype = marker.Type;
                        lm.latitude     = marker.Lat;
                        lm.longitude    = marker.Lng;
                        lm.name         = marker.Name;
                        ldb.landmarks.Add(lm);
                        ldb.SaveChanges();
                    }
                    if (0 != lm.landmark_id)
                    {
                        landmark mark = db.landmarks.Where(l => l.landmark_id == lm.landmark_id).Single();
                        db.landmarks.Attach(mark);
                        prop.landmarks.Add(mark);
                    }
                }
                db.SaveChanges();

                foreach (HttpPostedFileBase image in Images)
                {
                    //Checking file is available to save.
                    if (image != null)
                    {
                        image img = new image();
                        img.property_id = prop.property_id;
                        MemoryStream target = new MemoryStream();
                        image.InputStream.CopyTo(target);
                        img.image1 = target.ToArray();
                        db.images.Add(img);
                    }
                }
                db.SaveChanges();
                return(true);
            }
            catch (Exception ex)
            {
            }
            return(false);
        }