三维旋转变换可视化

最近做一个项目，机器人上有四个相机，如图所示放置，要求各个相机相对于车身中心的外参

aaa

关于Camera系和Body系之间的转换，Camera系Z轴超前，X轴朝右，Y轴朝下；Body系，X轴朝前，Y轴朝左，Z轴朝上。$\ R_{cb}$可由两步旋转得到。

aaa

为了验证旋转的正确性，我简单写了个可视化程序。以相机1为例，先绕$x_c$轴旋转$\ +\frac{\pi}{2}$，再绕$y_c$轴旋转$\ -\frac{3\pi}{4}$，就旋转成了Body系

aaa

完整代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
#include <iostream>
#include <stdio.h>
#include <vector>
#include <iomanip>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
    //cout << std::fixed << std::setprecision(4)<< endl;

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));


    vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;

    //Camera as local frame
    Eigen::Isometry3d T_camera = Eigen::Isometry3d::Identity();
    cout<<"T_camera: "<<endl; cout<<T_camera.matrix()<<endl;
    poses.push_back(T_camera);


    // //Compute transform matrix:  Body -> Camera
    Eigen::Isometry3d Tcb_1= Eigen::Isometry3d::Identity();
    //rotate by two step  The rotation axis of both steps is based on the initial frame
    //The positive direction of rotation angle is defined as the clockwise direction of the coordinate axis viewed from the origin
    Matrix3d R1 = AngleAxisd(M_PI/2,Vector3d(1, 0, 0)).toRotationMatrix();
    Matrix3d R2 = AngleAxisd(-3*M_PI/4,Vector3d(0, 1, 0)).toRotationMatrix();
    //Contrary to intuition, Rcb is a two-step multiplication of rotating the camera coordinate frame to the body frame, 
    //but the actual expression is to convert the points under the body frame to the camera coordinate frame
    Matrix3d Rcb_1 = R2 * R1 ;
    Eigen::Vector3d tcb_1; tcb_1 <<  -0.03, 0, -0.231;//In the camera frame, the coordinates of body system's origin
    Tcb_1.rotate(Rcb_1);
    Tcb_1.pretranslate(tcb_1);
    cout<<"Tcb_1: "<<endl; cout<<Tcb_1.matrix()<<endl;
    poses.push_back(Tcb_1);

    // Eigen::Isometry3d Tcb_2 = Eigen::Isometry3d::Identity();
    // Matrix3d R1 = AngleAxisd(M_PI/2,Vector3d(1, 0, 0)).toRotationMatrix();
    // Matrix3d R2 = AngleAxisd(-M_PI/4,Vector3d(0, 1, 0)).toRotationMatrix();
    // Matrix3d Rcb_2 = R2 * R1 ;
    // Eigen::Vector3d tcb_2; tcb_2 <<  -0.03, 0, -0.231;
    // Tcb_2.rotate(Rcb_2);
    // Tcb_2.pretranslate(tcb_2);
    // cout<<"Tcb_2: "<<endl; cout<<Tcb_2.matrix()<<endl;
    // poses.push_back(Tcb_2);

    // Eigen::Isometry3d Tcb_3 = Eigen::Isometry3d::Identity();
    // Matrix3d R1 = AngleAxisd(M_PI/2,Vector3d(1, 0, 0)).toRotationMatrix();
    // Matrix3d R2 = AngleAxisd(M_PI/4,Vector3d(0, 1, 0)).toRotationMatrix();
    // Matrix3d Rcb_3 = R2 * R1 ;
    // Eigen::Vector3d tcb_3; tcb_3 <<  -0.03, 0, -0.231;
    // Tcb_3.rotate(Rcb_3);
    // Tcb_3.pretranslate(tcb_3);
    // cout<<"Tcb_3: "<<endl; cout<<Tcb_3.matrix()<<endl;
    // poses.push_back(Tcb_3);

    // Eigen::Isometry3d Tcb_4 = Eigen::Isometry3d::Identity();
    // Matrix3d R1 = AngleAxisd(M_PI/2,Vector3d(1, 0, 0)).toRotationMatrix();
    // Matrix3d R2 = AngleAxisd(3 * M_PI/4,Vector3d(0, 1, 0)).toRotationMatrix();
    // Matrix3d Rcb_4 = R2 * R1 ;
    // Eigen::Vector3d tcb_4; tcb_4 <<  -0.03, 0, -0.231;
    // Tcb_4.rotate(Rcb_4);
    // Tcb_4.pretranslate(tcb_4);
    // cout<<"Tcb_4: "<<endl; cout<<Tcb_4.matrix()<<endl;
    // poses.push_back(Tcb_4);

    // Vector3d a0;a0<<0, 0, 1; //A point in Body frame
    // cout<<"a0_camera: "<<Tcb_2 * a0<<endl; 

    while (!pangolin::ShouldQuit()) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
        glLineWidth(2);

        for (size_t i = 0; i < poses.size(); i++) {
            // 画每个位姿的三个坐标轴
            Vector3d Ow = poses[i].translation();
            Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));//Red
            Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));//Green
            Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));//Blue
            glBegin(GL_LINES);
            glColor3f(1.0, 0.0, 0.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Xw[0], Xw[1], Xw[2]);
            glColor3f(0.0, 1.0, 0.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Yw[0], Yw[1], Yw[2]);
            glColor3f(0.0, 0.0, 1.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Zw[0], Zw[1], Zw[2]);
            glEnd();
        }
        pangolin::FinishFrame();
    }
}

Cmake

1
2
3
4
5
include_directories("/usr/include/eigen3") 
find_package( Pangolin )
include_directories( ${Pangolin_INCLUDE_DIRS} )
add_executable(rotation_visual rotation_visual.cpp)
target_link_libraries( rotation_visual ${Pangolin_LIBRARIES} )