eigen 构造变换矩阵(Eigen::Isometry3d或者Eigen::Matrix4d)的几种方式

0.写在前面

更新
2020_09_17 增加了用Eigen::Matrix4d构造变换矩阵的方式.

---

1.Eigen::Isometry3d构造变换矩阵

1.1.对各个元素赋值

Eigen::Isometry3d T1=Eigen::Isometry3d::Identity();
T1(0,0) = 1.000000e+00, T1(0,1) = 1.197624e-11, T1(0,2) = 1.704639e-10, T1(0,3) = 3.214096e-14;
T1(1,0) = 1.197625e-11, T1(1,1) = 1.197625e-11, T1(1,2) = 3.562503e-10, T1(1,3) = -1.998401e-15;
T1(2,0) = 1.704639e-10, T1(2,1) = 3.562503e-10, T1(2,2) = 1.000000e+00, T1(2,3) = -4.041212e-14;
T1(3,0) =            0, T1(3,1) =            0, T1(3,2) =            0, T1(3,3) =             1;

1.2.通过旋转矩阵和平移向量

Eigen::Matrix3d rotation_matrix1 = Eigen::Matrix3d::Identity();
rotation_matrix1 << 1.000000e+00, 1.197624e-11, 1.704639e-10,
                    1.197625e-11, 1.000000e+00, 3.562503e-10,
                    1.704639e-10, 3.562503e-10, 1.000000e+00;
Eigen::Vector3d t1;
t1 <<  3.214096e-14, -1.998401e-15, -4.041212e-14;
    
T1=Eigen::Isometry3d::Identity();
T1.rotate ( rotation_matrix1 );
T1.pretranslate ( t1 );

注意不能直接变换矩阵赋值,像这样子会报错

T1<< 1.000000e+00, 1.197624e-11, 1.704639e-10, 3.214096e-14,
     1.197625e-11, 1.197625e-11, 3.562503e-10, -1.998401e-15,
     1.704639e-10, 3.562503e-10, 1.000000e+00, -4.041212e-14,
                0,            0,            0,              1;

2.Eigen::Matrix4d构造变换矩阵

对每一个元素赋值的方法是可行的的,我这里采用的是按矩阵块赋值

// ----3.eigen::matrix4d----
Eigen::Matrix4d T2;
T2.setIdentity();
T2.block<3,3>(0,0) = rotation_matrix1;
T2.topRightCorner(3, 1) = t1;
//T2.topRightCorner<3, 1>() = t1;

3.代码实验

#include <iostream>
#include <cmath>
#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>
using namespace std;

int main()
{
    // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T1=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
    //不能直接赋值
//    T1<< 1.000000e+00, 1.197624e-11, 1.704639e-10, 3.214096e-14,
//            1.197625e-11, 1.197625e-11, 3.562503e-10, -1.998401e-15,
//            1.704639e-10, 3.562503e-10, 1.000000e+00, -4.041212e-14,
//                       0,            0,            0,              1;

    //----1.对各个元素赋值----
    T1(0,0) = 1.000000e+00, T1(0,1) = 1.197624e-11, T1(0,2) = 1.704639e-10, T1(0,3) = 3.214096e-14;
    T1(1,0) = 1.197625e-11, T1(1,1) = 1.197625e-11, T1(1,2) = 3.562503e-10, T1(1,3) = -1.998401e-15;
    T1(2,0) = 1.704639e-10, T1(2,1) = 3.562503e-10, T1(2,2) = 1.000000e+00, T1(2,3) = -4.041212e-14;
    T1(3,0) =            0, T1(3,1) =            0, T1(3,2) =            0, T1(3,3) =             1;
    cout<<"T1 from element assign:\n"<<T1.matrix()<<endl;

    //---2.通过旋转矩阵和平移向量赋值---
    Eigen::Matrix3d rotation_matrix1 = Eigen::Matrix3d::Identity();
    rotation_matrix1 << 1.000000e+00, 1.197624e-11, 1.704639e-10,
                        1.197625e-11, 1.000000e+00, 3.562503e-10,
                        1.704639e-10, 3.562503e-10, 1.000000e+00;

    Eigen::Vector3d t1;
    t1 <<  3.214096e-14, -1.998401e-15, -4.041212e-14;

    T1=Eigen::Isometry3d::Identity();
    T1.rotate ( rotation_matrix1 );
    T1.pretranslate ( t1 );

    cout<<"T1 from r,t:\n"<<T1.matrix()<<endl;


    // ----3.eigen::matrix4d----
    Eigen::Matrix4d T2;
    T2.setIdentity();
    T2.block<3,3>(0,0) = rotation_matrix1;
    T2.topRightCorner<3, 1>() = t1;
    cout<<"eigen::matrix4d T2 :\n"<<T2<<endl;

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

project(cal_trans)
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )

add_executable(cal_T "main.cpp")

可以看到,这几种方式对于变换矩阵的构造的结果是一样的