Ошибка OpenCV: неподдерживаемый формат или комбинация форматов (вектор векторов точек типа Point3f)

Я пытаюсь откалибровать свою веб-камеру на макинтош (и в конечном итоге Microsoft Kinect) обнаружить Маркеры аруко. Я выполняю все функции правильно, но с неверной калибровочной матрицей, потому что cameraCalibration() функция не работает должным образом и возвращает ошибки при запуске функции opencv calibrateCamera()

Ошибка: ошибка OpenCV: неподдерживаемый формат или комбинация форматов
(objectPoints должен содержать вектор векторов точек типа
Point3f) в collectCalibrationData

Мой код:

  #include "opencv2/core.hpp"#include "opencv2/imgcodecs.hpp"#include "opencv2/imgproc.hpp"#include "opencv2/highgui.hpp"#include "opencv2/aruco.hpp"#include "opencv2/calib3d.hpp"
#include <sstream>
#include <iostream>
#include <fstream>

#include <vector>

using namespace std;
using namespace cv;
using std::vector;

// Constants for Calibration
const float calibrationSquareDemension = .0239f; //meters
const float arucoSquareDimension = .080f;   //meters - 80mm = 8cm
const Size chessboardDimensions = Size(6, 9); //Size of calibration board (given)

// Known location of markers in Test Environment
std::vector<Vec3d> translationVectorsToOrigin;
Vec3d mk0 (0.040f, 0.304f, 0);
Vec3d mk1 (0.134f, 0.040f, 0);
Vec3d mk2 (0.148f, 0.204f, 0);

void createArucoMarkers() {
Mat outputMarker;

//Ptr<aruco::Dictionary> markerDictionary =
//  aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME::DICT_4x4_50);

Ptr<aruco::Dictionary> markerDictionary =
aruco::getPredefinedDictionary( 0 );for ( int i = 0 ; i < 50 ; i++ ) {
aruco::drawMarker(markerDictionary, i, 500, outputMarker, 1);
ostringstream convert;
string imageName = "4x4Marker_";
convert << imageName << i << ".jpg";
imwrite(convert.str(), outputMarker);
}
}

void createKnowBoardPositions(Size boardSize, float squareEdgeLength, vector<Point3f> corners ) {
for ( int i = 0 ; i < boardSize.height; i++ ) {
for ( int j = 0 ; j < boardSize.width; j++ ) {
// Push in all calculated of expected Point3f
corners.push_back( Point3f(j * squareEdgeLength, i * squareEdgeLength, 0.0f) );
}
}
}

void getChessboardCorners( vector<Mat> images, vector< vector<Point2f> >& allFoundCorners, bool showResults = false ) {
for ( vector<Mat>::iterator iter = images.begin(); iter != images.end(); iter++ ) {
vector<Point2f> pointBuf;
// Using opencv function
bool found = findChessboardCorners(*iter, Size(9,6), pointBuf, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE );

if ( found ) {
allFoundCorners.push_back(pointBuf);
}

if ( showResults ) {
drawChessboardCorners(*iter, Size(9,6), pointBuf, found);
imshow("Looking for Corners", *iter);
waitKey(0); //Does not terminate until done
}
}
}

void cameraCalibration( vector<Mat> calibrationImages, Size boardSize, float squareEdgeLength, Mat& cameraMatrix, Mat& distanceCoefficients ) {
printf("Enters function successfully\n");
vector< vector<Point2f> > checkerboardImageSpacePoints;
getChessboardCorners( calibrationImages, checkerboardImageSpacePoints, false );
printf("successfully gets chessboardCorners\n");
// Create known board positions
vector< vector<Point3f> > worldSpaceCornerPoints(1);

createKnowBoardPositions(boardSize, squareEdgeLength, worldSpaceCornerPoints[0]);
worldSpaceCornerPoints.resize(checkerboardImageSpacePoints.size(), worldSpaceCornerPoints[0]);

vector<Mat> rVectors, tVectors;
distanceCoefficients = Mat::zeros(8, 1, CV_64F);

printf("I successfully getChessboardCorners, createKnowBoardPositions, and set distanceCoefficients\n");

// TODO ISSUE! passing in correct points but not recognizing in opencv function
cv::calibrateCamera(worldSpaceCornerPoints, checkerboardImageSpacePoints, boardSize, cameraMatrix, distanceCoefficients, rVectors, tVectors);
}

