Proposal for New Algorithm Sample: Robust Least-Squares Rectangle Fitting (2D LiDAR)

[diogoccprado]

Algorithm Name
Robust Least-Squares Rectangle Fitting (with robust loss)

Description
This sample estimates an oriented rectangle for each detected object in a 2D LiDAR scan. The scan is first segmented into clusters (each cluster represents one object), then robust least-squares fitting is applied per cluster to output one rectangle per object. Using a robust loss reduces the influence of outliers within each cluster, improving stability under noise and partial occlusions.

Plan for Implementation

1. Integration and Interface

• Add a new sample script under src/simulations/perception/point_cloud_rectangle_fitting/ following the existing L-shape fitting sample structure (same I/O and plotting style).

2. Rectangle Parameterization + Initialization

• Reuse the existing clustering/object-segmentation step, then run the fitter independently for each cluster to produce multiple rectangles in a scene.
• Parameterize rectangle as (cx, cy, theta, width, length) (or half-sizes).
• Use a stable initialization (PCA-based orientation + extents, or the existing L-shape estimate as an initial guess).
• Ensure width/length remain positive during optimization (e.g., optimize half-sizes or log-parameters).

3. Residuals and Optimization

• Define one residual per point as the distance to the closest rectangle edge (≈ 0 when the point lies on the rectangle boundary).
• Solve with scipy.optimize.least_squares using a robust loss (soft_l1 or huber) and an f_scale value in meters that matches expected inlier noise.
• Use a solver method compatible with robust losses (e.g., trf).

4. Visualization and Testing

• Add a Matplotlib animation showing the rectangle updating during optimization (iterations / parameter updates).
• Add unit tests with synthetic rectangles: edge points + Gaussian noise + injected outliers (animation disabled).
• Validate using measurable errors (e.g., center error, yaw error, size error, and/or IoU vs ground truth rectangle).

References
SciPy least_squares (robust losses: huber, soft_l1, cauchy):
https://docs.scipy.org/doc/scipy/reference/generated/scipy.optimize.least_squares.html

Robust regression / M-estimation overview (intuition for downweighting outliers):
https://pmc.ncbi.nlm.nih.gov/articles/PMC6167755/

[ShisatoYano]

Hi @DiCas22,

Thank you for opening this issue!

Your proposal looks so great. So you can start implementing and testing.

I’m looking forward to your Pull Request!

[ShisatoYano]

Hi @DiCas22 , since this project has recently reached 1,130 stars and is gaining a lot of attention, I'd love to see this feature implemented soon to make my OSS more useful.

As I haven't heard back for a while, I'm opening this task to the community. If anyone is interested in implementing this new algorithm sample, please feel free to jump in!

[shreyshet]

@ShisatoYano I would like to work on this

[ShisatoYano]

That's great! Thank you for contributing, @shreyshet . I've assigned this issue to you. Looking forward to your Pull Request! If you have any questions about the current code structure, feel free to ask here. Happy coding! 🚀

[rai-sukant]

Hey can I work on this as well. @ShisatoYano

[ShisatoYano]

Hi @rai-sukant , thank you for suggesting. This issue has already been assigned to @shreyshet . So if you are familiar with any other LiDAR point cloud clustering or segmentation algorithms, I wanna ask you to implement the sample program. What do you think?

[belkacem-inelecer]

Hello, @DiCas22 @ShisatoYano. Kindly let me know where i could contribute. Thanks

[rai-sukant]

@ShisatoYano Sure, I shall implement the sample and let you know.

[ShisatoYano]

@rai-sukant Firstly, please read this contribution guide.
https://github.com/ShisatoYano/AutonomousVehicleControlBeginnersGuide/blob/main/HOWTOCONTRIBUTE.md

After that, please create a new issue to explain about what you implement for my project. Additionally, you can refer to this existing code to understand the current design and architecture.
https://github.com/ShisatoYano/AutonomousVehicleControlBeginnersGuide/tree/main/src/simulations/perception/point_cloud_rectangle_fitting

[ShisatoYano]

@belkacem-inelecer Thank you for your suggestion. If you can or are interested in, could you contribute for this issue #41 ?

[shreyshet]

Hi @ShisatoYano ,
I wanted to share an update on the progress so far. The journey from a basic geometric fit to a robust optimizer was... educational, to say the least.

1. The Naive Start and the "Reality Check"

I initially started with a straightforward approach:

• Initialization: Apply PCA (Principal Component Analysis) to the LiDAR point cloud.Estimation: Use PCA to find the initial guess for the rectangle parameters: center $(c_x, c_y)$, heading $(\theta)$, width $(w)$, and length $(l)$.
• Optimization: Program a residual loss function based on rectangle geometry and feed it into scipy.optimize.least_squares().The Result: To put it mildly, this did not work.The optimizer frequently failed to converge. When it did, the box was often "way off" the ground truth—either trapped in a 45-degree local minimum or skewed by noisy outliers.
• I realized that this fitting is a highly non-linear, non-convex problem. If the initial PCA guess is off by just a few degrees, the solver starts in the "wrong valley" and never recovers.

