C++ Mat3::determinant方法代码示例

double volume(OpenMesh::Vec3f& pointA, OpenMesh::Vec3f& pointB, OpenMesh::Vec3f& pointC)
{
	// http://www.ditutor.com/vectors/volume_tetrahedron.html
	Mat3 m = pointsToMat(pointA , pointB, pointC);
	// use minus sign or change the order of the points in above function call 
	return -m.determinant()/6.0;
}

// Check the properties of a fundamental matrix:
//
//   1. The determinant is 0 (rank deficient)
//   2. The condition x'T*F*x = 0 is satisfied to precision.
//
bool ExpectFundamentalProperties(const Mat3 &F,
                                 const Mat &ptsA,
                                 const Mat &ptsB,
                                 double precision) {
  bool bOk = true;
  bOk &= F.determinant() < precision;
  assert(ptsA.cols() == ptsB.cols());
  const Mat hptsA = ptsA.colwise().homogeneous();
  const Mat hptsB = ptsB.colwise().homogeneous();
  for (int i = 0; i < ptsA.cols(); ++i) {
    const double residual = hptsB.col(i).dot(F * hptsA.col(i));
    bOk &= residual < precision;
  }
  return bOk;
}

IGL_INLINE void igl::fit_rotations(
  const Eigen::PlainObjectBase<DerivedS> & S,
  const bool single_precision,
  Eigen::PlainObjectBase<DerivedD> & R)
{
  using namespace std;
  const int dim = S.cols();
  const int nr = S.rows()/dim;
  assert(nr * dim == S.rows());
  assert(dim == 3);

  // resize output
  R.resize(dim,dim*nr); // hopefully no op (should be already allocated)

  //std::cout<<"S=["<<std::endl<<S<<std::endl<<"];"<<std::endl;
  //MatrixXd si(dim,dim);
  Eigen::Matrix<typename DerivedS::Scalar,3,3> si;// = Eigen::Matrix3d::Identity();
  // loop over number of rotations we're computing
  for(int r = 0;r<nr;r++)
  {
    // build this covariance matrix
    for(int i = 0;i<dim;i++)
    {
      for(int j = 0;j<dim;j++)
      {
        si(i,j) = S(i*nr+r,j);
      }
    }
    typedef Eigen::Matrix<typename DerivedD::Scalar,3,3> Mat3;
    typedef Eigen::Matrix<typename DerivedD::Scalar,3,1> Vec3;
    Mat3 ri;
    if(single_precision)
    {
      polar_svd3x3(si, ri);
    }else
    {
      Mat3 ti,ui,vi;
      Vec3 _;
      igl::polar_svd(si,ri,ti,ui,_,vi);
    }
    assert(ri.determinant() >= 0);
    R.block(0,r*dim,dim,dim) = ri.block(0,0,dim,dim).transpose();
    //cout<<matlab_format(si,C_STR("si_"<<r))<<endl;
    //cout<<matlab_format(ri.transpose().eval(),C_STR("ri_"<<r))<<endl;
  }
}

void KRt_From_P(const Mat34 &P, Mat3 *Kp, Mat3 *Rp, Vec3 *tp) {
  // Decompose using the RQ decomposition HZ A4.1.1 pag.579.
  Mat3 K = P.block(0, 0, 3, 3);

  Mat3 Q;
  Q.setIdentity();

  // Set K(2,1) to zero.
  if (K(2,1) != 0) {
    double c = -K(2,2);
    double s = K(2,1);
    double l = sqrt(c * c + s * s);
    c /= l; s /= l;
    Mat3 Qx;
    Qx << 1, 0, 0,
          0, c, -s,
          0, s, c;
    K = K * Qx;
    Q = Qx.transpose() * Q;
  }
  // Set K(2,0) to zero.
  if (K(2,0) != 0) {
    double c = K(2,2);
    double s = K(2,0);
    double l = sqrt(c * c + s * s);
    c /= l; s /= l;
    Mat3 Qy;
    Qy << c, 0, s,
          0, 1, 0,
         -s, 0, c;
    K = K * Qy;
    Q = Qy.transpose() * Q;
  }
  // Set K(1,0) to zero.
  if (K(1,0) != 0) {
    double c = -K(1,1);
    double s = K(1,0);
    double l = sqrt(c * c + s * s);
    c /= l; s /= l;
    Mat3 Qz;
    Qz << c,-s, 0,
          s, c, 0,
          0, 0, 1;
    K = K * Qz;
    Q = Qz.transpose() * Q;
  }

  Mat3 R = Q;

  //Mat3 H = P.block(0, 0, 3, 3);
  // RQ decomposition
  //Eigen::HouseholderQR<Mat3> qr(H);
  //Mat3 K = qr.matrixQR().triangularView<Eigen::Upper>();
  //Mat3 R = qr.householderQ();

  // Ensure that the diagonal is positive and R determinant == 1.
  if (K(2,2) < 0) {
    K = -K;
    R = -R;
  }
  if (K(1,1) < 0) {
    Mat3 S;
    S << 1, 0, 0,
         0,-1, 0,
         0, 0, 1;
    K = K * S;
    R = S * R;
  }
  if (K(0,0) < 0) {
    Mat3 S;
    S << -1, 0, 0,
          0, 1, 0,
          0, 0, 1;
    K = K * S;
    R = S * R;
  }

  // Compute translation.
  Eigen::PartialPivLU<Mat3> lu(K);
  Vec3 t = lu.solve(P.col(3));

  if(R.determinant()<0) {
    R = -R;
    t = -t;
  }

  // scale K so that K(2,2) = 1
  K = K / K(2,2);

  *Kp = K;
  *Rp = R;
  *tp = t;
}

