32 assert(x1.rows() == x2.rows());
33 assert(x1.cols() == x2.cols());
36 Vec xx1 = (x1.col(0)).transpose();
37 Vec yx1 = (x1.col(1)).transpose();
39 double a12 = xx1.dot(yx1);
40 Vec xx2 = (x2.col(0)).transpose();
41 Vec yx2 = (x2.col(1)).transpose();
42 double b12 = xx2.dot(yx2);
44 double a1 = xx1.squaredNorm();
45 double a2 = yx1.squaredNorm();
47 double b1 = xx2.squaredNorm();
48 double b2 = yx2.squaredNorm();
56 P[0] = b1 * b2 * a12 * a12 - a1 * a2 * b12 * b12;
57 P[1] = -2 * a1 * a2 * b12 + 2 * a12 * b1 * b2 + b1 * a12 * a12 +
58 b2 * a12 * a12 - a1 * b12 * b12 - a2 * b12 * b12;
59 P[2] = b1 * b2 - a1 * a2 - 2 * a1 * b12 - 2 * a2 * b12 + 2 * a12 * b1 +
60 2 * a12 * b2 + a12 * a12 - b12 * b12;
61 P[3] = b1 + b2 - 2 * b12 - a1 - a2 + 2 * a12;
86 for (
int i = 0; i < num_roots; ++i) {
88 fs->push_back(
sqrt(roots[i]));
105 assert(x1.rows() == x2.rows());
106 assert(x1.cols() == x2.cols());
109 Mat3 K(Mat3::Identity() * 1.0 / focal);
113 Mat3 C = Mat3::Zero();
114 for (
int i = 0; i < x1.cols(); ++i) {
115 Mat r1i = (K * x1.col(i)).normalized();
116 Mat r2i = (K * x2.col(i)).normalized();
117 C += r2i * r1i.transpose();
121 Eigen::JacobiSVD<Mat> svd(
C, Eigen::ComputeThinU | Eigen::ComputeThinV);
124 ((svd.matrixU() * svd.matrixV().transpose()).determinant() > 0.0) ? 1.0
127 (*R) = svd.matrixU() *
scale * svd.matrixV().transpose();
#define assert(assertion)
float3x3 scale(float3x3 mat, float2 scale)
int SolveCubicPolynomial(Real a, Real b, Real c, Real *x0, Real *x1, Real *x2)
Eigen::Matrix< double, 3, 3 > Mat3
static bool Build_Minimal2Point_PolynomialFactor(const Mat &x1, const Mat &x2, double *P)
void F_FromCorrespondance_2points(const Mat &x1, const Mat &x2, vector< double > *fs)
void GetR_FixedCameraCenter(const Mat &x1, const Mat &x2, const double focal, Mat3 *R)
std::vector< ElementType, Eigen::aligned_allocator< ElementType > > vector