-1

I have wrong result of inverse matrix, using Eigen library. Here's my part of code:

Hessian_matrix = matrix.transpose() * matrix;
std::cout << "Hessian matrix:" << Hessian_matrix << std::endl;
std::cout << "inverse Hessian matrix" << std::endl << Hessian_matrix.inverse() << std::endl;
std::cout << "identity:" << std::endl << Hessian_matrix*Hessian_matrix.inverse() << std::endl << std::endl;

Here, matrix is a 6x6 matrix of type Eigen::MatrixXd, it's Jacobian matrix (1000 x 6). The function, that generated Jacobian matrix:

Eigen::MatrixXd tracking_module::Jacobian_matrix_of_residuals() {
    const auto start = std::chrono::system_clock::now();
    Eigen::MatrixXd Jacobian_matrix;
    unsigned int number_of_landmark = 0;
    for (auto& lm_curr_fr : curr_frm_.landmarks_) {
        if (!lm_curr_fr){
            continue;
        }
        if (lm_curr_fr->will_be_erased()){
            continue;
        }
        for (auto& lm_prev_fr : last_frm_.landmarks_) {
            if (!lm_prev_fr){
                continue;
            }
            if (lm_prev_fr->will_be_erased()){
                continue;
            }
            if (lm_curr_fr->id_ == lm_prev_fr->id_) {
                auto pose_1 = last_frm_.cam_pose_cw_.inverse();
                auto pose_2 = curr_frm_.cam_pose_cw_.inverse();
                
                Eigen::Matrix3d X1_rot = pose_1.block<3,3>(0,0);
                Eigen::Matrix3d X2_rot = pose_2.block<3,3>(0,0);

                Eigen::Vector3d roll_pitch_yaw1 = X1_rot.eulerAngles(0, 1, 2);
                Eigen::Vector3d roll_pitch_yaw2 = X2_rot.eulerAngles(0, 1, 2);

                double x1 = pose_1(0,3);
                double y1 = pose_1(1,3);
                double z1 = pose_1(2,3);
                double x2 = pose_2(0,3);
                double y2 = pose_2(1,3);
                double z2 = pose_2(2,3);

                double yaw1 = roll_pitch_yaw1(2);
                double pitch1 = roll_pitch_yaw1(1);
                double roll1 = roll_pitch_yaw1(0);
                double yaw2 = roll_pitch_yaw2(2);
                double pitch2 = roll_pitch_yaw2(1);
                double roll2 = roll_pitch_yaw2(0);
                Jacobian_matrix.conservativeResize(number_of_landmark+1,6);
                Jacobian_matrix(number_of_landmark, 0) = abs(lm_curr_fr->residual_error_from_optimization_)/(x2-x1);
                Jacobian_matrix(number_of_landmark, 1) = abs(lm_curr_fr->residual_error_from_optimization_)/(y2-y1);
                Jacobian_matrix(number_of_landmark, 2) = abs(lm_curr_fr->residual_error_from_optimization_)/(z2-z1);
                Jacobian_matrix(number_of_landmark, 3) = abs(lm_curr_fr->residual_error_from_optimization_)/(yaw2-yaw1);
                Jacobian_matrix(number_of_landmark, 4) = abs(lm_curr_fr->residual_error_from_optimization_)/(pitch2-pitch1);
                Jacobian_matrix(number_of_landmark, 5) = abs(lm_curr_fr->residual_error_from_optimization_)/(roll2-roll1);
                number_of_landmark++;
            }
        }
    }
    const auto end = std::chrono::system_clock::now();
    elapsed_ms_ = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    // std::cout << "time for Jacobian computing = " << elapsed_ms_ << std::endl;
    return Jacobian_matrix;
}

Here's output:

Hessian matrix: 4.26369e+15  -3.8184e+13 -5.26596e+14 -6.92714e+14 -2.29145e+14 -2.96259e+14
 -3.8184e+13  3.41962e+11  4.71599e+12  6.20369e+12  2.05214e+12  2.65318e+12
