Eigen

Examples

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

// declarations
Matrix<float, 2, 3> matrix_23; // type, row, col
Vector3d v_3d;      // Matrix<float, 3, 1>
Matrix3d matrix_33; // Matrix<double, 3, 3>
MatrixXd matrix_x;  // Matrix<double, Dynamic, Dynamic>

// input & output
matrix_33 = Matrix3d::Zero();
matrix_23 << 1, 2, 3, 4, 5, 6;
cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;
cout << "matrix(1, 2): " << matrix_23(0, 1) << endl;
v_3d << 3, 2, 1;

// requires explicit type conversion (float to double)
Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << endl;

// matrix operations
matrix_33 = Matrix3d::Random();
cout << "random matrix: \n" << matrix_33 << endl;
cout << "transpose: \n" << matrix_33.transpose() << endl;
cout << "sum: " << matrix_33.sum() << endl;
cout << "trace: " << matrix_33.trace() << endl;
cout << "times 10: \n" << 10 * matrix_33 << endl;
cout << "inverse: \n" << matrix_33.inverse() << endl;
cout << "det: " << matrix_33.determinant() << endl;

// eigen value
SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

// solve equations
Matrix<double, N, N> matrix_NN = MatrixXd::Random(N, N);
matrix_NN = matrix_NN * matrix_NN.transpose();  // for semi-definite
Matrix<double, N, 1> v_Nd = MatrixXd::Random(N, 1);
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);  // QR decomposition
x = matrix_NN.ldlt().solve(v_Nd);  // cholesky decomposition

// timing
clock_t tic = clock();
cout << "time is " << 1000 * (clock() - tic) / (double) CLOCKS_PER_SEC << "ms" << endl;

Linear Solver

We discuss some properties of the matrix ATAA^TA, constructed from a m-by-n matrix AA where m is significantly greater than n. This is normally the case in least-square problems.

  • The matrix ATAA^TA is always symmetric by definition.

  • The matrix ATAA^TA is positive semi-definite if it is invertible, since we havexTATAx=(Ax)TAx=∥Ax∥2≥0x^TA^TAx=(Ax)^TAx=∥Ax∥^2≥0.

  • The matrix ATAA^TA is almost always invertible, because m is significantly greater than n, and hence the constructed matrix is high likely to be full-rank.

  • The dense linear solver ldlt() in Eigen is the fastest for positive or negative semi-definite matrices. Remember to use double precision to avoid numerical instability issues.

References: Quora: Is ATA always positive definite?; StackExchange: Is a matrix multiplied with its transpose something special?; StackExchange: Proof for why a matrix multiplied by its transpose is positive semidefinite; Eigen Documentation: Basic linear solving;

Numerical Stability

The following logs reveal one of the potential issues of using A.ldlt().solve(b);

  • The solver was used for Gauss-Newton optimization in an Iterative Closest Point algorithm. It was tested under the termination condition of max_iteration only.

  • The result suggests that we should check if A and b are healthy before sending to A.ldlt().solve(b);, and it might be better to add epsilon as another termination condition.

  • Note that float type supports a value range of 1.2E-38 to 3.4E+38, and 6-7 decimal digits, while double type supports a value range of 2.3E-308 to 1.7E+308, and 15 decimal digits.

  • Updates: This issue was resolved by switching to double precision. (Tested multiple times towards 100 iterations. The produced result is stable.)

// Example Log 1
it = 0; cloud size = 29280; selected size = 7246
JTJ = 
 38016.2 -13125.2 -18423.8  6854.26 -1088.44  14895.9
-13125.2  6123.19  6490.22 -2408.94   365.29  -5207.7
-18423.8  6490.22   8958.4 -3330.11  527.498 -7219.55
 6854.26 -2408.94 -3330.11  1241.23 -196.889  2693.11
-1088.44   365.29  527.498 -196.889  31.7478 -427.549
 14895.9  -5207.7 -7219.55  2693.11 -427.549  5856.14
