Esempio n. 1
0
  static void Solve(const Mat &x1, const Mat &x2, std::vector<Mat3> *pvec_E)
  {
    assert(3 == x1.rows());
    assert(8 <= x1.cols());
    assert(x1.rows() == x2.rows());
    assert(x1.cols() == x2.cols());

    MatX9 A(x1.cols(), 9);
    EncodeEpipolarEquation(x1, x2, &A);

    Vec9 e;
    Nullspace(&A, &e);
    Mat3 E = Map<RMat3>(e.data());

    // Find the closest essential matrix to E in frobenius norm
    // E = UD'VT
    if (x1.cols() > 8) {
      Eigen::JacobiSVD<Mat3> USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
      Vec3 d = USV.singularValues();
      double a = d[0];
      double b = d[1];
      d << (a+b)/2., (a+b)/2., 0.0;
      E = USV.matrixU() * d.asDiagonal() * USV.matrixV().transpose();
    }
    pvec_E->push_back(E);

  }
void FourPointSolver::Solve(const Mat &x, const Mat &y, vector<Mat3> *Hs) {
  assert(2 == x.rows());
  assert(4 <= x.cols());
  assert(x.rows() == y.rows());
  assert(x.cols() == y.cols());

  Mat::Index n = x.cols();

  Vec9 h;
  if (n == 4)  {
    // In the case of minimal configuration we use fixed sized matrix to let
    //  Eigen and the compiler doing the maximum of optimization.
    typedef Eigen::Matrix<double, 16, 9> Mat16_9;
    Mat16_9 L = Mat::Zero(16, 9);
    BuildActionMatrix(L, x, y);
    Nullspace(&L, &h);
  }
  else {
    MatX9 L = Mat::Zero(n * 2, 9);
    BuildActionMatrix(L, x, y);
    Nullspace(&L, &h);
  }
  Mat3 H = Map<RMat3>(h.data()); // map the linear vector as the H matrix
  Hs->push_back(H);
}