[SLAM] 3D 회전과 이동, Eigen & Pangolin 라이브러리를 활용한 실습

3D Rigid Body Motion

Rigid Body

강체

rigid body는 강체

특정 물체를 벡터로 표현할 수 있다

강체는 3차원 벡터와 방향성도 가지고 있다

 

카메라를 나타낼 때 위치와 방향을 나타낼 수 있다

pose = 방향 + 위치

 

Coordinate Transformation

강체의 움직임을 파악하기 위해 강체의 좌표계를 전체 좌표계에 변환하여 분석하기 위해

좌표계 변환이 필요하다

두 좌표계 간의 orientation과 position 값을 알면 변환할 수 있다

 

다양한 회전 표현법

1. Rotations

Roll / Pitch / Yaw

각각의 축을 의미한다

차량의 좌우 운동이 심할 경우 롤이 심하다고 한다

 

3차원 회전을 표현하기 위해 차례로 변환해야 한다

 

이해하기 쉽지만 최적화하기 어려워 SLAM에는 사용되지 않는다

 

 

- Gimbal Lock

회전된 상태에서 다른 축을 회전할 때 회전축이 겹친다면

다시 회전한다면 자유도가 사라질 수 있다

 

Axis-angle

물리학에서 벡터를 바라보는 방식

방향과 크기로 나타낸다

축에 대한 방향과 회전하는 각도를 나타낸다

 

회전 축 e와 회전 각 theta를 통해 구할 수 있다

4개의 값으로 나타낼 수 있기 때문에 메모리 효율이 좋다

Quaternion

3개의 축을 표현하기 위해 

w, x, y, z를 통해 나타낸다

w는 복소수이고 이를 사용해 singularity 문제를 해결할 수 있다

미분이 가능하여 최적화할 수 있다

 

4차원 연산이 익숙하지 않아 파악하기 쉽지 않다

SO(3) Rotation matrix

3x3 matrix 회전 방법이다

회전 행렬을 통해 회전해도 x, y, z 축은 서로 수직을 유지해야 하며, 유닛 벡터의 크기가 1이여야 한다

역행렬을 곱하면 1이 나와야 한다

 

여러 개의 매트릭스를 하나로 통합할 수 있다는 장점이 있다

 

singular 문제가 없지만

차지하는 메모리가 크다

 

최적화가 어렵다

Lie algebra 공간에서 미분 가능하여 최적화할 수 있다

 

2. Translation(이동)

Metric Translation Vector

3차원 이동은 축에 따른 이동 범위만 나타내면 된다

대신 scale을 통일해야 오류가 발생하지 않는다

SE(3) Translation Matrix

강체를 하나의 행렬로 표현하는 방식

SO(3)와 translation을 한 번에 표현할 수 있다

 

Special Euclidean Group에 속해있다

 

대부분의 경우는 i는 1이다

행렬을 풀면 아래와 같다

 

Eigen을 이용한 Rotation 표현

Eigen 설치

https://www.cyberithub.com/how-to-install-eigen3-on-ubuntu-20-04-lts-focal-fossa/

How to Install Eigen3 on Ubuntu 20.04 LTS (Focal Fossa) | CyberITHub

 

g++ -I/usr/include your_program.cpp -o your_program

 

 

Pangolin 설치

https://github.com/stevenlovegrove/Pangolin/tree/v0.6

GitHub - stevenlovegrove/Pangolin: Pangolin is a lightweight portable rapid development library for managing OpenGL display / in

 

https://stackoverflow.com/questions/23284473/fatal-error-eigen-dense-no-such-file-or-directory

fatal error: Eigen/Dense: No such file or directory

 

 

implementation code

#include<iostream>
#include<eigen3/Eigen/Dense>
//#include <eigen3/Eigen/src/Core/Matrix.h>
#include<eigen3/Eigen/Geometry>
#include<cmath>

int main(){
    // Matrix initialization method 1
    // initial matrix and indicate values directly(2x3)
    Eigen::Matrix<double,2,3> matrix23_double;
    matrix23_double << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0;

    // Matrix initialization method 2
    // initial matrix and indicate values using methods(3x3)
    Eigen::Matrix3d matrix33_double = Eigen::Matrix3d::Zero();

    // Matrix initialization method 3
    // initial matrix and indicate values(dynamic size)
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;



    // Vector initialization method 1
    Eigen::Vector3d vec= {x:1.0, y:2.0, z:3.0};

    // Vector initialization method 2
    Eigen::Vector3d vec_zeros = Eigen::Vector3d::Zero();

    Eigen::Matrix3d mat33;
    mat33 << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;

    for(int row = 0; row < mat33.rows(); ++row){
        for(int col = 0; col < mat33.cols(); ++col){
            std::cout << mat33(row,col) << "\n";
        }
    }


    // Rotate 45 degrees along z axis
    Eigen::AngleAxisd rotation_vector(M_PI / 4.0, Eigen::Vector3d(0.0, 0.0, 1.0));

    std::cout << "rotation vector = " << "\n";
    std::cout << rotation_vector.matrix() << "\n";
    std::cout << "\n";

    Eigen::Vector3d vector(1.0, 0.0, 0.0);
    Eigen::Vector3d rotated_vector = rotation_vector * vector;

    std::cout << "rotated vector = " << "\n";
    std::cout << rotated_vector.transpose() << "\n";



    // Rotate 45 degrees along z axis using Quaternion
    Eigen::Quaterniond quat = Eigen::Quaterniond(rotation_vector);

    std::cout << "quaternion = " << quat.coeffs().transpose() << "\n";
    std::cout << "\n";

    std::cout << "rotated vector = " << (quat * vector).transpose() << "\n";

    return 0;
}

 

output

1
2
3
4
5
6
7
8
9
rotation vector = 
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1

rotated vector = 
0.707107 0.707107        0
quaternion =        0        0 0.382683  0.92388

rotated vector = 0.707107 0.707107        0
[1] + Done                       "/usr/bin/gdb" --interpreter=mi --tty=${DbgTerm} 0<"/tmp/Microsoft-MIEngine-In-lbm3fyuq.qzu" 1>"/tmp/Microsoft-MIEngine-Out-cidsx2a5.kyl"
hyunlaptop@laptopdesktop:~/Desktop/AutoDev/SLAM$

Coordinate Transformation

 

 

 

---

 

 

https://goodgodgd.github.io/ian-flow/archivers/vscode-tutorial

Visual Studio Code setup for C++ « IanFlow

https://github.com/stevenlovegrove/Pangolin/issues/697

A lot of gl/glsl.h and gl/gl.hpp functions not declared · Issue #697 · stevenlovegrove/Pangolin