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

MatrixXd Utils::calculateHomographyMatrix(vector<Vector3i> selectedPoints, vector<Vector3d> realWorldPoints)
{
    MatrixXd A(8, 8);
    MatrixXd B(8,1);

    for (uint i = 0; i < realWorldPoints.size(); i++)
    {
        double realWorldX = realWorldPoints.at(i).x();
        double realWorldY = realWorldPoints.at(i).y();
        int selectedPointX = selectedPoints.at(i).x();
        int selectedPointY = selectedPoints.at(i).y();

        A.row(i*2) << realWorldX, realWorldY, 1, 0, 0, 0, -realWorldX*selectedPointX, -realWorldY*selectedPointX;
        A.row(i*2 + 1) << 0, 0, 0, realWorldX, realWorldY, 1, -realWorldX*selectedPointY, -realWorldY*selectedPointY;

        B.row(i*2) << selectedPointX;
        B.row(i*2 +1) << selectedPointY;
    }
    //A  = A.inverse().eval();

    MatrixXd x = A.fullPivLu().solve(B);
    Matrix3d H;

    H << x(0), x(1), x(2), x(3), x(4), x(5), x(6), x(7), 1;

    return H.inverse();
}

Vector6d logmap_se3(Matrix4d T){
    Matrix3d R, Id3 = Matrix3d::Identity();
    Vector3d Vt, t, w;
    Matrix3d V = Matrix3d::Identity(), w_hat = Matrix3d::Zero();
    Vector6d x;
    Vt << T(0,3), T(1,3), T(2,3);
    w  << 0.f, 0.f, 0.f;
    R = T.block(0,0,3,3);
    double cosine = (R.trace() - 1.f)/2.f;
    if(cosine > 1.f)
        cosine = 1.f;
    else if (cosine < -1.f)
        cosine = -1.f;
    double sine = sqrt(1.0-cosine*cosine);
    if(sine > 1.f)
        sine = 1.f;
    else if (sine < -1.f)
        sine = -1.f;
    double theta  = acos(cosine);
    if( theta > 0.000001 ){
        w_hat = theta*(R-R.transpose())/(2.f*sine);
        w = skewcoords(w_hat);
        Matrix3d s;
        s = skew(w) / theta;
        V = Id3 + s * (1.f-cosine) / theta + s * s * (theta - sine) / theta;
    }
    t = V.inverse() * Vt;
    x.head(3) = t;
    x.tail(3) = w;
    return x;
}

double distPtTri(Vector3d p, Matrix4d m){
    /// compute distance between a triangle and a point
    double s[4];
    Vector3d a,b,c,n;
    a << m(0,0), m(0,1), m(0,2);
    b << m(1,0), m(1,1), m(1,2);
    c << m(2,0), m(2,1), m(2,2);
    n << m(3,0), m(3,1), m(3,2);
    double k=(n-a).dot(a-p);
    if(k<0) return HUGE_VALF;
    s[0]=distPtLin(p,a,b);
    s[1]=distPtLin(p,b,c);
    s[2]=distPtLin(p,c,a);
    Matrix3d A;
    A << b(0)-a(0), c(0)-a(0), n(0)-a(0),
    b(1)-a(1), c(1)-a(1), n(1)-a(1),
    b(2)-a(2), c(2)-a(2), n(2)-a(2);
    Vector3d v = A.inverse()*(p-a);
    if(v(0)>0 && v(1)>0 && v(0)+v(1)<1){
        s[3]=k*k;
    }else{
        s[3] = HUGE_VALF;
    }
    return min(min(min(s[0],s[1]),s[2]),s[3]);
}

int main(int, char**)
{
  cout.precision(3);
  Matrix3d m = Matrix3d::Random();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Its inverse is:" << endl << m.inverse() << endl;

  return 0;
}

