Skip to content

Commit

Permalink
Fix bug in eigenvalues classifying points
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
jlblancoc authored Jan 24, 2019
1 parent ef1b9b7 commit b5b326e
Showing 1 changed file with 5 additions and 5 deletions.
10 changes: 5 additions & 5 deletions src/lib/BasicLaserMapping.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -684,14 +684,14 @@ void BasicLaserMapping::optimizeTransformTobeMapped()
Vector3 a = Vector3(_laserCloudCornerFromMap->points[pointSearchInd[j]]) - vc;

mat_a(0, 0) += a.x() * a.x();
mat_a(0, 1) += a.x() * a.y();
mat_a(0, 2) += a.x() * a.z();
mat_a(1, 0) += a.x() * a.y();
mat_a(2, 0) += a.x() * a.z();
mat_a(1, 1) += a.y() * a.y();
mat_a(1, 2) += a.y() * a.z();
mat_a(2, 1) += a.y() * a.z();
mat_a(2, 2) += a.z() * a.z();
}
matA1 = mat_a / 5.0;

// This solver only looks at the lower-triangular part of matA1.
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> esolver(matA1);
matD1 = esolver.eigenvalues().real();
matV1 = esolver.eigenvectors().real();
Expand Down Expand Up @@ -926,4 +926,4 @@ void BasicLaserMapping::optimizeTransformTobeMapped()
}


} // end namespace loam
} // end namespace loam

0 comments on commit b5b326e

Please sign in to comment.