fffhyun

참고 : https://darkpgmr.tistory.com/82

오늘은 PnP 에 대해 알아보겠습니다.

PnP : Perspective-n-Point 란

2차원에서의 점들과 이에 대응되는 3차원의 점 n개가 주어진 경우에

카메라의 pose R,t를 구하는 문제를 말합니다.

실제 우리가 카메라의 외부 파라미터 ( pose ) 추정을 한다고 생각하며 예시를 들어보겠습니다.

우리는 이미 알고 있는 object에 대한 사진을 찍었고, 해당 사진을 찍었을 때 카메라의 pose [R |t] 를 구하려고 합니다.

object의 국소적 특징 모서리, edge등을 통해 이미지(사진)에 찍힌 2D상의 점과

대응되는 3D point cloud상의 모델의 점(실제 3차원 현실에서의 점)에서 2D-3D 대응점들을 찾을 수 있었습니다.

해당 대응점이 3개 이상 존재하면 PnP알고리즘으로 카메라의 pose를 구할 수 있고, 

이 pose는 object 좌표계 -> camera 좌표계 사이의 변환행렬을 의미하며,

반대로 생각하면 카메라 좌표계에서 object 좌표계의 변환행렬을 알 수 있음을 말합니다.

다시 생각해보면 물체의 pose 추정을 풀 수 있는 방법과  같음을 알 수 있습니다.

Xi는 object 좌표계에서의 3D 점, RXi+t는 카메라 좌표계에서의 3D점 입니다. pi를 통해 xi 즉 2D image 상의 한 점으로 mapping됨을 알 수 있습니다. 이 3개 변환 간의 관계를 잘 이해해야 해당 문제에 대해서 이해할 수 있습니다.

결국 우리가 풀고자하는 문제는 xi 와 Xi를 알고 있을 때, 이 사이에 있는 변환행렬 , 카메라 외부 파라미터 R,t를 추정하는 것입니다.

그러나 perspetive projection으로 인해, 해당 문제는 비선형 방정식을 푸는 문제로 변하고 풀기가 매우 복잡해집니다.

3차원에서 한 점은 Xi = (X, Y, Z) 의 성분을 가집니다. 그러나 2D 이미지 좌표는 xi = (u,v) 2개의 성분을 가집니다.

이러한 u,v와 X,Y,Z간 관계는 아래와 같습니다.

정규화 이미지 평면이 무엇인지, 왜 Z는 나눗셈으로 들어가는지 헷갈릴 수 있는데 

보통 카메라 모델에서 좌표계는 총 3개로 표현되며

1. 카메라 좌표계 (Camera frame)
2. 정규 이미지 좌표계 (Normalized image plane)
3. 픽셀 좌표계 (Image / pixel frame) 로 구분할 수 있습니다.  1. 은 카메라를 원점으로한 3D 좌표계이고, 2,3은 이미지상의 2D 좌표계입니다.

여기서 정규 이미지 좌표계란, 카메라 좌표계에서 위치한 3D 공간상의 점에서 카메라 좌표계의 원점으로 직선을 긋습니다.

여기서 z = 1의 거리에 2D 이미지 평면이 존재한다고 생각하고 

해당 평면과 직선이 만나는 점을 정규 좌표계에 3차원 상의 P를 투영한 점 P'으로 정의합니다.

따라서 이러한 원리로 직선과 평면의 교점을 정규 이미지 좌표계 projection이라고 하는 것입니다.

이는 카메라가 보는 2D 좌표를 깊이(Z)의 영향을 제거한 순수한 방향 정보로 표현한 것입니다.

또한, 이는 우리가 최종적으로 만나게 되는 픽셀 좌표계에서 카메라 내부 파라미터 영향을 제거하고, 순수하게 기하적인 의미만을 가지는 좌표이기도 합니다.

- EPnP

EPnP는 존재하는 모든 대응점들을 사용해 R,t를 구하는 것이 아니라, 대표되는 4개의 대응점을 사용해 R,t를 구하는 방식입니다.

이는 3차원 공간에서 1개의 점은 4개의 대응점으로 항상 표현이 가능하다는 사실에 기반합니다.

추가적으로 3차원 공간에서 점2개면 직선, 점 3개면 평면을 표현할수 있듯이, 4개의 점이면 3D 공간 전체를 표현할 수 있다는 것입니다.

공간으로 설명하면 어려우니, 예시를 들어봅니다. 임의의 삼각형이 있고, 삼각형 안의 임의의 점 A가 있습니다.

