I have recently been researching several types of laser range finder with the goal of developing an effective robot navigation capability within an internal space.

There are so many laser range finders available on the market it can be difficult to know what to choose from. Just browsing Google shopping, there’s a huge list, which include:

• Parallax 15-122cm Laser Rangefinder (£86)
• Hokuyo URG-04LX-UG01 Scanning Laser Rangefinder (£880)
• SF10-C Laser Rangefinder (£430)
• LIDAR-Lite 2 Laser Rangefinder (£90)
• RoboPeak RPLIDAR 360° Laser Scanner (£250)

I really like the RPLIDAR 360° laser scanner – it sits at the lower end of the price range and is a 360 degree 2D scanner (LIDAR) solution, with more than 6 meters of effective range detection. The 2D point cloud data it creates can easily be consumed by a simultaneous localisation and mapping (SLAM) algorithm.

Incidentally, you’ll find a very similar laser range finder in a Neato XV robot vacuum cleaner – so if you’re a hacker at heart, you could tear one down, get the sensor, plus harvest a host of other useful parts.

Due to environmental conditions, a point cloud generated from any laser scanning device will contain noise – so how can we clean the data and establish clean landmarks? Landmarks are features which can easily be distinguished from the environment and repeatedly be reacquired, allowing the robot to localise itself.

I decided to implement a derivative of the RANSAC algorithm. Random sample consensus (RANSAC) is an iterative method to estimate parameters of a mathematical model from a set of observed data which contains outliers. For the point cloud data I’ve been looking at, I decided to drop the random element of the algorithm, which gives me more consistent results.

Given a dataset whose data elements contain both inliers and outliers, RANSAC uses a voting scheme to find the optimal fitting result. Data elements in the dataset are used to vote for one or more models. The implementation of this voting scheme is based on two assumptions: that the noisy features will not vote consistently for any single model and there are enough features to agree on a good model.

The code below firstly creates a noisy data set, simulating a laser scan point cloud for a square room. Then it applies my hacked implementation of the RANSAC algorithm to correctly identify walls from the noisy data.

 

