입문 Visual SLAM 제 3장 공부

출처 : https://www.cv-learn.com/20210123-slam-book-translation/

SLAM에 대해 공부를 하려고 보니 어디서부터 진행해야될지 몰라 찾아보던 중 SLAMKR 커뮤니티 분들이 중국어 원서인 "입문 Visual SLAM 이론에서 연습까지 14 강(视觉SLAM十四讲 从理论到实践)"을 한국어로 번역 후 자료를 올려주셔서 그 책으로 공부한 내용에 대한 키워드 및 수식들을 간단하게만 정리를 진행하려고 합니다.

 

입문 Visual SLAM 제 3장의 링크는 다음과 같습니다.

https://docs.google.com/document/d/10gJsFWGAiNaFc5za8IJTZHU3JFYHzStvLNeLVKyrpUA/

입문 Visual SLAM 14강 (제3장)

 

 

• 글에 나온 수식 및 이미지의 출처는 3장입니다.
• 실습 코드에 대한 내용은 https://github.com/gaoxiang12/slambook에 나와있습니다.
• 책의 내용 중 키워드와 수식들로 정말 간단하게 키워드를 위주로 정리했습니다.
• 실습 관련 코드에서는 주석이 전부 중국어로 되어있어, 구글 번역을 이용해 한국어로 번역했습니다.
• 틀린 부분이 있거나, 보완할 점이 보이면 댓글이나 이메일로 연락주시면 바로 피드백을 반영하도록 하겠습니다.
• 본 글이 문제가 된다면 언제든지 삭제하겠습니다.

 

 

3장(3차원 공간 강체 변환)의 내용에 대한 목차

3.1 회전 행렬

3.2 연습 : Eigen

3.3 회전 벡터와 오일러 각

3.4 쿼터니언

3.5 유사, 아핀, 투영 변환

3.6 실습 : Eigen Geometry 모듈

3.7 시각적표현

 

 

3.1 회전행렬

 

3.1.1 점과 벡터, 좌표계

• 벡터의 내적, 외적
  • 벡터의 내적 

    

  • 벡터의 외적

    

    외적의 경우 반대칭 기호 ^를 사용해 반대칭행렬을 만들 수 있다.

3.1.2 좌표계 간의 유클리드 변환

• SO(n)
  • n 차원 공간의 회전 행렬로 구성

• • 회전행렬에 대한 설명

3.1.3 변환 행렬 및 동질 좌표

• SE(3)
  • 변환행렬에 대한 설명

3.2 연습 : Eigen

책에서는 코드에 대한 설명은 주석에 자세하게 나와있다고 함.(그러나 주석이 중국어이기에 구글 번역으로 한국어로 변환한 코드를 올립니다.)

#include <iostream>
using namespace std;
#include <ctime>
// Eigen 부분
#include <Eigen/Core>
// 조밀한 행렬에 대한 대수 연산(역, 고유값 등)
#include <Eigen/Dense>

#define MATRIX_SIZE 50

/****************************
* 이 프로그램은 고유 기본 유형의 사용을 보여줍니다.
****************************/