// TODO: Test if it is working. Cannot without camera calibration
bool saveCameraCalibration( string name, Mat cameraMatrix, Mat distanceCoefficients ) {
printf("ENTERS CAMERA CALIBRATION SUCCESSFULLY\n");

ofstream outStream;
outStream.open(name.c_str());

if ( outStream ) {
uint16_t rows = cameraMatrix.rows;
uint16_t columns = cameraMatrix.cols;

// Push out rows and cols to file << endl = \n
outStream << rows << endl;
outStream << columns << endl;

for ( int r = 0 ; r < rows ; r++  ) {
for ( int c = 0 ; c < columns ; c++ ) {
double value = cameraMatrix.at<double>(r, c);
outStream << value << endl;
}
}

rows = distanceCoefficients.rows;
columns = distanceCoefficients.cols;

// Push out rows and cols to file << endl = \n for distanceCoefficients
outStream << rows << endl;
outStream << columns << endl;

for ( int r = 0 ; r < rows ; r++  ) {
for ( int c = 0 ; c < columns ; c++ ) {
double value = distanceCoefficients.at<double>(r, c);
outStream << value << endl;
}
}

outStream.close();
return true;
}
return false;
}

// Fully working. Takes in precalculated calibration values of the camera for analysis
bool loadCameraCalibration( string name, Mat& cameraMatrix, Mat& distanceCoefficients ) {

// Bring in the file that we are loading information from
ifstream inStream;
inStream.open(name.c_str());

if ( inStream ) {
uint16_t rows;
uint16_t columns;

inStream >> rows;
inStream >> columns;

cameraMatrix = Mat( Size(columns, rows), CV_64F );

for ( int r = 0 ; r < rows ; r++ ) {
for ( int c = 0 ; c < columns ; c++ ) {
double temp = 0.0f;
inStream >> temp;
cameraMatrix.at<double>(r,c) = temp;
cout << cameraMatrix.at<double>(r,c) << "\n";
}
}

// Find distanceCoefficients
inStream >> rows;
inStream >> columns;

distanceCoefficients = Mat::zeros(rows, columns, CV_64F);

for ( int r = 0 ; r < rows ; r++ ) {
for ( int c = 0 ; c < columns ; c++ ) {
double temp = 0.0f;
inStream >> temp;
distanceCoefficients.at<double>(r,c) = temp;
cout << distanceCoefficients.at<double>(r,c) << "\n";
}
}

inStream.close();
return true;
}

return false;
}

// Function is called by calibrateStepOne
int analyzeFrame( vector< vector<Point2f> > markerCorners, const Mat& cameraMatrix, const Mat& distanceCoefficients, vector<Vec3d> rotationVectors, vector<Vec3d> translationVectors, vector<int> markerIds ) {

// Start by printing markerCorners and determing what the data looks like
printf("MarkerId:\t");
printf("(  A  )\t\t");
printf("(  B  )\t\t");
printf("(  C  )\t\t");
printf("(  D  )\n");

for ( int r = 0 ; r < markerCorners.size() ; r++ ) {
printf("MarkerId: %d\t", markerIds[r]);
for ( int c = 0 ; c < 4 ; c++ ) {
printf("(%f, %f)\t", markerCorners[r][c].x, markerCorners[r][c].y );
}
printf("\n");
}
printf("\n");

// Print out rotationVectors and translationVectors
for ( int r = 0 ; r < rotationVectors.size() ; r++ ) {
printf("MarkerId: %d\n", markerIds[r]);
cout << "[x,y,z] " << rotationVectors.at(r)[0] <<", " << rotationVectors.at(r)[1] <<", " << rotationVectors.at(r)[2] << endl;
cout << "[r,p,y] " << translationVectors.at(r)[0] <<", " << translationVectors.at(r)[1] <<", " << translationVectors.at(r)[2] << endl;
printf("\n");
}
return 1;
}

