Sign inversion in R matrix

[sgk-000]

Dear Authors, Thank you for your great work. I tested this repository on our lidartag, I found detection is often failed because initial pose is not stable. I debugged and found signs of some values in R matrix are inverted in fitGrid_new function. I fixed GridVertices and payload_vertices, these are augments of fitGrid_new function, but the sign inversion are occurred. If I fix R matrix, sign inversion is not occurred and initial pose is stable. below is the video of the test(Payload_vertices is not fixed in this case) We can find

• Pose optimization status is 1 if R element of 1 line 1 row is negative.
• pose optimization status is 2 or -3 if R element of 1 line 1 row is positive.

https://user-images.githubusercontent.com/32808802/127618912-ecdf6d86-34b5-4b48-a9d7-f81a712ecff9.mp4

I reproduced this problem using sample bag, ccw_6m.bag on default setting.

https://user-images.githubusercontent.com/32808802/127620627-8cbeb46f-9073-4ee3-b46e-0e3a65dca707.mp4

Do you have any suggestion?

thanks,

[sgk-000]

Finally I solved this problem using OpenCV for SVD instead of eigen.

[brucejk]

Hi,

I am sorry! I have been working on another submission and haven't checked my email.

I hope my response is not too late.

Could you define "initial pose is not stable" and "payload vertices are not fixed"?

Do you mean the target is moving or the lidar is moving while detecting?

[sgk-000]

Thank you for reply. "Initial pose is not stable" and "payload vertices are not fixed" mean just values are not stable(Target and lidar are not moving). Finally I found something of SVD function of eigen went wrong. Arguments of fitgrid_new function are fixed(it means inputs are fixed), but singular values are not stabled(The values occasionally were NaN), so return value of _optimizePose sometimes is 1, then it is 2 or -3 again and again. This problem reproduced if I try sample data on default setting. My eigen version is 3.3.90.

https://user-images.githubusercontent.com/32808802/128953724-fd9fad5e-41ef-4d50-9228-88dd7bcd1f92.mp4

[brucejk]

Hi,

That is really helpful! Thank you for debugging into this.

Just to be clear, the problem happens when estimating the initial vertices of a LiDARTag at here, in which the function calls Eigen SVD function at here, where it sometimes gives NAN values or gives inconsistent values:

1. If it gives NAN, the payload will be rejected and reported as a line-fitting failure at here.
2. If it gives inconsistent values, it will mess up the solver at here.

I acknowledge that I also see the diverging issue occasionally. Initially, I thought it was the solver that I use that was not robust, which leads to the divergence. However, after I dug into this issue, I also found that the principal axis output from the SVD is not consistent. My thoughts on this issue are the shape of the target is symmetric and thus causes an ambiguous principal axis for the SVD function.

To solve this problem inherently, we need to change the shape of the target. I'd like to give a big thank you again for providing a workaround to this issue by changing the SDV function from Eigen to OpenCV :)!

Do you mind posting your code where you use the OpenCV function to do it?

[sgk-000]

Thank you for your reply. I have the same understanding as you in terms of this problem. I think output from SVD should be consistent if lidar and target is not moving even if target is symmetric because target edge(input of SVD) is stabled. Is my understanding of this correct?

OK, I will make pull request today . I am grad if I could help you.

[brucejk]

Hi

Thanks for your pull request! I watched the video again carefully. The singular values are HUGE and that makes me feel like the values might overflow. Could you try to change Eigen::MatrixXf to Eigen::Matrix3d to see if that solves the problem?

Additionally, I wrote a minimal testing script attempting to reproduce the problem:

    Eigen::Matrix3f M; // input matrix
    M <<        0,         0,        0, 
        -0.660054, -0.510275, 0.651784,   
         0.631292,  0.154421,  0.80545;

    Eigen::Matrix3f U; // save for comparision
    Eigen::Matrix3f V; // save for comparision
    Eigen::Vector3f S; // save for comparision

    // run 100 times to check consistency
    for (int i = 0; i < 100; ++i)
    {
        std::cout << "=============" << std::endl;
        std::cout << "i: " << i << std::endl;
        std::cout << "M: \n" << M << std::endl;
        Eigen::JacobiSVD<Eigen::MatrixXf> svd(
                M, Eigen::ComputeFullU | Eigen::ComputeFullV);

        std::cout << "U: \n" << svd.matrixU() << std::endl;
        std::cout << "S: \n" << svd.singularValues() << std::endl;
        std::cout << "V: \n" << svd.matrixV() << std::endl;

        // take the first results and then compare them with the other
        if (i == 0)
        {
            U = svd.matrixU();
            S = svd.singularValues();
            V = svd.matrixV();
        }

        std::cout << "norm(U): \n" << (U - svd.matrixU()).norm() << std::endl;
        std::cout << "norm(S): \n" << (S - svd.singularValues()).norm() << std::endl;
        std::cout << "norm(V): \n" << (V - svd.matrixV()).norm() << std::endl;

        // raise error if not consistent
        assert((U - svd.matrixU()).norm() < 1e-5);
        assert((S - svd.singularValues()).norm() < 1e-5);
        assert((V - svd.matrixV()).norm() < 1e-5);
    }

The outputs look pretty consistent:

i: 99
M: 
        0         0         0
-0.660054 -0.510275  0.651784
 0.631292  0.154421   0.80545
U: 
 5.40374e-09  2.00547e-08           -1
    0.906831    -0.421495 -3.55265e-09
    0.421495     0.906831  2.04638e-08
S: 
    1.06517
    1.02831
4.37043e-16
V: 
 -0.31213  0.827267 -0.467124
-0.373317  0.345336  0.861033
 0.873618  0.443139  0.201043
norm(U): 
0
norm(S): 
0
norm(V): 
0

Is this what you were trying to test?

PS. OpenCV is a huge library, so I am a bit hesitant to include OpenCV just because of the SVD function. Indeed, having a workaround is fantastic and will be definitely useful to others if they already have OpenCV installed!

Just for the record, a workaround is Solving the SVD from OpenCV instead of Eigen as follows:

    Eigen::Matrix3f M = 
        GridVertices.rightCols(4)*payload_vertices.transpose();
    Eigen::Matrix<float, 3, 3, Eigen::DontAlign> R;

    // Solving SVD using eigen (having issues and still being investigating)
    // Eigen::JacobiSVD<Eigen::MatrixXf> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
    // R = svd.matrixU() * svd.matrixV().transpose();

    // Solving SVD using OpenCV
    cv::Mat cv_M;
    cv::eigen2cv(M, cv_M);

    // compute SVD
    cv::Mat cv_S;
    cv::Mat cv_U;
    cv::Mat cv_Vt;
    cv::SVD::compute(cv_M, cv_S, cv_U, cv_Vt);

    // parsing results back to Eigen
    Eigen::Matrix3f U;
    Eigen::Matrix3f V;
    cv::cv2eigen(cv_U, U);
    cv::cv2eigen(cv_Vt, V);
    R = U * V; // final result