-5.26596e+14  4.71599e+12  6.50382e+13   8.5555e+13  2.83011e+13    3.659e+13
-6.92714e+14  6.20369e+12   8.5555e+13  1.12544e+14  3.72288e+13  4.81326e+13
-2.29145e+14  2.05214e+12  2.83011e+13  3.72288e+13  1.23151e+13  1.59219e+13
-2.96259e+14  2.65318e+12    3.659e+13  4.81326e+13  1.59219e+13  2.05852e+13
inverse Hessian matrix
  0.84104   33.9674  -3.73255   1.02025 -0.993682   12.7437
  122.317   23835.5  -1591.36   675.752  -4342.53   3295.63
 -10.0169  -2080.49   91.1274  -57.0409   407.123  -219.511
  5.80446   851.824  -39.0675   37.1296  -144.223   67.9237
  4.67209   210.571   4.27676  -33.7183   176.976  -25.5461
 -3.04185  -1039.82    77.451  -31.7602   22.0809   9.75697
identity:
    0.684563     -9.97159      1.56548     -0.85569      2.44585      2.93436
-0.000541409      1.07273    0.0069068  -0.00905051    0.0821457   -0.0551281
  -0.0207974    -0.494565      1.19223    -0.123652      1.03656     -1.45191
  -0.0516072      9.20116    -0.429967      1.31109     -1.10355     -2.08863
   0.0222311      3.14692    -0.354085     0.188195     0.516471     -0.29575
  0.00806517     -4.08476     0.280319      0.15838     -1.06946    0.0306264

As you can see product Hessian * Hessian^(-1) is not even close to the identity matrix. I thought, that I got some kind of singilar matrix or anything like that, and that's why Eigen didn't manage. Then I decided to check inverse matrix in python. Here's code:

a = np.array([[4.26369e+15, -3.8184e+13, -5.26596e+14, -6.92714e+14, -2.29145e+14, -2.96259e+14],
              [-3.8184e+13, 3.41962e+11, 4.71599e+12, 6.20369e+12, 2.05214e+12, 2.65318e+12],
              [-5.26596e+14, 4.71599e+12, 6.50382e+13, 8.5555e+13, 2.83011e+13, 3.659e+13],
              [-6.92714e+14, 6.20369e+12, 8.5555e+13, 1.12544e+14, 3.72288e+13, 4.81326e+13],
              [-2.29145e+14, 2.05214e+12, 2.83011e+13, 3.72288e+13, 1.23151e+13, 1.59219e+13],
              [-2.96259e+14, 2.65318e+12, 3.659e+13, 4.81326e+13, 1.59219e+13, 2.05852e+13]])
np.linalg.inv(a)

And the output is:

array([[-9.50157030e-11,  2.42050992e-08, -4.09485425e-10,
        -1.51298368e-09,  1.44172809e-10, -3.33168349e-10],
       [ 2.42050992e-08, -4.13924685e-06, -8.24132506e-08,
         4.50069824e-07,  6.55282742e-08, -7.47002292e-08],
       [-4.09485425e-10, -8.24132506e-08, -2.73532375e-09,
         1.42546534e-09,  6.50633717e-09,  1.22536282e-09],
       [-1.51298368e-09,  4.50069824e-07,  1.42546535e-09,
        -3.58572413e-08, -6.40345760e-09,  6.47787736e-09],
       [ 1.44172809e-10,  6.55282742e-08,  6.50633717e-09,
        -6.40345760e-09,  2.88168850e-09, -5.19205954e-09],
       [-3.33168349e-10, -7.47002292e-08,  1.22536282e-09,
         6.47787736e-09, -5.19205954e-09, -8.47577970e-09]])

If I execute this code in python:

a @ np.linalg.inv(a)

Then the output is:

