Planar object Homography calculation and Pose Estimation

Question

I'm trying to implement my own Homography and Pose estimation in OpenCV. Suppose I have a square image as a model which I want to locate in input frame from camera. My question is about how to prepare model data to calculate Homography.

I did the following procedure:

1-I extracted 3 corresponding set in both images.

 Input features:       Model Features:
p1_Input(278,278)---> p1_model(137,273)
p2_Input(317,235)---> p2_model(176,230)
p3_Input(217,204)---> p3_model(76,199)  //all in pixel

2- Solving P3P problem:

a) normalizing input points using camera Intrinsic parameters:

u.x=(p1_Input.x - cx) / fx 
u.y=(p1_Input.y - cy) / fy  
u.z=1

v.x=(p2_Input.x - cx) / fx 
v.y=(p2_Input.y - cy) / fy  
v.z=1

w.x=(p3_Input.x - cx) / fx 
w.y=(p3_Input.y - cy) / fy  
w.z=1

b) normalizing the lenght in order to have a unit vector:

d = sqrt(u.x*u.x + u.y*u.y + 1);
u.x=u.x/d;
u.y=u.y/d;
u.z=u.z/d;  // and same for v and w

c)Finding distance between u and camera focus.(same for v and w) by solving p3p and storing in a,b and c

d)computing 3D coordinates:

A_Input.x=a*u.x;
A_Input.y=a*u.y;
A_Input.z=a*u.z;  // same for B_input and C_Input

like:
A_Input:(-0.0899342 ,0.0570672 ,0.976046)
B_Input:(-0.0197703 ,-0.0194311 ,0.955101) 
C_Input:(-0.197233 ,-0.0746457 ,0.967379)

3-Computing Homography

My question is arising here. How should I prepare and modify p1_model,p2_model and p3_model to be prepared for Homography computing?

Obviously A_Input is 3D vector with normalized date while p1_model is 2D vector in pixel.

After solving the problem the rest would be as follows:

a)finding centeroid point for both set.

b)finding H using this formula dot prodoct

4- finding rigid transform using H and SVD

more detail is available here and here

Answer 1

As far as coordinate systems go, there is no specific "format" that you need to put your points in to get good results from a P3P (or PnP) algorithm, just be consistent. The solution the algorithm is looking for given a correspondence between p1 and p2 is

p2 = [resulting_3x4_transform]*p1;

for each correspondence. It may be advisable for numerical reasons to not have any coordinate values be very large or very small but other than that, feel free to put your points in any coordinate system you like.