Reputation: 683
I am trying to use pnp algorithm to compute the pose of a camera which has a low fov (15 degree). The calibrated intrinsic parameters(focal length and center location) match the expected values fairly well. For pnp, I have a set of six 3D points. Four of these points form a plane, while the other two are not on this plane. These 3D points are well separated in x, y values, but have less variation for z values. I have marked the corresponding locations in the image. These are well spread in the image. The coordinate frame of the 3D points is approximately the coordinate frame of the camera which has been rotated about x axis by 180 degrees. Also the world coordinate frame is along the z axis of the camera coordinate frame at a distance d. Thus the camera pose rvec should be approximately (pi,0,0). The tvec should approximately be (0,0, d).
When I use pnp algorithm to compute the pose, I get results which are off from the expected values by a significant amount.
My question is how to get accurate pose estimation from pnp algorithm for a low fov camera. What are the precautions while setting 3D point locations for pnp(I understand they need to be non planar and non linear). Is four points adequate for accurate results if I use the p3p model? Is my code correct or further transformations are necessary to get the camera pose?
My code, results and image is as below:
#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp>
#include <iostream>
#include <fstream>
std::vector<cv::Point2f> imagePoints;
bool pointsSelected = false;
using namespace std;
using namespace cv;
void onMouse(int event, int x, int y, int, void*) {
if (event == cv::EVENT_LBUTTONDOWN && imagePoints.size() < 6) {
imagePoints.emplace_back(x, y);
std::cout << "Point selected: (" << x << ", " << y << ")\n";
}
}
int main(int argc, char** argv) {
// Camera intrinsics (example values)
cv::Mat cameraMatrix = (cv::Mat_<double>(3,3) << 4316.19346, 0, 935.16587,
0, 4316.19346, 571.24591,
0, 0, 1);
cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64F);
// 3D world points in X,Y,Z order
std::vector<cv::Point3f> objectPoints = {
{-38.4 ,56.4 ,8.5},{-39 ,95.5 , 0},
{38.4 , 56.4, 0},{38, 95.5, 7},
{ 0, 56.4 ,8.5}, {0, 95.5 ,7}
};
// load image for pnp
cv::Mat img = cv::imread(argv[1]);
cv::namedWindow("Select Points",cv::WINDOW_NORMAL);
cv::resizeWindow("Select Points)",1920,1080);
cv::setMouseCallback("Select Points", onMouse);
// capture point locations in the image
cv::putText(img, "Click 6 points in sequence", {30, 50}, cv::FONT_HERSHEY_SIMPLEX, 1, {255, 255, 255}, 2);
while (imagePoints.size() < 6) {
cv::imshow("Select Points", img);
if (cv::waitKey(1) == 27) { // Escape key to exit
return 0;
}
}
cv::destroyWindow("Select Points");
cout<< "imagePoints="<< imagePoints<<endl;
cout<< "objectPoints="<< objectPoints<<endl;
// Compute initial pose using PnP
cv::Mat rvec, tvec;
bool success = cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, cv::SOLVEPNP_ITERATIVE );
if (!success) {
std::cerr << "PnP failed!" << std::endl;
return -1;
}
// Refine using Levenberg-Marquardt (Bundle Adjustment)
cv::solvePnPRefineLM(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
// Compute angle (theta) from the rotation vector's magnitude
double theta = (cv::norm(rvec)); // Angle in radians
// Compute unit axis of rotation
cv::Mat axis = rvec / theta; // Normalize to get the rotation axis
cout<<"Axis:- "<<axis<<" Theta:-"<<theta*180/3.14159<<endl;
std::cout << "Refined Rotation Vector:\n" << rvec << std::endl;
std::cout << "Refined Translation Vector:\n" << tvec << std::endl;
return 0;
}
Results obtained:
Axis:- [0.007625887701268117;
0.8235109123348147;
0.5672491719714086] Theta:-181.03
Refined Rotation Vector:
[0.02409447332177289;
2.601934684160179;
1.792259547513409]
Refined Translation Vector:
[-18.36243419369148;
-22.3985613463705;
-365.1093664945685]
the axis is not correct in the results above. The rotation should be around x axis
I am attaching the image for reference.
I had posted on opencv forum but havent recieved any replies yet. So I am positing on SO.
Upvotes: 0
Views: 32