JTJ determinant = 9.95116e+07
JTr = 
 -577.63
 228.602
 281.135
-105.983
 16.6081
-230.903
X = 
  -0.112421
-0.00316341
  -0.102398
  -0.291014
   0.121551
   0.339041
transformation in this iteration
  0.994757   0.101927 0.00834039  -0.291014
 -0.102219   0.988446   0.111918   0.121551
 0.0031634  -0.112183   0.993683   0.339041
         0          0          0          1
current transformation estimation
  0.994757   0.101927 0.00834039  -0.291014
 -0.102219   0.988446   0.111918   0.121551
 0.0031634  -0.112183   0.993683   0.339041
         0          0          0          1
it = 1; cloud size = 29280; selected size = 24791
JTJ = 
 92950.4 -42927.1 -45647.5  19505.3 -2807.42  42299.9
-42927.1  27583.8  21442.2 -9525.14   1324.4 -20755.5
-45647.5  21442.2  22517.7 -9641.93  1382.54 -20829.7
 19505.3 -9525.14 -9641.93  4282.87 -598.361  9236.44
-2807.42   1324.4  1382.54 -598.361   90.107 -1298.59
 42299.9 -20755.5 -20829.7  9236.44 -1298.59  20018.1
JTJ determinant = 2.59903e+14
JTr = 
-138.758
 66.6306
  68.979
-33.9927
 3.96181
 -71.508
X = 
  0.0378539
-0.00402526
  0.0855123
   0.203131
   0.024896
 -0.0837208
transformation in this iteration
    0.996338   -0.0854987 -0.000775407     0.203131
   0.0854074     0.995619   -0.0380502     0.024896
  0.00402525    0.0378446     0.999276   -0.0837208
           0            0            0            1
current transformation estimation
   0.999851   0.0171299 -0.00202947  -0.0974733
 -0.0169317     0.99709   0.0743299    0.108159
 0.00329683  -0.0742844    0.997232    0.258503
          0           0           0           1
it = 2; cloud size = 29280; selected size = 27278
JTJ = 
  106650 -49865.1 -52593.7  21952.8  -3155.5  47461.4
-49865.1  32271.1  24970.3 -10820.4  1500.07 -23530.8
-52593.7  24970.3  26055.8 -10891.8  1559.69 -23452.8
 21952.8 -10820.4 -10891.8   4732.8 -661.023  10178.9
 -3155.5  1500.07  1559.69 -661.023  99.2445 -1430.34
 47461.4 -23530.8 -23452.8  10178.9 -1430.34    22006
JTJ determinant = 6.6466e+14
JTr = 
-69.9797
 44.7118
 35.9486
-17.6539
 1.80711
-36.3435
X = 
  0.0231947
-0.00401701
  0.0506327
    0.13484
  0.0331909
 -0.0589205
transformation in this iteration
    0.99871  -0.0506905 -0.00283697     0.13484
  0.0506107    0.998445  -0.0233662   0.0331909
   0.004017   0.0231924    0.999723  -0.0589205
          0           0           0           1
current transformation estimation
   0.999411  -0.0332245 -0.00862379    0.031276
  0.0336208    0.998142   0.0508101    0.130209
 0.00691963  -0.0510701    0.998671    0.201628
          0           0           0           1
it = 3; cloud size = 29280; selected size = 28816
JTJ = 
  117818 -54770.5 -58277.6  23776.2 -3413.26  51235.2
-54770.5  34908.8  27502.5 -11604.7   1608.9 -25130.5
-58277.6  27502.5  28969.2 -11832.1  1691.92 -25385.1
 23776.2 -11604.7 -11832.1  5025.81 -703.071  10773.6
-3413.26  1608.89  1691.92 -703.071  105.135 -1516.08
 51235.2 -25130.5 -25385.1  10773.6 -1516.08  23220.2
JTJ determinant = 1.12071e+15
JTr = 
-29.1478
 17.9882
 14.8802
  -7.199
