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

출처 : https://docs.google.com/document/d/1pIYlL3cVf9gXT9O7p_5H5az0HYkALuhrnrBQeZI0HUw/

입문 Visual SLAM 14강 (제6장)

 

제 6장 내용은 비선형 최적화 관련된 내용으로 최소자승법(Least-Square Method), Gradient descent, 가우스-뉴턴 방법(Gauss-Newton Method), Levenberg-Marquardt Method에 대한 내용이 나온다.

 

실습 코드로는 Ceres 라이브러리와 g2o 라이브러리에 대한 실습 코드가 있다.

 

제 6장을 읽으면서 수식에 대한 이해에 상당한 어려움이 있었고, 직관적으로 이해가 안되는 것들이 조금 있었다.

소단원마다 참고자료 링크를 올려주셨었는데 해당 링크를 통해 직관적으로 이해를 하는데 도움이 되었다.

 

최소자승법(Least Square Method) 참고자료 : https://darkpgmr.tistory.com/56

뉴턴법(Newton's Method) 참고자료 : https://darkpgmr.tistory.com/58

비선형 최적화 방법 참고자료 : https://darkpgmr.tistory.com/142

목적함수의 최솟값을 찾아감에 있어 미분을 이용한다는 것이 특징인 것 같다.

책에 나와있다시피 Levenberg-Marquardt 방법을 SLAM에서는 애용한다고 한다.

 

 

주관적인 결론을 내리자면 최적화로 목적함수의 최솟값을 찾는 것이 목적인 것 같다.

이는, SLAM에서 자신의 포즈를 추정하는데 에러를 최소화 시키는 것이 목적이지 않을까 생각한다.

 

 

 

ch6/ceres_curve_fitting의 실습코드는 다음과 같다.

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

// 비용 함수의 계산 모델
struct CURVE_FITTING_COST
{
    CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
	// 잔차 계산
	template <typename T>
	bool operator()(
		const T *const abc, // 모델 매개변수, 3차원
		T *residual) const	// 잔여
	{
        residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c)
        return true;
    }
	const double _x, _y; // x,y 데이터
};

int main ( int argc, char** argv )
{
	double a = 1.0, b = 2.0, c = 1.0;	// 실제 매개변수 값
	int N = 100;						// 데이터 포인트
	double w_sigma = 1.0;				// 노이즈 시그마 값
	cv::RNG rng;						// OpenCV 난수 생성기
	double abc[3] = {0, 0, 0};			// abc 매개변수의 예상 값

	vector<double> x_data, y_data; // 데이터

	cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

	// 최소 제곱 문제 만들기
	ceres::Problem problem;
    for ( int i=0; i<N; i++ )
    {
		problem.AddResidualBlock( // 문제에 오류 용어 추가
								  // 자동 파생, 템플릿 매개변수 사용: 오류 유형, 출력 차원, 입력 차원, 차원은 이전 구조체와 일치해야 합니다.
			new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
				new CURVE_FITTING_COST(x_data[i], y_data[i])),
			nullptr, // 여기서 사용되지 않는 커널 함수, 비어 있음
			abc		 // 추정할 매개변수
		);
	}

	// 솔버 구성
	ceres::Solver::Options options;				   // 여기에 입력해야 할 구성 항목이 많습니다.
	options.linear_solver_type = ceres::DENSE_QR;  // 증분 방정식을 푸는 방법
	options.minimizer_progress_to_stdout = true;   // cout으로 출력

	ceres::Solver::Summary summary; // 최적화 정보
	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	ceres::Solve(options, &problem, &summary); // 최적화 시작
	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

	// 출력 결과
	cout<<summary.BriefReport() <<endl;
    cout<<"estimated a,b,c = ";
    for ( auto a:abc ) cout<<a<<" ";
    cout<<endl;

    return 0;
}

 

출력 결과

x와 y에 대한 100개의 출력 값은 너무 세로로 길어 배제하였다.

 

 

 

ch6/g2o_curve_fitting의 실습코드는 다음과 같다.

#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std;

// 곡선 모델의 정점, 템플릿 매개변수: 변수 차원 및 데이터 유형 최적화
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	virtual void setToOriginImpl() // 초기화
	{
        _estimate << 0,0,0;
    }

	virtual void oplusImpl(const double *update) // 업데이트
	{
        _estimate += Eigen::Vector3d(update);
    }
	// 저장 및 불러오기: 비워두기
	virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

// ErrorModel 템플릿 매개변수: 관찰 치수, 유형, 연결된 정점 유형
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
	// 곡선 모델 오류 계산
	void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
	double _x; // x 값, _measurement에 대한 y 값
};

int main( int argc, char** argv )
{
	double a = 1.0, b = 2.0, c = 1.0;	// 실제 매개변수 값
	int N = 100;						// 데이터 포인트
	double w_sigma = 1.0;				// 노이즈 시그마 값
	cv::RNG rng;						// OpenCV 난수 생성기
	double abc[3] = {0, 0, 0};			// abc 매개변수의 예상 값

	vector<double> x_data, y_data; //데이터

	cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

	// 그래프 최적화 빌드, 먼저 g2o 설정
	typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> Block;								 // 각 오차 항의 최적화 변수 차원은 3이고 오차 값 차원은 1입니다.
	Block::LinearSolverType *linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 선형 방정식 솔버
	Block *solver_ptr = new Block(linearSolver);												 // 매트릭스 블록 솔버
	// GN, LM, DogLeg 중에서 선택되는 경사하강법
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
	g2o::SparseOptimizer optimizer;		// 그래프 모델
	optimizer.setAlgorithm(solver);		// 솔버 설정
	optimizer.setVerbose(true);			// 디버그 출력 켜기

	// 그래프에 정점 추가
	CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );

	// 그래프에 가장자리 추가
	for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);
		edge->setVertex(0, v);					// 연결된 정점 설정
		edge->setMeasurement(y_data[i]);		// 관찰된 값
		edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 정보 행렬: 공분산 행렬의 역행렬
		optimizer.addEdge( edge );
    }

	// 최적화 수행
	cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

	// 출력 최적화 값
	Eigen::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
    
    return 0;
}

 

출력 결과

 

1. LM 최적화 사용

2. Gauss-Newton 최적화 사용

 

3. dogleg 최적화 사용

 

 

전반적으로 수식적인 이해는 되었으나, 실질적으로 구현은 연습을 많이 해봐야할 것 같다.