| 60 | } |
| 61 | |
| 62 | inline std::array<Eigen::Vector4d, 3> fetzer_ds( |
| 63 | const Eigen::Matrix3d& i1_G_i0) { |
| 64 | Eigen::JacobiSVD<Eigen::Matrix3d> svd( |
| 65 | i1_G_i0, Eigen::ComputeFullU | Eigen::ComputeFullV); |
| 66 | Eigen::Vector3d s = svd.singularValues(); |
| 67 | |
| 68 | Eigen::Vector3d v_0 = svd.matrixV().col(0); |
| 69 | Eigen::Vector3d v_1 = svd.matrixV().col(1); |
| 70 | |
| 71 | Eigen::Vector3d u_0 = svd.matrixU().col(0); |
| 72 | Eigen::Vector3d u_1 = svd.matrixU().col(1); |
| 73 | |
| 74 | Eigen::Vector3d ai = |
| 75 | Eigen::Vector3d(s(0) * s(0) * (v_0(0) * v_0(0) + v_0(1) * v_0(1)), |
| 76 | s(0) * s(1) * (v_0(0) * v_1(0) + v_0(1) * v_1(1)), |
| 77 | s(1) * s(1) * (v_1(0) * v_1(0) + v_1(1) * v_1(1))); |
| 78 | |
| 79 | Eigen::Vector3d aj = Eigen::Vector3d(u_1(0) * u_1(0) + u_1(1) * u_1(1), |
| 80 | -(u_0(0) * u_1(0) + u_0(1) * u_1(1)), |
| 81 | u_0(0) * u_0(0) + u_0(1) * u_0(1)); |
| 82 | |
| 83 | Eigen::Vector3d bi = Eigen::Vector3d(s(0) * s(0) * v_0(2) * v_0(2), |
| 84 | s(0) * s(1) * v_0(2) * v_1(2), |
| 85 | s(1) * s(1) * v_1(2) * v_1(2)); |
| 86 | |
| 87 | Eigen::Vector3d bj = |
| 88 | Eigen::Vector3d(u_1(2) * u_1(2), -(u_0(2) * u_1(2)), u_0(2) * u_0(2)); |
| 89 | |
| 90 | Eigen::Vector4d d_01 = fetzer_d(ai, bi, aj, bj, 1, 0); |
| 91 | Eigen::Vector4d d_02 = fetzer_d(ai, bi, aj, bj, 0, 2); |
| 92 | Eigen::Vector4d d_12 = fetzer_d(ai, bi, aj, bj, 2, 1); |
| 93 | |
| 94 | std::array<Eigen::Vector4d, 3> ds; |
| 95 | ds[0] = d_01; |
| 96 | ds[1] = d_02; |
| 97 | ds[2] = d_12; |
| 98 | |
| 99 | return ds; |
| 100 | } |
| 101 | |
| 102 | class FetzerFocalLengthCost { |
| 103 | public: |
no test coverage detected