我正在尝试在Mac（最终是Microsoft Kinect）上校准我的网络摄像头以检测Aruco 标记。我正在正确运行所有函数，但校准矩阵不正确，因为该cameraCalibration()函数未按预期工作并在运行 opencv 函数时返回错误calibrateCamera()
错误：OpenCV 错误：collectCalibrationData 中不支持的格式或格式组合（objectPoints 应包含 Point3f 类型的点向量的向量）
我的代码：
#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);
}
关于我做错了什么的任何想法？谢谢。