Factor Graph

Grahp SLAM이나 Pose Graph optimization, Bundle Adjustment 에 많이 사용함

• For example, in simultaneous localization and mapping (SLAM) and many other estimation problems we are after a maximum a posteriori estimate, i.e., we try to maximize posterior probability of the variables given a set of measurements.

• When attempting to act optimally, we try to maximize a performance index, or conversely minimize a penalty function. And even in classical planning, we are trying to find an assignment to a set of discrete variables that minimizes the plan length or optimizes for some other desirable property of the plan.

• For example, in a tracking application, a particular video frame only provides information about the position of a target at a particular time. At the next time step, a different variable is associated with the target. Of course, this depends on the parametrization chosen: if a track is globally parametrized, for example as a polynomial, this locality is destroyed.

• Factor graphs are a class of graphical models in which there are variables and factors. The variables represent unknown quantities in the problem, and the factors represent functions on subsets of the variables. Edges in the factor graph are always between factors and variables, and indicate that a particular factor depends on a particular variable.

• 간단한 식은 위에와 같다. 모든 relative variable들을 곱한것.

Factor Graph를 쓰면 probability distrubution이 간단해 진다.

위에 예와 같이 엣지는 노드(로봇의 포즈 상태)간의 상관성(예를 들어 : Odometery, 예측값)이고, 만약 관측값이 있다면, Least square 방법을 통해서

e(x-x^) M e(x-x^)^t 와 같이

(관측값 - 예측값) x information Matrix(covariance matrix) x (콴측값- 예측값)^t 을 통해 에러를 최적화 시킨다.

Object Function 안에 있는 것을 Factor 라고 하는구만!

VIO factor,

GPS factor

Semantic Factor

IMU Factor

etc,

이미지 매칭 방법

Ransac(Random Sample Consensus)

센서에서 얻어지는 데이터는 불완벽한데

RANSAC알고리즘은

이런 불완벽한 데이터에

inlier과 outlier 데이터를 구분을 하여서 사용자가 inlier데이터를 사용할수 있게 한다.

위와 같이 랜덤으로 선택된 포인트간에 라인에 가까이 있거나, 안에 들어가 있다면 inlier로 간주한다.

위와 같이 iterate를 하고, 제일 높은 inlier의 케이스를 선택을 하여서 데이터 분류를 한다.

즉 세가지 스텝인데

1. sampling a small subset of data points(treat them as inlier)
2. compute model parameters using the sampled data points.
3. score by fraction of inlier within a present threshold of the model.
   • 즉 어떤 inlier 포인트가 모델 스코어에게 큰 영향을 미치는 지 찾아 어떤 데이터가 inlier인지 outlier인지 찾는다.

SLAM에서는 Visual Odometer에 사용하게 된다.

• estimating motion of camera through the environment, by taking in acount the Corresponding points in pairs of consecutive image so they can compute relative orientation of the camera just based on the Correspondences
• and given that this approach is a kind of senstive to outlier so you need to make sure we have only inlier in order to estimate trajectory
• 즉 두 이미지 상에 효과적인 Corresponding matching 점을 찾는 것을 도와주면서 결국 안정적인 Corresponding matching point들로 인해 Transformation을 구할 수 있게 한다

즉 아래와 같이 엉망징창으로 코레스폰딩을 하게 되는데, 이때 불필요한 데이터를 outlier한다.

즉 내가 했던 프로젝트로 보면, 두 이미지상 ORB Feature Extractor로 특징점을 얻게 되는데, Corresponding mathcing을 하는데 있어 잘못된 특징점이 매칭 되는경우가 있으므로 이런 잘못된 매칭을 제거하고자 RANSAC이 쓰인다.

https://www.youtube.com/watch?v=Cu1f6vpEilg

Graph SLAM

Graph SLAM은 Pose graph optimization의 방식으로 odometery(휠엔코더)로 생성된 로봇 포즈에 대한 값을 Node로 저장을 한다.

주행중 생긴 여러 노드들간의 Rotation이나 Traslation의 상대적인 값(odometery measurement)들은 edge로 여겨지고 이는 관찰값이 된다.

그리고 만약 현재 주행하고 있는 포즈에 대한 추정 값이 이전에 방문한 노드에 존재한다면, 루프크로져를 통해 그래프 최적화가 일어나게 되는데

이때, 이전에 생성된 edge들에 대한 상대적 Odometer값들은 예측값으로 변화하게 되고, 카메라 센서다 라이다 센서를 통해 얻어진 Visual odometery는 관측값으로 사용하게

되면서 Nonlinear least square method 방식을 통해 관측값과 예측값을 iterate하여서 에러값을 미니멀하게 만들어 글로벌 맵을 최적화 한다.

에러 펑션 Minimize하는 방식은

e(x-x^) M e(x-x^)^t

에러 펑션과 인포메이션 매트릭스(corvariance matrix)를 통해 최적화를 한다.

방식에는 Gaussian Newton과 Levenberg-Marquardt(LM)

Essential Matrix, Fundamental Matrix

스테레오 카메라를 이용해서 3D mappoints를 구하는데 있어 필요한 essential Matrix와 Fundamental matrix가 있다.

이는 epipolar contraint을 통해서 Essential matrix나 Fundamental matrix를 구할 수 있는데

두 이미지에 매칭하는 피처포인트에 대해서 frame과 frame 간의 연결 할때 생기는 epipole 그리고 3차원 공간에서서 교차해서 만나는 포인트를 찾기 위해

epipole과 interest 특징점을 연결을 한 선이 epipoline으로, 프래임간 에피폴라라인으로 서칭을 통해 매칭점에 대한 3차원 위치를 찾는다.

이 찾게 된 3D 상에 매칭점은 epipoline과 baseline, epipole 등으로 통해 Fundamental Matrix를 구할 수 있으며

Essential Matrix는 E = K^t* F * K인 카메라 파라미터를 통해 구할 수 있다.

구해진 Essential Matrix는 essential matrix triangulation을 통해서 3D Mappoints가 구해지게 된다.

번외로, 구해진 3D Mappoints들은 카메라 포즈를 최적화 할때도 사용하게 되는데. 현재 프레임에서 추출된 이미지 플래인에서의 특징점과 3D Mappoints에 이미 가지고 있는 특

징점을 리프레젝션을 하여서 Bundle Adjustment(Least Square)를 통해 iterate를 하여서 최적화를 시킨다.

e(x-x^)M e^t(x-x^) = 0

위와 같은 에러 펑션과 Information matrix(Correspond matrix)를 이용한 알고리즘(BA)이다