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

double Distance( const Conic& c1, const Conic& c2, double circle_radius )
{
  const Matrix3d Q = c1.Dual * c2.C;
  //  const double dsq = 3 - trace(Q)* pow(determinant(Q),-1.0/3.0);
  const double dsq = 3 - Q.trace()* 1.0 / cbrt(Q.determinant());
  return sqrt(dsq) * circle_radius;
}

TEST(Rotation, TestEulerAnglesAndMatrices) {
  // Randomly generate euler angles, convert to a matrix, then convert back.
  // Check that (1) the rotation matrix has det(R)==1, and (2), that we get the
  // same angles back.
  math::RandomGenerator rng(0);
  for (int ii = 0; ii < 1000; ++ii) {
    Vector3d e1;
    e1.setRandom();

    // Converting from rotation matrices to euler angles is only valid when phi,
    // theta, and psi are all < 0.5*PI. Otherwise the problem has multiple
    // solutions, and we can only return one of them with our function.
    e1 *= 0.5 * M_PI;

    Matrix3d R = EulerAnglesToMatrix(e1);
    EXPECT_NEAR(1.0, R.determinant(), 1e-8);

    Vector3d e2 = MatrixToEulerAngles(R);
    EXPECT_NEAR(0.0, S1Distance(e1(0), e2(0)), 1e-8);
    EXPECT_NEAR(0.0, S1Distance(e1(1), e2(1)), 1e-8);
    EXPECT_NEAR(0.0, S1Distance(e1(2), e2(2)), 1e-8);

    EXPECT_TRUE(R.isApprox(EulerAnglesToMatrix(e2), 1e-4));
  }
}

double JacobianEva::evaluate(HexVertex *hv, HexVertex* nhv[3]) {
	Matrix3d J = Matrix3d::Zero();
	for (int i = 0; i < 3; ++i) {
		Point pt = nhv[i]->point() - hv->point();
		pt /= pt.norm();
		for (int j = 0; j < 3; ++j) {
			J(j, i) = pt[j];
		}
	}
	
	return J.determinant();
}