Vector3d Compound::PointOfReference::vectorFromPointAndNearVector(Compound::PointPtr point, Vector3d vector, int i) {
	if(i > 10 | vector.squaredNorm() > 10000 | vector != vector) {
		//std::cout << "Compound.cpp vector:\n" << vector << std::endl;
		//return vector;
		return Vector3d(0,0,0);
	}
	Vector3d v1 = point->getPosition()->getVector();
	//Vector3d v0 = point->getPosition()->getVector();
	//assert(v0 == v0);
	double epsilon = 0.00001;
	Manifold* space = point->getPosition()->getSpace();
	//std::cout << "Compound.cpp i:\t" << i << std::endl;
	Vector3d v0 = this->pointFromVector(vector)->getVector(space);
	Vector3d vx = this->pointFromVector(vector + Vector3d(epsilon,0,0))->getVector(space);
	Vector3d vy = this->pointFromVector(vector + Vector3d(0,epsilon,0))->getVector(space);
	Vector3d vz = this->pointFromVector(vector + Vector3d(0,0,epsilon))->getVector(space);
	assert(vz == vz);
	Matrix3d jacobean;
	jacobean << vx-v0,vy-v0,vz-v0;
	jacobean /= epsilon;
	/*std::cout << "getPosition()->getVector():\n" << getPosition()->getVector() << std::endl;
	std::cout << "vector:\n" << vector << std::endl;
	std::cout << "v0:\n" << v0 << std::endl;
	std::cout << "vx:\n" << vx << std::endl;
	std::cout << "vy:\n" << vy << std::endl;
	std::cout << "vz:\n" << vz << std::endl;*/
	//std::cout << "jacobean:\n" << jacobean << std::endl;
	Vector3d delta = jacobean.inverse()*(v1-v0);
	//std::cout << "v1-v0:\n" << v1-v0 << std::endl;
	//std::cout << "delta:\n" << delta << std::endl;
	if(delta != delta) {
		return Vector3d(0,0,0);
	}
	//std::cout << "delta.norm():\t" << delta.norm() << std::endl;
	double squaredNorm = delta.squaredNorm();
	double max = 5;
	if(squaredNorm > max*max) {
		delta = max*delta/sqrt(squaredNorm);
	}
	if(squaredNorm < EPSILON*EPSILON) {
		/*std::cout << "v1-v0:\n" << v1-v0 << std::endl;
		std::cout << "delta:\n" << delta << std::endl;
		std::cout << "vector:\n" << vector << std::endl;
		std::cout << "delta+vector:\n" << delta+vector << std::endl;
		std::cout << "pointOfReference->vectorFromPoint(point):\n" << pointOfReference->getGeodesic(point->getPosition())->getVector() << std::endl;*/
		assert(pointFromVector(delta+vector)->getPosition()->getSpace() == point->getPosition()->getSpace());
		assert((pointFromVector(delta+vector)->getPosition()->getVector() - point->getPosition()->getVector()).squaredNorm() < EPSILON);
		/*assert((pointOfReference->getSpace()->pointFromVector(pointOfReference, vector)->getVector() - point->getPosition()->getVector()).squaredNorm() < EPSILON);
		assert((this->pointFromVector(delta+vector)->getPosition()->getVector() - pointOfReference->getSpace()->pointFromVector(pointOfReference, vector)->getVector()).squaredNorm() < EPSILON);
		assert((delta+vector - pointOfReference->getSpace()->vectorFromPoint(pointOfReference, point->getPosition())).squaredNorm() < EPSILON);*/	//Only for portals that connect to themselves.
		//std::cout << "i:\t" << i << std::endl;
		return delta+vector;
	} else {
		return vectorFromPointAndNearVector(point, delta+vector, i+1);
	}
}

