I am trying to write a simple augmented reality program that, given and output video and the camera intrinsic parameters, projects 3 lines into each video frame. These lines are the x,y and z axis.
I'm asking for help not for the program itself but for the solvePnP function, because I just donĀ“t understand what I am doing wront to get this error:
terminate called after throwing an instance of 'cv::Exception' what(): OpenCV(4.4.0) /home/nel/opencv-4.4.0/modules/calib3d/src/solvepnp.cpp:753: error: (-215:Assertion failed) ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) ) && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) in function 'solvePnPGeneric'
This is my code:
#include <vector>
#include <opencv2/highgui/highgui_c.h>
using namespace cv;
using namespace std;
int main(int, char**)
{
VideoCapture cap("./video.mp4"); // open the default camera
if(!cap.isOpened()) // check if we succeeded
return -1;
Mat image;
Size patternsize(9,6); //interior number of corners
vector<Point2f> corners;
vector<Point3f> axis={Point3f(0.0,0.0,0.0), Point3f(0.0,0.1,0.0),Point3f(0.0,0.0,0.1), Point3f(0.1,0.0,0.0)};
FileStorage fs("intrinsics.yml", FileStorage::READ);
if (!fs.isOpened())
{
cerr << "failed to open " << "intrinsic.yml" << endl;
return 0;
}
Mat camera_matrix, distortion_coefficients;
cout << "reading R and T" << endl;
fs["camera_matrix"] >> camera_matrix;
fs["distortion_coefficients"] >> distortion_coefficients;
fs.release();
for(;;)
{
Mat frame;
Mat rvec, tvec, points2d;
cap >> frame; // get a new frame from camera
cvtColor(frame, image, COLOR_BGR2GRAY);
bool patternfound = findChessboardCorners(image, patternsize, corners,
CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE
+ CALIB_CB_FAST_CHECK);
if(patternfound) cornerSubPix(image, corners, Size(11, 11), Size(-1, -1),TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
else{cerr<<"Error: pattern not found"<<endl;return 0;}
cv::solvePnP(axis, corners, camera_matrix, distortion_coefficients, rvec, tvec);
cout<<"Extrinsic params calculated"<<endl;
cv::projectPoints(axis, rvec, tvec, camera_matrix, distortion_coefficients, points2d);
MatIterator_<double> it, end;
for( it = points2d.begin<double>(), end = points2d.end<double>(); it != end; ++it)
{
cv::line(image, (Point)points2d.at<double>(0,0), (Point)*it, (255,0,0), 3,8);
}
imshow("image", image);
if(waitKey(30) >= 0) break;
}
// the camera will be deinitialized automatically in VideoCapture destructor
return 0;
}