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

bool depthFromTriangulation(
    const SE3& T_search_ref,
    const Vector3d& f_ref,
    const Vector3d& f_cur,
    double& depth)
{
  Matrix<double,3,2> A; A << T_search_ref.rotationMatrix() * f_ref, f_cur;
  const Matrix2d AtA = A.transpose()*A;
  if(AtA.determinant() < 0.000001)
    return false;
  const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();
  depth = fabs(depth2[0]);
  return true;
}

int getBestSearchLevel(
    const Matrix2d& A_cur_ref,
    const int max_level)
{
  // Compute patch level in other image
  int search_level = 0;
  double D = A_cur_ref.determinant();
  while(D > 3.0 && search_level < max_level)
  {
    search_level += 1;
    D *= 0.25;
  }
  return search_level;
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  using namespace Eigen;

  if(nrhs < 1 || nlhs != 1) {
    mexWarnMsgTxt("Minimum one input and one output parameter required");
    return;
  }

  //Parse opts
  bool fast2D = false;

  if(nrhs > 1) {
    const mxArray *opt = prhs[1];

    const mxArray *opt_fast2D = mxGetField(opt, 0, "fast2D");
    if(opt_fast2D)
      fast2D = mxGetScalar(opt_fast2D);
  }

  const mwSize* dims = mxGetDimensions(prhs[0]);

  const mwSize m = dims[0], n = dims[1];

  if(n%m != 0)
    mexErrMsgTxt("Invalid input (inconsistent dimensions).");

  mxArray *R        = mxCreateNumericMatrix(m, n,
                                         mxDOUBLE_CLASS, mxREAL);

  const double *data_in  = mxGetPr(prhs[0]);
        double *data_out = mxGetPr(R);

  const mwSize nt = n / m;

  if(m == 2) {

    if(fast2D) {
    //special case closed form solution for 2x2

    for(mwSize t = 0; t < nt; ++t) {
      //get transformation
      Map<const Matrix2d> T(data_in + 4*t);

      //closed form solution
      double trace        = T.trace();
      double offdiff      = T(0,1) - T(1,0);
      double off_term     = offdiff / trace;
      double denom_global = 1 / sqrt(off_term*off_term + 1);

      Matrix2d R;
      R(0,0) = denom_global; R(0,1) = off_term * denom_global;
      R(1,0) = -R(0,1);      R(1,1) = R(0,0);

      if(R.determinant() < 0) //safety checks
        R *= 1;

      Map<Matrix2d>(data_out + 4*t) = R;
    }

    } else {
      //hope for better compiler optimization
      for(mwSize t = 0; t < nt; ++t) {
        //get transformation
        Map<const Matrix2d> T(data_in + 4*t);

        //get svd
        const JacobiSVD<Matrix2d> svd = T.jacobiSvd(ComputeFullU | ComputeFullV);

        Matrix2d R(svd.matrixU() * svd.matrixV().transpose());

        if(svd.singularValues().prod() < 0) //safety checks
          R *= -1;

        Map<Matrix2d>(data_out + 4*t) = R;
      }
    }

  } else if(m == 3) {
    //hope for better compiler optimization
    for(mwSize t = 0; t < nt; ++t) {
      //get transformation
      Map<const Matrix3d> T(data_in + 9*t);

      //get svd
      const JacobiSVD<Matrix3d> svd = T.jacobiSvd(ComputeFullU | ComputeFullV);

      Matrix3d R(svd.matrixU() * svd.matrixV().transpose());

      if(svd.singularValues().prod() < 0) //safety checks
        R *= -1;

      Map<Matrix3d>(data_out + 9*t) = R;
    }

  } else {
    //generic nd implementation
    for(mwSize t = 0; t < nt; ++t) {
      //get transformation
      Map<const MatrixXd> T(data_in + m*m*t, m, m);
//.........这里部分代码省略.........