| 100 | } |
| 101 | |
| 102 | void FindOptimalImageObservations(const Eigen::Matrix3d& E, |
| 103 | const Eigen::Vector2d& point1, |
| 104 | const Eigen::Vector2d& point2, |
| 105 | Eigen::Vector2d* optimal_point1, |
| 106 | Eigen::Vector2d* optimal_point2) { |
| 107 | const Eigen::Vector3d& point1_homogeneous = point1.homogeneous(); |
| 108 | const Eigen::Vector3d& point2_homogeneous = point2.homogeneous(); |
| 109 | |
| 110 | Eigen::Matrix2x3d S; |
| 111 | S << 1, 0, 0, 0, 1, 0; |
| 112 | |
| 113 | // Epipolar lines. |
| 114 | Eigen::Vector2d n1 = S * E * point2_homogeneous; |
| 115 | Eigen::Vector2d n2 = S * E.transpose() * point1_homogeneous; |
| 116 | |
| 117 | const Eigen::Matrix2d E_tilde = E.block<2, 2>(0, 0); |
| 118 | |
| 119 | const double a = n1.transpose() * E_tilde * n2; |
| 120 | const double b = (n1.squaredNorm() + n2.squaredNorm()) / 2.0; |
| 121 | const double c = point1_homogeneous.transpose() * E * point2_homogeneous; |
| 122 | const double d = std::sqrt(b * b - a * c); |
| 123 | double lambda = c / (b + d); |
| 124 | |
| 125 | const Eigen::Vector2d delta1 = lambda * n1; |
| 126 | const Eigen::Vector2d delta2 = lambda * n2; |
| 127 | |
| 128 | n1 -= E_tilde * delta2; |
| 129 | n2 -= E_tilde.transpose() * delta1; |
| 130 | |
| 131 | lambda *= (2.0 * d) / (n1.squaredNorm() + n2.squaredNorm()); |
| 132 | |
| 133 | *optimal_point1 = |
| 134 | (point1_homogeneous - S.transpose() * lambda * n1).hnormalized(); |
| 135 | *optimal_point2 = |
| 136 | (point2_homogeneous - S.transpose() * lambda * n2).hnormalized(); |
| 137 | } |
| 138 | |
| 139 | Eigen::Vector3d EpipoleFromEssentialMatrix(const Eigen::Matrix3d& E, |
| 140 | const bool left_image) { |
no outgoing calls