int main( int argc, char** argv )
{
    // Eigen의 모든 벡터와 행렬은 템플릿 클래스인 Eigen::Matrix입니다. 처음 세 개의 매개변수는 데이터 유형, 행, 열입니다.
    // 2*3 float 행렬 선언
    Eigen::Matrix<float, 2, 3> matrix_23;

    // 동시에 Eigen은 typedef를 통해 많은 내장 유형을 제공하지만 맨 아래 계층은 여전히 ​​Eigen::Matrix입니다.
    // 예를 들어 Vector3d는 본질적으로 Eigen::Matrix<double, 3, 1>, 3차원 벡터입니다.
    Eigen::Vector3d v_3d;
	// 동일합니다.
    Eigen::Matrix<float,3,1> vd_3d;

    // Matrix3d는 본질적으로 Eigen::Matrix<double, 3, 3>
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //0으로 초기화
    // 행렬 크기가 확실하지 않은 경우 동적으로 크기가 조정된 행렬을 사용할 수 있습니다.
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
    // 간단하게 다음과 같이 표현할 수 있습니다.
    Eigen::MatrixXd matrix_x;
    // 이 유형에는 다른 많은 유형이 있습니다. 하나씩 나열하지는 않겠습니다.

    // 다음은 고유 행렬에 대한 연산입니다.
    // 입력 데이터(초기화)
    matrix_23 << 1, 2, 3, 4, 5, 6;
    // 출력
    cout << matrix_23 << endl;

    // ()를 사용하여 행렬의 요소에 액세스
    for (int i=0; i<2; i++) {
        for (int j=0; j<3; j++)
            cout<<matrix_23(i,j)<<"\t";
        cout<<endl;
    }

    // 행렬과 벡터 곱하기(실제로는 여전히 행렬과 행렬)
    v_3d << 3, 2, 1;
    vd_3d << 4,5,6;
    // 그러나 Eigen에서는 두 가지 다른 유형의 행렬을 혼합할 수 없습니다. 이와 같은 것은 잘못된 것입니다.
    // Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
    // matrix_23은 float이기 때문에 double로 형변환
    // 명시적으로 변환해야 합니다.
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

    Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
    cout << result2 << endl;

    // 또한 행렬의 차원을 잘못 얻을 수 없습니다.
    // 다음의 주석을 제거하여 Eigen이 보고하는 오류를 확인하십시오.
    // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;

    // 일부 행렬 연산
    // 네 가지 산술 연산은 시연되지 않으며 +-*/를 직접 사용하십시오.
    matrix_33 = Eigen::Matrix3d::Random();      // 난수 행렬
    cout << matrix_33 << endl << endl;

    cout << matrix_33.transpose() << endl;      // 반전
    cout << matrix_33.sum() << endl;            // 합
    cout << matrix_33.trace() << endl;          // 추적하다
    cout << 10*matrix_33 << endl;               // 곱셈
    cout << matrix_33.inverse() << endl;        // 역
    cout << matrix_33.determinant() << endl;    // Det()

    // 고유값
    // 실제 대칭 행렬은 성공적인 대각화를 보장합니다.
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose()*matrix_33 );
    cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

    // 방정식 풀기
    // 우리는 방정식 matrix_NN * x = v_Nd를 풉니다.
    // N의 크기는 난수에 의해 생성되는 이전 명령어에서 정의됩니다.
    // 직접 반전은 당연히 가장 직접적이지만 반전 계산량이 많습니다.

    Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
    Eigen::Matrix< double, MATRIX_SIZE,  1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 );

    clock_t time_stt = clock(); // 타이밍
    // 직접 반전
    Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
    cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;
    
	// 일반적으로 QR 분해와 같은 행렬 분해를 사용하여 속도가 훨씬 빠릅니다.
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout <<"time use in Qr decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    return 0;
}

 

3.3 회전 벡터와 오일러 각

 

3.3.1 회전 벡터

• Rodrigues rotation fomula
  • 3차원 회전변환을 4개의 값(회전축 벡터 + 회전각)만으로 표현
  • https://darkpgmr.tistory.com/99 내용 참고

3.3.2 오일러

• 요우, 피치, 롤(yaw-pitch-roll)

출처 : https://en.wikipedia.org/wiki/Aircraft_principal_axes

• 짐벌락 현상

 

3.4 쿼터니언

 

3.4.1 쿼터니언의 정의

• 쿼터니언

쿼터니언 회전

쿼터니언으로부터 회전 축과 각도를 구한 식

3.4.2 쿼터니언 연산

• 더하기 및 빼기

• 곱셈

벡터 형식으로 작성하면 조금 더 간결하다.

• 켤레

쿼터니언의 켤레는 허수부를 역으로 사용한다.

