입문 Visual SLAM 제 4장 실습 코드 중국어 주석 번역

출처 : https://docs.google.com/document/d/1icPjUyT3nPvjZ1OVMtWp9afUtuJ4gXLJL-ex7A9FpNs

입문 Visual SLAM 14강 (제4장)

입문 Visual SLAM 책을 공부하면서 실습 코드를 직접 하나씩 돌려보고 있는데, 실습 코드에서의 추가적인 이해를 위해 주석이 중국어로 되어있어서 주석을 구글 번역을 이용해 한국어로 번역했다.

 

제 4장은 Lie 군과 Lie 대수에 대한 내용과 지수/로그 맵핑에 대한 내용, 섭동 모델에 대한 내용이였다.

나중에 제 4장은 따로 하나씩 정리해가면서 수식에 대한 이해를 조금 더 높이도록 하겠다.

 

Lie 군과 Lie 대수의 지수/로그 맵핑을 위해 Sophus라는 라이브러리를 사용하는데, 해당 라이브러리에는 Lie 군, Lie 대수 및 지수/로그 맵핑에 대한 내용들이 정리가 되어있다.

 

SLAMKR에서 해당 책을 가지고 만든 스터디 영상 제 4장에 대한 내용도 있다.

책을 읽은 후 유튜브 영상까지 활용하면 더욱 이해가 잘 갔다.

 

Lie 군과 Lie 대수 지수/로그 맵핑 : https://www.youtube.com/watch?v=_uLRPqjdHjk&list=PLubUquiqNQdOTNocmWCSWk9ZaWhV7ubCD&index=6 

 

Lie 대수 미분과 섭동 모델, 비선형 최적화 방법 : https://www.youtube.com/watch?v=fFn8EfHgWfc&list=PLubUquiqNQdOTNocmWCSWk9ZaWhV7ubCD&index=7 

 

 

ch4/useSophus에 대한 실습코드는 다음과 같다.

 

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

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

#include "sophus/so3.h"
#include "sophus/se3.h"

int main( int argc, char** argv )
{
	// Z축을 따라 90도 회전 행렬
	Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix(); // Z축을 중심으로 90도 회전한 회전 벡터를 생성 후 회전행렬로 변환

	Sophus::SO3 SO3_R(R);				// Sophus::SO(3)는 회전 행렬에서 직접 구성할 수 있습니다.
	Sophus::SO3 SO3_v(0, 0, M_PI / 2);	// 회전 벡터에서 구성할 수도 있습니다.
	Eigen::Quaterniond q(R);			// 또는 쿼터니언
	Sophus::SO3 SO3_q( q );
	// 위의 표현은 모두 동일합니다.
	// SO(3)이 출력되면 so(3)과 같이 출력된다.
	cout<<"SO(3) from matrix: "<<SO3_R<<endl;
    cout<<"SO(3) from vector: "<<SO3_v<<endl;
    cout<<"SO(3) from quaternion :"<<SO3_q<<endl;

	// 로그 맵을 사용하여 Lie 대수를 얻습니다.
	Eigen::Vector3d so3 = SO3_R.log();
    cout<<"so3 = "<<so3.transpose()<<endl;
	// ^는 반대칭 행렬에 대한 벡터입니다.
	cout<<"so3 hat=\n"<<Sophus::SO3::hat(so3)<<endl;
	// 대조적으로, vee는 벡터에 대해 반대칭입니다.
	cout << "so3 hat vee= " << Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose() << endl; // Transpose는 순전히 아름다운 출력을 위한 것입니다.

	// 증분 섭동 모델 업데이트
	Eigen::Vector3d update_so3(1e-4, 0, 0); //업데이트의 양이 다음과 같다고 가정합니다.
	Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R;
    cout<<"SO3 updated = "<<SO3_updated<<endl;

	/********************귀여운 구분선*****************************/
	cout << "************구분선*************" << endl;
	// SE(3)에 대한 작업은 유사합니다.
	Eigen::Vector3d t(1, 0, 0);			// X축을 따라 1씩 이동
	Sophus::SE3 SE3_Rt(R, t);			// R,t에서 SE(3) 구성
	Sophus::SE3 SE3_qt(q, t);			// q,t에서 SE(3) 구성
	cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;
    cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;
	// Lie 대수 se(3)는 편의를 위해 6차원 벡터입니다.
	typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout<<"se3 = "<<se3.transpose()<<endl;
	// 출력을 관찰하면 Sophus에서 se(3)의 변환이 앞에 있고 회전이 뒤에 있음을 알 수 있습니다.
	// 마찬가지로 두 개의 연산자 hat과 vee가 있습니다.
	cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;
    cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;

	// 마지막으로 업데이트 시연
	Vector6d update_se3; //업데이트 금액
	update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
    cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;
    
    return 0;
}

 

 

실행 결과는 다음과 같다.