| 189 | } |
| 190 | |
| 191 | bool TriangulateOptimalPoint(const Eigen::Matrix3x4d& cam1_from_world_mat, |
| 192 | const Eigen::Matrix3x4d& cam2_from_world_mat, |
| 193 | const Eigen::Vector2d& cam_point1, |
| 194 | const Eigen::Vector2d& cam_point2, |
| 195 | Eigen::Vector3d* xyz) { |
| 196 | const Rigid3d cam1_from_world( |
| 197 | Eigen::Quaterniond(cam1_from_world_mat.leftCols<3>()), |
| 198 | cam1_from_world_mat.col(3)); |
| 199 | const Rigid3d cam2_from_world( |
| 200 | Eigen::Quaterniond(cam2_from_world_mat.leftCols<3>()), |
| 201 | cam2_from_world_mat.col(3)); |
| 202 | const Rigid3d cam2_from_cam1 = cam2_from_world * Inverse(cam1_from_world); |
| 203 | const Eigen::Matrix3d E = EssentialMatrixFromPose(cam2_from_cam1); |
| 204 | |
| 205 | Eigen::Vector2d optimal_point1; |
| 206 | Eigen::Vector2d optimal_point2; |
| 207 | FindOptimalImageObservations( |
| 208 | E, cam_point1, cam_point2, &optimal_point1, &optimal_point2); |
| 209 | |
| 210 | return TriangulatePoint(cam1_from_world_mat, |
| 211 | cam2_from_world_mat, |
| 212 | optimal_point1, |
| 213 | optimal_point2, |
| 214 | xyz); |
| 215 | } |
| 216 | |
| 217 | double CalculateTriangulationAngle(const Eigen::Vector3d& proj_center1, |
| 218 | const Eigen::Vector3d& proj_center2, |
nothing calls this directly
no test coverage detected