Efficient L-shape fitting of laser scanner data for vehicle pose estimation

In this paper, we propose an efficient algorithm to fit a cluster of laser scan points with an L-shape. The algorithm partitions a cluster into two disjoint sets optimally in the sense of the least square error, and then fits them with two perpendicular lines. By exploiting the characteristics of both the laser scanner sensor and the fitting problem, the algorithm can test all the possible corner points while keeping the complexity as low as 9 times that of fitting a single pair of orthogonal lines, where the 9 times scaling factor is independent of the number of points in the cluster. Specifically, we exploit the property that the scanner data points are ordered either clockwise or counterclockwise, and incrementally construct the L-shape fitting problem rather than from scratch when the corner point is different. We extend our algorithm to provide multiple hypotheses on pose estimation, which are derived from L-shape fitting, to account for the ambiguity on the corner points. The extended algorithm only requires slightly more computation, which is tested and verified with real laser scanner data. The experimental results justify the correctness and efficacy of our algorithm.