Pose Estimation Methods Overview and Techniques

This content delves into various pose estimation methods, including the problem definition, linear techniques, iterative techniques, comparisons, implementation issues, and more. It explores the concepts of pose estimation, its applications, and different techniques such as POSIT, Epnp, P3P, P4P, and P7P. The content covers the basics of pose, defining what it is and how it relates to object coordinates, camera coordinates, and image projections. With detailed explanations and images, it provides insights into the algorithms and computations involved in pose estimation.

• Pose Estimation
• Techniques
• Problem Definition
• Linear Methods
• Iterative Techniques

quintav Follow

Uploaded on Mar 04, 2025 | 1 Views

Download Presentation

Please find below an Image/Link to download the presentation.

The content on the website is provided AS IS for your information and personal use only. It may not be sold, licensed, or shared on other websites without obtaining consent from the author.If you encounter any issues during the download, it is possible that the publisher has removed the file from their server.

You are allowed to download the files provided on this website for personal or commercial use, subject to the condition that they are used lawfully. All files are the property of their respective owners.

The content on the website is provided AS IS for your information and personal use only. It may not be sold, licensed, or shared on other websites without obtaining consent from the author.

Presentation Transcript

1. Pose estimation methods By KH Wong Pose estimation methods v01.b 1

2. Overview Introduction Problem definition Linear techniques Iterative techniques Comparisons Implementation issues Conclusion Pose estimation methods v01.b 2

3. Introduction What is Pose estimation? What is pose estimation used for? Pose estimation methods v01.b 3

4. Problem definition What is pose? Pose estimation methods v01.b 4

5. Pose estimation techniques POSIT Epnp (solve pnp in opencv) P3P P4P P7P Linear methods P3P Power4 methods Pose estimation methods v01.b 5

