Eigen中 Isometry3d、matrix的Identity()

Eigen中 Isometry3d与 matrix的区别

1、Identity()

    Eigen::Isometry3d A;
    A.Identity();

 Identity（）初始化的结果 并不是一个4*4的单位矩阵；

正确做法：

    Eigen::Isometry3d A= Eigen::Isometry3d::Identity();
    //Matrix<double, 4, 4> A = Matrix<double, 4, 4>::Identity();
    std::cout << "A::" << "\n";
    std::cout << A.matrix() << "\n";

 

 得到一个4*4的单位矩阵。

 

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;

int main()
{
    //Vector3d v0(-0.125664, -0.0000000015, 99.99), v49(-6.42184,0.873476,100.232), v1(-0.250786, -0.0328449, 99.981);
    //Quaterniond q0(-0.0000000043,0.706662,0.000000000043, 0.707551), q49(0.0408578,0.708267,-0.0891035,0.699106), q1(-0.046529,0.705413,0.0432253,0.705946);
    //
    //Vector3d delta_v = q0.conjugate()*(v49 - v0);//
    //Quaterniond delta_q = q0.conjugate()*q49;

    //std::cout << delta_v.x() << " " << delta_v.y() << " " << delta_v.z() << " "\
    //    << delta_q.w() << " " << delta_q.x() << " " << delta_q.y() << " " << delta_q.z() << "\n";

    Eigen::Vector3d t1(-1.0, 0, 0);
    Eigen::AngleAxisd v1(3.1415926 / 4.0, Eigen::Vector3d::UnitX());
    Eigen::Quaterniond q1(v1);
    Eigen::Matrix3d R1(v1);
    Eigen::Matrix4d T4x4 = Eigen::Matrix4d::Identity();
    T4x4.block(0, 0, 3, 3) = R1.block(0, 0, 3, 3);
    T4x4.block(0, 3, 3, 1) = t1.block(0, 0, 3, 1);

    Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
    T1.rotate(q1.toRotationMatrix());
    T1.pretranslate(t1);
    //T1.inverse();

    std::cout << "Isometry3d::  \n" << T1.matrix() << "\n";
    std::cout << "Isometry3d::  \n"<<T1.inverse().matrix() << "\n";
     
    std::cout << "Matrix4d::  \n" << T4x4<< "\n";
    std::cout << "Matrix4d::  \n" << T4x4.inverse() << "\n";

    ///////////////////////////////////////////////////
    Eigen::Quaterniond q_con = q1.conjugate();
    Eigen::Matrix3d R2(q_con);
    std::cout << "Matrix3d::  \n"  << R2 << "\n";

    ///////////////////////////////////////////////////
    return 0;
}