A는 삼각형의 세변 기준점 a,b,c로 항상 표현될 수 있습니다 . ex ) 0.2a + 0.2b + 0.1c ... a,b,c는 벡터라고 생각하면 편합니다..

이러한 A의 a,b,c로의 비율은 삼각형이 회전/이동하여도 불변합니다.

이와 동일한 맥락으로 3D공간에서의 한개 object의 모든 점도 4개의 점의 결합으로 표현할 수 있습니다. 

고정된 카메라에 대해 물체가 이동한다고 가정합시다.

이 경우에도 삼각형의 경우와 동일하게 한 개 object의 4개 기준점의 결합 비율은 불변합니다.

이제 카메라의 입장으로 가봅시다.

위와 같이 , 물체 좌표계에서와 같이 카메라 좌표계에서도 동일하게 한 점은 , 기준점 4개의 가중합임을 확인할 수 있습니다.

해당 식을 사용해서 기존에 풀고자 했던 R,t 추정을 단순화합니다. 

원래 우리는 실제 3D 점 X가 2D기존에는 R,t 각각 6,3개의 자유도를 가진 방정식을 풀어야 했는데 반편에 

EKF에 대해 리뷰해보겠습니다. 아래 유튜브가 EKF에 대해 굉장히 쉽게 설명하여 많은 도움을 얻었습니다.

 참고자료

-  제어공학 수업자료

-  Probabilistic Robotics

https://youtu.be/9X3jGGnbcvU?si=qCTI6pfvvzVehR-t&t=204 

앞선 포스트에서 말했다시피 , EKF는 KF에서 있는 시스템의 선형화 가정을 완화한 필터입니다.

따라서 실제 존재하는 비선형 시스템에 적용했을 때, 이점을 얻게 됩니다.

기본적인 뼈대는 Kalman filter와 같지만, 시스템을 A,B와 같은 행렬이 아니라 비선형 term으로 표현합니다.

*noise, 초기값에 대한 가정은 kalman filter와 동일합니다.

기존에 있던 prediction과 correction은 선형 시스템을 대상으로 만들어진 수식이기에 , 당연히 비선형 시스템에는 적용이 안됩니다.

하지만 비선형 시스템을 그대로 다루는 것은 어려운 일이기에 EKF에서는 시스템에 대한 선형화 linearization 를 진행합니다.

여기서 first order Taylor series expansion을 사용하여 비선형 시스템을 선형으로 근사합니다.

이게 뭐지 .. 싶을 수도 있지만 사실은 단순합니다. 

first order Taylor series expansion은 Taylor series의 1차항까지를 썻다고 해서 하는 말이고 ,

사실은 해당 지점에서의 접선의 방정식을 의미합니다 ㅎㅎ

우리가 쓰는 시스템은 n 차원의 상태에서 m을 매핑하기에 ,

단순 미분이 아닌 사전지식에서 언급한 jacobian을 통해 선형 근사를 하게 되는 것입니다 ㅎㅎ

이렇게 비선형 시스템 g와 h를 선형화하는 방법을 통해 아래의 EKF 를 수행합니다. 

KF 와 비교하며 어떤게 달라졌는지 확인해봅시다. 

2: 에서 선형인 A,B를 EKF에서는 g로 표현함을 알 수 있습니다.

5: 에서의 식도 동일합니다.

여기서 3:과 4에서 G,H를 확인할 수 있는데

G는 비선형 함수 g에 대한 이전 시점 상태 평균 (t-1)에서의 jacobian 이고,

H는 비선형 함수 h에 대한 이번 시점(t) 상태의 prediction 평균에서의 jacobian입니다.

또한 해당 수식에서의 g,h 선형화는 아래와 같습니다.

현재 가지고 있는 값에 대한 함수값과 기울기를 가지고 인접한 부분에 대한 함수값을 

해당 점 근처에서 유효한 직선의 방정식을 통해 구하는 것이 선형화입니다.

*여기서 헷갈릴 수 있는데 비선형이라고 해서 unknown인 시스템인게 아니라 단지 다루기 힘든 시스템이라 선형화를 진행하는 것입니다. 알고있는 함수에 대입하여 Taylor의 0차를 구합니다 ㅎ ( 전 헷갈렸었음 ) 

