C++ Matrix4d::transpose方法代码示例

Pose getDirTransform(Pose transformation) {
    Matrix4d dirTransformation = transformation.asMatrix();
    dirTransformation.invert();
    dirTransformation.transpose();
    Pose dirTransform(dirTransformation);
    return dirTransform;
}

int main(int, char**)
{
  cout.precision(3);
  Matrix4d X = Matrix4d::Random(4,4);
Matrix4d A = X + X.transpose();
cout << "Here is a random symmetric 4x4 matrix:" << endl << A << endl;
Tridiagonalization<Matrix4d> triOfA(A);
Vector3d hc = triOfA.householderCoefficients();
cout << "The vector of Householder coefficients is:" << endl << hc << endl;

  return 0;
}

    /**
     * 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;
    }

LieAlgebra MinEnFilter::computeGradient(const LieGroup& S, const MatrixXd& Gk, const MatrixXd& Disps_inhom) {
	/* DO NOT CHANGE THIS FUNCTION */
	LieAlgebra L;
	MatrixXd iEg = ((S.E).lu().solve(Gk.transpose())).transpose();
	VectorXd kappa = iEg.col(2);
	MatrixXd h = iEg.leftCols(2).cwiseQuotient(kappa*MatrixXd::Ones(1,2));
	MatrixXd z = Disps_inhom.leftCols(2) - h;
	MatrixXd Qvec(1,2);
	Qvec << q1/nPoints,q2/nPoints;
	MatrixXd qvec = MatrixXd::Ones(nPoints,1) * Qvec;
	MatrixXd b12 = (kappa.cwiseInverse()*MatrixXd::Ones(1,2)).cwiseProduct(qvec.cwiseProduct(z));
	MatrixXd kappa2 = kappa.cwiseProduct(kappa);
	MatrixXd aux1 = (iEg.leftCols(2)).cwiseProduct(z.cwiseProduct(qvec));
	MatrixXd b3 = -((kappa2.cwiseInverse()*MatrixXd::Ones(1,2)).cwiseProduct(aux1)).rowwise().sum();
	MatrixXd B(nPoints,4);
	B << b12, b3, MatrixXd::Zero(nPoints,1);
	MatrixXd Ones = MatrixXd::Ones(1,4);
	MatrixXd A1 = iEg.col(0) * Ones;
	MatrixXd A2 = iEg.col(1) * Ones;
	MatrixXd A3 = iEg.col(2) * Ones;
	MatrixXd A4 = iEg.col(3) * Ones;
	
	MatrixXd G1 = (B.cwiseProduct(A1)).colwise().sum();
	MatrixXd G2 = (B.cwiseProduct(A2)).colwise().sum();
	MatrixXd G3 = (B.cwiseProduct(A3)).colwise().sum();
	MatrixXd G4 = (B.cwiseProduct(A4)).colwise().sum();

	Matrix4d G;
	G << 	G1, 
			G2,
			G3,
			G4;

	G = MEFcommon::prSE(G.transpose());
	if (order == 1) {
		L = LieAlgebra(G);
	} else if (order >=2) {
		L = LieAlgebra(G, VectorXd::Zero((order-1)*6),order);
	} else {
		cerr << "Parameter order must be greater than zero." << endl;
		exit(1);
	}
	
	return L;
}

pair<Matrix4d, Matrix4d> C3Trajectory::transformation_pair(const Vector6d &q) {
	Matrix4d R;
	R.block<3,3>(0, 0) = AttitudeHelpers::EulerToRotation(q.tail(3));
	R.block<1,3>(3, 0).fill(0);
	R.block<3,1>(0, 3).fill(0);
	R(3, 3) = 1;

	Matrix4d T = Matrix4d::Identity();
	T.block<3,1>(0, 3) = -q.head(3);

	pair<Matrix4d, Matrix4d> result;
	result.first = R.transpose()*T; // NED -> BODY

	T.block<3,1>(0, 3) = q.head(3);
	result.second = T*R; // BODY -> NED

	return result;
}