| 154 | } |
| 155 | |
| 156 | bool essential_solver::decompose(const Mat33_t &E_21, eigen_alloc_vector<Mat33_t> &init_rots, eigen_alloc_vector<Vec3_t> &init_transes) |
| 157 | { |
| 158 | // https://en.wikipedia.org/wiki/Essential_matrix#Determining_R_and_t_from_E |
| 159 | |
| 160 | const Eigen::JacobiSVD<Mat33_t> svd(E_21, Eigen::ComputeFullU | Eigen::ComputeFullV); |
| 161 | |
| 162 | Vec3_t trans = svd.matrixU().col(2); |
| 163 | trans.normalize(); |
| 164 | |
| 165 | Mat33_t W = Mat33_t::Zero(); |
| 166 | W(0, 1) = -1; |
| 167 | W(1, 0) = 1; |
| 168 | W(2, 2) = 1; |
| 169 | |
| 170 | Mat33_t rot_1 = svd.matrixU() * W * svd.matrixV().transpose(); |
| 171 | if (rot_1.determinant() < 0) |
| 172 | { |
| 173 | rot_1 *= -1; |
| 174 | } |
| 175 | |
| 176 | Mat33_t rot_2 = svd.matrixU() * W.transpose() * svd.matrixV().transpose(); |
| 177 | if (rot_2.determinant() < 0) |
| 178 | { |
| 179 | rot_2 *= -1; |
| 180 | } |
| 181 | |
| 182 | init_rots = {rot_1, rot_1, rot_2, rot_2}; |
| 183 | init_transes = {trans, -trans, trans, -trans}; |
| 184 | |
| 185 | return true; |
| 186 | } |
| 187 | |
| 188 | Mat33_t essential_solver::create_E_21(const Mat33_t &rot_1w, const Vec3_t &trans_1w, const Mat33_t &rot_2w, const Vec3_t &trans_2w) |
| 189 | { |
no test coverage detected