2. Mathematical formulation of Residual Function

The optimization objective is to find the parameter vector $\mathbf{p} = [c_x, c_y, \theta, w, l]^\top$ that minimizes the total cost across $N$ LiDAR points:

$$\min_{\mathbf{p}} \sum_{i=1}^{N} \rho(r_i(\mathbf{p})^2)$$

Where $\rho$ is the loss function, eg Cauchy or Huber and $r_i$ is the geometric residual for point $i$.

---

2.1 Local Coordinate Transformation

Each world-frame point $\mathbf{P}i = (P{x,i}, P_{y,i})$ is transformed into the box's local coordinate system (translation followed by rotation):

$$ \begin{bmatrix} \Delta x_i \ \Delta y_i \end{bmatrix} = \begin{bmatrix} \cos\theta & \sin\theta \\ -\sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} P_{x,i} - c_x \\ P_{y,i} - c_y \end{bmatrix} $$

2.2 Signed Distance Field (SDF) Residual

We calculate the distance from the point to the box boundary by comparing the local coordinates to the box half-extents ($w/2, l/2$). We first find the "overflow" values:

$$e_{x,i} = |\Delta x_i| - \frac{w}{2}$$ $$e_{y,i} = |\Delta y_i| - \frac{l}{2}$$

The SDF residual $r_i$ is then computed as:

$$r_i = \sqrt{\max(e_{x,i}, 0)^2 + \max(e_{y,i}, 0)^2} + \min(\max(e_{x,i}, e_{y,i}), 0)$$

• Outside the box: $r_i$ is the Euclidean distance to the nearest face or corner (positive).
• Inside the box: $r_i$ is the distance to the nearest face (negative).

2.3 Robust Loss Function (Huber)

In my testing, I found that Huber Loss provided the best balance for this application, yielding a significantly better RMSE than other robust estimators like Cauchy.

Huber combines the benefits of $L2$ (squared error) for small residuals and $L1$ (absolute error) for large ones:

$$ \rho(r) = \begin{cases} \frac{1}{2}r^2 & \text{for } |r| \le \delta \\ \delta(|r| - \frac{1}{2}\delta) & \text{for } |r| > \delta \end{cases} $$

By using Huber with $\delta = 0.05$, the optimizer still "listens" to the minor noise on the walls—allowing the box to average out surface jitters—while preventing large outliers from dominating the gradient. This results in a tighter fit that respects the overall distribution of the point cloud.

3. The Breakthrough: A Hybrid Geometric-Optimization Approach

After analyzing the failures of the naive approach, I realized the optimizer needed a "guided hand." I moved away from pure optimization and developed a Hybrid Strategy that combines global search with local refinement:

Global Orientation Search: Instead of trusting PCA for the angle, I used your approach of a 90-degree geometric sweep. This "pre-scan" finds the correct valley for the heading by minimizing the variance of the points along the axes. This completely eliminated the 45-degree diagonal trap.

Geometric Diameter Centering: I replaced the density-biased "mean" center with a diameter-based midpoint logic. By finding the two farthest points in the cloud, I ensured the box starts exactly in the middle of the object’s physical extent.

The Guided "Polish": I now use the least_squares() solver only for the final 2% of the work. By initializing it with the high-quality previous angle and the diameter center, the optimizer simply "snaps" the box edges to the points with sub-degree precision.

The Result: The system is now extremely stable. By using Huber loss for this final polish, the box ignores the "noisy tails" while maintaining a low RMSE. The "fixed theta" issue was solved by giving the optimizer a small, safe window to vibrate into the perfect alignment without ever losing its way.

4. Results

BLUE - Previous Approach RED - Optimized Approach

While the visual improvement in the simulation GIFs is subtle to the naked eye, the underlying data tells a much more compelling story. To validate the hybrid approach, I tracked the Root Mean Square Error (RMSE) of the residuals across the entire simulation.

As shown in the plot below, the advantage of the Optimized Polish (Red) over the Geometric Sweep (Blue) is clearly distinguishable:

• Residual Reduction: The optimized approach consistently maintains a lower RMSE, proving that the Huber-weighted SDF provides a much "tighter" fit to the actual point distribution than a purely geometric limit-based box.

• Stability: The optimized fit exhibits fewer spikes in error when the object's aspect ratio changes or when the sensor noise increases at the "tails" of the point cloud.

RMSE on a static object (Obstacle 3: 10m x 5m at (-15,-15, 0 deg))

RMSE on a moving object (Obstacle 1: Small moving car started from (-5, 15))

I'd love to get your thoughts on this direction. Further I am open to collaborate on ideas with other people.