void DecomposeProjectionMatrix(const Mat& P, Mat3* K, Mat3* R, Vec3* t)
{
    // This is a modified version of the function "KRt_From_P", found in libmv and openMVG.
    // It is subject to the terms of the Mozilla Public License, v. 2.0, a copy of the MPL
    // can be found under "license.openMVG"

    // Decompose using the RQ decomposition HZ A4.1.1 pag.579.
    Mat3 Kp = P.block(0, 0, 3, 3);
    Mat3 Q; Q.setIdentity();

    // Set K(2, 1) to zero.
    if (Kp(2, 1) != 0)
    {
        double c = -Kp(2, 2);
        double s =  Kp(2, 1);
        double l = sqrt(c * c + s * s);
        c /= l; s /= l;
        Mat3 Qx;
        Qx <<  1,  0,  0,
               0,  c, -s,
               0,  s,  c;
        Kp = Kp * Qx;
        Q = Qx.transpose() * Q;
    }

    // Set K(2, 0) to zero.
    if (Kp(2, 0) != 0)
    {
        double c = Kp(2, 2);
        double s = Kp(2, 0);
        double l = sqrt(c * c + s * s);
        c /= l; s /= l;
        Mat3 Qy;
        Qy <<  c,  0,  s,
               0,  1,  0,
              -s,  0,  c;
        Kp = Kp * Qy;
        Q = Qy.transpose() * Q;
    }

    // Set K(1, 0) to zero.
    if (Kp(1, 0) != 0)
    {
        double c = -Kp(1, 1);
        double s =  Kp(1, 0);
        double l = sqrt(c * c + s * s);
        c /= l; s /= l;
        Mat3 Qz;
        Qz <<  c, -s,  0,
               s,  c,  0,
               0,  0,  1;
        Kp = Kp * Qz;
        Q = Qz.transpose() * Q;
    }

    Mat3 Rp = Q;

    // Ensure that the diagonal is positive and R determinant == 1
    if (Kp(2, 2) < 0)
    {
        Kp = -Kp;
        Rp = -Rp;
    }

    if (Kp(1, 1) < 0)
    {
        Mat3 S;
        S <<  1,  0,  0,
              0, -1,  0,
              0,  0,  1;
        Kp = Kp * S;
        Rp = S  * Rp;
    }

    if (Kp(0, 0) < 0)
    {
        Mat3 S;
        S << -1,  0,  0,
              0,  1,  0,
              0,  0,  1;
        Kp = Kp * S;
        Rp = S  * Rp;
    }

    // Compute translation.
    Vec3 tp = Kp.colPivHouseholderQr().solve(P.col(3));

    if(Rp.determinant() < 0)
    {
        Rp = -Rp;
        tp = -tp;
    }

    // Scale K so that K(2, 2) = 1
    Kp = Kp / Kp(2, 2);

    *K = Kp;
    *R = Rp;
    *t = tp;
}

bool FindExtrinsics(const Mat3& E, const Point2Vec& pts1, const Point2Vec& pts2, Mat3* R, Vec3* t)
{
    const auto num_correspondences = pts1.size();

    // Put first camera at origin
    Mat3 R0;    R0.setIdentity();
    Vec3 t0;    t0.setZero();

    // Find the SVD of E
    Eigen::JacobiSVD<Mat3> svd{E, Eigen::ComputeFullU | Eigen::ComputeFullV};
    Mat3 U  = svd.matrixU();
    Mat3 Vt = svd.matrixV().transpose();

    // Find R and t
    Mat3 D;
    D << 0,  1,  0,
        -1,  0,  0,
         0,  0,  1;

    Mat3 Ra = U * D * Vt;
    Mat3 Rb = U * D.transpose() * Vt;

    if (Ra.determinant() < 0.0) Ra *= -1.0;
    if (Rb.determinant() < 0.0) Rb *= -1.0;

    Vec3 tu = U.col(2);

    // Figure out which configuration is correct using the supplied points
    int c1_pos = 0, c2_pos = 0, c1_neg = 0, c2_neg = 0;

    for (std::size_t i = 0; i < num_correspondences; i++)
    {
        const Point3 Q = Triangulate(Observations{Observation{pts1[i], R0, t0}, Observation{pts2[i], Ra, tu}});
        const Point3 PQ = (Ra * Q) + tu;

        if (Q.z() > 0)  c1_pos++;
        else            c1_neg++;
        
        if (PQ.z() > 0) c2_pos++;
        else            c2_neg++;
    }

    if (c1_pos < c1_neg && c2_pos < c2_neg)
    {
        *R =  Ra;
        *t =  tu;
    } else if (c1_pos > c1_neg && c2_pos > c2_neg)
    {
        *R =  Ra;
        *t = -tu;
    } else
    {
        // Triangulate again
        c1_pos = c1_neg = c2_pos = c2_neg = 0;

        for (std::size_t i = 0; i < num_correspondences; i++)
        {
            const Point3 Q = Triangulate(Observations{Observation{pts1[i], R0, t0}, Observation{pts2[i], Rb, tu}});
            const Point3 PQ = (Rb * Q) + tu;

            if (Q.z() > 0)  c1_pos++;
            else            c1_neg++;
        
            if (PQ.z() > 0) c2_pos++;
            else            c2_neg++;
        }

        if (c1_pos < c1_neg && c2_pos < c2_neg)
        {
            *R =  Rb;
            *t =  tu;
        } else if (c1_pos > c1_neg && c2_pos > c2_neg)
        {
            *R =  Rb;
            *t = -tu;
        } else
        {
            std::cerr << "[FindExtrinsics] Error: no case found!";
            return false;
        }
    }

    return true;
}