요렇게 해서 EKF 를 전부 확인해보았습니다. 추가로 시간이 된다면 code에서의 사용 , sensor fusion에서 EKF를 사용하는 이유 등을 알아보려고 합니다 . 감사합니다. 

오류와 수정사항 환영합니다.

EKF 를 공부하기 위해서는 기본적인 KF에 대한 지식이 당연히 필요하겠죠 ? 

오늘은 kalman filter에 대해서 간단하게 review하는 글을 써보겠습니다.

참고자료 

-  제어공학 수업자료

-  Probabilistic Robotics

Kalman filter는 선형 가우시안 시스템의 연속 상태 공간에서 사후 확률 분포 (Posteriors)를 정확하게 계산하기 위한 필터입니다.

말이 어려울 수 있는데, 아래의 3가지 조건을 만족하면 우리가 알고자하는 시스템의 state의 확률 분포를 알 수 있다는 말입니다.

1)  상태 전이 시스템이 선형

2)  관측 시스템이 선형 

3)  초기 상태 분포가 가우시안 + 관측, 상태 noise가 가우시안이라는 전제 

앞의 3가지 전제를 만족하면,

모든 시간 t에서 belief( = Posteriors , t시간에서의 state의 확률 분포)는 가우시안으로 생각할 수 있고

따라서 재귀적인 방법으로 시간 t에서의 평균과 공분산을 계속하여 구함으로써 

칼만 필터는 우리가 알고자하는 state의 확률 분포를 추정합니다.

다시 한번 위의 각 요소에 대하여 다시 확인하며 넘어가겠습니당

쉽게 설명해보자.

우선 기본적으로 시스템에 대해서 , 선형인 전제를 하는 것이 1) 2) 이다. 당연하게도 시스템이 선형이면 해당 시스템을 해석하기가 매우 간단하다.

상태공간 방정식에서 X가 포함된 상을 이산적으로 표현한 식이 여기서 말하는 1) 상태 전이 방정식이고, Y가 포함된 식이 2) 관측 시스템이다. 생각해보면 Y는 output이라고 배웠고, x는 state라고 배웠다. 당연한 사실이다. 

* 위에서는 x에 대한 미분으로 표현되어있지만, 실제 시스템에서 sampling된 time으로 생각하면 xn - xn-1/ td = x dot임을 알 수 있고 넘기면 아래와 같은 수식으로 표현 가능하다.

현실 시스템의 sensor와 측정에서는 당연하게도 noise가 있고, 위 식은 해당 사항을 반영하지 않고 있다 . 이를 반영한 것이 아래의 1),2) 수식이다. 

1에서 noise를 제외한 부분은 기존 상공방과 동일하게 state와 제어기 입력이다. 

여기서 나오는 noise는 제어기 입력으로 인해 생기는 noise ( input noise ) 이다.

당연하게도, 우리가 실제로 입력한 제어기 입력과 다르게 조금은 다른 값이 input으로 들어갈 것이기에 해당 noise를 가정한다.

kalman filter에서는 해당 noise가 평균이 0이고, 공분산이 Q인 가우시안을 따른다고 가정하고, input noise라고 한다.

동일하게 아래 식 2에서 있는 delta는 measurement noise라고 부르며 센서 측정에 있어서 나오는 실제 y와 측정 y간의 간격을 noise로 표현한다. 동일하게 평균이 0이고 공분산이 R인 가우시안을 따른다고 가정한다.

* 또한 추가적으로 t = 0일때의 초기 변수에 대한 확률분포도 가우시안 분포를 따른다고 가정한다.

앞서 말했다시피 해당 조건을 만족하면 모든 시점 t에 대해서 Posteriors 사후 확률분포는 가우시안 분포를 만족합니다 여기서, 

가우시안 분포는 평균mean과 공분산covariance로  완전히 표현되므로
이 최소 파라미터들을 구하는 방식으로 확률 분포 전체를 계산하는 것을 대체한다.

이렇게 재귀적인 방법으로 매 시간 t에 대한 상태변수의 mean과 covariance를 구하는 것이 kalman filter인 것이다.

물론 실제 시스템에서 noise나 초기 상태가 가우시안이라는 전제 등 다양한 전제를 사용하여 실제 값과 오차가 있을 수 있지만 , 이러한 방식으로 다양한 시스템에서 효과적인 상태 추정을 하게 도와줍니다 ㅎㅎ

이제 칼만 필터가 어떻게 재귀적으로 상태변수의 mean 과 covariance를 업데이트하는지 알아보겠습니다.