0.732457
-14.8477
X = 
 0.00985068
-0.00153022
  0.0214971
  0.0555449
  0.0158326
 -0.0239884
transformation in this iteration
   0.999768  -0.0215095 -0.00131805   0.0555449
  0.0214954     0.99972 -0.00988113   0.0158326
 0.00153022  0.00985051     0.99995  -0.0239884
          0           0           0           1
current transformation estimation
  0.998446  -0.054619  -0.011031  0.0837472
 0.0550257   0.997653  0.0407425   0.144685
0.00877979 -0.0412862   0.999109    0.17896
         0          0          0          1
it = 4; cloud size = 29280; selected size = 29062
JTJ = 
 inf -nan -nan -inf -nan -nan
-nan  inf -inf -nan -inf -inf
-nan -inf  inf -nan  inf  inf
-inf -nan -nan  inf -nan -nan
-nan -inf  inf -nan  inf  inf
-nan -inf  inf -nan  inf  inf
JTJ determinant = -nan
JTr = 
 inf
-nan
-nan
-inf
-nan
-nan
X = 
-nan
-nan
-nan
-nan
-nan
   0
transformation in this iteration
-nan -nan -nan -nan
-nan -nan -nan -nan
-nan -nan -nan    0
   0    0    0    1
current transformation estimation
-nan -nan -nan -nan
-nan -nan -nan -nan
-nan -nan -nan -nan
   0    0    0    1
color_icp: /usr/include/pcl-1.8/pcl/kdtree/impl/kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, std::vector<int>&, std::vector<float>&) const [with PointT = pcl::PointXYZRGBNormal; Dist = flann::L2_Simple<float>]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
Aborted (core dumped)
// Example Log 2
it = 0; cloud size = 29280; selected size = 7246
JTJ =  38016.2 -13125.2 -18423.8  6854.26 -1088.44  14895.9
-13125.2  6123.19  6490.22 -2408.94   365.29  -5207.7
-18423.8  6490.22   8958.4 -3330.11  527.498 -7219.55
 6854.26 -2408.94 -3330.11  1241.23 -196.889  2693.11
-1088.44   365.29  527.498 -196.889  31.7478 -427.549
 14895.9  -5207.7 -7219.55  2693.11 -427.549  5856.14
JTr =  -577.63
 228.602
 281.135
-105.983
 16.6081
-230.903
X =   -0.112421
-0.00316341
  -0.102398
  -0.291014
   0.121551
   0.339041
transformation in this iteration
  0.994757   0.101927 0.00834039  -0.291014
 -0.102219   0.988446   0.111918   0.121551
 0.0031634  -0.112183   0.993683   0.339041
         0          0          0          1
current transformation estimation
  0.994757   0.101927 0.00834039  -0.291014
 -0.102219   0.988446   0.111918   0.121551
 0.0031634  -0.112183   0.993683   0.339041
         0          0          0          1
it = 1; cloud size = 29280; selected size = 24791
JTJ =  92950.4 -42927.1 -45647.5  19505.3 -2807.42  42299.9
-42927.1  27583.8  21442.2 -9525.14   1324.4 -20755.5
-45647.5  21442.2  22517.7 -9641.93  1382.54 -20829.7
 19505.3 -9525.14 -9641.93  4282.87 -598.361  9236.44
-2807.42   1324.4  1382.54 -598.361   90.107 -1298.59
 42299.9 -20755.5 -20829.7  9236.44 -1298.59  20018.1
JTr = -138.758
 66.6306
  68.979
-33.9927
 3.96181
 -71.508
X =   0.0378539
-0.00402526
  0.0855123
   0.203131
   0.024896
 -0.0837208
transformation in this iteration
    0.996338   -0.0854987 -0.000775407     0.203131
   0.0854074     0.995619   -0.0380502     0.024896
  0.00402525    0.0378446     0.999276   -0.0837208
           0            0            0            1