vector<pair<int, Vector3d>> DataGenerator::getImage()
{
    vector<pair<int, Vector3d>> image;
    Vector3d position = getPosition();
    Matrix3d quat = getRotation();           //R_gb
    printf("max: %d\n", current_id);
    for (int i = 0; i < NUM_POINTS; i++)
    {
        double xx = pts[i * 3 + 0] - position(0);
        double yy = pts[i * 3 + 1] - position(1);
        double zz = pts[i * 3 + 2] - position(2);
        Vector3d local_point = Ric.inverse() * (quat.inverse() * Vector3d(xx, yy, zz) - Tic);
        xx = local_point(0);
        yy = local_point(1);
        zz = local_point(2);

        if (std::fabs(atan2(xx, zz)) <= PI * FOV / 2 / 180
                && std::fabs(atan2(yy, zz)) <= PI * FOV / 2 / 180
                && zz > 0)
        {
            int n_id = before_feature_id.find(i) == before_feature_id.end() ?
                       current_id++ : before_feature_id[i];
//#if WITH_NOISE
//            Vector3d disturb = Vector3d(distribution(generator) * sqrt(pts_cov(0, 0)) / zz / zz,
//                                        distribution(generator) * sqrt(pts_cov(1, 1)) / zz / zz,
//                                        0
//                                       );
//            image.push_back(make_pair(n_id, disturb + Vector3d(xx / zz, yy / zz, 1)));
//#else
            image.push_back(make_pair(n_id, Vector3d(xx, yy, zz)));
            printf ("id %d, (%d %d %d)\n", n_id,
                pts[i * 3 + 0] ,
                pts[i * 3 + 1] ,
                pts[i * 3 + 2] 
            );
                            
//#endif
            current_feature_id[i] = n_id;
        }
    }
    before_feature_id = current_feature_id;
    current_feature_id.clear();
    //sort(image.begin(), image.end(), [](const pair<int, Vector3d> &a,
    //                                    const pair<int, Vector3d> &b)
    //{
    //    return a.first < b.first;
    //});
    return image;
}

/**
*@brief 手先速度から関節角速度を取得
* @param v 手先速度
* @return 関節角速度
*/
Vector3d RobotArm::calcJointVel(Vector3d v)
{
	Matrix3d J = calcJacobian(theta);
	Matrix3d Jinv = J.inverse();

	Vector3d vf(v(0), v(1), v(2));
	

	Vector3d A = Jinv * vf;

	//std::cout << Jinv << std::endl << std::endl;
	
	return A;

}

double Tetrahedra<ConcreteShape>::estimatedElementRadius() {
  Matrix3d invJ;
  double detJ;

  std::tie(invJ, detJ) = ConcreteShape::inverseJacobian(mVtxCrd);
  Matrix3d J = invJ.inverse();
  VectorXcd eivals = J.eigenvalues();

  // get minimum h (smallest direction)
  Vector3d eivals_norm;
  for(int i=0;i<3;i++) {
    eivals_norm(i) = std::norm(eivals[i]);
  }
  return eivals_norm.minCoeff();
  
}

    /**
     * input needs at least 2 correspondences of non-parallel lines
     * the resulting R and t works as below: x'=Rx+t for point pair(x,x');
     * @param vLineA
     * @param vLineB
     * @param R
     * @param t
     */
    void
    LineExtract::ComputeRelativeMotion_svd(vector<Line3d> vLineA, vector<Line3d> vLineB, Matrix3d &R, Vector3d &t) {
        if (vLineA.size() < 2) {
            cerr << "Error in computeRelativeMotion_svd: input needs at least 2 pairs!\n";
            return;
        }
        // convert to the representation of Zhang's paper
        for (int i = 0; i < vLineA.size(); ++i) {
            Vector3d l, m;
            if (vLineA[i].u.norm() < 0.9) {
                l = vLineA[i].EndB - vLineA[i].EndA;
                m = (vLineA[i].EndA + vLineA[i].EndB) * 0.5;
                vLineA[i].u = l / l.norm();
                vLineA[i].d = vLineA[i].u.cross(m);
                //	cout<<"in computeRelativeMotion_svd compute \n";
            }
            if (vLineB[i].u.norm() < 0.9) {
                l = vLineB[i].EndB - vLineB[i].EndA;
                m = (vLineB[i].EndA + vLineB[i].EndB) * 0.5;
                vLineB[i].u = l * (1 / l.norm());
                vLineB[i].d = vLineB[i].u.cross(m);
            }
        }

        Matrix4d A = Matrix4d::Zero();
        for (int i = 0; i < vLineA.size(); ++i) {
            Matrix4d Ai = Matrix4d::Zero();
            Ai.block<1, 3>(0, 1) = vLineA[i].u - vLineB[i].u;
            Ai.block<3, 1>(1, 0) = vLineB[i].u - vLineA[i].u;

            Ai.bottomRightCorner<3, 3>(1, 1) = SO3d::hat((vLineA[i].u + vLineB[i].u)).matrix();
            A = A + Ai.transpose() * Ai;
        }
        Eigen::JacobiSVD<Matrix4d> svd(A, Eigen::ComputeFullV | Eigen::ComputeFullV);

        Vector4d q = svd.matrixU().col(3);
        R = Eigen::Quaterniond(q).matrix();

        Matrix3d uu = Matrix3d::Zero();
        Vector3d udr = Vector3d::Zero();
        for (int i = 0; i < vLineA.size(); ++i) {
            uu = uu + SO3d::hat(vLineB[i].u) * SO3d::hat(vLineB[i].u).matrix().transpose();
            udr = udr + SO3d::hat(vLineB[i].u).transpose() * (vLineB[i].d - R * vLineA[i].d);
        }
        t = uu.inverse() * udr;
    }

