Hello, I want to receive position from Aruco of marker. The following code, I draw the axes on the marker and very good catches it. How do I read the value of the matrix from the camera position of marker? Are needed axes x, y, z and the rotation axis. Does anyone have any idea?
#include
#include
#include
using namespace std;
using namespace cv;
static bool readCameraParameters(string filename, Mat &camMatrix, Mat &distCoeffs) {
FileStorage fs(filename, FileStorage::READ);
if (!fs.isOpened())
return false;
fs["camera_matrix"] >> camMatrix;
fs["distortion_coefficients"] >> distCoeffs;
return true;
}
/**
*/
static bool readDetectorParameters(string filename, Ptr¶ms) {
FileStorage fs(filename, FileStorage::READ);
if (!fs.isOpened())
return false;
fs["adaptiveThreshWinSizeMin"] >> params->adaptiveThreshWinSizeMin;
fs["adaptiveThreshWinSizeMax"] >> params->adaptiveThreshWinSizeMax;
fs["adaptiveThreshWinSizeStep"] >> params->adaptiveThreshWinSizeStep;
fs["adaptiveThreshConstant"] >> params->adaptiveThreshConstant;
fs["minMarkerPerimeterRate"] >> params->minMarkerPerimeterRate;
fs["maxMarkerPerimeterRate"] >> params->maxMarkerPerimeterRate;
fs["polygonalApproxAccuracyRate"] >> params->polygonalApproxAccuracyRate;
fs["minCornerDistanceRate"] >> params->minCornerDistanceRate;
fs["minDistanceToBorder"] >> params->minDistanceToBorder;
fs["minMarkerDistanceRate"] >> params->minMarkerDistanceRate;
fs["doCornerRefinement"] >> params->doCornerRefinement;
fs["cornerRefinementWinSize"] >> params->cornerRefinementWinSize;
fs["cornerRefinementMaxIterations"] >> params->cornerRefinementMaxIterations;
fs["cornerRefinementMinAccuracy"] >> params->cornerRefinementMinAccuracy;
fs["markerBorderBits"] >> params->markerBorderBits;
fs["perspectiveRemovePixelPerCell"] >> params->perspectiveRemovePixelPerCell;
fs["perspectiveRemoveIgnoredMarginPerCell"] >> params->perspectiveRemoveIgnoredMarginPerCell;
fs["maxErroneousBitsInBorderRate"] >> params->maxErroneousBitsInBorderRate;
fs["minOtsuStdDev"] >> params->minOtsuStdDev;
fs["errorCorrectionRate"] >> params->errorCorrectionRate;
return true;
}
/**
*/
int main(int argc, char *argv[]) {
int dictionaryId = 0;
bool showRejected = false;
bool estimatePose = true;
float markerLength = 200;
Ptr detectorParams = aruco::DetectorParameters::create();
detectorParams->doCornerRefinement = true; // do corner refinement in markers
Ptr dictionary =
aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId));
Mat camMatrix, distCoeffs;
if (estimatePose) {
bool readOk = readCameraParameters("kalibracja", camMatrix, distCoeffs);
if (!readOk) {
cerr << "Invalid camera file" << endl;
return 0;
}
}
VideoCapture inputVideo;
inputVideo.open(0);
double totalTime = 0;
int totalIterations = 0;
while (inputVideo.grab()) {
Mat image, imageCopy;
inputVideo.retrieve(image);
double tick = (double)getTickCount();
vector< int > ids;
vector< vector< Point2f >> corners, rejected;
vector< Vec3d > rvecs, tvecs;
// detect markers and estimate pose
aruco::detectMarkers(image, dictionary, corners, ids, detectorParams, rejected);
if (estimatePose && ids.size() > 0)
aruco::estimatePoseSingleMarkers(corners, markerLength, camMatrix, distCoeffs, rvecs, tvecs);
double currentTime = ((double)getTickCount() - tick) / getTickFrequency();
totalTime += currentTime;
totalIterations++;
if (totalIterations % 30 == 0) {
cout << "Detection Time = " << currentTime * 1000 << " ms "<< "(Mean = " << 1000 * totalTime / double(totalIterations) << " ms)" << endl;
}
if (totalIterations % 30 == 0) {
cout << "Detection Time = " << currentTime * 1000 << " ms "<< "(Mean = " << 1000 * totalTime / double(totalIterations) << " ms)" << endl;
}
// draw results
image.copyTo(imageCopy);
if (ids.size() > 0) {
aruco::drawDetectedMarkers(imageCopy, corners, ids);
if (estimatePose) {
for (unsigned int i = 0; i < ids.size(); i++)
aruco::drawAxis(imageCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i],
markerLength * 0.5f);
}
}
if (showRejected && rejected.size() > 0)
aruco::drawDetectedMarkers(imageCopy, rejected, noArray(), Scalar(100, 0, 255));
imshow("out", imageCopy);
char key = (char)waitKey(100);
if (key == 27) break;
}
system("pause");
return 0;
}
↧