• 모듈 길이

• 역함수

3.29 정의에 따르면 쿼터니언과 그 역행의 곱은 실수 쿼터니언 1이다.

q가 단위쿼터니언(??)이면 곱셈의 역행렬은 행렬과 비슷한 특성을 가진다.

• 곱셈과 내적

 

3.4.3 회전을 나타 내기 위해 쿼터니언을 사용

• 3차원 점 p가 회전하여 점 p`이 되었을 때 회전행렬을 이용해 나타내면 p`=Rp가 된다.
  • 이를 쿼터니언으로 설명하면 다음과 같다.

실수는 0이고 허수부분에 x, y, z

회전

식 3.19에 의해 점 p`는 다음과 같이 쿼터니언을 통해 나타낼 수 있다.

3.4.4 쿼터니언에서 회전 행렬로 변환

쿼터니언 q

3.5 유사, 아핀, 투영 변환

유클리드 변환은 벡터의 길이와 각도를 유지(모양 변경 x)

유클리드 변환 외의 변환은 모양을 바꾼다.

 

3.5.1 Similarity transform(유사 변환)

• x, y, z의 좌표를 균등하게 스케일 할 수 있는 배율 인수 s를 가진다.

3.5.2 Affine transform(아핀 변환)

3.5.3 Projective transform(투영 변환)

• 가장 일반적인 변환
• 실제 세계에서 카메라 사진으로의 변환은 투영 변환으로 볼 수 있다.

 

3.6 실습 : Eigen Geometry 모듈

주석이 중국어이기에 구글 번역으로 한국어로 변환한 코드를 올립니다.

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// 고유 기하학 모듈
#include <Eigen/Geometry>

/****************************
* 이 프로그램은 Eigen 기하학 모듈의 사용을 보여줍니다.
****************************/

int main ( int argc, char** argv )
{
    // Eigen/Geometry 모듈은 다양한 회전 및 변환 표현을 제공합니다.
    // 3D 회전 행렬은 Matrix3d 또는 Matrix3f를 직접 사용
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 회전 벡터는 AngleAxis를 사용합니다. 아래쪽 레이어는 직접적으로 Matrix가 아니지만 연산을 행렬로 사용할 수 있습니다(연산자가 오버로드되기 때문에).
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //Z축을 따라 45도 회전
    cout .precision(3);
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;                //matrix()를 사용하여 행렬로 변환
    // 직접 할당 가능
    rotation_matrix = rotation_vector.toRotationMatrix();
    // AngleAxis를 사용하여 좌표 변환 수행
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // 또는 회전 행렬 사용
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    // 오일러 각도: 회전 행렬을 오일러 각도로 직접 변환할 수 있습니다.
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX 순서, 즉 롤 피치 요 순서
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    // Eigen::Isometry를 사용한 유클리드 변환 행렬
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();                //3d라고 하지만 실제로는 4*4의 행렬이다.
    T.rotate ( rotation_vector );                                     // rotation_vector에 따라 회전
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );                     // 변환 벡터를 (1,3,4)로 설정
    cout << "Transform matrix = \n" << T.matrix() <<endl;

    // 좌표 변환에 변환 행렬 사용
    Eigen::Vector3d v_transformed = T*v;                              // R*v+t와 같음
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

    // 아핀 및 투영 변환의 경우 Eigen::Affine3d 및 Eigen::Projective3d를 약간 사용합니다.

    // 쿼터니언
    // 쿼터니언에 AngleAxis를 직접 할당할 수 있으며 그 반대도 가능합니다.
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 계수의 순서는 (x, y, z, w)이고, w는 실수 부분이고 처음 세 개는 허수 부분입니다.
    // 회전 행렬을 할당할 수도 있습니다.
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // 쿼터니언을 사용하여 벡터를 회전하고 오버로드된 곱셈을 사용하십시오.
    v_rotated = q*v; // 수학적으로 qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    return 0;
}