array([[ 1.00000000e+00, -1.49011612e-08, -2.32830644e-10,
        -1.86264515e-09,  0.00000000e+00, -1.16415322e-10],
       [-6.82121026e-13,  1.00000000e+00,  5.45696821e-12,
         2.91038305e-11,  7.27595761e-12,  9.09494702e-13],
       [ 0.00000000e+00,  7.45058060e-09,  1.00000000e+00,
         5.82076609e-10,  5.82076609e-11,  4.36557457e-11],
       [-2.91038305e-11,  0.00000000e+00,  5.82076609e-11,
         1.00000000e+00,  0.00000000e+00,  2.91038305e-11],
       [-3.63797881e-12,  9.31322575e-10,  1.45519152e-11,
         2.32830644e-10,  1.00000000e+00,  2.91038305e-11],
       [-1.81898940e-12,  0.00000000e+00,  1.45519152e-11,
         1.16415322e-10,  0.00000000e+00,  1.00000000e+00]])

What's wrong with matrix Eigen function inverse()?

I used VSCode. My tasks.json is:

{
    "version": "2.0.0",
    "tasks": [
        {
            "type": "shell",
            "label": "build_OpenVSLAM",
            "command": "cd /home/cds-s/workspace/openvslam/build && cmake -DBUILD_WITH_MARCH_NATIVE=ON \
                        -DCMAKE_BUILD_TYPE=Release \
                        -DUSE_PANGOLIN_VIEWER=ON \
                        -DUSE_SOCKET_PUBLISHER=OFF \
                        -DUSE_STACK_TRACE_LOGGER=ON \
                        -DBOW_FRAMEWORK=DBoW2 \
                        -DBUILD_TESTS=ON \
                        .. && make -j4",
            "args": [],
            "options": {
                "cwd": "${workspaceFolder}"
            },
            "problemMatcher": [
                "$gcc"
            ],
            "group": "build"
        }
    ]
}

My launch.json file is:

{
    // Use IntelliSense to learn about possible attributes.
    // Hover to view descriptions of existing attributes.
    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
    "version": "0.2.0",
    "configurations": [
        {
            "name": "Build and debug OpenVSLAM",
            "type": "cppdbg",
            "request": "launch",
            "program": "${workspaceFolder}/build/run_tum_rgbd_slam",
            "args": [
                "-v", "/media/cds-s/data/Vocab/orb_vocab/orb_vocab.dbow2",
                "-c", "/media/cds-s/data/Datasets/NKBVS_calib_17_03_2020_rgbd_1200_600.yaml",
                "-d", "/media/cds-s/data/Datasets/Husky-NKBVS/00_map_half_2020-03-17-14-21-57/RGBD"
            ],
            "stopAtEntry": false,
            "cwd": "${workspaceFolder}",
            "environment": [],
            "externalConsole": false,
            "MIMode": "gdb",
            "setupCommands": [
                {
                    "description": "Enable pretty-printing for gdb",
                    "text": "-enable-pretty-printing",
                    "ignoreFailures": true
                }
            ],
            "preLaunchTask": "build_OpenVSLAM",
            "miDebuggerPath": "/usr/bin/gdb",
            "default": true
        }
    ]
}

While looking for problem, I decided to define Hessian matrix plicitly with the following code:

    Eigen::MatrixXd mat(6,6);
    mat << 4.26369e+15,  -3.8184e+13, -5.26596e+14, -6.92714e+14, -2.29145e+14, -2.96259e+14,
           -3.8184e+13,  3.41962e+11,  4.71599e+12,  6.20369e+12,  2.05214e+12,  2.65318e+12,
          -5.26596e+14,  4.71599e+12,  6.50382e+13,   8.5555e+13,  2.83011e+13,    3.659e+13,
          -6.92714e+14,  6.20369e+12,   8.5555e+13,  1.12544e+14,  3.72288e+13,  4.81326e+13,
          -2.29145e+14,  2.05214e+12,  2.83011e+13,  3.72288e+13,  1.23151e+13,  1.59219e+13,
          -2.96259e+14,  2.65318e+12,    3.659e+13,  4.81326e+13,  1.59219e+13,  2.05852e+13;

Then the inverse matrix is correct after building in VScode and running. It seems that I put values into Jacobian or Hessian incorrectly ?

