Perspective n points

PnP problem

2D-3D간 correspondences를 구했다면 이를 통해 한측 데이터로 다른측 pose를 estimate한다. (6 degree of freedom)

- noise를 제거하기 위해 RANSAC이 필요하다.

cv::solvePnP(InputArray objectpoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=SOLVEPNP_ITERATIVE)

 

P3P Grunert

조건 

world coordinate points 3개, image coordinate 3개를 알아야한다., camera calibaration 완료

1. estimate length of projection rays

2. estimate the orientation

 

$s_{i}$: ray vector의 scale

$x^{s}_i$: ray vector의 unit vector

$X_{i}$: object 3d좌표

$X_{o}$: camera 3d좌표

$s_{i} ^{k}x_{i]^{s} = R(X_{i} - X_{o})$

$^{k}x_{i]^{s} = -sign(c)N(K^{-1}x_{i})$ (c=focal length, unit vector를 만들기 위한 normalize용 변)

vector의 방향을 object방향으로 바꾸기 위한 - 곱

 

algorithm

- s값 구하는 것

cos(θ) = $(X_{1} - X_{0}) · (X_{2} - X_{0}) /(|X_{1} - X_{0}||X_{2} - X_{0}|)$

이처럼 3좌표에 모두 구한다.

$\alpha = \arccos{(^{k}x_{2}^{s}, ^{k}x_{3}^{s})}$, $a = |X_{3} - X_{2}|$

$\beta = \arccos{(^{k}x_{3}^{s}, ^{k}x_{1}^{s})}$, $b = |X_{3} - X_{1}|$

$\gamma = \arccos{(^{k}x_{1}^{s}, ^{k}x_{2}^{s})}$, $c = |X_{2} - X_{1}|$

$c^{2} = a^{2} + b^{2} - 2abcos(θ)$ 를 3개의 삼각형에 적용하고 $u=s_{2}/s_{1}, v=s_{3}/s_{1}$로 치환하면

$a^{2} = s_{1}^{2}(u^{2} + v^{2} - 2uvcos\alpha)$로 정리되고, 한번더 정리하면$s_{1}^{2} = a^{2}/(u^{2} + v^{2} - 2uvcos\alpha)$로 정리된다.위의 cos법칙을 이식에 정리하면 아래와 같다.$s_{1}^{2} = a^{2}/(u^{2} + v^{2} - 2uvcos\alpha)$      $= b^{2}/(1 + v^{2} - 2uvcos\beta)$      $=c^{2}/(u^{2} + 1 - 2uvcos\gamma)$v에 대해 정리하고 치환하면 4차방정식으로 비교적 간단해진다.(복잡해서 생략)즉 4개의 solution이 있다. -> 즉 4개의 point가 필요하게 된다 (p3p이름값 못한다)이후 2가지 방식1. 다른 센서를 통해 방향성과 x1 x2 x3를 검증 -> 타 센서 검증으로 안 끌림2. 또다른 2d correspondense를 가져와 검증한다 -> only visual 방식이지만 4개의 point가 필요

최근 연구에는 4번째 point없이 기하학적 방식을 사용하기도 한다.