Vector4d rigidBodyDynamics::quaternionFromRot(Matrix3d& R) const{
	Vector4d q;
	double div1, div2, div3, div4;

	double numerical_limit = 1.0e-4;

	if (abs(R.determinant()-1) > numerical_limit ) {
		std::cerr << "R does not have a determinant of +1" << std::endl;
	} else {
		div1 = 0.5*sqrt(1+R(0,0)+R(1,1)+R(2,2));
		div2 = 0.5*sqrt(1+R(0,0)-R(1,1)-R(2,2));
		div3 = 0.5*sqrt(1-R(0,0)-R(1,1)+R(2,2));
		div4 = 0.5*sqrt(1-R(0,0)+R(1,1)-R(2,2));

		//if (div1 > div2 && div1 > div3 && div1 > div4) {
		if (fabs(div1) > numerical_limit) {
			q(3) = div1;
			q(0) = 0.25*(R(1,2)-R(2,1))/q(3);
			q(1) = 0.25*(R(2,0)-R(0,2))/q(3);
			q(2) = 0.25*(R(0,1)-R(1,0))/q(3);
		} else if (fabs(div2) > numerical_limit) {
		//} else if (div2 > div1 && div2 > div3 && div2 > div4) {
			q(0) = div2;
			q(1) = 0.25*(R(0,1)+R(1,0))/q(0);
			q(2) = 0.25*(R(0,2)+R(2,0))/q(0);
			q(3) = 0.25*(R(1,2)+R(2,1))/q(0);
		} else if (fabs(div3) > numerical_limit) {
		//} else if (div3 > div1 && div3 > div2 && div3 > div4) {
			q(2) = div3;
			q(0) = 0.25*(R(0,2)+R(2,0))/q(2);
			q(1) = 0.25*(R(1,2)+R(2,1))/q(2);
			q(3) = 0.25*(R(0,1)-R(1,0))/q(2);
		//} else {
		} else if (fabs(div4) > numerical_limit) {
			q(1) = div4;
			q(0) = 0.25*(R(0,1)+R(1,0))/q(1);
			q(2) = 0.25*(R(1,2)+R(2,1))/q(1);
			q(3) = 0.25*(R(2,0)-R(0,2))/q(1);
		} else {
			std::cerr << "quaternionFromRot didn't convert: [" << div1 << ", " << div2 << ", " << div3 << ", " << div4 << std::endl;
			std::cerr << "Rotation Matrix: " << R << std::endl;
		}
	}
/*
	if (q(3) < 0) {
		q *= -1;
	}
*/
	q /=q.norm();

	return q;
}

        // Original code from the ROS vslam package pe3d.cpp
        // uses the SVD procedure for aligning point clouds
        //   SEE: Arun, Huang, Blostein: Least-Squares Fitting of Two 3D Point Sets
        Eigen::Matrix4d threePointSvd(Eigen::MatrixXd const & p0, Eigen::MatrixXd const & p1)
        {
            using namespace Eigen;

            SM_ASSERT_EQ_DBG(std::runtime_error, p0.rows(), 3, "p0 must be a 3xK matrix");
            SM_ASSERT_EQ_DBG(std::runtime_error, p1.rows(), 3, "p1 must be a 3xK matrix");
            SM_ASSERT_EQ_DBG(std::runtime_error, p0.cols(), p1.cols(), "p0 and p1 must have the same number of columns");

            Vector3d c0 = p0.rowwise().mean();
            Vector3d c1 = p1.rowwise().mean();

            Matrix3d H(Matrix3d::Zero());
            // subtract out
            // p0a -= c0;
            // p0b -= c0;
            // p0c -= c0;
            // p1a -= c1;
            // p1b -= c1;
            // p1c -= c1;

            // Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
            // 	p1c*p0c.transpose();
            for(int i = 0; i < p0.cols(); ++i)
            {
                H += (p0.col(i) - c0) * (p1.col(i) - c1).transpose();
            }
      

            // do the SVD thang
            JacobiSVD<Matrix3d> svd(H,ComputeFullU | ComputeFullV);
            Matrix3d V = svd.matrixV();
            Matrix3d R = V * svd.matrixU().transpose();          
            double det = R.determinant();
        
            if (det < 0.0)
            {
                V.col(2) = V.col(2) * -1.0;
                R = V * svd.matrixU().transpose();
            }
            Vector3d tr = c0-R.transpose()*c1;    // translation 
      
            // transformation matrix, 3x4
            Matrix4d tfm(Matrix4d::Identity());
            //        tfm.block<3,3>(0,0) = R.transpose();
            //        tfm.col(3) = -R.transpose()*tr;
            tfm.topLeftCorner<3,3>() = R.transpose();
            tfm.topRightCorner<3,1>() = tr;
      
            return tfm;
      
        }