Vector3d DataGenerator::getAngularVelocity()
{
    const double delta_t = 0.00001;
    Matrix3d rot = getRotation();
    t += delta_t;
    Matrix3d drot = (getRotation() - rot) / delta_t;
    t -= delta_t;
    Matrix3d skew = rot.inverse() * drot;
#ifdef WITH_NOISE
    Vector3d disturb = Vector3d(distribution(generator) * sqrt(gyr_cov(0, 0)),
                                distribution(generator) * sqrt(gyr_cov(1, 1)),
                                distribution(generator) * sqrt(gyr_cov(2, 2))
                               );
    return disturb + Vector3d(skew(2, 1), -skew(2, 0), skew(1, 0));
#else
    return Vector3d(skew(2, 1), -skew(2, 0), skew(1, 0));
#endif
}

//NOTE: this must be done after all parameters are computed
Vector4d CProxyCamera::getOpticalCenter(void) const 
{
	// orthographic case
	Vector4d ans;
	if (m_KR(2,0) == 0.0 && m_KR(2,1) == 0.0 &&
		m_KR(2,2) == 0.0) 
	{
		Vector3d vtmp[2];
		for (int i = 0; i < 2; ++i)
			for (int y = 0; y < 3; ++y)
			{
				vtmp[i][y] = m_KR(i,y);
			}

			Vector3d vtmp2 = vtmp[0].cross(vtmp[1]);
			vtmp2.normalize();
			for (int y = 0; y < 3; ++y)
			{
				ans[y] = vtmp2[y];
			}
			ans[3] = 0.0;
	}
	else 
	{
		Matrix3d A;
		Vector3d b;
		for (int y = 0; y < 3; ++y) 
		{
			for (int x = 0; x < 3; ++x)
			{
				A(y,x) = m_KR(y,x);
			}
			b[y] = - m_KT[y];
		}
		Matrix3d iA = A.inverse();
		b = iA * b;
		for (int y = 0; y < 3; ++y)
		{
			ans[y] = b[y];
		}
		ans[3] = 1.0;
	}
	return ans;
}

JNIEXPORT jobject JNICALL Java_com_mousebird_maply_Matrix3d_inverse
(JNIEnv *env, jobject obj)
{
    try
    {
        Matrix3dClassInfo *classInfo = Matrix3dClassInfo::getClassInfo();
        Matrix3d *inst = classInfo->getObject(env,obj);
        if (!inst)
            return NULL;
        
        Matrix3d matInv = inst->inverse();
        return MakeMatrix3d(env,matInv);
    }
    catch (...)
    {
        __android_log_print(ANDROID_LOG_VERBOSE, "Maply", "Crash in Matrix3d::inverse()");
    }
    
    return NULL;
}

MoveTo_TaskSpace::MoveTo_TaskSpace(const Affine3d &pose, const Vector3d &dp, double pos_tol,
                     const Vector3d &rpy_minus, const Vector3d &rpy_plus)
{
    double X, Y, Z, roll, pitch, yaw ;

    poseToXYZRPY(pose, X, Y, Z, roll, pitch, yaw) ;

    c0 = Vector3d(X, Y, Z) ;
    a0 = Vector3d(roll, pitch, yaw) ;

    Vector3d c1 = c0 + dp ;

    // We define a coordinate system with the Z axis pointing towards the target point

    Matrix3d r = getLookAtMatrix(dp) ;

    frame = r  ;
    iframe = r.inverse() ;

    // we use a cylinder parameterization of the position

    lower[0] = 0.0      ; upper[0] = dp.norm() ; // cylinder length
    lower[1] = -M_PI    ; upper[1] = M_PI ; // polar angle
    lower[2] = 0.0      ; upper[2] = pos_tol ; // radius

    const double small_ = 0.001 ;

    double roll_min = std::max(a0.x() - fabs(rpy_minus.x()) - small_, -M_PI) ;
    double roll_max = std::min(a0.x() + fabs(rpy_plus.x()) + small_, M_PI) ;

    double pitch_min = std::max(a0.y() - fabs(rpy_minus.y()) - small_, -M_PI) ;
    double pitch_max = std::min(a0.y() + fabs(rpy_plus.y()) + small_, M_PI) ;

    double yaw_min = std::max(a0.z() - fabs(rpy_minus.z()) - small_, -M_PI) ;
    double yaw_max = std::min(a0.z() + fabs(rpy_plus.z()) + small_, M_PI) ;

    upper[3] = roll_max     ; lower[3] = roll_min ;
    upper[4] = pitch_max    ; lower[4] = pitch_min ;
    upper[5] = yaw_max      ; lower[5] = yaw_min ;

}