int startWebcamMonitoring( const Mat& cameraMatrix, const Mat& distanceCoefficients, bool showMarkers) {

Mat frame;  //Hold frame of info from webcam

vector<int> markerIds;
vector< vector<Point2f> > markerCorners, rejectedCandidates;

// Aruco part
aruco::DetectorParameters parameters;

Ptr<aruco::Dictionary> markerDictionary =
aruco::getPredefinedDictionary( 0 );

VideoCapture vid(0);  //TODO: When using on Machine with Kinect change to source 1 if not working!!!!

if ( !vid.isOpened() ) {
return -1;
}

namedWindow("Webcam", CV_WINDOW_AUTOSIZE);  //Makes the GUI

vector<Vec3d> rotationVectors, translationVectors;

while(true) {
if (!vid.read(frame)) {
break;
}

// Finds them
aruco::detectMarkers(frame, markerDictionary, markerCorners, markerIds);

// Outline the markers and label with ids
if (showMarkers) { aruco::drawDetectedMarkers(frame, markerCorners, markerIds); }

// Estimate pose
aruco::estimatePoseSingleMarkers(markerCorners, arucoSquareDimension, cameraMatrix, distanceCoefficients, rotationVectors, translationVectors );

// Continually draw the axis
if ( showMarkers ) {
for ( int i = 0 ; i < markerIds.size() ; i++ ) {
aruco::drawAxis(frame, cameraMatrix, distanceCoefficients, rotationVectors[i], translationVectors[i], 0.1f);
// printf("Marker %i found\n", markerIds[i] ); // Only print once but already shown visually
}
imshow("Webcam", frame);
}

if (waitKey(30) >= 0) break;

}
// TODO: Remove
analyzeFrame( markerCorners, cameraMatrix, distanceCoefficients, rotationVectors, translationVectors, markerIds );

return 1;
}

// Update to not show imshow when testing complete
// TODO: Update to ensure it catches a frame > frame is not reading
// TODO: Keep in a while loop > Then upon escape it calls the next function for analysis
int calibratePositioningStepOne( const Mat& cameraMatrix, const Mat& distanceCoefficients, bool showMarkers) {

Mat frame;  //Hold frame of info from webcam

vector< vector<Point2f> > markerCorners, rejectedCandidates;
vector<int> markerIds;

// Aruco part
aruco::DetectorParameters parameters;

Ptr<aruco::Dictionary> markerDictionary =
aruco::getPredefinedDictionary( 0 );

VideoCapture vid(0);  //TODO: When using on Machine with Kinect change to source 1 if not working!!!!

if ( !vid.isOpened() ) {
return -1;
}

namedWindow("Webcam", CV_WINDOW_AUTOSIZE);  //Makes the GUI

vector<Vec3d> rotationVectors, translationVectors;

if (!vid.read(frame)) {
return -1;
}

// Finds them
aruco::detectMarkers(frame, markerDictionary, markerCorners, markerIds);

// Outline the markers and label with ids
if (showMarkers) { aruco::drawDetectedMarkers(frame, markerCorners, markerIds); }

// Estimate pose
aruco::estimatePoseSingleMarkers(markerCorners, arucoSquareDimension, cameraMatrix, distanceCoefficients, rotationVectors, translationVectors );

// Continually draw the axis
if ( showMarkers ) {
for ( int i = 0 ; i < markerIds.size() ; i++ ) {
aruco::drawAxis(frame, cameraMatrix, distanceCoefficients, rotationVectors[i], translationVectors[i], 0.1f);
// printf("Marker %i found\n", markerIds[i] ); // Only print once but already shown visually
}
while (true) {
imshow("Webcam", frame);
if (waitKey(30) >= 0) break;
}
}

// Test calling next steps to analyze shot
analyzeFrame( markerCorners, cameraMatrix, distanceCoefficients, rotationVectors, translationVectors, markerIds );

return 1;
}

