I am working on a project where I require to track aerial objects and calculate the six degree of freedom.
I am currently tracking colored balls , and calculating their center in rgb_frame, and using the center values to find the depth in the depth_frame.
After finding the depth(Z) in real-world co-ords,I am calculating the real-world X and Y using equations: X = (Zu)/fx and Y = (Zv)/fy , where fx,fy are the focal length obtained from the kinect's intrinsic params ,and u and v in this case are the center's x,y values.
I am treating (u,v) as image point and (X,Y,Z) as image point and feeding into this method: solvePnP
obj_pts = np.array([[X,Y,Z]],np.float64)
img_pts = np.array([[u,v]],np.float64)
ret,rvecs,tvecs = cv2.solvePnP(obj_pts,img_pts,camera_matrix2,np_dist_coefs)
I expect to find the rvecs which I will use as input for:
cv2.Rodrigues(rvecs)
to get the euler angles namely, pitch, yaw, roll.
I am presently having issues with the solvePnP call, which gives me the following error:
/opencv-3.0.0/modules/calib3d/src/solvepnp.cpp:61: error: (-215) npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) in function solvePnP
I also understand that sending just the center's object and image points is not recommended. By this is my first step towards the realization. I intend to use feature detectors like SIFT to make it more interesting later.
Can anyone please suggest on my approach and help me accomplish finding the six degrees of freedom:
forward/back, up/down, left/right, pitch, yaw, roll.
While my approach was exactly correct, I missed the fact that to calculate the orientation of a 3D object in real-coordinates we need atleast 4+ corresponding point sets in pixel and object coordinates.
I just had to check for contours in the image that had more than 4 points , and my code works as intended, and I am able to compute the 6-degrees of freedom with accuracy and also by now I have made it more sophisticated and soon I will be able to share a git link to my work.
I would still like to hear about, how to successfully test the correctness of orientation that I am able to compute using the rodrigues formulae.