칼만 필터는 크게 2번의 step으로 최종적인 공분산과 평균을 구합니다. 

첫번째는 prediction입니다.

prediction은 말 그대로 한 스탭 전의 상태변수의 평균과 공분산을 사용해, 이미 알고있는 시스템 식에 적용해 어떤 값이 나올지 예상하는 것입니다. 

이미 알고 있는 모델로 미래를 예측 !

이는 table 3.1의 2 : 식으로 표현되며 식을 자세히 살펴보면 단순히 시스템 식에서 상태변수 대신 , 이전 스탭의 평균이 들어간 것을 확인할 수 있습니다. 여기서 제어기 입력은 우리가 의도한 값이 들어간다고 가정하기에 t시간에서의 ut를 그대로 사용합니다.

이어서 식 3: 을 통해서 prediction error에 대한 공분산을 계산합니다. (수학적인 유도는 생략)

이로써 prediction이 종료되고 , 결과론적으로 t-1의 평균과

error에 대한 공분산을 사용해서 시스템 모델과 제어기 입력값을 통해 t시점에서의 

평균과 공분산을 구했음을 알 수 있습니다.

그 다음은 Correction ( Measurement Update ) 입니다.

prediction이 모델의 정보를 통해 미래를 예측했다면, Correction은 해당 값을 참조하여 최종적으로 real value를 추정하는 과정입니다. 

5: 의 식을 주목하여 보면 식의 형태가 Observer의 수식과 굉장히 유사함을 확인할  수 있습니다.

측정 output인 zt를 prediction 된 값을 사용해 최적화된 Observer gain, 즉 Kalman gain이라고 불리는 4번 식을 통하여

이 전에 prediction했던 t시간에서의 평균을 correction합니다. 

이는 이번에 측정한 z값을 얼마나 믿을건지 ? 를 Kalman gain을 참조하여 본다고 보면 됩니다.

결과적으로 모델을 통해 예측한 값과 실제로 측정된 값을 합쳐 noise를 제거한 실제 값을 추정하는 과정 이라고 보면 됩니다.

즉, Kalman filter의 correction step은
확률적으로 최적화된 observer를 사용하는 상태 추정 과정이라고 볼 수 있습니다.

6: 식도 Kalman gain을 사용하여 error에 대한 공분산을 계산합니다.

이렇게 최종적으로 시간 t에서의 평균과 공분산을 구하고 해당 파라미터들은 다음 time step에서의 값들로 재귀적으로 사용됩니다.

해당 과정에서의 증명은 생략하였지만 찾아보면 재미있습니다 ㅎㅎ

또한 correction에서 하는 과정이 이해가 가지 않으면 observer에 대한 기초 개념을 한번 더 리뷰하면 좋을 것 같습니다.

다음 글에서는 이러한 선형 가정을 완화하여,
비선형 시스템에서도 Kalman filter를 적용할 수 있도록 확장한 EKF에 대해 정리해보겠습니다.

오류나 지적 환영합니다 감사합니다

EKF를 공부하기 전 필요한 배경지식들을 정리하고 넘어가고자 합니다.

1. covariance matrix (공분산 행렬)

먼저 공분산의 정의를 짚고 넘어가고자 합니다.

공분산(共分散, 영어: covariance)은 2개의 확률변수의 선형 관계를 나타내는 값이다

수학적인 정의는 아래와 같습니다.

스칼라 값을 가지는 두 확률변수 X,Y에 대해 상관관계를 파악하기 위해 공분산을 사용하고,

공분산이 양수인 경우 확률변수 X가 커지면  Y도 커지는 양의 상관관계를

공분산이 음수인 경우 확률변수 X가 커짐에도 Y는 작아지는 음의 상관관계를

공분산이 0이면 확률변수 X의 변화와 Y의 변화는 양도, 음도 아닌 관계를 가짐을 의미합니다.

(공분산은 선형 관계를 말하는 것임으로 공분산이 0임이 관계가 없음을 의미하는 것이 아님 !, 대표적인 반례로 원의 방정식을 예로 들 수 있다) 

추가로 기댓값의 성질을 사용하면 수식을 아래와 같이 변형할 수 있습니다.

여기서 잠깐  !

보통 분산이라고 하면 , 확률분포가 얼마나 퍼져있나 ? 를 상상하게 되는데요

공분산은 두 변수간의 상관관계다 라는 정의가 분산을 먼저 배우고 , 공분산을 배운 사람은 이해가 잘 되지 않을 수 있습니다.