Code Snippet
  1. using System;
  2. using System.Drawing;
  3. using System.Windows.Forms;
  4. public class Ransac : Form
  5. {
  6.     const int MAX_LINES = 4;
  7.     const int SAMPLE_SIZE = 10;
  8.     const int RANSAC_CONSENSUS = 50;
  9.     const double RANSAC_TOLERANCE = 7.5;
  10.     const double D2R = Math.PI / 180.0;
  11.     double[] LinesA = new double[MAX_LINES];
  12.     double[] LinesB = new double[MAX_LINES];
  13.     double[] PointCloud = new double[360];
  14.     bool Draw = false;
  15.     int LineCount = 0;
  16.     [STAThread]
  17.     static void Main()
  18.     {
  19.         Application.Run(new Ransac());
  20.     }
  21.     public Ransac()
  22.     {
  23.         Width = 500;
  24.         Height = 525;
  25.         Text = “RANSAC Demo”;
  26.         CreateNoisyPointCloud();
  27.         ExtractLines();
  28.         Draw = true;
  29.     }
  30.     private void CreateNoisyPointCloud()
  31.     {
  32.         Random rnd = new Random();
  33.         for (int i = 0; i < 90; i++)
  34.         {
  35.             double length = 150 / Math.Cos((i – 45) * D2R);
  36.             PointCloud[i] = length + rnd.NextDouble() * 16 – 8;
  37.             PointCloud[i + 90] = length + rnd.NextDouble() * 16 – 8;
  38.             PointCloud[i + 180] = length + rnd.NextDouble() * 16 – 8;
  39.             PointCloud[i + 270] = length + rnd.NextDouble() * 16 – 8;
  40.         }
  41.     }
  42.     private void ExtractLines()
  43.     {
  44.         int[] pointStatus = new int[360];
  45.         for (int seed = 0; seed < 360; seed++)
  46.         {
  47.             int[] sample = new int[SAMPLE_SIZE];
  48.             for (int i = 0; i < SAMPLE_SIZE; i++) sample[i] = (seed + i) % 360;
  49.             int[] fitPoints = new int[360];
  50.             int fitCount = 0;
  51.             double a = 0;
  52.             double b = 0;
  53.             LeastSquaresFit(PointCloud, sample, SAMPLE_SIZE, ref a, ref b);
  54.             for (int i = 0; i < 360; i++)
  55.             {
  56.                 if (pointStatus[i] == 0)
  57.                 {
  58.                     // Convert scan vectors to cartesian coordinates.
  59.                     double x = Math.Cos(i * D2R) * PointCloud[i];
  60.                     double y = Math.Sin(i * D2R) * PointCloud[i];
  61.                     // Claim points close to sample line.
  62.                     if (DistanceToLine(x, y, a, b) < RANSAC_TOLERANCE)
  63.                         fitPoints[fitCount++] = i;
  64.                 }
  65.             }
  66.             if (fitCount > RANSAC_CONSENSUS)
  67.             {
  68.                 // Refresh line and add to collection.
  69.                 LeastSquaresFit(PointCloud, fitPoints, fitCount, ref a, ref b);
  70.                 LinesA[LineCount] = a;
  71.                 LinesB[LineCount] = b;
  72.                 LineCount++;
  73.                 // Update point cloud status.
  74.                 for (int i = 0; i < fitCount; i++) pointStatus[fitPoints[i]] = LineCount;
  75.             }
  76.         }
  77.     }
  78.     private void LeastSquaresFit(double[] pointCloud, int[] selection, int count, ref double a, ref double b)
  79.     {
  80.         double sumX = 0;
  81.         double sumY = 0;
  82.         double sumXX = 0;
  83.         double sumYY = 0;
  84.         double sumXY = 0;
  85.         for (int i = 0; i < count; i++)
  86.         {
  87.             double x = Math.Cos(selection[i] * D2R) * pointCloud[selection[i]];
  88.             double y = Math.Sin(selection[i] * D2R) * pointCloud[selection[i]];
  89.             sumX += x;
  90.             sumXX += Math.Pow(x, 2);
  91.             sumY += y;
  92.             sumYY += Math.Pow(y, 2);
  93.             sumXY += x * y;
  94.         }
  95.         a = (count * sumXY – sumX * sumY) / (count * sumXX – Math.Pow(sumX, 2));
  96.         b = (sumY * sumXX – sumX * sumXY) / (count * sumXX – Math.Pow(sumX, 2));
  97.     }
  98.     private double DistanceToLine(double x, double y, double a, double b)
  99.     {
  100.         double ao = -1.0 / a;
  101.         double bo = y – ao * x;
  102.         double px = (b – bo) / (ao – a);
  103.         double py = ((ao * (b – bo)) / (ao – a)) + bo;
  104.         return Math.Sqrt(Math.Pow(x – px, 2) + Math.Pow(y – py, 2));
  105.     }
  106.     protected override void OnPaint(PaintEventArgs e)
  107.     {
  108.         if (Draw)
  109.         {
  110.             Draw = false;
  111.             Graphics g = e.Graphics;
  112.             Pen pen = new Pen(Color.Gray, 4);
  113.             for (int i = 0; i < LineCount; i++)
  114.             {
  115.                 int x1 = 0;
  116.                 int y1 = 250 + (int)(LinesA[i] * -250 + LinesB[i]);
  117.                 int x2 = 500;
  118.                 int y2 = 250 + (int)(LinesA[i] * 250 + LinesB[i]);
  119.                 g.DrawLine(pen, x1, y1, x2, y2);
  120.             }
  121.             pen = new Pen(Color.Red, 4);
  122.             for (int i = 0; i < 360; i++)
  123.             {
  124.                 int x = 250 + (int)(Math.Cos(i * D2R) * PointCloud[i]);
  125.                 int y = 250 + (int)(Math.Sin(i * D2R) * PointCloud[i]);
  126.                 g.DrawEllipse(pen, x, y, 1, 1);
  127.             }
  128.         }
  129.     }
  130. }