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 ATA, constructed from a m-by-n matrix A where m is significantly greater than n. This is normally the case in least-square problems.
The matrix ATA is always symmetric by definition.
The matrix ATA is positive semi-definite if it is invertible, since we havexTATAx=(Ax)TAx=∥Ax∥2≥0.
The matrix ATA 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 usedoubleprecision 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
floattype supports a value range of 1.2E-38 to 3.4E+38, and 6-7 decimal digits, whiledoubletype supports a value range of 2.3E-308 to 1.7E+308, and 15 decimal digits.Updates: This issue was resolved by switching to
doubleprecision. (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 1Last updated