void CameraDirectLinearTransformation::decomposePMatrix(const Eigen::Matrix<double,3,4> &P)
{
    Matrix3d M = P.topLeftCorner<3,3>();
    Vector3d m3 = M.row(2).transpose();
    // Follow the HartleyZisserman - "Multiple view geometry in computer vision" implementation chapter 3

    Matrix3d P123,P023,P013,P012;
    P123 << P.col(1),P.col(2),P.col(3);
    P023 << P.col(0),P.col(2),P.col(3);
    P013 << P.col(0),P.col(1),P.col(3);
    P012 << P.col(0),P.col(1),P.col(2);

    double X = P123.determinant();
    double Y = -P023.determinant();
    double Z = P013.determinant();
    double T = -P012.determinant();
    C << X/T,Y/T,Z/T;

    // Image Principal points computed with homogeneous normalization
    this->principalPoint = (M*m3).eval().hnormalized().head<2>();

    // Principal vector  from the camera centre C through pp pointing out from the camera.  This may not be the same as  R(:,3)
    // if the principal point is not at the centre of the image, but it should be similar.
    this->principalVector  =  (M.determinant()*m3).normalized();
    this->R = this->K = Matrix3d::Identity();
    this->rq3(M,this->K,this->R);
    // To understand how K is formed http://ksimek.github.io/2013/08/13/intrinsic/
    // and also read http://ksimek.github.io/2012/08/14/decompose/
    K/=K(2,2); // EXTREMELY IMPORTANT TO MAKE THE K(2,2)==1 !!!

    // K = [ fx, s, x0;
    //       0, fy, y0;
    //       0,  0,  1];
    // Where fx is the focal length on x measured in pixels, fy is the focal length ony  measured in pixels

    // Negate the second column of K and R because the y window coordinates and camera y direction are opposite is positive
    // This is the solution I've found personally to correct the behaviour using OpenGL gluPerspective convention
    this->R.row(2)=-R.row(2);
    // Our 3x3 intrinsic camera matrix K needs two modifications before it's ready to use in OpenGL. First, for proper clipping, the (3,3) element of K must be -1. OpenGL's camera looks down the negative z-axis, so if K33 is positive, vertices in front of the camera will have a negative w coordinate after projection. In principle, this is okay, but because of how OpenGL performs clipping, all of these points will be clipped.
    //this->K.col(2) = -K.col(2);

    // t is the location of the world origin in camera coordinates.
    t = -R*C;
}

void ConsState::prepareOutputs()
{
	Matrix3d rhoF;
	ConsState temp = *this;	
	rhoF <<  v[3], v[4], v[5],
			 v[6], v[7], v[8],
			 v[9], v[10], v[11];
	/* rho = pow(rhoF.determinant()/2., 2.); */
	rho = sqrt(rhoF.determinant()/rho_0);
	//deformation gradient
	Matrix3d F = rhoF/rho;
	// Calculate finger tensor
	Matrix3d G = strainTensor(F);

	//calculate invariants
	/* cout << B_0 << endl; */
	invariants(G);
	dInvariants_G();

	//Stress
	stress(G);
}

Matrix3d
getOrientationAndCentriods(Vector3d & p0a,
                           Vector3d & p0b,
                           Vector3d & p0c,
                           Vector3d & p1a,
                           Vector3d & p1b,
                           Vector3d & p1c,
                           Vector3d & c0,
                           Vector3d & c1)
{
  c0 = (p0a+p0b+p0c)*(1.0/3.0);
  c1 = (p1a+p1b+p1c)*(1.0/3.0);

  // subtract out
  p0a -= c0;
  p0b -= c0;
  p0c -= c0;
  p1a -= c1;
  p1b -= c1;
  p1c -= c1;

  Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
               p1c*p0c.transpose();

  JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
  Matrix3d V = svd.matrixV();

  Matrix3d  R;
  R = V * svd.matrixU().transpose();
  double det = R.determinant();

  if (det < 0.0)
  {
    V.col(2) = V.col(2)*-1.0;
    R = V * svd.matrixU().transpose();
  }
  return R;
}