여기서 분산은 공분산의 special한 case라는 점을 확인할 수 있습니다. 

공분산 -> 함께 얼마나 변하는가 ?

분산 -> 혼자 얼마나 변하는가 ?  - 한 확률변수가 평균을 중심으로 얼마나 퍼져있는가 ? 

라고 생각하면서 이해를 할 수 있었습니다.

다시 돌아와서 ... covariance matrix (공분산 행렬) 이란

우리는 행렬과 벡터를 학습함으로써 단순 스칼라에서 행렬을 통해 더 고차원의 것을 표현하는 방법을 배우는데요, 이 또한 해당 맥락으로 이해할 수 있습니다.

단순히 스칼라로 표현되던 확률 변수를 , 1차원이 아닌 n차원으로 표현되는 vector로 표현할 수 있고이는 기존 공분산 또한 행렬로 계산될 수 있음을 말해줍니다. (스칼라 확률변수를 벡터 확률변수로 확장)

여기서 보다시피 벡터 확률변수 안에는 여러 확률 변수가 들어있고 , 이 안의 각각의 변수에 대한 상관관계가 궁금할 수 있습니다. 이를 표현한 것이 공분산 행렬입니다.

확률 이론 및 통계학에서 공분산 행렬 ( 자기공분산 행렬 , 분산 행렬, 분산 -공분산 행렬이라고도 함 ) 은 주어진 확률 벡터 의 각 요소 쌍 사이의 공분산을 나타내는 정사각 행렬입니다 .

정의에 따라 정리하면, 대각성분은 각 확률변수의 자기자신에 대한 공분산 ( 분산 ) 이고, 그 외 성분은 다른 성분들과의 공분산입니다. 

이에 대한 성질로 

- 대칭행렬 ( 당연하죵 )

- 양의 준정부호행렬 ( 설명하려면 조금 길거같아 나중으로 미룬다 , 양의 준정부호행렬은 행렬의 고유값이 모두 >=0 인 행렬을 말하고, 대칭행렬임을 이용하면 유도할 수 있다! )

2. Jacobian 

로봇공학 전공에게 익숙한 jacobian이다.

jacobian의 정의는 특정 변수 x가 변할 때, y값은 얼마나 변함 ? 을 편미분으로 표현한 행렬이고 아래와 같다. 

https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant

함수 f가 n->m을 매핑하는 함수로써 변수로 n개의 state를 가지고 , m개의 결과값을 가진다고 생각하면 된다.

n개의 state 각각이 m개의 결과 각각에 어떤 영향을 미치는지를 표현한 행렬이다. 

(로봇으로 예를 들면 , 6개 joint 각각의 theta가 Cartesian space에서 end effector의 position ( x, y, z, roll , pitch, raw ) 6개 변수를 어떻게 변화시키는지를 생각해보면 직관적으로 이해가 된다. )

3. Gaussian

고등학교 때 많이들 배우는 종모양의 그놈이다.  다른 말로는 정규분포이다. 

KF에서 다루는 확률은 가우시안의 평균과 공분산을 활용하여 다룬다 . 차이가 있다면

우리가 고등학교 시절에 배운 아래와 같은 그래프를 가지는 표준정규분포, 가우시안은 앞서 언급한 것처럼 1D 즉 , 스칼라 확률변수를 가지는 가우시안이고 앞으로 다룰 가우시안은 벡터 확률변수의 가우시안이다. 

상태를 결정론적인 관점에서 어떠한 값이 존재한다고 보는 것이 아닌 확률을 가진 확률변수로써 바라보고 ,

이러한 확률변수가 확률을 가지는 분포를 표현한 확률밀도함수(PDF)를

가우시안 분포 - 정규분포로 가정하여

해당 분포의 대표값인 평균( mean ) , 공분산 (covariance) 으로 해석한다.

* 가우시안의 확률밀도함수는 평균(mean)을 중심으로 퍼져있으며, covariance로 퍼진 크기와 방향을 표현한다.

고 생각하면 될 듯하다.

이렇게 g선생이 말하주는 EKF를 공부하기 위한 사전지식들을 정리해보았습니다. 알고 있었지만 가물가물한 내용도 있었는데 다시 보니 기분이 좋네요. 그럼 다음 포스팅에 본격적인 EKF 리뷰를 해보도록 하겠습니다.

오류 , 수정사항 지적 환영합니다 ~