current transformation estimation
   0.999851   0.0171299 -0.00202947  -0.0974733
 -0.0169317     0.99709   0.0743299    0.108159
 0.00329683  -0.0742844    0.997232    0.258503
          0           0           0           1
it = 2; cloud size = 29280; selected size = 27278
JTJ =   106650 -49865.1 -52593.7  21952.8  -3155.5  47461.4
-49865.1  32271.1  24970.3 -10820.4  1500.07 -23530.8
-52593.7  24970.3  26055.8 -10891.8  1559.69 -23452.8
 21952.8 -10820.4 -10891.8   4732.8 -661.023  10178.9
 -3155.5  1500.07  1559.69 -661.023  99.2445 -1430.34
 47461.4 -23530.8 -23452.8  10178.9 -1430.34    22006
JTr = -69.9797
 44.7118
 35.9486
-17.6539
 1.80711
-36.3435
X =   0.0231947
-0.00401701
  0.0506327
    0.13484
  0.0331909
 -0.0589205
transformation in this iteration
    0.99871  -0.0506905 -0.00283697     0.13484
  0.0506107    0.998445  -0.0233662   0.0331909
   0.004017   0.0231924    0.999723  -0.0589205
          0           0           0           1
current transformation estimation
   0.999411  -0.0332245 -0.00862379    0.031276
  0.0336208    0.998142   0.0508101    0.130209
 0.00691963  -0.0510701    0.998671    0.201628
          0           0           0           1
it = 3; cloud size = 29280; selected size = 28816
JTJ =   117818 -54770.5 -58277.6  23776.2 -3413.26  51235.2
-54770.5  34908.8  27502.5 -11604.7   1608.9 -25130.5
-58277.6  27502.5  28969.2 -11832.1  1691.92 -25385.1
 23776.2 -11604.7 -11832.1  5025.81 -703.071  10773.6
-3413.26  1608.89  1691.92 -703.071  105.135 -1516.08
 51235.2 -25130.5 -25385.1  10773.6 -1516.08  23220.2
JTr = -29.1478
 17.9882
 14.8802
  -7.199
0.732457
-14.8477
X =  0.00985068
-0.00153022
  0.0214971
  0.0555449
  0.0158326
 -0.0239884
transformation in this iteration
   0.999768  -0.0215095 -0.00131805   0.0555449
  0.0214954     0.99972 -0.00988113   0.0158326
 0.00153022  0.00985051     0.99995  -0.0239884
          0           0           0           1
current transformation estimation
  0.998446  -0.054619  -0.011031  0.0837472
 0.0550257   0.997653  0.0407425   0.144685
0.00877979 -0.0412862   0.999109    0.17896
         0          0          0          1
it = 4; cloud size = 29280; selected size = 29062
JTJ =  2.67447e+16 -7.84049e+15  -3.6278e+16 -5.63107e+15 -2.42087e+16  1.07807e+15
-7.84049e+15  1.86729e+16 -3.04592e+16  9.04303e+15 -3.79936e+15 -4.65635e+15
 -3.6278e+16 -3.04592e+16  1.52343e+17 -1.09139e+16  6.01844e+16  9.43043e+15
-5.63107e+15  9.04303e+15 -1.09139e+16  4.52285e+15  1.77921e+14 -2.18642e+15
-2.42087e+16 -3.79936e+15  6.01844e+16  1.77921e+14  2.91641e+16  1.91243e+15
 1.07807e+15 -4.65635e+15  9.43043e+15 -2.18642e+15  1.91243e+15  1.19393e+15
JTr =   1.1874e+14
-2.52785e+13
-1.84986e+14
-2.06976e+13
-1.13823e+14
 2.25993e+12
X =  0.00351562
0.000976562
 0.00117368
 0.00585938
 0.00500283
 -0.0078125
transformation in this iteration
    0.999999  -0.00117024  0.000980682   0.00585938
  0.00117368     0.999993  -0.00351447   0.00500283