MatrixXd der_logarithm_map(Matrix4d T)
{

    MatrixXd dlogT_dT = MatrixXd::Zero(6,12);

    // Approximate derivative of the logarithm_map wrt the transformation matrix
    Matrix3d L1 = Matrix3d::Zero();
    Matrix3d L2 = Matrix3d::Zero();
    Matrix3d L3 = Matrix3d::Zero();
    Matrix3d Vinv = Matrix3d::Identity();
    Vector6d x = logmap_se3(T);

    // estimates the cosine, sine, and theta
    double b;
    double cos_ = 0.5 * (T.block(0,0,3,3).trace() - 1.0 );
    if(cos_ > 1.f)
        cos_ = 1.f;
    else if (cos_ < -1.f)
        cos_ = -1.f;
    double theta  = acos(cos_);
    double theta2 = theta*theta;
    double sin_   = sin(theta);
    double cot_   = 1.0 / tan( 0.5*theta );
    double csc2_  = pow( 1.0/sin(0.5*theta) ,2);

    // if the angle is small...
    if( cos_ > 0.9999 )
    {
        b = 0.5;
        L1(1,2) = -b;
        L1(2,1) =  b;
        L2(0,2) =  b;
        L2(2,0) = -b;
        L3(0,1) = -b;
        L3(1,0) =  b;
        // form the full derivative
        dlogT_dT.block(3,0,3,3) = L1;
        dlogT_dT.block(3,3,3,3) = L2;
        dlogT_dT.block(3,6,3,3) = L3;
        dlogT_dT.block(0,9,3,3) = Vinv;
    }
    // if not...
    else
    {
        // rotation part
        double k;
        Vector3d a;
        a(0) = T(2,1) - T(1,2);
        a(1) = T(0,2) - T(2,0);
        a(2) = T(1,0) - T(0,1);
        k = ( theta * cos_ - sin_ ) / ( 4 * pow(sin_,3) );
        a = k * a;
        L1.block(0,0,3,1) = a;
        L2.block(0,1,3,1) = a;
        L3.block(0,2,3,1) = a;
        // translation part
        Matrix3d w_skew = skew( x.tail(3) );
        Vinv += w_skew * (1.f-cos_) / theta2 + w_skew * w_skew * (theta - sin_) / pow(theta,3);
        Vinv  = Vinv.inverse().eval();
        // dVinv_dR
        Vector3d t;
        Matrix3d B, skew_t;
        MatrixXd dVinv_dR(3,9);
        t = T.block(0,3,3,1);
        skew_t = skew( t );
        // - form a
        a =  (theta*cos_-sin_)/(8.0*pow(sin_,3)) * w_skew * t
            + ( (theta*sin_-theta2*cos_)*(0.5*theta*cot_-1.0) - theta*sin_*(0.25*theta*cot_+0.125*theta2*csc2_-1.0))/(4.0*theta2*pow(sin_,4)) * w_skew * w_skew * t;
        // - form B
        Vector3d w;
        Matrix3d dw_dR;
        w = x.tail(3);
        dw_dR.row(0) << -w(1)*t(1)-w(2)*t(2), 2.0*w(1)*t(0)-w(0)*t(1), 2.0*w(2)*t(0)-w(0)*t(2);
        dw_dR.row(1) << -w(1)*t(0)+2.0*w(0)*t(1), -w(0)*t(0)-w(2)*t(2), 2.0*w(2)*t(1)-w(1)*t(2);
        dw_dR.row(2) << -w(2)*t(0)+2.0*w(0)*t(2), -w(2)*t(1)+2.0*w(1)*t(2), -w(0)*t(0)-w(1)*t(1);
        B = -0.5*theta*skew_t/sin_ - (theta*cot_-2.0)*dw_dR/(8.0*pow(sin_,2));
        // - form dVinv_dR
        dVinv_dR.col(0) = a;
        dVinv_dR.col(1) = -B.col(2);
        dVinv_dR.col(2) = B.col(1);
        dVinv_dR.col(3) = B.col(2);
        dVinv_dR.col(4) = a;
        dVinv_dR.col(5) = -B.col(0);
        dVinv_dR.col(6) = -B.col(1);
        dVinv_dR.col(7) = B.col(0);
        dVinv_dR.col(8) = a;
        // form the full derivative
        dlogT_dT.block(3,0,3,3) = L1;
        dlogT_dT.block(3,3,3,3) = L2;
        dlogT_dT.block(3,6,3,3) = L3;
        dlogT_dT.block(0,9,3,3) = Vinv;
        dlogT_dT.block(0,0,3,9) = dVinv_dR;
    }

    return dlogT_dT;

}

