SolvePNP Bad Result Planar Marker
Hello,
I'm working with solvePnp with some bad results for a planar marker. The marker is a basket ball court. So the model size is 14 x 15.
Marker is (-7,-7.5,0) to (7,7.5,0). (Should this be normalized to 1 for homogeneous coordinates??) The chessboard size was 6x9 with 0.025 square size. Chessboard and marker are in same units of measurement.
We have 3d Object points and 2d image points. The image is undistorted when when 2d points are detected so distCoeffs can be cv::Mat() from here on out. (i.e. no distortion)
The 2d points are for a marker that is of known size is 14m by 15m. The chessboard square size was also in meters (0.025m square size).
//Make sure 64bit
rvec = cv::Mat(3, 1, CV_64F);
tvec = cv::Mat(3, 1, CV_64F);
bool flag = cv::solvePnP(allImagePoints3d, allImagePoints, camMatrix64, cv::Mat(), rvec, tvec);
std::vector<cv::Point2d> projectedPoints;
cv::Mat J;
cv::projectPoints(allImagePoints3d, rvec, tvec, camMatrix64, cv::Mat(), projectedPoints, J);
cv::Mat Sigma = cv::Mat(J.t() * J, cv::Rect(0,0,6,6)).inv();
// Compute standard deviation
cv::Mat std_dev;
sqrt(Sigma.diag(), std_dev);
std::cout << "rvec1, tvec1 standard deviation:" << std::endl << std_dev << std::endl;
I then draw these projected points in red and original points in green.
Any ideas? Other posts have suggested that rvec and tvec should be 64bit.
Using CV_P3P gets three correct marker locations, but the final point is waay out. (CV_EPNP fails entirely)
Full model...
- Green line: marker points detected. Used for homography.
- Hollow circles: model points under homography transform (z=0 ignored).
- Full circles: projectedPoints following solvePnp.
solvePnpRansac
How many points do you use for solvePnP ?
If the matching between the 2D image points and the 3D object points are correct, one possibility is that the solution returned is a local minimum. What you could do is:
This is something to check in the litterature, the difference between the P3P and PnP methods is that the P3P will found 3 points over N with the pose that minimizes the reprojection error whereas the PnP will minimize the reprojection error over all the points.
@Eduardo Thank you for your reponse. The pose is computed with all points and with just 4 points. Both setups return the same result. The code works with chessboards and i can draw them with a 3d axis. How do i add a reasonable initial pose for rvec and tvec?
If you set the parameter useExtrinsicGuess to true for cv::solvePnP, it will use the rvec and tvec values as an initial approximations (only for SOLVEPNP_ITERATIVE).
I remember to have encountered the same situation where the points look good but the pose returned was upside-down and I supposed this was due to some local minima in the minimization process. I you have Matlab, you could try another method for the pose estimation with the same data to see if the result is similar (for example RPnP).
Unfortunately, I don't have much information about that.
Also, it looks like the second image is undistorted twice ?
No, the distortion is not applied twice. But i will investigate. I have heard of some 'mirror ambiguities' in other posts. http://stackoverflow.com/questions/32.... Both images are the same, i've just cropped the unused regions.
@Eduardo How do i reverse rvec? If this is a mirror ambiguity, i should be able to invert the rotation. I have tried Rodriquez to convert to a rotation matrix. This matrix is transposed and converted back to rvec via rodriques. It didn't work. How should i convert this?