[SLAM} Opencv Camera model 정리
Opencv에는 다양한 카메라 모델을 다루기 위한 기본 Class들이 정의되어 있다. 그 중에서 Pinhole model과 Fish-eye model이 어떻게 모델되어 있는지 식으로 살펴보려고 한다. 카메라 모델에는 이 두가지 모델 이외에도 다양한 모델이 있으며, 이러한 다양한 카메라 모델에 대해서는 다음의 이전 글을 참고하자.
Pinhole Camera Model
Pin-hole 카메라 모델은 일반적으로 많이 사용되는 카메라 모델이며, 물체로부터 오는 lay를 focal length 가 1인 normalized image plane으로 projection하고, image coordinate으로 변환시키는 모델이다. 이때 일반적으로 distortion은 radial distortion과 tangential distortion을 고려한다.
먼저 distortion을 고려하지 않는 경우 다음과 같다. \[\begin{array}{l} {\left[\begin{array}{l} x \\ y \\ z \end{array}\right]=R\left[\begin{array}{l} X \\ Y \\ Z \end{array}\right]+t} \end{array}\]
Distortion을 고려하지 않은 모델은 매우 단순하다. $P=(X,Y,Z)$ 는 global coordinate에서의 물체의 위치이다. 이때 global coordinate에서 카메라의 rotation과 translation 이 각각 $R$, $t$ 라면 첫번째 수식을 통해 P는 Camera coordinate에서의 좌표인 $(x,y,z)$ 로 변환된다. \[\begin{array}{l} x^{\prime}=x / z \\ y^{\prime}=y / z \end{array}\]
그 다음은 Camera coordinate으로 변환된 좌표를 normalized image plane으로 변환합니다. normalized image plane은 원점으로 부터 거리가 1인 평면을 의미하며, 이때의 x, y값에 f를 곱함으로써 초첨거리가 f인 평면으로 변환할 수 있다. 따라서 첫번째로 camera coordinate으로 변환된 좌표를 각각 $z$ 로 나눠줌으로써 normalized image plane으로 변환한다. \[\begin{array}{l} u=f_{x} * x^{\prime}+c_{x} \\ v=f_{y} * y^{\prime}+c_{y} \end{array}\]
그리고 마지막으로는 $x’, y’$ 각각에 focal length ($f$)를 곱하고 principal point ($c$) 를 더해줌으로써 image coordinate으로 변환시킨다. focal length를 곱해주는것은 위에서 설명한 것 처럼 normalized image plane에서 초점거리가 f인 image plane으로 변환하는 과정이다. 그리고 camera coordinate의 경우 이미지의 중심이 $(0,0)$ 이지만 이미지의 경우 일반적으로 좌측상단이 $(0,0)$ 이다. 따라서 이러한 좌표의 변환을 해주는 것이 principal point를 더해주는 것이다. 이 계산은 일반적으로 3X3의 Camera Intrinsic matrix ($K$)를 곱해주는 과정으로 표현하기도 한다.
이러한 과정을 통해 3차원 포인트를 카메라로 바라보았을 때 해당하는 물체가 어느 픽셀에 도달할지를 계산할 수 있게 되었다. 하지만 실제 카메라 모델은 이렇게 이상적이지 않다. 렌즈의 물리적인 이유, 센서와 렌즈의 misalignment와 같은 이유로 발생하는 distortion을 보정해 주어야 한다. 일반적인 카메라 모델의 경우 보통 Radial distortion과 Tangential distortion 모델을 활용한다. 두가지 distortion model에 대해서 익숙하지 않다면, 위 링크의 글을 살펴보길 바란다.
일반적인 Radial distortion은 다음과 같은 식으로 표현된다. \[x_{\text {distorted }}=x\left(1+k_{1}^{*} r^{2}+k_{2}^{*} r^{4}+k_{3}^{*} r^{6}\right)\] \[y_{\text {distorted }}=y\left(1+k_{1}^{*} r^{2}+k_{2}^{*} r^{4}+k_{3}^{*} r^{6}\right)\]
그리고 Tangential distortion은 다음과 같이 표현된다. \[x_{\text {distorted }}=x+\left[2^{*} p_{1}^{*} x^{*} y+p_{2}^{*}\left(r^{2}+2^{*} x^{2}\right)\right]\] \[y_{\text {distorted }}=y+\left[p_{1}^{*}\left(r^{2}+2^{*} y^{2}\right)+2^{*} p_{2}^{*} x^{*} y\right]\]
각 Distortion 식에서 $x,y$는 이상적인 (undistorted) pixel이며, $k_1, k_2, k_3$는 radial distortion parameter, 그리고 $p_1, p_2$는 tangential distortion parameter이다. 이러한 Distortion model을 적용한 모델은 다음과 같다. \[\begin{array}{l}{\left[\begin{array}{l}x \\y \\z\end{array}\right]=R\left[\begin{array}{l}X \\Y \\z\end{array}\right]+t} \\ x'=x / z \\y^{\prime}=y / z \end{array}\]
Normalized image plane으로 변환하는 과정은 동일하다. \[\begin{array}{l}x^{\prime \prime}=x^{\prime} \frac{1+k_{1} r^{2}+k_{2 }r^4+k_{3} r^{6}}{1+k_{4} r^{2}+k_{5} r^{4}+k_{6} r^{6}}+2 p_{1} x^{\prime} y^{\prime}+p_{2}\left(r^{2}+2 x^{\prime 2}\right) \\y^{\prime \prime}=y^{\prime} \frac{1+k_{1} r^{2}+k_{2} r^4+ k_{3} r^{6}}{1+k_{4} r^{2}+k_{5} r^{4}+k_{6} r^{6}}+p_{1}\left(r^{2}+2 y^{\prime 2}\right)+2 p_{2} x^{\prime} y^{\prime} \\\text { where } r^{2}=x^{\prime 2}+y^{\prime 2} \end{array}\]
Normalized image plane으로 변환된 값 $x’, y’$ 은 radial, tangential distortion 모델에 의해서 distortion이 발생한다. 위 식에서 우항의 첫번째 항은 radial distortion, 그리고 두번째 세번째 항은 tangential distortion이다. 여기서 $r$은 normalized image plane에서 각 점의 principal axis로 부터의 거리를 의미한다. \[\begin{array}{l}u=f_{x} * x^{\prime \prime}+c_{x} \\v=f_{y} * y^{\prime \prime}+c_{y}\end{array}\]
Distortion model을 적용한 이후의 normalized image plane 의 pixel ($x’’, y’’$)은 Camera intrinsic matrix를 곱해줌으로써 image plane으로 변환된다. 위와 같이 distortion이 포함된 카메라 모델을 통해 3차원 좌표상에 있는 물체가 실제 핀홀 카메라 모델에서 어느 위치의 픽셀에 나타날지 계산할 수 있게 되었다. 반대로 이미지 plane에 있는 픽셀이 실제 3차원 좌표에서 어디에 위치하는지 계산할 수 있다. 하지만 이런 경우에 위 모델을 통해 알 수 것은 normalized image plane에서의 좌표인 $x’,y’$ 이기 때문에 실제 3차원 좌표로 복원되기 위해서는 카메라로 부터 떨어진 거리인 depth ($z$)을 알고 있어야 한다.
Fish-eye Camera Model
Fish-eye camera는 물고기의 눈처럼 렌즈가 볼록하여 넓은 화각을 갖는 카메라 모델을 의미한다. 일반적인 핀홀 카메라와는 다르게 Object point의 빛이 렌즈를 통과하여 직진으로 image plane에 도달하는 것이 아닌 렌즈의 왜곡에 의해 빛이 꺽여서 image plane에 도달하게 된다. Fish-eye camera를 모델링하기 위해 가장 많이 활용되는 모델은 Equidistant model로 빛의 입시각 $\theta$와 image plane에서 principal point와의 거리인 $r$ 가 비례적인 관계를 갖는 모델이다. 즉, \[\frac{r_1}{\theta_1} = \frac{r_2}{\theta_2}\]
이다. OpenCV의 fish-eye 카메라 모델도 이와 같은 equidistant model을 이용하고 있다. 보통 equidistant model의 경우에는 radial distortion 만을 고려한다. 지금부터 3차원 point인 $X$가 실제 fish-eye 모델을 통해 image plane으로 변환되는 과정을 살펴보자. \[X_c = RX + T\] \[x = X_{c1}, y = X_{c2}, z= X_{c3}\]
핀홀카메라와 동일하게 3차원 공간의 점 \(X\)는 카메라의 rotation, translation인 $R, T$를 이용하여 camera coordinate인 \(X_c\)로 변환된다. \[a = \frac{x}{z}, b=\frac{y}{z}\]
Camera coordinate으로 변환된 점은 depth인 $z$로 normalize를 함으로써 normalized image plane으로 변환된다. \[r^2 = a^2 + b^2\] \[\theta=atan(r)\]
이때 중요한것은 여기서의 $r$은 일반적은 핀홀 카메라의 normalized image plane에서 중심점으로 부터 해당 픽셀까지의 euclidean distance를 의미한다. $\theta$는 빛의 입사각을 의미한다. \[\theta_d = \theta(1+k_1 \theta^2 + k_2 \theta^4 + k_3 \theta^6 + k_4 \theta^8)\]
위 식은 radial distortion을 적용하는 식을 나타낸다. 일반적인 핀홀 카메라와 가장 큰 차이점의 있는 부분 중에 하나이다. 핀홀 카메라의 경우에는 pixel의 x, y좌표인 $x’, y’$에 distortion 모델이 적용되었으나, equidistant model에서는 입사각인 $\theta$에 distortion 모델이 적용된다. 이때 일반적으로 4개의 radial distortion parameter가 사용된다. \[x' =\frac{\theta_d}{r} * a\] \[y' = \frac{\theta_d}{r}*b\]
위 식은 equidistant model의 특징인 $\frac{r_1}{\theta_1} = \frac{r_2}{\theta_2}$의 특징을 이용하여 pixel의 값을 scaling 하는 식이다. normalized image plane에서의 pixel 값 ($a, b$)는 $r$과 $\theta_d$의 비율로 scaling된다. 위에서는 distortion model이 적용되었기 때문에 $\theta_d$가 사용되었다. 만약 distortion model을 적용하지 않고 이상적인 모델이라면 $\theta_d$ 대신 $\theta$를 사용하면 된다. \[u= f_x(x' + ay') + c_x\] \[v=f_yy'+c_y\]
마지막으로 normalized image plane에서 scaling된 pixel값에 Intrinsic matrix를 곱해 줌으로써 image coordinate으로 변환하게 된다. 이러한 계산을 통해 실제 3차원에 있는 물체가 fish-eye 카메라 모델에서 어느 pixel에 위치할지 계산할 수 있다. 보통 이러한 Fish-eye 카메라의 이미지는 많은 경우 undistortion과정 (실제 직선은 직선으로 보이도록 변환) 을 통해 이미지를 변환하여 사용하게 된다. Undistortion 과정은 위의 과정과 반대로 실제 fish-eye model로 변환된 픽셀값 $u, v$ 로부터 normalized image plane의 점인 $a, b$를 계산하는 과정으로 생각할 수 있다. 이때 depth 값은 모르기 때문에 $z$를 복원할 수는 없다.
Reference
Camera Calibration and 3D Reconstruction - OpenCV 2.4.13.7 documentation
OpenCV: Fisheye camera model
[SLAM] Camera Models and distortion (Perspective, Fisheye, Omni)
Visual SLAM, SfM 등 카메라를 이용한 연구를 하기 위해서 가장 기본적으로 알아야 할 부분이 바로 카메라 모델이다. 일반적인 카메라는 대부분 핀홀(Pin-hole) 카메라 형태로 모델링하여 사용하고 있으며, 처음 카메라에 대해서 배울 때 대부분의 설명 자료들이 핀홀 모델을 기반으로 설명을 하고 있다. 이러한 핀홀 모델은 렌즈를 통해 들어오는 빛이 굴절되지 않고 바로 이미지 센서로 들어오는 perspective projection을 기반으로 하고 있다. 하지만 실제 연구를 진행하다 보면 넓은 화각이 이점이 되는 어플리케이션 (ex. Visual SLAM)이 있기 때문에 FOV (Field of View)가 170도 이상인 어안렌즈 (Fisheye lens)를 사용하는 경우도 많다. 이러한 화각이 매우 넓은 렌즈의 경우에는 일반적인 모델로 표현하기 어렵다. 따라서 이러한 경우에는 perspective projection이 아닌 equidistance projection 모델을 사용한다.
카메라를 통해 이미지를 얻는다는 것은 3차원 공간에 있는 물체에 반사되어 오는 빛이 이미지 센서에 들어오는 신호가 변형되는 것이다. 따라서 카메라를 모델링 한다는 것은 실제 3차원에 있는 점을 카메라를 통해 보았을 때 실제 이미지상에 어떤 위치에 놓이게 되는지 추정하는 것이다. 이러한 카메라 모델이 정확히 정의 되어야 3차원 공간의 정보와 2D 이미지 상의 매칭을 수학적으로 표현 가능하다. VIsual SLAM, Visual odometry, SfM (Structure from Motion)등이 이러한 카메라 모델이 필요한 대표적인 분야라고 할 수 있다.
Camera Models
카메라 모델은 물체에 반사되는 빛이 이미지 센서까지 도달하는 방법에 대한 모델이다. 이러한 과정에서 빛의 왜곡에 의한 영향은 distortion 파라미터로 보정하게 된다. 먼저 다양한 카메라 모델을 살펴보자.
여기서 $f$ 는 빛이 모이는 중심점에서 image plane 까지의 거리인 Focal length, $\theta$ 는 principal axis인 $Z_c$ 와 빛이 들어오는 각도, 그리고 $r$은 이미지의 중심인 principal point와 image point의 거리를 의미한다.
- 다양한 카메라 모델들\[r = f tan \theta\]\[r = 2ftan(\theta/2)\]\[r=f\theta\]
- Equisolid angle projection
\[r = 2fsin(\theta/2)\]\[r = f sin(\theta)\]
빛이 이미지로 변환되는 과정은 다양한 모델로 표현이 되지만, 주로 사용되는 모델은 perspective projection, Equidistance projection 모델이다. 이 글에서는 이 두 모델, 그리고 추가적으로 omni camera model까지 다뤄본다.
Perspective Projection
Perspective projection은 기본적인 pin-hole 카메라 모델이며, Principal axis (노란색 line) 기준으로 빛이 들어오는 각도와 image plane으로 나아가는 빛의 각도가 동일한 모델이다. 즉 위 그림 기준으로 $\alpha = \beta$ 이다. 다양한 카메라 모델을 설명할 때의 식으로 표현하면 다음과 같다. \[r = f tan \beta\]
여기서 $f$ 는 focal length 로 image plane과 빨간색 선의 거리를 의미하며, $r$ 은 principal axis (노란색 line)에서 부터 image plane상의 빛까지의 거리를 의미한다. 즉 빛이 들어오는 각도와 focal length에 의해 이미지에서의 위치가 결정된다. 이러한 perspective model은 가장 기본적인 카메라 모델로, 대부분의 카메라 모델에서 활용된다.
Equidistance Projection (Fisheye projection)
최근에는 Visual odometry 분야에서는 빠른 움직임에서 tracking loss를 최소화 하기 위하여 화각이 넓은 Fish-eye 카메라를 많이 활용한다. 이러한 Fish-Eye camera는 렌즈 설계부터 넓은 화각을 목적으로 설계되었기 때문에 일반적인 Perspective model로 모델링하기 어렵다. 이런 Fish-Eye 카메라에서 가장 많이 활용되는 모델이 Equidistance projection 모델이다. Perspective projection 모델은 들어오는 빛과 나가는 빛의 각도가 동일하였다면, Equidistance projection 모델은 들어오는 빛의 각도와 principal axis (노란색 선)으로 부터 떨어진 거리가 선형적인 모델이다. 즉 각도가 거리와 선형적이기 때문에 이름이 Equidistance 인 것 같다. 식으로 표현하면 위의 그림처럼 \[\frac{\alpha_1}{d_1} = \frac{\alpha_2}{d_2}\]
와 같이 표현할 수 있다.
이러한 렌즈 설계는 오른쪽 그림과 같이 넓은 화각의 데이터를 얻을 수 있지만, 상당한 왜곡 (Distortion)을 발생시킴을 알 수 있다.
Omni Directional 카메라 모델 (Catadioptric Camera)
Omni-direction 카메라는 주변, 즉 360도를 전부 바라보는 카메라를 의미한다. 위 그림은 대표적인 omni-directional camera들의 종류를 보여준다. 첫번째 그림은 앞에서 설명한 Fish-eye lens로 180 도 이상의 화각을 갖기도 한다. 두번째는 일반적인 카메라에 거울을 붙여 수평 360도를 바라보는 카메라이다. 세번째는 여러개의 카메라를 사방으로 부착하여 사용하는 카메라이다. 여기서는 두번째 모델인 Catadioptic camera model을 설명한다.
위 그림은 Catadioptric camera model을 설명하기 위한 그림이다. 이 모델은 central catadioptric camera를 위한 unified model로 다양한 곡선의 거울과, perspective model을 동시에 고려가 가능한 모델이다.
모델을 이해하기 위한 단계는 크게 4단계로 나눠진다.
Scene point인 $P$ 를 unit sphere로 projection 한다. \[P_s = \frac{P}{||P||} = (x_s, y_s, z_s)\]
중심점이 $C_{\epsilon} = (0,0,\epsilon)$ 인 새로운 reference frame으로 $P_s$를 옮긴다. 여기서 $\epsilon$ 은 conic의 foci인 $d$ 와 latus rectum인 $l$ 에 의해서 결정된다.
\[P_{\epsilon} = (x_s, y_s, z_s + \epsilon)\]
$C_{\epsilon}$ 으로 부터 거리 1떨어진 normalized image plane으로 $P_{\epsilon}$ 을 projection 한다. \[\tilde{m}=\left(x_{m}, y_{m}, 1\right)=\left(\frac{x_{s}}{z_{s}+\epsilon}, \frac{y_{s}}{z_{s}+\epsilon}, 1\right)=g^{-1}\left(P_{s}\right)\]
마지막으로 일반적인 intrinsic matrix $K$ 를 곱해서 image coordinate으로 변환한다. \[\tilde{p} = K \tilde{m}\]
위 모델을 다시 간단히 살펴보면, $\epsilon$ 값에 따라 다양한 모델을 표현할 수 있다. 즉 일반적인 perspective model은 곡선이 없는, 즉 $\epsilon = 0$ 인 경우이며, parabolic 형태의 거울일 경우에는 $\epsilon=1$ 인 경우이다. 더욱 자세한 내용은 아래 링크를 참고바란다.
link
Distortion Models
위에서는 주로 많이 사용되는 카메라 모델에 대해서 알아보았다. 위의 카메라 모델들은 렌즈의 형태에 의해 빛이 굴절되는 모델을 표현하는 것이며, 이상적인 경우를 의미한다. 하지만 실제로는 다양한 이유로 (렌즈의 왜곡 등) 정확히 해당 모델로 표현이 되지 않는다. 일반적으로 이러한 왜곡은 두가지 distortion 모델로 정의된다.
Radial Distortion
Radial distortion은 렌즈의 중앙부와 바깥영역의 굴절률이 달라짐에 따라서 발생한다. Distortion이 없는 경우 (No distortion)에는 정면으로 바로보았을 때 모든 직선이 직선으로 보인다. 하지만 Barrel distortion 혹은 Pincushion distortion이 발생하였을 때는 실제 환경의 직선이 휘어져 보이게 된다. Barrel, 그리고 Pincusion이라는 이름은 실제 Barrel (통)의 형태와 Cusion(쿠션)을 눌렀을때의 형태에서 따온 것이다. \[x_{\text {distorted }}=x\left(1+k_{1}^{*} r^{2}+k_{2}^{*} r^{4}+k_{3}^{*} r^{6}\right)\] \[y_{\text {distorted }}=y\left(1+k_{1}^{*} r^{2}+k_{2}^{*} r^{4}+k_{3}^{*} r^{6}\right)\]
Radial distortion은 일반적으로 위와 같이 모델링된다. $x, y$ 는 normalized image coordinate에서의 undistorted된 pixel의 위치이다. 즉 distortion이 발생하지 않았을 때의 pixel 위치를 의미한다. $r$은 중심축에서의 각 픽셀의 거리로 $r^2 = x^2 + y^2$ 이다. 그리고 나머지 $k_1, k_2, k_3$는 radial distortion을 표현하는 parameter이다.
Tangential Distortion
Tangential distortion은 실제 물리적으로 카메라 렌즈와 이미지 센서의 mis-alignment로 인해 발생하는 distortion이다. 최근에는 기술의 발달로 하드웨어 적인 mis-alignment가 매우 적기 때문에 크게 신경쓰지는 않는 추세이긴 하나 상황에 따라서 필요하기도 하다. \[x_{\text {distorted }}=x+\left[2^{*} p_{1}^{*} x^{*} y+p_{2}^{*}\left(r^{2}+2^{*} x^{2}\right)\right]\] \[y_{\text {distorted }}=y+\left[p_{1}^{*}\left(r^{2}+2^{*} y^{2}\right)+2^{*} p_{2}^{*} x^{*} y\right]\]
Tangential distortion은 위와 같이 모델링된다. Radial distortion과 마찬가지로 $x,y$ 는 undistorted pixel 위치이다. Tangential distortion은 $p_1, p_2$ coefficient로 표현된다.
정리
이 글에서는 가장 기본적으로 많이 사용되는 카메라 모델과 Distortion 모델에 대해서 설명하였다. 대부분의 카메라 calibration 을 위한 tool 들 (Matlab, Kalibr 등)은 대부분 위에서 설명한 camera model들과 distortion model들을 지원한다. Kalibr의 경우 Pinhole camera 모델의 경우 radial distortion과 tangential distortion을 모두 추정하지만, equidistance model의 경우에는 radial distortion만 추정하여 사용한다.
Reference
Camera Model
A generic camera model and calibration method for conventional, wide-angle, and fish-eye lenses
What Is Camera Calibration?
ethz-asl/image_undistort
omni directional camera
Bundle Adjustment의 Jacobian 계산
Jacobian with respect to Lie Algebra
일반적으로 최적화를 수행할 때 가장 쉬운 방법은 함수의 미분을 계산하고, 현재의 값에서 미분값이 작아지는 방향으로 값을 변경해 가면서 최적화를 수행하는 방법이다. 이러한 방법을 gradient descent 방법이라고 하는데, Jacobian은 이러한 함수가 multi-variable 일때의 미분을 의미한다.
즉 Jacobian은 multi-variable 문제에서 내가 최적화 하고 싶은 parameter들에 대한 편미분을 matrix로 표현한 것이다.
Graph-based SLAM 뿐만 아니라 BA(Bundle Adjustment), Visual SLAM쪽을 공부하다 보면 계속 이러한 Jacobian을 만나게 되는데 정확히 어떻게 계산이 되는지 알고 넘어가는게 좋을 것 같아서 수식을 정리해 보았다.
수식이 너무 많아서 손필기로 정리하였으며, Error function은 일반적인 BA에서 사용하는 reprojection error에 대한 Jacobian을 구하는 방법이다.
(혹시 수식적으로 틀린 부분이 있거나, 더 간단하게 풀수있는 방법이 있으면 댓글로 알려주세요)
최종 rotation의 Jacobian을 보면 상당히 복잡하다.
이 식을 조금 단순화 시키기 위해서 roation matrix를 단순화 시킨다.
Multiple view geometry 책의 Appendix 6의 A6.9.1에 이런 말이 있다.
Lie algebra t가 작으면, rotation matrix는 다음과 같이 근사 가능하다. \[R = I + [t]_{\times}\]
따라서 근사화된 R에 대해서 jacobian을 구해보면 다음과 같다.
즉 근사화된 Rotation matrix를 이용해서 Jacobian을 구해보면 입력 measurement의 skew symmetric matrix가 됨을 알 수 있다.
대부분의 tracking 문제에서는 계산하고자 하는 state는 두 frame 사이의 relative pose이고, 이 relative는 크지 않다.
따라서 R을 위와같이 근사화 해서 풀 수 있다.
Jacobian with respect to Quaternion
그렇다면 state가 quaternion으로 rotation을 표현한다면 어떻게 될까?
visual SLAM 코드를 공부하기 좋은 Pro-SLAM의 코드는 quaternion으로 rotation state를 표현하고 있으며, w를 제외한 3개로 state를 표현한다.
w는 3개의 state로 recovery가 가능하다.
Point를 Quaternion으로 회전 시키는 식을 미분하면 다음과 같다.
state를 3개만으로 표현하였기 때문에 최종 matrix에서 뒤의 3 column 즉 \[2 (v^TaI+va^T-av^T-w[a]_{\times})\]
만 해당이 된다.
위에서 설명한 것 처럼 대부분의 tracking 문제에서는 두 frame 사이의 relative를 계산하는 것이며, 이 relative는 크지 않다.
따라서 Jacobian은 회전이 0 (identity) 에서 미분한 값으로 단순화 할 수 있다. (찾고자 하는 relative rotation이 작기 때문에)
회전이 0, identity일 때 quaternion의 w 는 1, 그리고 i,j,k term (위 식에서는 v vector)은 0이 된다.
따라서 quaternion의 jacobian은 다음이 된다. \[-2[a]_{\times}\]
Pro-SLAM 코드에서 rotation state는 quaternion으로 표현되기 때문에 위의 Jacobian을 사용한다.
Rotation의 Jacobian을 계산하는 코드를 보면 일치함을 알 수 있다.
//ds update total error
_total_error += _errors[u];
//ds compute the jacobian of the transformation
Matrix3_6 jacobian_transform;
//ds translation contribution (will be scaled with omega)
// proj(M_i)를 p_cam 으로 미분한 부분에서 translation부분
jacobian_transform.block<3,3>(0,0) = _weights_translation[u]*Matrix3::Identity();
//ds rotation contribution - compensate for inverse depth (far points should have an equally strong contribution as close ones)
// *******************여기가 Rotation의 Jacobian 계산하는 부분, 위에서 설명한대로 넣어주고 있다 **********************
jacobian_transform.block<3,3>(0,3) = -2*srrg_core::skew(sampled_point_in_camera_left);
//ds precompute
// Intrinsic matrix를 곱해주는 부분
const Matrix3_6 camera_matrix_per_jacobian_transform(_camera_calibration_matrix*jacobian_transform);
//ds precompute
const real inverse_sampled_c_left = 1/sampled_c_left;
const real inverse_sampled_c_right = 1/sampled_c_right;
const real inverse_sampled_c_squared_left = inverse_sampled_c_left*inverse_sampled_c_left;
const real inverse_sampled_c_squared_right = inverse_sampled_c_right*inverse_sampled_c_right;
//ds jacobian parts of the homogeneous division: left
Matrix2_3 jacobian_left;
// 각 카메라의 coordinate로 이동한 measurement로 만든 matrix (미분 term에서 가장 앞에 곱해지는 matrix)
jacobian_left << inverse_sampled_c_left, 0, -sampled_abc_in_camera_left.x()*inverse_sampled_c_squared_left,
0, inverse_sampled_c_left, -sampled_abc_in_camera_left.y()*inverse_sampled_c_squared_left;
//ds we compute only the contribution for the horizontal error: right
Matrix2_3 jacobian_right;
jacobian_right << inverse_sampled_c_right, 0, -sampled_abc_in_camera_right.x()*inverse_sampled_c_squared_right,
0, inverse_sampled_c_right, -sampled_abc_in_camera_right.y()*inverse_sampled_c_squared_right;
//ds assemble final jacobian
_jacobian.setZero();
//ds we have to compute the full block
// 최종 p_cam으로 미분 한 matrix, 여기에선 left, right의 reporjection error를 concat해서 사용
_jacobian.block<2,6>(0,0) = jacobian_left*camera_matrix_per_jacobian_transform;
//ds we only have to compute the horizontal block
_jacobian.block<2,6>(2,0) = jacobian_right*camera_matrix_per_jacobian_transform;
//ds precompute transposed
const Matrix6_4 jacobian_transposed(_jacobian.transpose());
//ds update H and b
// 최적화 문제를 풀기 위해 계산된 jacobian으로 hessian 계산 (이 부분은 이전 Graph SLAM post 참고)
_H += jacobian_transposed*_omega*_jacobian;
_b += jacobian_transposed*_omega*error;
아래의 github 링크는 Pro-SLAM 에 대해서 한글로 주석을 달아놓은 code이다.
https://github.com/JinyongJeong/proSLAM.git
Ubuntu CPU Frequency 최적화 하기
Ubuntu CPU Frequency
일반적으로 전력 관리를 위해 CPU의 frequency는 가변적으로 설정되어 있다. 일반적으로 800MHz-4200MHz정도로 설정되어 있다. 이런 CPU의 Frequency는 OS가 자동적으로 load을 보고 조절하는 것으로 알고 있으나, 실제 Ubuntu에서 구현된 code를 돌려보면 CPU의 frequency의 변화에 의해서 함수 구동 속도가 변하는 현상을 볼 수 있다. 일반적으로 Code가 최대 performance로 돌아야 하는 경우는 CPU frequency가 일정하게 최대 속도인것이 좋다.
현재 frequency 및 min/max frequency는 아래 명령어로 확인 가능하다
watch -n 0.1 "lscpu | grep MHz"
Ubuntu CPU Frequency 조절
다음 package를 설치한다.
sudo apt-get install cpufrequtils
CPU frequnecy min 설정
sudo cpufreq-set -d 0.8Ghz
CPU frequnecy max 설정
sudo cpufreq-set -u 4.2Ghz
아래 명령어를 통해 현재 CPU의 max frequency를 확인 후 약간 아래값으로 minimum 값을 설정해주면 일정한 속도의 code 성능을 확인할 수 있다.
watch -n 0.1 "lscpu | grep MHz"
이 방법을 통해 minimum frequency를 높이게 되면 노트북의 경우 배터리 성능에 영향이 있을 수도 있으니, 적절히 사용하면 좋을 것 같다.
IMU Filter 관련 정리
IMU는 Angular velocity를 측정하는 Gyroscope, 가속도를 측정하는 Accelerometer, 자기장을 측정하는 Magnetometer 센서를 포함하고 있다. IMU Filter (AHRS: Attitude Heading Reference System)는 이 데이터를 이용해서 센서의 Global 회전 (Orientation)을 계산한다.
관련 ROS package http://wiki.ros.org/imu_filter_madgwick
관련 Report https://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf
Pagination