search

laboshinl / loam_velodyne

Laser Odometry and Mapping (Loam) is a realtime method for state estimation and mapping using a 3D lidar.
http://wiki.ros.org/loam_velodyne
Other
1.71k stars 952 forks source link

Fix bug in eigenvalues classifying points #119

Closed jlblancoc closed 5 years ago

jlblancoc commented 5 years ago

Eigen::SelfAdjointEigenSolver docs say: Only the lower triangular part of the input matrix is referenced. The original code was filling the upper triangular part. As a result, the obtained eigenvalues were actually... the diagonal elements, while totally ignoring their correlation.

In practice: the original code was not doing its job while classifying point patches that were not at 90 degrees with the (arbitrary) XYZ axes.

Closes #118

ojura commented 5 years ago

Hi, a question: why not do a JacobiSVD on the 3x5 data matrix with the subtracted centroid from every column? The first column of U contains the dominant line, while the third contains the normal. This is also the method described in the original loam paper.

jlblancoc commented 5 years ago

Hmm.... what 3x5 matrix? I guess you meant 3x3, right? You're right, the SVD method would also work, and is probably more numerically stable, but my patch just fixed the original LOAM code which uses Eigen::SelfAdjointEigenSolver :-)

Anyway, it should be benchmarked to decide whether it really represents a significant improvement in either speed or accuracy...

ojura commented 5 years ago

Nope, 3x5 :) 5 points are drawn from the kd tree to which we want to fit a line or a plane. If you stack them in columns, you get a 3 (xyz) by 5 (column for each point) data matrix. Compute and subtract the centroid from each column and fire up SVD. The result is really elegant - the 3x3 U of the decomposition has the line direction and plane normal as the first and third columns.

More details in https://www.ltu.se/cms_fs/1.51590!/svd-fitting.pdf

jlblancoc commented 5 years ago

Ah, ok now I got your idea! Sure, it's a good approach... should be benchmarked to check if it's more or less efficient than the current one...