OpenCV Assertion Failed with SolvePnP - c++

Question

I'm trying to use solvePnP in a C++ program that detects a chessboard in a video stream. Every time the calibration finishes, I try to run solvePnP, but I keep getting errors that I think are related to the translation and rotation vectors.
This is the error:
The error
This is my code:
#include <cstdio>
#include <cstdlib>
#include <string>
#include <iostream>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace cv;
using namespace std;
//Global variables
vector<vector<Point2f> > corner_list;
vector<vector<Point3f> > point_list;
vector<Mat> rotation_vecs, translation_vecs;
int i = 0;
bool calibMode = true, drawMode = false, drawMode2 = false, camMatrixInit = false, calibrated = false;
Mat originalCameraMatrix, cameraMatrix, distCoefficients, frame, gray, rvec, tvec;
//Function declarations
vector<Point3f> genWorldPoints(int cols, int rows);
void printCameraMatrix();
void printDistCoeff();
void printRotVecs();
/*Generates the world points (0, 0, 0), (1, 0, 0), etc. for the camera calibration.*/
vector<Point3f> genWorldPoints(int cols, int rows){
vector<Point3f> ret;
for (int i=0; i<rows; i++) {
for (int j=0; j<cols; j++) {
int tempi = i*(-1);
ret.push_back(Point3f((float)j, (float)tempi, 0.0));
}
}
return ret;
}
/*Print the camera matrix*/
void printCameraMatrix(){
cout << "Original Camera Matrix" << endl << originalCameraMatrix << endl;
cout << "Current Camera Matrix" << endl << cameraMatrix << endl;
}
/*Prints the distortion coefficients*/
void printDistCoeff(){
cout << "Distortion Coefficients:"<< endl << distCoefficients << endl;
}
int main(int argc, char *argv[]){
VideoCapture *capdev;
cameraMatrix = Mat::eye(3, 3, CV_64F);
distCoefficients = Mat::zeros(8, 1, CV_64F);
// open the video device
capdev = new VideoCapture(0);
if (!capdev->isOpened()) {
printf("Unable to open video device\n");
return(-1);
}
namedWindow("Video", 1);
bool found;
*capdev >> frame;
rvec = Mat::zeros(3, 1, CV_64F);
tvec = Mat::zeros(3, 1, CV_64F);
//Initialize camera matrix
cameraMatrix.at<double>(0,2) = (frame.size().width)/2;
cameraMatrix.at<double>(1,2) = (frame.size().height)/2;
cameraMatrix.copyTo(originalCameraMatrix);
printCameraMatrix();
for(;;){
*capdev >> frame;
if (!found) {
imshow("Video", frame);
}
Size patternsize(9,6);
vector<Point2f> corner_set;
vector<Point3f> point_set;
int code = waitKey(10);
cvtColor(frame, gray, CV_BGR2GRAY);
found = findChessboardCorners(gray, patternsize, corner_set, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
//Code to add a calibration frame
if (found && calibMode) {
cornerSubPix(gray, corner_set, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
drawChessboardCorners(frame, patternsize, Mat(corner_set), found);
//s key press
if (code == 115){
printf("Adding calibration frame\n");
printf("Number of corners found: %lu\n", corner_set.size());
printf("Point 0 x: %f\ty: %f\n", corner_set[0].x, corner_set[1].y);
//Add the corner set
corner_list.push_back(corner_set);
//Generate point set
vector<Point3f> point_set = genWorldPoints(9,6);
//Add point set to point set list
point_list.push_back(point_set);
//Save the image
string filename = "../data/images/p4_calib_image_"+to_string(i)+".jpeg";
imwrite(filename, frame);
//if there are more than 5 saved calibration images, run calibration procedure
if (i>4) {
//Calculate the reprojection error by running calibrateCamera
double rpe = calibrateCamera(point_list, corner_list, frame.size(), cameraMatrix,
distCoefficients, rotation_vecs, translation_vecs,
CV_CALIB_FIX_ASPECT_RATIO | CV_CALIB_FIX_K4);
//Print the camera matrix
printCameraMatrix();
printDistCoeff();
//Print the reprojection error
cout << "Reprojection error: " << rpe << endl;
calibrated = true;
}
//Increment i
i++;
}
imshow("Video", frame);
}
else if (found && drawMode){
cout << "Draw mode" << endl;
bool solved = solvePnP(point_list, corner_list, cameraMatrix, distCoefficients,
rvec, tvec);
//rotation_vecs.front(), translation_vecs.front());
}
else if (found && drawMode2){
cout << "Draw mode 2" << endl;
bool solved = solvePnP(point_list, corner_list, cameraMatrix, distCoefficients,
rvec, tvec);
//rotation_vecs.front(), translation_vecs.front());
}
//Switching between drawing modes
if (calibrated && code == 49) {
calibMode = false;
drawMode2 = false;
drawMode = true;
}
else if (calibrated && code == 50){
calibMode = false;
drawMode = false;
drawMode2 = true;
}
//Switch back to calibration mode
else if (calibrated && code == 51){
drawMode = false;
drawMode2 = false;
calibMode = true;
}
if (code == 27) {
printf("Terminating\n");
delete capdev;
return(0);
}
}
capdev = new VideoCapture(0);
printf("Terminating\n");
delete capdev;
return(0);
}
Please excuse the indentation...

Answer 1

The problem is that sovlePnP requires vector<Point2/3f> as input, instead of vector<vector<Point2/3f> >. In your code, "point_list" is vector<vector<Point3f> >, and "corner_list" is vector<vector<Point2f> >.
The documentation of solvePnP can be found here: http://docs.opencv.org/3.0-beta/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html