bool Homography::
decompose()
{
  decompositions.clear();
  JacobiSVD<MatrixXd> svd(H_c2_from_c1, ComputeThinU | ComputeThinV);

  Vector3d singular_values = svd.singularValues();

  double d1 = fabs(singular_values[0]); // The paper suggests the square of these (e.g. the evalues of AAT)
  double d2 = fabs(singular_values[1]); // should be used, but this is wrong. c.f. Faugeras' book.
  double d3 = fabs(singular_values[2]);

  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV();                    // VT^T

  double s = U.determinant() * V.determinant();

  double dPrime_PM = d2;

  int nCase;
  if(d1 != d2 && d2 != d3)
    nCase = 1;
  else if( d1 == d2 && d2 == d3)
    nCase = 3;
  else
    nCase = 2;

  if(nCase != 1)
  {
    printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. ");
    return false;
  }

  double x1_PM;
  double x2;
  double x3_PM;

  // All below deals with the case = 1 case.
  // Case 1 implies (d1 != d3)
  { // Eq. 12
    x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3));
    x2    = 0;
    x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3));
  };

  double e1[4] = {1.0,-1.0, 1.0,-1.0};
  double e3[4] = {1.0, 1.0,-1.0,-1.0};

  Vector3d np;
  HomographyDecomposition decomp;

  // Case 1, d' > 0:
  decomp.d = s * dPrime_PM;
  for(size_t signs=0; signs<4; signs++)
  {
    // Eq 13
    decomp.R = Matrix3d::Identity();
    double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
    double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2;
    decomp.R(0,0) = dCosTheta;
    decomp.R(0,2) = -dSinTheta;
    decomp.R(2,0) = dSinTheta;
    decomp.R(2,2) = dCosTheta;

    // Eq 14
    decomp.t[0] = (d1 - d3) * x1_PM * e1[signs];
    decomp.t[1] = 0.0;
    decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs];

    np[0] = x1_PM * e1[signs];
    np[1] = x2;
    np[2] = x3_PM * e3[signs];
    decomp.n = V * np;

    decompositions.push_back(decomp);
  }

  // Case 1, d' < 0:
  decomp.d = s * -dPrime_PM;
  for(size_t signs=0; signs<4; signs++)
  {
    // Eq 15
    decomp.R = -1 * Matrix3d::Identity();
    double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
    double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2;
    decomp.R(0,0) = dCosPhi;
    decomp.R(0,2) = dSinPhi;
    decomp.R(2,0) = dSinPhi;
    decomp.R(2,2) = -dCosPhi;

    // Eq 16
    decomp.t[0] = (d1 + d3) * x1_PM * e1[signs];
    decomp.t[1] = 0.0;
    decomp.t[2] = (d1 + d3) * x3_PM * e3[signs];

    np[0] = x1_PM * e1[signs];
    np[1] = x2;
    np[2] = x3_PM * e3[signs];
    decomp.n = V * np;

//.........这里部分代码省略.........

//.........这里部分代码省略.........
        Vector3f p0a = t3d[a];
        Vector3f p0b = t3d[b];
        Vector3f p0c = t3d[c];

        Vector3f p1a = q3d[a];
        Vector3f p1b = q3d[b];
        Vector3f p1c = q3d[c];

        Vector3f c0 = (p0a+p0b+p0c)*(1.0/3.0);
        Vector3f c1 = (p1a+p1b+p1c)*(1.0/3.0);

        // subtract out
        p0a -= c0;
        p0b -= c0;
        p0c -= c0;
        p1a -= c1;
        p1b -= c1;
        p1c -= c1;

        Matrix3f Hf = p1a*p0a.transpose() + p1b*p0b.transpose() +
	              p1c*p0c.transpose();
	Matrix3d H = Hf.cast<double>();

#if 0
        cout << p0a.transpose() << endl;
        cout << p0b.transpose() << endl;
        cout << p0c.transpose() << endl;
#endif

        // do the SVD thang
        JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
        Matrix3d V = svd.matrixV();
        Matrix3d R = V * svd.matrixU().transpose();          
        double det = R.determinant();
        //ntot++;
        if (det < 0.0)
          {
            //nneg++;
            V.col(2) = V.col(2)*-1.0;
            R = V * svd.matrixU().transpose();
          }

	Vector3d cd0 = c0.cast<double>();
	Vector3d cd1 = c1.cast<double>();
        Vector3d tr = cd0-R*cd1;    // translation 

	//      cout << "[PE test] R: " << endl << R << endl;
	//	cout << "[PE test] t: " << tr.transpose() << endl;

        Vector3f trf = tr.cast<float>();      // convert to floats
        Matrix3f Rf = R.cast<float>();
	Rf = Kf*Rf;
	trf = Kf*trf;

        // find inliers, based on image reprojection
        int inl = 0;
        for (int i=0; i<nmatch; i++)
          {
            const Vector3f &pt1 = q3d[i];
            Vector3f ipt = Rf*pt1+trf; // transform query point
            float iz = 1.0/ipt.z();
	    Vector2f &kp = t2d[i];
	    //	    cout << kp.transpose() << " " << pt1.transpose() << " " << ipt.transpose() << endl;
            float dx = kp.x() - ipt.x()*iz;
            float dy = kp.y() - ipt.y()*iz;
            float dd = fb/t3d[i].z() - fb/ipt.z(); // diff of disparities, could pre-compute t3d

  int PoseEstimator3d::estimate(const Frame& f0, const Frame& f1, 
                                const std::vector<cv::DMatch> &matches)
  {
    // convert keypoints in match to 3d points
    std::vector<Vector4d, aligned_allocator<Vector4d> > p0; // homogeneous coordinates
    std::vector<Vector4d, aligned_allocator<Vector4d> > p1;

    int nmatch = matches.size();

    // set up data structures for fast processing
    // indices to good matches
    vector<int> m0, m1;
    for (int i=0; i<nmatch; i++)
      {
        if (f0.disps[matches[i].queryIdx] > minMatchDisp && 
            f1.disps[matches[i].trainIdx] > minMatchDisp)
          {
            m0.push_back(matches[i].queryIdx);
            m1.push_back(matches[i].trainIdx);
          }
      }

    nmatch = m0.size();
    if (nmatch < 3) return 0;   // can't do it...

    int bestinl = 0;

    // RANSAC loop
    #pragma omp parallel for shared( bestinl )
    for (int i=0; i<numRansac; i++) 
      {
        // find a candidate
        int a=rand()%nmatch;
        int b = a;
        while (a==b)
          b=rand()%nmatch;
        int c = a;
        while (a==c || b==c)
          c=rand()%nmatch;

        int i0a = m0[a];
        int i0b = m0[b];
        int i0c = m0[c];
        int i1a = m1[a];
        int i1b = m1[b];
        int i1c = m1[c];

        if (i0a == i0b || i0a == i0c || i0b == i0c ||
            i1a == i1b || i1a == i1c || i1b == i1c)
          continue;

        // get centroids
        Vector3d p0a = f0.pts[i0a].head<3>();
        Vector3d p0b = f0.pts[i0b].head<3>();
        Vector3d p0c = f0.pts[i0c].head<3>();
        Vector3d p1a = f1.pts[i1a].head<3>();
        Vector3d p1b = f1.pts[i1b].head<3>();
        Vector3d p1c = f1.pts[i1c].head<3>();

        Vector3d c0 = (p0a+p0b+p0c)*(1.0/3.0);
        Vector3d c1 = (p1a+p1b+p1c)*(1.0/3.0);

        // subtract out
        p0a -= c0;
        p0b -= c0;
        p0c -= c0;
        p1a -= c1;
        p1b -= c1;
        p1c -= c1;

        Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
                     p1c*p0c.transpose();

#if 0
        cout << p0a.transpose() << endl;
        cout << p0b.transpose() << endl;
        cout << p0c.transpose() << endl;
#endif

        // do the SVD thang
        JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
        Matrix3d V = svd.matrixV();
        Matrix3d R = V * svd.matrixU().transpose();          
        double det = R.determinant();
        //ntot++;
        if (det < 0.0)
          {
            //nneg++;
            V.col(2) = V.col(2)*-1.0;
            R = V * svd.matrixU().transpose();
          }
        Vector3d tr = c0-R*c1;    // translation 

        //        cout << "[PE test] R: " << endl << R << endl;
        //        cout << "[PE test] t: " << tr.transpose() << endl;

#if 0
        R << 0.9997, 0.002515, 0.02418,
          -0.002474, .9999, -0.001698,
          -0.02418, 0.001638, 0.9997;
//.........这里部分代码省略.........