Notifications
Clear all
Topic starter
2025-09-03 11:50 pm
So I’m going to scratch the surface of this topic with my efforts to implement a few algorithms that extract lines from a 2D point cloud. I performed this exercise with a hobby servo and a rangefinder impersonating a static robot. Is finding corners and openings just a matter of recognizing lines in a 2D point cloud and measuring the angle of their intersection? Well, that’s the approach I took.
I decided to try out two algorithms that I found in this paper “A Comparison of Line Extraction Algorithms using 2D Laser Rangefinder for Indoor Mobile Robotics“ by Nguyen, Martinelli, Tomatis & Siegwart. Those roboticists in Lausanne, Switzerland are so generous with their publications!
https://infoscience.epfl.ch/server/api/core/bitstreams/aab830b5-436f-4cc7-a580-7611e42d0a87/content
Split-and-Merge and Incremental were the preferred approaches discussed in the paper because of their speed and correctness. So those are the two algorithms that I implemented on a Raspberry Pi Pico using sample data gathered from the servo - rangefinder combination.
As a level set this is a 2D scan of a room I’ve described here https://forum.dronebotworkshop.com/sensors-modules/characterizing-a-vl53l1x-tof-based-ranging-sensor/ but now a cardboard box, a pile of stuff, a bed and a desk have been added to the scan.
These are the results of running Split-and-Merge (orange) and Incremental (purple) on the 2D scan of the room. I adjusted the parameters so that both results had the same number of line segments. To my eye it looks like the Split-and-Merge did a better job of including corners.
After getting these two algorithms to work I altered the spilt merge code to take the line segments created by the program and identify corners by using the angles created by two line segments and openings by identifying areas that the sensor could not detect.
Groups of three red boxes in this graph identify corners. I loosely defined a corner as any angle >70 degrees and less than 120 degrees.
uint8_t findMaxDistance(float angle[], float range[], uint8_t start, uint8_t end, float threshold) {
// Find the point with the maximum absolute distance from the trendline, using threshold to limit.
float firstTerm, secondTerm, thirdTerm, fourthTerm, theta, dist;
uint8_t n;
float maxDistance = 0.0f;
float distance = 0.0f;
uint8_t location = 0;
firstTerm = 0.0f;
secondTerm = 0.0f;
thirdTerm = 0.0f;
fourthTerm = 0.0f;
theta = 0.0f;
dist = 0.0f;
n = 0; // count of points in the set
if ((end - start) <= 1)
location = 0;
else {
for (int i = start; i <= end; i++ ) {
firstTerm += range[i] * range[i] * (sinf(2.0f * angle[i]));
thirdTerm += range[i] * range[i] * (cosf(2.0f * angle[i]));
n += 1;
}
firstTerm = firstTerm / n;
thirdTerm = thirdTerm / n;
for (int i = start; i <= end; i++) {
for (int j = start; j <= end; j++) {
secondTerm += range[i] * range[j] * cosf(angle[i]) * sinf(angle[j]);
fourthTerm += range[i] * range[j] * cosf(angle[i] + angle[j]);
}
}
secondTerm = (2.0f/(n*n)) * secondTerm;
fourthTerm = fourthTerm / (n*n);
theta = 1.0f/2.0f * atan2f((firstTerm - secondTerm),(thirdTerm - fourthTerm));
for (int i = start; i <= end; i++)
dist += range[i] * cosf(angle[i] - theta);
dist = dist / n;
for (int i = start; i <= end; i++) {
distance = range[i] * cos(angle[i] - theta) - dist;
//printf("distance from trendline for index=%d is %2.4f units\n",i,distance);
if ((fabs(distance) > fabs(maxDistance)) && (fabs(distance) > threshold)) {
maxDistance = distance;
location = i;
}
}
}
return location;
} // end findMaxDistance
The secret sauce in these two programs is the findMaxDistance( ) function. It uses the total-least-squares method for line fitting that the authors wrote about. Although my version is unweighted so it’s a bit simpler. I use the same routine in the Incremental and the Split-and-Merge algorithms. So if you’re curious and want to try out the Split and Merge (which I think did a better job on my data) the procedure is in the paper and the code in this post has what you need to determine distance from the line…
Tom
To err is human.
To really foul up, use a computer.
Topic starter
2025-10-13 6:57 pm
I want to explain how the Split-and-Merge algorithm works and the strange results that I’m sometimes getting.
A key part of this algorithm is to find a trend line that fits a set of sensor measurements and then using a total least squares method determine the distance of the sensor measurement from the trend line. The details can be found in “Introduction to Autonomous Mobile Robots “by Siegwart, Nourbakhsh & Scaramuzza.
Given a vector with angle alpha and length r that forms a perpendicular angle to a trend line it is possible to use trigonometry to determine the distance d of a sensor measurement to the trend line. This is the equation to find that distance. The point x is in polar coordinate form.
So the next piece to this approach are the equations that give the angle alpha and length r for a set of measurements. Siegwart’s use of weighted sensor measurements gave me pause, so I used an un-weighted version that I found in “Introduction to Autonomous Robots: Mechanisms, Sensors, Actuators and Algorithms” by Correll, Hayes, Heckman & Roncone.
This is the Split-and-Merge algorithm from the “A Comparison of Line Extraction Algorithms” paper I cited above.
1) Initial: set S1 consists of N points, Put S1 in a list L
2) Fit a line to the next set Si in L
3) Detect point p with maximum distance Dp to the line
4) If Dp is less than a threshold, continue (go to step 2)
5) Otherwise, split Si at p into Si1 and Si2, replace Si in L by Si1 and Si2, continue (go to 2)
6) When all sets (segments) in L have been checked, merge collinear segments
We make a slight modification to line 3 so that we scan for a splitting position where 2 adjacent points P1 and P2 are at the same side to the line and both have distances to the line greater than the threshold (if only 1 such point is found, it is ignored as a noisy point).
I didn’t implement their slight modification, but it sounds useful.
In my implementation I came up with two parameters. The first is the threshold used to identify where to split a line into two segments and the second is used to control how close to collinear two lines need to be before they can be merged.
Pressing on. Siegwart gives a “concrete” example of 17 measurements taken with a laser range sensor. Applying the previous equations he arrived with a trend line defined by angle a = 37.36 degrees and length r = 0.4 meters. When I run the equations I get angle a = 37.95 degrees and r = 0.3999. So I’m not spot on, but I thought that I was close enough to continue.
The following two graphs show the trend line for the first two iterations through the algorithm using Siegwart’s example. The measurement that exceeds the threshold the most is number 17, which I show as a circle. So I hold out the line segment of points 16 & 17 (to be merged later) and next create a new trend line using points 1 through 16. With this trend line point 3 is identified as exceeding the threshold by the greatest amount and it becomes the next dividing point of the remaining points. This process continues until all the line segments have been identified, then the merge process combines those segments that are collinear.
But things get a little weird when it creates the trend line using points 3 through 5. The trend line that the equations come up with looks like this.
The numerator that arctan is given is negative so it comes up with a positive slope line, instead of this one.
I didn’t catch this until I started comparing least squares trend lines to the trend lines created by these equations.
In practice the split process breaks down the set of points into many more 2 point line segments then it needs to and the merge process puts them back together because they are collinear :/
After trying many different approaches to fix this problem I settled on creating three versions of the alpha angle and its length:
alpha = 1/2 arctan(numerator/denominator)
alpha = 1/2 (arctan(numerator/denominator) + Pi)
alpha = 1/2 (arctan(numerator/denominator) - Pi)
I then compare the slope of these angles to the slope of a least squares regression line and select the closest match. Using this hack of arctan2 I’m getting better results with the two line extraction algorithms I’ve been testing.
Maybe I should puzzle out Siegwart’s use of weighted sensor measurements to come up with the alpha and r parameters and see if that clears up the weirdness.
Tom
To err is human.
To really foul up, use a computer.