-0.000976562   0.00351562     0.999993   -0.0078125
           0            0            0            1
current transformation estimation
  0.998389 -0.0558269 -0.0100988  0.0896127
 0.0561664   0.997727  0.0372179   0.149156
0.00799813 -0.0377252   0.999256   0.171573
         0          0          0          1
it = 5; cloud size = 29280; selected size = 29094
JTJ =  3.85228e+15   -8.865e+15  5.95232e+15 -4.71678e+15 -6.04553e+14  2.15227e+15
  -8.865e+15  2.04018e+16 -1.37005e+16  1.08551e+16  1.39049e+15 -4.95323e+15
 5.95232e+15 -1.37005e+16  9.20306e+15 -7.28945e+15 -9.32601e+14  3.32629e+15
-4.71678e+15  1.08551e+16 -7.28945e+15   5.7756e+15  7.39871e+14 -2.63544e+15
-6.04553e+14  1.39049e+15 -9.32601e+14  7.39871e+14  9.52667e+13 -3.37575e+14
 2.15227e+15 -4.95323e+15  3.32629e+15 -2.63544e+15 -3.37575e+14  1.20256e+15
JTr =  2.08085e+13
-4.78883e+13
 3.21585e+13
-2.54797e+13
-3.26387e+12
 1.16265e+13
X =   0.0128433
 0.00216505
-0.00500124
 0.00834517
  0.0198864
  0.0139678
transformation in this iteration
   0.999985  0.00502861  0.00210061  0.00834517
-0.00500121    0.999905  -0.0128536   0.0198864
-0.00216505   0.0128429    0.999915   0.0139678
          0           0           0           1
current transformation estimation
   0.998674  -0.0508881 -0.00781249    0.099067
  0.0510651    0.998397   0.0244209    0.166375
 0.00655723  -0.0247874    0.999671    0.187248
          0           0           0           1
it = 6; cloud size = 29280; selected size = 21480
JTJ =  83223.5 -51361.8 -36190.9  12769.4 -5665.84    37284
-51361.8  38320.7  19571.6 -5566.25  4479.49 -22739.5
-36190.9  19571.6  21818.4 -9771.92  778.722 -17248.9
 12769.4 -5566.25 -9771.92  5948.69  1186.69   7115.4
-5665.84  4479.49  778.722  1186.69  1809.97 -1772.63
   37284 -22739.5 -17248.9  7115.41 -1772.63  17702.3
JTr =  1628.63
-937.616
-809.984
 357.062
 -48.507
 769.675
X =  -0.0178255
-0.00240211
8.74875e-05
 -0.0128786
 -0.0203239
-0.00579425
transformation in this iteration
    0.999997 -4.46571e-05  -0.00240328   -0.0128786
 8.74873e-05     0.999841    0.0178244   -0.0203239
   0.0024021   -0.0178245     0.999838  -0.00579425
           0            0            0            1
current transformation estimation
  0.998653  -0.050873  -0.010216  0.0857307
 0.0512612   0.997792  0.0422348   0.149371
0.00804488 -0.0427016   0.999055   0.178696
         0          0          0          1
it = 7; cloud size = 29280; selected size = 29168
JTJ =   137245 -70218.5 -54756.4  17307.5 -9947.02  55566.1
-70218.5  57113.8  8509.89 -857.801   3813.2 -30683.9
-54756.4  8509.89  60285.3 -22262.6  6150.86 -21120.7
 17307.5 -857.801 -22262.6    10854  653.366  8318.85
-9947.02   3813.2  6150.86  653.366  4836.75  -1860.1
 55566.1 -30683.9 -21120.7  8318.85  -1860.1  24691.2
JTr = -11.5479
 43.9814
-97.1369
 29.5074
-2.42588
-1.77437
X =  0.00564612
-0.00134065
  0.0130597
  0.0252539
 -0.0116627
 -0.0125163
