Fixed optimization bug under degeneracy

[KitKat7]

According to the paper, "J. Zhang, M. Kaess and S. Singh, On degeneracy of optimization-based state estimation problems, 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, 2016", when degeneracy happens, the update should be remapped by "matV.inverse() * matV2".

However, in the original paper Vu and Vf are transposed matrix of eigenvalues, which should correspond to matV2 and matV in the current implementation a4c364a.

Thus, to update the states towards well-conditioned directions only, "matV.transpose().inverse() * matV2.transpose()" should be used, or equivalently "matV2 * matV.inverse()".

[ojura]

I tried out your PR and it helped with wild pose divergences indoors (VLP-16). They still happen (especially in confined spaces), but less often.

[KitKat7]

Glad it helped :).
IMHO, this approach will prefer to make the weights of the degeneracy parts (eigen vectors) close to the ones from predictions.
In some confined spaces, the predictions (constant velocity assumption or erroneous laser odometry poses) may be incorrect. Using only the constraints from the sparse point clouds can cause the alignment slip.

[ojura]

Indeed, thank you :) Yup, this implementation trusts the IMU too little. It only uses the gyro/accelerometer to unwarp the point cloud, even though the IMU provides a stable pitch/roll reference with respect to gravity which does not drift.

[ojura]

@KitKat7 can you PTAL at this line? Do you think that the transform should be scaled according to scan time (s = (1.f / _scanPeriod) * (pointOri.intensity - int(pointOri.intensity)) like in BasicLaserOdometry::transformToStart), instead of s = 1?

[KitKat7]

Actually, the s=1 here does not make any effects. I think some scale can be considered, but at least not according to the scan time, since this s is for weighting for the optimization instead of of point cloud undistortion.

[ojura]

Hmmm, are you sure it's just scaling? The angle inside the sines/cosines a couple of lines below is also multiplied by s.

If the costs d (inside coeff.intensity) are calculated by transforming the point by s fraction of transform according to the point scan time, shouldn't the point scan time also appear in the Jacobian of the costs?

[KitKat7]

I think in the original paper, the point scan time is not optimized here. The scan time of the point is corrected before the optimization in transformToStart .

[tungdanganh]

Hi @KitKat7,
I think you are right because the eigenvectors are in column order in this implementation. However, we have to change also in the for loop. We could simply transpose the eigenvectors in begining and keep the same like previous version:
matV = esolver.eigenvectors().real();
matV = matV.transpose();

[ojura]

@tungdanganh You need to use "transposeInPlace()". https://eigen.tuxfamily.org/dox/group__TutorialMatrixArithmetic.html

[narutojxl]

Hi, @KitKat7,
I have some stupid questions about this section, If you have some time to have a look, i will be much appreciated a lot !