VectorXd rigidBodyDynamics::f(VectorXd x) {
	Vector3d dr, dv, da, dw;
	Matrix<double,12,12> lambda, dLambda;
	VectorXd vec_dLambda;

	int vecsize;
	if (covProp) {
		vecsize = 90;
	} else {
		vecsize = 12;
	}

	VectorXd dx(vecsize);

	Vector3d r = x.segment<3>(0);
	Vector3d v = x.segment<3>(3);
	Vector3d a = x.segment<3>(6);
	Vector3d w = x.segment<3>(9);

	MatrixXd Bw = getBw();
	Matrix3d J = _ir.getJ();


	//Nonlinear State Model \dot x = f(x)

	/*
	 * 	\mathbf{\dot r} = \mathbf{v}
	 */
	dr = v;

	/*
	 * 	\mathbf{\dot v} = 0
	 */
	dv = Vector3d::Zero();

	/*
	 * \frac{d \mathbf{a}_p}{dt} =
	 * 			\frac{1}{2}\left(\mathbf{[\omega \times]} +
	 * 			\mathbf{\omega} \cdot \mathbf{\bar q} \right) \mathbf{a}_p +
	 * 			\frac{2 q_4}{1+q_4} \mathbf{\omega}
	 */
	double c1, c2, c3;
	c1 = 0.5;
	c2 = 0.125 * w.dot(a);		//NEW simplification
	c3 = 1 - a.dot(a)/16;
	da = -c1 * w.cross(a) + c2* a + c3 * w;

	/*
	 * \dot \mathbf{w} = -\mathbf{J}^{-1} \mathbf{\omega} \times \mathbf{J} \mathbf{\omega}
	 */
	dw = - J.inverse() * w.cross(J * w);

	if (covProp) {

		//Covariance Propagation according to Lyapunov function
		//see Brown & Hwang pg 204

		//Compute Linear transition matrix
		Matrix<double,12,12> A = Matrix<double,12,12>::Zero();

		//position derivative
		A.block<3,3>(0,3) = Matrix<double,3,3>::Identity();

		//mrp kinematics
		A.block<3,3>(6,6) = -0.5*crossProductMat(w) + w.dot(a)/8 * Matrix3d::Identity();
		A.block<3,3>(6,9) = (1-a.dot(a/16))*Matrix3d::Identity();

		//angular velocity dynamics
		A.block<3,3>(9,9) = - J.inverse() * crossProductMat(w) * J;


		lambda = vec2symmMat(x.segment<78>(12));
		dLambda = A * lambda + lambda *A.transpose() + Bw * _Q * Bw.transpose();
		vec_dLambda = symmMat2Vec(dLambda);
	}

	//write to dx
	dx.segment<3>(0) = dr;
	dx.segment<3>(3) = dv;
	dx.segment<3>(6) = da;
	dx.segment<3>(9) = dw;
	if(covProp){
		dx.segment<78>(12) = vec_dLambda;
	}

	return dx;
}