일 | 월 | 화 | 수 | 목 | 금 | 토 |
---|---|---|---|---|---|---|
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 |
- 제 6장
- Raspberry Pi
- IMAGE
- OpenCV 모듈
- 코딩 테스트
- logarithm mapping
- PointNet++
- C++
- 부자 아빠 가난한 아빠
- Slam
- 논문 리뷰
- rigid KPConv
- 코딩테스트
- ros2
- deformable KPConv
- OpenCV
- exponential mapping
- FeatureMatching
- 코딩테스트 공부
- 경제 공부
- Docker
- 입문 Visual SLAM
- visual slam
- point cloud
- KPConv
- TURTLEBOT3
- SLAMKR
- 2 포인터 알고리즘
- PointNet
- 백준 1253번
- Today
- Total
꿈꾸는 개발자
입문 Visual SLAM 제 6장 실습 코드 중국어 주석 번역 본문
출처 : https://docs.google.com/document/d/1pIYlL3cVf9gXT9O7p_5H5az0HYkALuhrnrBQeZI0HUw/
입문 Visual SLAM 14강 (제6장)
Preface 이 문서는 중국어 원서인 “입문 Visual SLAM 이론에서 연습까지 14 강(视觉SLAM十四讲 从理论到实践)” 책의 원저자로부터 한글 번역 허가를 받고 구글 번역기를 이용하여 작성된 문서입니다.
docs.google.com
제 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;
}
출력 결과
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 최적화 사용
전반적으로 수식적인 이해는 되었으나, 실질적으로 구현은 연습을 많이 해봐야할 것 같다.
'SLAM' 카테고리의 다른 글
리 군과 리 대수(Lie Group, Lie Algebra) (0) | 2024.01.18 |
---|---|
입문 Visual SLAM 제 5장 실습 코드 중국어 주석 번역 (2) | 2022.12.01 |
입문 Visual SLAM 제 4장 실습 코드 중국어 주석 번역 (0) | 2022.12.01 |
입문 Visual SLAM 제 3장 공부 (0) | 2022.11.29 |
SLAM 개발자를 위한 공부 - 1 (0) | 2022.10.24 |