6. POSIT are M ,.., M n 1 M , features 3D n on the object, M centroid the is o 0 T i i i i R i j k u v w u u u = = = = = = = = T j R , , , j j j R R i i R j j R k k u v w i v j v k v s T k k k k R i j k u v w w w w th Object corrdinate ( : s is ) the point object in corrdinate U ,W object in ) ,V i i i i = M is o (M M M corrdinate s, M object center , plane , ou ov ow o = G image th Camera corrdinate : s (X )is the point camera in corrdinate s ,Y ,Z i i i i x 0 = = O camera center y 0 = G imageplane = K plane the parallel to K z , from along O z axis 0 = Pi orhograhic point 3D (orthograh projection ic plane to K x i = = perspectiv image e point of Mi , m i y i ' ' x i = = perspectiv image e point of Pi p i y i f = = + MoMi ' 1 ( ) ( ) i x x x i 0 i i i z 0 f = = + MoMi ' 1 ( ) ( ) j y y y ii 0 i i i z 0 1 = = MoMi where , k k i j i z 0 Pose estimation methods v01.b 6

7. POSIT f f correct, are used terms If the we can find from = ' x + x = = Assume , put , into (i) , (ii) i I j J i i i z = z = orginally We assume 0, we now have ' x 1 ( i x ) , x 0 0 i 0 i i = + = MoMi ' 1 ( y ) , I x x x stable. is i improved the find can we repeat the , operation until 0 y i i i i i = = + = MoMi ' 1 ( i ) J y 0 i i i stack T = = , U V W I I I U V W I i i i u v w i i i i T = = , U V W J J J U V W J i i i u v w i i i i points 3D become to A, correspond 2D and x AI = become to ences x',y' = ' ' AJ y ( ( ) ) 1 = = T T ' ' I Bx A A A x 1 = = T T ' ' J By A A A y Pose estimation methods v01.b 7

8. POSIT Algorithm 1; = = = Step1 : (0) (i 0, 1...N - 1_, n i Step2 begining : of loop Compute i, j, Zo : + - compute the image vector x' with corrdinate 1 - N s of teh form 1 , x ( ) x + y 1 0 i i(n ) + and the image vector with y' coordinate 1 - N s of the form 1 , y ( ) + 1 0 i i(n ) Multiply t 3x(N he - coordinate 1 - (N vectors image the and B matrix object 1) s) - Compute = the scale = s of [rojection the = avaerage the as betwwen th norms e of I and s : J + 1/2 1/2 s (I I) s , (J J) s , (s s = )/2 1 2 1 2 = Compute unit vecto rs i and i : j I/s j , 1 J/s 2 Step3 Compute : new : i compute unit vecto r k as the cross - product of i and j; = = compute the z - coordinate Z0 of translati the on vector as Zo f/s, f focal length cunit vect or k as the cross - product of i and j; 1 = = compute MoMi k; i(n) z 0 = + step4 If threshold n , n Goto 1; step2 i(n) i(n) 1 - step5 iteration last at found values using pose output Else : : = tramnslat full the ion vector OMo is OMo = OMo/s; rotation the matrix has row vectors i, j, k make sure i, j, unit vecto are k and rs k i j Pose estimation methods v01.b 8

9. POSIT Because the camera model is SOP scaled orthographic, it is an approximated camera POSIT is based on POS but add a few iteration to improve the result. Pose estimation methods v01.b 9

10. = estimate to ) overview An model 3D : points : , 2 , 1 ,.., Based . n on the barycentri coord. c sys., P i i EPnP: Efficient Perspective-n- Point Camera Pose Estimation control 4 use can we points 3D ( feature 3D any point ( space in ) c p = control 3D Four points 3D : , 1 ,., 4 c j j 4 4 = j = j = = = w w j w j th , with , 1 control point in world coordinate s P c c j i ij ij 1 1 4 = j = = c c j c j th , control point camera in coordinate s. P c c j i ij 1 between th pose the find to is Our task world e camera and coordinate We s. choose [1] Barycentric coordinate system https://en.wikipedi a.org/wiki/Barycen tric_coordinate_sys tem [2] Vincent Lepetit Francesc Moreno- Noguer Pascal Fua, "EPnP: An Accurate O(n) Solution to the PnP Problem", International Journal of Computer Vision , February 2009 control one is center model point, other aligned with principal directions of data. = homogeneou barycentri s coordinate c estimated be can s [1] ij u U 4 = j i i = = = c c j 2D case i, , where therefore , w AP A c U i i ij i 1 v 1 i c j u f u x i u cox u 4 = j cox = c j 3D case i, , where optical the is center w v f v y i i v coy ij v coy 1 c j 1 1 z = th Then, for each feature, 1 totally features, n rearrange the above formulas Control point j i i ,..,n, 4 ( ) = j + = c j c j 0 f x u u z World corrd. sys ij u ij cox i 1 ( ) 4 = j MX Camera Coord. sys + = c j c j 0 f y v v z ij v ij coy i 1 = = , 0 solved be can this by i.e. , SVD (u,s,v) SVD(M) T T T T T = c c c c a is 12 column last the is vector, 1 of . v X c c c c 1 2 3 4 a is M 2n measuremen are entries matrix, 12 find ts, X 10 4 = j Pose estimation methods v01.b = Solution is X v i i 1

11. Application of Epnp (solvepnp in opencv Aruco pose tracker https://docs.opencv.org/3.4.0/d5/dae/tutorial_aruco_detection.html From //https://github.com/njanirudh/Aruco_tracke r Download project.zip , unzip to a dir. e.g. \Aruco_Tracker-master Copy Aruco_Tracker- master\calib_images\checkerboard\*.jpg to Aruco_Tracker-master\calib_images change line 6 of aruco_tracker.py to : cap = cv2.VideoCapture(0). Run it conda>>pythonaruco_tracker.py Print the 6x6 aruco marker (in \Aruco_Tracker- master\images\marker_66.jpg) and show to the camera, it will be tracked https://youtu.be/IsXWrcB_Hvs?t=6 https://www.youtube.com/wat ch?v=sV7vOTvUCx8 Stem4: Arduino and Computer vision, v.0.b2 11

12. Barycentric coordinate system https://en.wikipedia.org/wiki/Barycentric_coordinate_system [1] Any 3D world feature (or called model) point Piw ( ) can be represented by N(e.g. N=4) 3D world control points ( ) Cjw=1,2,3,4 using the formula below 4 4 w j ij Ia c = = = = w ( , ) with , 1 the same point camera in coord. is P i ij 1 1 j j 4 = = c c j c ( where ), control the is j points camera in coord. P c Ib c i ij 1 j Barycentri same the share systems camera) (world, Both control c param. ij =3D feature point Pwi=1,2,3,4 in world coord. =3D Control point Cjw=1,2,3,4in world coord. World Coord. sys Our pose estimation task is to find Rotation (R), Translation (T) between the world and camera coord. Systems if model points (Piw) and their image points Ui=[u,v]iT are given Choose the center of the model points as one of the control points Camera Coord.sys R T ) 1 3 ( ) 3 3 ( = = c j w j , H P HP ( 4 ) 4 0 1 12 Pose estimation methods v01.b

13. The Barycentric coordinate parameter calibration If you are given Pwi=1,2,..,n and cwi=1,2,3,4 you may recover ij 4 = j = w w j definition By homogenous using , shown with corrd. dimension P c i ij 1 ( ) ( ) T T = = w w x w y w z w j w x w y w z 1 and , 1 P P P P c c c c = = = = , , , 1 , 1 , 1 , 1 i i i i j j j ) 1 ) 1 ( 4 ( 4 w x w x w x w x w x P c c c c = = , 1 , 1 i j i j = = = = , , 1 , 2 , 3 , 4 i j j j j w y w y w y w y w y P c c c c = = , 2 , 2 i j i j = = = = = = = w w j w j w j w j , , 1 , 2 , 3 , 4 i j j j j P c c c c = = = = 1 2 3 4 i ) 4 w z w z w z w z w z ( 4 P c c c c = = , 3 , 3 i j i j = = = = , , 1 , 2 , 3 , 4 i j j j j 1 1 1 1 1 = = , 4 , 4 i j i j ) 1 ) 4 ) 1 ) 1 ( 4 ( 4 1 ( ( 4 w For single a feature (model) point P i 1 w x w x w x w x w x c c c c P = , 1 i j = = = = , 1 , 2 , 3 , 4 , j j j j i w y w y w y w y w y c c c c P = , 2 i j = = = = = , 1 , 2 , 3 , 4 , j j j j i , w z w z w z w z w z c c c c P = , 3 i j = = = = , 1 , 2 , 3 , 4 , j j j j i 1 1 1 1 1 = , 4 i j Pose estimation methods v01.b 13

14. Barycentric coordinate parameter calibration If you are given Pwi=1,2,..,n and cwi=1,2,3,4 you may recover all ij point (model) feature single a For i P = w 1 w x w x w x w x w x c c c c P = , 1 i j = = = = , 1 , 2 , 3 , 4 , j j j j i w y w y w y w y w y c c c c P = , 2 i j = = = = , 1 , 2 , 3 , 4 , j j j j i , w z w z w z w z w z c c c c P = , 3 i j = = = = , 1 , 2 , 3 , 4 , j j j j i 1 1 1 1 1 = , 4 i j = w feature all For points ( 1 2 , ) and i ,...,n P 2 , 1 = ,..., i n w j also four can be all recovered be can c = 2 , 1 = = 1 2 , 3 , 4 , ,..., 4 , 3 , 2 , 1 i n,j , 1 = = , 2 = = = = 1 1 , 1 i j i j i n j , 1 = = , 2 = = = = 2 2 , 2 i j i j i n j = ... Pose estimation methods v01.b , 1 = = , 2 = = = = 3 3 , 3 i j i j i n j , 1 = = , 2 = = = = 4 4 , 4 i j i j i n j ( 4 ) n 1 w x w x w x w x w x w x w x c c c c P P P = = = = = = = , 1 , 2 , 3 , 4 , 1 , 2 , j j j j i i i n w y w y w y w y w y w y w y c c c c P P P = = = = = = = , 1 , 2 , 3 , 4 , 1 , 2 , j j j j i i i n ... w z w z w z w z w z w z w z c c c c P P P = = = = = = = , 1 , 2 , 3 , 4 , 1 , 2 , j j j j i i i n 1 1 1 1 1 1 1 14 ( 4 ) n ( 4 4 )

15. EPnP problem setting = ,.., 2 , 1 , : coord. in world points model 3D w . P i n i = w Based on the Barycentri coord. c sys., represente be can d by ( control 4) P N i w j w points 3D ( estimate to ) feature 3D any point ( space, in ) i.e. c P = 4 , 3 , 2 , 1 i 4 4 = = = w w j w j th , with , 1 control point in world coordinate s P c c j i ij ij = = 1 1 j j f u _ u c ox = = w T i Also, image 2D of [ is , ] . camera matrix P u v A f v _ i v c oy 1 4 = = c c j Using Barycentri coord., c feature 3D point camera in coord. , P c i ij = 1 j = c j th where found be (to coord. cam. in point control 3D Epnp) by c j Epnp algorithm will achive this : w = 2 , 1 T i w j Input : [ , , ] , selected by user, P u v A,c = = ,.., 2 , 1 ,.., 4 , 3 , 2 , 1 i n n R T ) 1 3 ( ) 3 3 ( = = w j c j c j w j Output find : c then from (Ib) finally , since , P H P HP = 4 , 3 , 2 , 1 ( 4 ) 4 0 1 Pose estimation methods v01.b 15

16. EPnP basic formulation Our task is to find the pose between th (R,T) world e camera and coordinate s. Choose the model center control a as point, other aligned with principal = directions of data generate will good result estimated be can coord. c barycentri s homogeneou previous (see slides) [1] ij 2 , 1 = . ) 4 , 3 , 2 , 1 = = 2D case ( i, ,.., and n camera = intrinsic parameters i j A f u _ u c ox u U 4 = i i = = = c c j , where , , then w AP A c U A f v _ i i ij i v c oy 1 v 1 j i 1 c j u f u x _ i u c ox 4 = = c j , ( ) w v f v y II _ i i v c oy ij 1 j c j 1 1 z u _ c ox where optical the is center, , found be can camera by calibratio n f f u v v _ c oy Pose estimation methods v01.b 16

17. EPnP : Our plan to find the pose between the camera and world: find Pcj=1,2,..,n, then compute Rotation (R(3x3)), Translation(T(3x1)) rearrange , 4 3 2 1 & 1 features , eq.(I) For , , , j ,..,n, i = = terms is i gone, and w ( ) 4 + = c j c j 0 ( ) f x u u z III _ ij u ij c ox i = 1 j ( ) 4 + = c j c j 0 ( ), f y v v z IV Pose estimation methods v01.b _ ij v ij c oy i = 1 j Combining ( ) & ( ), we have I II 4 4 + + + = (V) c j c j c j c j c j c j 0 f x f y u z u z v z v z _ _ ij u ij v ij c ox ij i ij c oy ij i = = 1 1 j j In , Barycentri by found are param. c calibratio discussed n earlier, (IV) ij c j x c j y = c j c , , , are cam. param. From find , ( ) f f u u (V) c from Ib find P _ _ u v c ox c oy i c j z 1 R T ( ) 1 - ) 3 ) 1 3 ( 3 ( = = = w j c j w j c j w j Since is given, and then , find ! P P HP H P P done ) 4 ( 4 0 1 17

18. EPnP continue to find Pcj=1,2,3,4 = For each feature , i since 1 2 , 3 , , 4 , equation 4 are there so from j (IV) 4 4 + + + = c j c j c j c j c j c j 0 f x f y u z u z v z v z = = = = = = 1 1 _ 1 1 _ 1 1 ij u ij v ij c ox ij i ij c oy ij i = = 1 1 j j 4 4 + + + = c j c j c j c j c j c j 0 f x f y u z u z v z v z = = = = = = 2 2 _ 2 2 _ 2 2 ij u ij v ij c ox ij i ij c oy ij i = = 1 1 j j 4 4 + + + = c j c j c j c j c j c j 0 f x f y u z u z v z v z = = = = = = 3 3 _ 3 3 _ 3 3 ij u ij v ij c ox ij i ij c oy ij i = = 1 1 j j 4 4 + + + = c j c j c j c j c j c j 0 f x f y u z u z v z v z = = = = = = 4 4 _ 4 4 _ 4 4 ij u ij v ij c ox ij i ij c oy ij i = = 1 1 j j = equations 4 above the Formula matrix the into form : , 0 M X 12 ) 1 6 ( ) 12 ( T T T T T = c c c c where making or , more it explicit X c c c c ) 1 12 ( 1 2 3 4 T = c j c j c j c j c j c j c j c j c j c j c j c j , , , , , , , , , , , ( ) X x y z x y z x y z x = y z V ) 1 = = = = = = = = = = = = 12 ( 1 1 1 2 2 (U 2 3 3 3 4 4 4 solved be can This by i.e. , = , , , SVD S V ) SVD(M ) ) 6 12 12 6 ( 6 ( ) 12 ( 12 V ) 6 ( ) ( ) = simple A solution is column last of with _ X scale factor 1 1 solution c i c From recover can we c , n ( ) done! X from I find P 2 , 1 = ,.., solution i 18 Pose estimation methods v01.b

19. EPnP continue to find Pcj=1,2,3,4 = For each feature , i since 1 2 , 3 , , 4 , equation 4 are there so from j (IV) T = c j c j c j c j c j c j c j c j c j c j c j c j , , , , , , , , , , , 0 M x y z x y z x y z x y z 12 = = = = = = = = = = M = = 6 ( ) 1 1 1 2 2 2 3 3 3 4 4 4 = equations 4 above the Formula matrix the into form : , 0 X 12 ) 1 6 ( ) 12 ( exercise. an as M Write 19 Pose estimation methods v01.b

20. EPnP : what to do next after V is found general In = 4 N = = Solution is X v i i 1 j ? T T T T T = c 1 c 2 c 3 c 4 where X c c c c ) 1 12 ( reprojecti use will We How to find i error to on find the best set of i Pose estimation methods v01.b 20

21. How to recover ? Case: N=1 = 4 N = = = = Solution is , N 1 so X v X v i i 1 j column last the is v = of from V SVD(M) T ] 1 = = = = [ [ 2 ] [ 3 ] [ 4 ] j j j j v v v v v ) 3 ) 3 ) 3 ) 3 12 ( ) 1 1 ( 1 ( 1 ( 1 ( T T T T T = c c c c where X c c c c ) 1 12 ( 1 2 3 4 4 ; 1 = = Distance between control 2 points e.g. : 1 2 , where { , } [a ], [b ] a b 2 2 ] 1 = ] 2 = = [ [ a b w a w b camera in coord , or world cord. v v c c ] 1 = ] 2 = [ [ should remain unchanged, hence. 2 2 ] 1 = ] 2 = = [ [ a b w a w b v v c c ] 1 = ] 2 = [ [ w a w b since distances 6 and known, is among control 4 points c c Hence scaling the factor is [ ] [ ] a b w a w b v v c c 4 ; 1 { , } a b = ( ) VI 2 [ ] [ ] a b v v 4 ; 1 { , } a b Pose estimation methods v01.b 21

22. How to recover ? Case: N=2 v = + Solution v v is X v 1 1 2 2 , columns 2 last the are = of = from find , , scale factors V SVD(M) 1 2 1 2 = 2 2 2 Let , Preserve . length the between control 2 pts. 1 11 1 2 12 22 ( ) ( ) ) 2 2 ] 1 = ] 1 = ] 2 = ] 2 = + + = [ 1 [ 2 [ 1 [ 2 a a b b w a w b v v v v c c ] 1 = ] 2 = 1 2 1 2 [ [ ( ( ( )( ) ( + ) 2 2 ] 1 = ] 1 = ] 1 = ] 1 = ] 2 = ] 2 = ] 2 = ] 2 = = + + + + [ 1 [ 2 [ 1 [ 2 [ 1 [ 2 [ 1 [ 2 a a a a b b b b v v v v v v v v 1 2 1 2 1 2 1 2 ) = 2 2 ] 1 = ] 1 = ] 1 = ] 1 = = + + 2 [ 1 [ 1 v [ 2 2 2 [ 2 a a a a 2 v v v v 1 v 1 2 ] 1 = ] 2 = ] 1 = ] 2 = ] 2 = ] 1 = ] 2 = [ 1 [ 1 + [ 2 [ 2 [ 2 ] 1 [ 1 [ 2 [ 2 a b a b a b a b v v v v v v 1 1 1 2 2 1 2 2 ( ) 2 2 ] 2 = ] 2 = ] 2 = ] 2 = + + 2 [ 1 [ 1 [ 2 2 2 [ 2 b b b b 2 v v v v 1 1 2 2 2 ] 1 = ] 1 = ] 1 = ] 1 = = + + [ 1 [ 1 [ 2 [ 2 a a a a 2 v v v v 11 12 22 ] 1 = ] 2 = ] 1 = ] 2 = ] 1 = ] 2 = ] 1 = ] 2 = [ 1 [ 1 [ 1 [ 2 [ 2 [ 1 [ 2 [ 2 a b a b a b a b v v v v v v v v 11 12 12 22 2 2 ] 2 = ] 2 = ] 2 = ] 2 = + + + [ 1 [ 1 [ 2 + [ 2 b b b b 2 v v v v 11 12 22 ( v ) ] 1 = 2 2 ] 1 = ] 1 = ] 2 = ] 2 = = + [ 1 a [ 1 [ 1 [ 1 a a b b v v v v 11 ( ) ] 1 = ] 1 = ] 1 = ] 2 = ] 2 = ] 2 = ] 2 = + [ 1 [ 2 [ 1 [ 2 [ 2 [ 1 [ 1 [ 2 a a b a b b b 2 2 v v v v v v v 12 ( ) 2 2 2 ] 1 = ] 1 = ] 2 = ] 2 = + + = [ 2 [ 2 [ 2 [ 2 a a b b w a w b ( ) v v v v c c VII ] 1 = ] 2 = 22 [ [ Pose estimation methods v01.b 22

23. How to recover ? Case: N=2(Continue) ] 2 [ 1 1 v v v ( v ) ] 2 = 2 2 ] 1 = ] 1 = ] 2 = = + + [ 1 a [ 1 [ a a b b Recall v v v v 11 ( ) ] 1 = ] 1 = ] 1 = ] 2 = ] 1 = ] 2 = ] 2 = + [ 1 [ 2 [ 1 [ 2 [ 2 [ 1 [ 1 [ 2 a a b a b b b 2 2 v v v v 12 ( ) 2 2 2 ] 1 = ] 1 = ] 2 = ] 2 = + + = [ 2 [ 2 [ 2 [ 2 a a b b w a w b ( ) v v v v c c VII ] 1 = ] 2 = 22 [ [ = This is for pt1 and the 2, linear system becomes , or LB 11 = ] 2 , 1 [ ] 2 , 1 [ 1 l ] 2 , 1 [ 2 l ] 2 , 1 [ 3 l where , 12 22 ( ( ( ) [ 2 2 2 ] 1 = ] 1 = ] 2 = ] 2 = = + ] 2 , 1 [ 1 l [ 1 [ 1 [ 1 [ 1 a a b b v v v v ) ] 1 = ] 1 = ] 1 = ] 2 = ] 1 = ] 2 = ] 2 = ] 2 = = + ] 2 , 1 [ 2 l [ 1 [ 2 [ 1 [ 2 [ 1 [ 1 [ 2 a a a b a b b b 2 2 v v v v v v v v ) 2 2 ] 1 = ] 1 = ] 2 = ] 2 = = + ] 2 , 1 [ 3 l [ 2 [ 2 [ 2 [ 2 a a b b v v v v ] 2 , 1 [ = = = distance between pt(a and 1) (b 2). Pose estimation methods v01.b 23

24. How to recover ? Case: N=2(Continue) ] 2 [ 1 1 v v v ( v ) ] 2 = 2 2 ] 1 = ] 1 = ] 2 = = + + [ 1 a [ 1 [ a a b b Recall v v v v 11 ( ) ] 1 = ] 1 = ] 1 = ] 2 = ] 1 = ] 2 = ] 2 = + [ 1 [ 2 [ 1 [ 2 [ 2 [ 1 [ 1 [ 2 a a b a b b b 2 2 v v v v 12 ( ) 2 2 2 ] 1 = ] 1 = ] 2 = ] 2 = + + = [ 2 [ 2 [ 2 [ 2 a a b b w a w b ( ) v v v v c c VII ] 1 = ] 2 = 22 [ [ This is for pt1 and the 2, linear system becomes 11 = ] 2 , 1 [ ] 2 , 1 [ 1 l ] 2 , 1 [ 2 l ] 2 , 1 [ 3 l ( where ), VIII 12 22 ( ( ( ) [ 2 2 2 ] 1 = ] 1 = ] 2 = ] 2 = = + ] 2 , 1 [ 1 l [ 1 [ 1 [ 1 [ 1 a a b b v v v v ) ] 1 = ] 1 = ] 1 = ] 2 = ] 1 = ] 2 = ] 2 = ] 2 = = + ] 2 , 1 [ 2 l [ 1 [ 2 [ 1 [ 2 [ 1 [ 1 [ 2 a a a b a b b b 2 2 v v v v v v v v ) 2 2 ] 1 = ] 1 = ] 2 = ] 2 = = + ] 2 , 1 [ 3 l [ 2 [ 2 [ 2 [ 2 a a b b v v v v ] 2 , 1 [ = = = distance between point (a and 1) (b 2). Pose estimation methods v01.b 24

25. How to recover ? Case: N=2(Continue) From 11 = ] 2 , 1 [ ] 2 , 1 [ 1 l ] 2 , 1 [ 2 l ] 2 , 1 [ 3 l ( ) VIII 12 22 control 6 all stack We l l point - pair cases [1,2], : [1,3], [1,4], [2,3], [2,4], [3,4] ] 2 , 1 [ 1 l ] 2 , 1 [ 2 l ] 2 , 1 [ 3 l ] 2 , 1 [ ] 2 , 1 [ ] 3 , 1 [ 1 ] 3 , 1 [ 2 l ] 3 , 1 [ 3 l 11 ] 2 , 1 [ ] 4 , 1 [ 1 l ] 4 , 1 [ 2 l ] 4 , 1 [ 3 l = ( ) IX 12 ] 2 , 1 [ ] 3 , 2 [ 1 l ] 3 , 2 [ 2 ] 3 , 2 [ 3 l 22 ] 2 , 1 [ ] 4 , 2 [ 1 l ] 4 , 2 [ 2 ] 4 , 2 [ 3 l l ] 2 , 1 [ ] 4 , 3 [ 1 l ] 4 , 3 [ 2 l ] 4 , 3 [ 3 l L = = , hence psudo_inve rse B B (L) ) 3 ) 1 ) 1 6 ( 3 ( 6 ( 11 = = = Solve for , then , B 12 1 11 2 22 22 ( ) = + [ 1 ] [ 2 ] c i i i According ref[2], to , c v v = 1 1 2 N = where is 1 = the scale factor for case 1 N N Pose estimation methods v01.b 25

26. Verify which case is better Use image projection to judge which case (N=1 or 2) can generate the best result with minimum re-projection error If the re-projection error is still large, Use case N=3 and 4 to find the scaling factors. Details can be found in [2] and the program in [3]. Pose estimation methods v01.b 26

27. Re-projection error https://en.wikipedia.org/wiki/Reprojection_error Pose estimation methods v01.b 27

28. Epnp references and program [1] Barycentric coordinate system https://en.wikipedia.org/wiki/Barycentric_coordinate_system [2] Vincent Lepetit Francesc Moreno-Noguer Pascal Fua, "EPnP: An Accurate O(n) Solution to the PnP Problem", International Journal of Computer Vision , February 2009 web-ink [3] https://www.epfl.ch/labs/cvlab/software/multi-view- stereo/epnp/ https://www.epfl.ch/labs/cvlab/wp- content/uploads/2018/08/EPnP_matlab.zip [4] Aruco tracker https://docs.opencv.org/trunk/d5/dae/tutorial_aruco_detection.ht ml Pose estimation methods v01.b 28

29. References POSIT: POSE estimation Daniel F. Dementhon,Larry S. Davis, Model Based Object Pose in 25 Lines of Code , http://home.in.tum.de/~grembowi/ar2004_05/3dPoseEstimation_presentation.pdfPOSIT, International Journal of Computer Vision June 1995, Volume 15, Issue 1 2, pp 123 141 the original paper Sebastian Grembowietz , 3D Pose Estimation http://home.in.tum.de/~grembowi/ar2004_05/3dPoseEstimation_elaboration.pdf a tutorial on pose estimation methods: 3pt , p4+p, POSIT, linear method, perspective p7p, Thomas Petersen , A comparison of 2D-3D Pose Estimation Methods Aalborg University 2008 http://www.haowuhw.com/pose/report.pdf a report on POS, POSIT Daniel Grest,ThomasPetersen,Volker Kr ger, A Comparison of Iterative 2D-3D Pose Estimation Methods for Real-Time Applications , Scandinavian Conference on Image Analysis, SCIA 2009: Image Analysis pp 706-715 Tutorial ppt Sebastian Grembowietz, Algorithms for Augmented Reality 3D Pose Estimation , 2004 nov 19, http://home.in.tum.de/~grembowi/ar2004_05/3dPoseEstimation_presentation.pdf Epnp Vincent Lepetit Francesc Moreno-Noguer Pascal Fua, "EPnP: An Accurate O(n) Solution to the PnP Problem", International Journal of Computer Vision , February 2009 https://pdfs.semanticscholar.org/6ed0/083ff42ac966a6f37710e0b5555b98fd7565.pdf Khwong Code http://www.cse.cuhk.edu.hk/%7Ekhwong/j2004_IEEE_chang_MM_xlowe_draft.pdf http://people.rennes.inria.fr/Eric.Marchand/pose-estimation/tutorial-pose-dementhon-opencv.html http://www.cfar.umd.edu/~daniel/Site_2/Code.html (posit matlab , c++ etc) Pose estimation methods v01.b 29

30. appendix https://math.stackexchange.com/questions/1 80418/calculate-rotation-matrix-to-align- vector-a-to-vector-b-in-3d https://stackoverflow.com/questions/5188876 /rotation-matrix-given-angle-and-point-in-x-y- z Pose estimation methods v01.b 30

31. Ref :Thomas Petersen , A comparison of 2D-3D Pose Estimation Methods Aalborg University 2008 http://www.haowuhw.com/pose/report.pdf a report on POS, POSIT POS,POSIT (alternative formulation) A simplified model , focal length =f, ox=oy=0 Kint= intrinsic parameters, Kext=extrinsic parameters, projection=Kint*kext = 1 32 31 R R R R R f R R R T 11 12 13 x = , , K f K R R R T x int 21 22 23 ext y 1 = + w / ' 1 T R R R y R R R T 31 32 33 z 33 z T z z T R R R 11 12 13 1 T x = = = T i where , where row a vector R R 21 22 23 2 T hence if we set large enough, that so T R R R y T R R R R 31 32 33 z z 31 32 33 3 z R 1 i x = = = = T i for , unit vecto r, R R R R R R R R R 1 = = + 2 1 2 3 3 1 2 i i i i i / ' 1 1 w w T R R R y 31 32 33 z T R z z 3 i x x = Using w , 1 and put / , hence s f T ' w u f R R R T z 11 12 13 x y y x = = ' w v K K f R R R T int 21 22 23 ext y z z sR sR sR sT u y ' 1 w R R R T 11 11 11 x = , 31 32 33 z 1 1 sR sR sR sT v z 1 21 22 23 y x ' w u fR fR fR fT 11 12 13 x Transpose both sides y = ' w v fR fR fR fT 21 22 23 y sR sR z 11 21 ' w R R R T 31 32 33 z sR sR 1 = 12 22 1 u v x y z sR sR Multiply both sides with 1/T , Then 13 23 z sT sT x x y ( ( ) ) / ' ( / ) ( / ) ( / ) w T u f T R f T R f T R fT 11 12 13 z z z z x y = / ' w ( / ) ( / ) ( / ) w T v f T R f T R f T R fT 21 22 23 z z z z y z Pose estimation methods v01.b / ' / 1 ( ) / 1 ( ) / 1 ( ) 1 T T R T R T R 31 31 32 33 z z z z 1

32. POS Algo sR sR 11 21 sR sR = 12 22 1 u v x y z sR sR 13 23 sT sT x y Stacking 1 x y z u u 0 0 0 0 0 sR sR 11 21 1 1 1 x y z u v 1 1 1 sR sR 12 22 = sR sR 13 23 sT sT x y 1 x y z u v n n n n n = AK b = = 3 model points, image 2D correspond ences A D b ( ) 1 = T T , after found, is = we can find , since K A A A b K R R R R 11 21 31 = = = , , where , unit vecto are rs, R R R R R R R R R R R 1 12 2 22 3 32 1 2 3 1 2 R R R 13 23 33 sR K K 11 11 11 = = , ' unit vecro The r is length the , of vector the is sR K K K' R K s 12 12 1 12 sR Similarly, = K we K 13 13 13 = can = find then , R R R R 2 3 1 2 / , / T K s T K s 41 42 x y Pose estimation methods v01.b 32