Ceres Solver로 직접 해보면 이해하기 쉬움!

http://ceres-solver.org/tutorial.html

1. 각 frame image에서의 2d feature point와 pair되는 3d points 글로벌 좌표에서 각 프레임 카메라 좌표로 전환 P’=Rp + t
2. 카메라 좌표 Nomal plane화 (z값 1로)
3. Distortion과 카메라 내부파라미터를 이용하여 이전 이미지 좌표계로 나타냄
4. 이를 실제 관측값과 이미지 좌표로 투영된 픽셀(3d-2d) 값에 대한 error function를 구함
5. cost function을 통해 최적화
6. 이때 카메라 자세 T에서 얻어지는 랜드마크 값 p들을 관측값으로 비교하기 위해서는, 비선형방정식을 선형 방정식으로 변형 시켜줘야한다.
7. 첫번째로 Tranformation Matrix(Lie Group) 형태를 Rodrigues formula(log) 를 통해 lie algibra(선형) 형태로 바꾼다.(Rotation Matrix에 대하여 vector화)ㄴ
8. 이유는 9개의 요소와 3개의 자유도를 가지고 있는데, 자유도보다 요소가 많으면 singularity에 빠기지 쉽다. 또한 Rotation Matrix는 +,-,x,나누기가 있는데, 이는 최적화 할때 Computational이 증가한다. 그렇기 때문에 벡터화를 하여서 Targent space(선형 공간)에서의 최적화를 해야 된다.
9. 두번째 error function(cost function)에 대해 first order & second order tayor series하여서 선형화를 만든다.
10. 이때 T에 대한 카메라 error function에 대한 편미분과 랜드마크에 대한 편미분을 하여서 Jacobian Matrix(각 요소에 대한 vector field를 구한다)를 구한다(선형근대화를 시킨다.)
11. 얻어진 자코비안 매트릭스들을 Information 곱을 통한(J * Omega * J^t = H) 통해서 Hessian Matrix를 만들거나, 편미분을 한번 더하여서 Hessian Matrix를 만든다.
12. 9번째 스텝을 통해서 가우시안 뉴턴, LM같은 최적화 공식인 [H * deltax = -b]이라는 식이 나오고 LM,가우시안뉴턴 같은 최적화 방법으로 최적화가 된다.
13. Jacobian 및 2차 편미분으로 Hessian Matrix를 만들게 되면 엄청나게 큰 희소성의 행렬이 되고, 희소성은 역행렬을하여서 문제를 푸는데 많은 메모리 소모와 시간이 걸린다.
14. 이에 Schur Elimination 이용, 행렬안에 특징별 block 매트릭스를 통하여서 계산량을 적게, 쉽게 dx(예측값)을 구하고 이를 실제 관측값과 cost function을 통해 최적화를 한다.
15. 모든 맵포인트를 고려하게 되면, 계산량이 많고 불필요한 맵포인트도 포함될수 있으므로, RANSAC을 통해서 가치있는 맵포인트들과 정해진 이미지 프래임 숫자만 큼 Bundle Adjustment를 한다.
16. 즉 Covisibiltiy graph에 대한 문제를 푼다.
17. 또한 cost function를 통해 오차를 최적화 하는데는 오버 피팅이 되지 않도록 Huber같은 것을 사용하여서 outlier 제거를 통해 오차의 크기 수정을 결정한다.
18. 이와 같이 Hessian Matrix에 맺친 카메라 포즈에서 관찰된 Landmark 포인트들을 Gassian Newton or LM같은 방식을 써서 실제 관측값에 대한 오차를 계산하고 Mappoints와 camera 자세를 업데이트 해준다.