transformation in this iteration
   0.999914  -0.0130667 -0.00126678   0.0252539
  0.0130593    0.999899 -0.00566312  -0.0116627
 0.00134065  0.00564609    0.999983  -0.0125163
          0           0           0           1
current transformation estimation
  0.997887 -0.0638524 -0.0120326   0.108799
 0.0642522   0.997268  0.0364394     0.1378
0.00967301 -0.0371355   0.999263   0.167135
         0          0          0          1
it = 8; cloud size = 29280; selected size = 29052
JTJ =  2.84468e+16 -5.16613e+16  5.65076e+16 -2.70914e+16 -1.62882e+15  1.21495e+16
-5.16613e+16  9.41613e+16 -1.03494e+17  4.94006e+16  2.77454e+15 -2.21357e+16
 5.65076e+16 -1.03494e+17  1.14481e+17 -5.43289e+16 -2.76576e+15  2.43169e+16
-2.70914e+16  4.94006e+16 -5.43289e+16  2.59188e+16  1.44315e+15 -1.16127e+16
-1.62882e+15  2.77454e+15 -2.76576e+15  1.44315e+15  1.92091e+14 -6.57227e+14
 1.21495e+16 -2.21357e+16  2.43169e+16 -1.16127e+16 -6.57227e+14  5.20397e+15
JTr = -6.10337e+13
 1.20158e+14
 -1.4509e+14
 6.36122e+13
-1.52288e+12
-2.80187e+13
X =   0.00356821
 0.000681689
  -0.0012749
-0.000841346
   0.0530048
   0.0107272
transformation in this iteration
    0.999999   0.00127732  0.000677135 -0.000841346
  -0.0012749     0.999993  -0.00356907    0.0530048
-0.000681689    0.0035682     0.999993    0.0107272
           0            0            0            1
current transformation estimation
  0.997974 -0.0626036 -0.0113094   0.108247
  0.062945   0.997475   0.032888   0.190069
0.00922196 -0.0335333   0.999395   0.178278
         0          0          0          1
it = 9; cloud size = 29280; selected size = 29188
JTJ =  1.17564e+17 -5.70052e+16 -1.79488e+16 -3.13878e+16 -6.93931e+16  1.01122e+16
-5.70052e+16  9.88215e+16  -1.0835e+17  4.84635e+16  1.02865e+15 -2.44858e+16
-1.79488e+16  -1.0835e+17  2.32446e+17  -4.8552e+16  7.34678e+16  3.03591e+16
-3.13878e+16  4.84635e+16  -4.8552e+16  2.39647e+16   3.6365e+15 -1.18484e+16
-6.93931e+16  1.02865e+15  7.34678e+16   3.6365e+15  5.82194e+16  2.94149e+15
 1.01122e+16 -2.44858e+16  3.03591e+16 -1.18484e+16  2.94149e+15  6.26501e+15
JTr = -1.63446e+14
 1.16082e+14
-1.03395e+14
  5.7563e+13
 6.16021e+13
-2.42421e+13
X =  -0.011483
  0.011887
 0.0103372
 0.0439453
-0.0367641
  0.119141
transformation in this iteration
  0.999876 -0.0104728  0.0117666  0.0439453
 0.0103363   0.999879   0.011605 -0.0367641
-0.0118867  -0.011482   0.999863   0.119141
         0          0          0          1
current transformation estimation
     0.9973  -0.0734368 0.000106994    0.152286
  0.0733597    0.996318   0.0443652     0.15647
-0.00336464  -0.0442375    0.999015    0.293926
          0           0           0           1
it = 10; cloud size = 29280; selected size = 0
JTJ = 0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
JTr = 0
0
0
0
0
0
X = 0
0
0
0
0
0
transformation in this iteration
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
current transformation estimation
     0.9973  -0.0734368 0.000106994    0.152286
  0.0733597    0.996318   0.0443652     0.15647
-0.00336464  -0.0442375    0.999015    0.293926
          0           0           0           1

Last updated