void cameraCalibrationProcess( Mat& cameraMatrix, Mat& distanceCoefficients ) {
Mat frame;
Mat drawToFrame;

vector<Mat> savedImages;

vector< vector<Point2f> > markerCorners, rejectedCandidates;

VideoCapture vid(0);

if (!vid.isOpened()) { return; }

int framesPerSecond = 20;

namedWindow( "Webcam", CV_WINDOW_AUTOSIZE );

while(true) {
if (!vid.read(frame)) {
break;
}

vector<Vec2f> foundPoints;
bool found = false;

found = findChessboardCorners( frame, chessboardDimensions, foundPoints, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE );
frame.copyTo( drawToFrame );
drawChessboardCorners(drawToFrame, chessboardDimensions, foundPoints, found);
if (found) {
imshow("Webcam", drawToFrame);
} else {
imshow("Webcam", frame);
}
char character = waitKey(1000/framesPerSecond);

switch (character) {
case ' ':
printf("SPACE: Looking for image\n");
// Saving image
if (found) {
printf("Image found\n");
Mat temp;
frame.copyTo(temp);
savedImages.push_back(temp);
}
break;
case 'f':
// Enter Key - start calibration
// First check that we have enough
printf("ENTER: Save File\n");
printf("COUNT Image found: %i\n", savedImages.size());
if ( savedImages.size() > 15 ) {
// TODO: Issue with this call in my method to other method
printf("Enters. Correct number of images found\n");
cameraCalibration(savedImages, chessboardDimensions, calibrationSquareDemension, cameraMatrix, distanceCoefficients);
printf("Calibration Complete...Waiting to save file\n");
saveCameraCalibration("ILoveCameraCalibration", cameraMatrix, distanceCoefficients);
printf("SUCCESSFULLY saved calibration\n");
}
printf("Not enough images found\n");
break;
case 27:
// ESC Key
return;
break;

}
}
}

// Given analysis figure out where the markers are
// then take this and determine their location relation to OriginWorld
vector<Point3f> getCornersInCameraWorld( Vec3d rvec, Vec3d tvec ) {

double half_side = arucoSquareDimension/2;// compute rot_mat
Mat rot_mat;
Rodrigues(rvec, rot_mat);

// transpose of rot_mat for easy columns extraction
Mat rot_mat_t = rot_mat.t();

// the two E-O and F-O vectors
double * tmp = rot_mat_t.ptr<double>(0);
Point3f camWorldE(tmp[0]*half_side,
tmp[1]*half_side,
tmp[2]*half_side);

tmp = rot_mat_t.ptr<double>(1);
Point3f camWorldF(tmp[0]*half_side,
tmp[1]*half_side,
tmp[2]*half_side);

// convert tvec to point
Point3f tvec_3f(tvec[0], tvec[1], tvec[2]);

// return vector:
vector<Point3f> ret(4,tvec_3f);

ret[0] +=  camWorldE + camWorldF;
ret[1] += -camWorldE + camWorldF;
ret[2] += -camWorldE - camWorldF;
ret[3] +=  camWorldE - camWorldF;

return ret;
}

int main(int argv, char** argc) {

//createArucoMarkers(); // Done

// Add elements to translationVectorsToOrigin vector
translationVectorsToOrigin.push_back(mk0);
translationVectorsToOrigin.push_back(mk1);
translationVectorsToOrigin.push_back(mk2);

Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
Mat distanceCoefficients;
vector<int> markerIds;
vector< vector<Point2f> > markerCorners;

// Not working, using temp file
cameraCalibrationProcess( cameraMatrix, distanceCoefficients );

//loadCameraCalibration("ILoveCameraCalibration", cameraMatrix, distanceCoefficients);
//startWebcamMonitoring( cameraMatrix, distanceCoefficients, true);

// Tom's tests
// TODO: Issue passing markerIds and markerCorners for further analysis
// TODO: Try calling next function and pass in values once created
// calibrateStepOne( cameraMatrix, distanceCoefficients, true);

}

Есть идеи о том, что я сделал не так? Благодарю.