Yaroslav Solomentsev
  • 1
  • 2
  • 2
    Using double precision, I don't see any problem with your matrix and Eigen: https://godbolt.org/z/v38a3M. You need to provide a [mre]! – chtz Dec 02 '20 at 15:40
  • 2
    N.B.: For symmetric matrices you should of course prefer a Cholesky factorization (LLT or LDLT) and if your original Jacobian is near rank-deficient, you should consider calculating a QR factorization of that (this is more expensive, but much more robust). – chtz Dec 02 '20 at 15:42
  • I've recently updated my problem description. – Yaroslav Solomentsev Dec 03 '20 at 09:19
  • Please read again how to provide a [mre]. – chtz Dec 03 '20 at 15:42

1 Answers1

0

The problem was in values of matrix. The output into the console, that was posted in my question, was done with the default precision. But if I output into the console with precision 18 with code:

    std::cout << std::setprecision(18);
    std::cout << "Hessian:\n" << Hessian_matrix << "\n";
    std::cout << "Inverse Hessian:\n" << Hessian_matrix.inverse() << "\n";
    std::cout << "Identity:\n" << Hessian_matrix*Hessian_matrix.inverse() << "\n";

Then the output will be:

Hessian:
 86446085861.6839294 -1513344485728.35767  17432338937932.8184 -538583928534.592041 -475594265084.251648 -8164195672312.20605
-1513344485728.35767  26492946553402.3945 -305174418738621.438  9428570538796.44531  8325859422494.65723   142924232808755.75
 17432338937932.8184 -305174418738621.438     3515329095792667 -108608475388471.484  -95906255826910.625  -1646355930366449.5
-538583928534.592041  9428570538796.44531 -108608475388471.484    3355532470722.604  2963088786777.93896  50865282501679.7969
 -475594265084.25177  8325859422494.65527 -95906255826910.6719  2963088786777.93799  2616543047917.05029  44916373044234.8438
 -8164195672312.2041  142924232808755.688  -1646355930366449.5  50865282501679.8125  44916373044234.8438    771048108325571.5
Inverse Hessian:
 -76769.1724561170122   -35.184041803614889   223.286725546342609   2234.88813205763381    3609.5108733544248  -687.278544549406661
 -487.047700329240001  -33.5283296666579531   1.63812060566774975   39.7504696737151377  -268.451900461737239   17.5716071815450299
  223.474775163196966   1.99499386512538579 -0.380585808962619954  -4.07344900325281589  -26.6842017680237795    3.0069898250188154
  9923.27589135810013   145.562085706210269    18.341392753088158  -794.922763661831482   1007.33308116673004   111.012398519574077
 -9659.87772674228108  -35.2602570858584201  -3.94184294338451791   1002.28966418411221  -2.87189036254061758  -170.116512637237861
 -337.322875780041727   2.55358596006699923  0.267639393724612762    1.6512149922244419  -35.2817747831255843  -1.52723250381498521
Identity:
     1.03585266918102858  -0.00960887212501091575 -0.000504251118909580472    0.0124370876957572169   0.00558495808806040717   -0.0106859493546394551
   -0.154766613273494613      1.04896358761167297  -0.00759519087047519359     0.387423196900574307    0.0712581634887534676     0.318551462464354895
    -87.9184433005332835    -0.673832372684966874       1.6704924171867408      5.77536829662169904      2.45667899074825158     -1.39016134938339508
     3.78115310740339261 -0.000273350980715444947   -0.0109397309123419658     0.668410206460676837   -0.0459331081400999874    0.0717009567151854765
    -0.86945917709489251    0.0276154623277449496   -0.0135497323864693202    -0.365008131408886216     0.602766770305086741    0.0523544126516358801
    -96.4255602940492622    -0.380973192464549637    -0.185535418139582686      2.68175292986019853      3.52222438717650022      1.74509372657112971

As you can see there's still no Identity matrix. But you can check in https://godbolt.org/z/KxfKP3 that with these values you won't get Identity matrix.

Yaroslav Solomentsev
  • 1
  • 2