I have modified the ARToolkit nftSimple Application to work with OpenCV Mat and without OpenGL dependencies. I have got the tracking to work successfully and i am able to extract the pose matrix ARPose.
Code for project :- https://github.com/njanirudh/OpenCV-ARTK
Output
The ARPose is of the format (16 x 1) :
_Documentation : // Position and orientation, column-major order. (position(x,y,z) = {T[12], T[13], T[14]}_
T = 0.833453 0.148213 0.532342 0.000000 -0.055585 0.980959 -0.186091 0.000000
-0.549787 0.125508 0.825822 0.000000 60.233962 -307.641967 -964.580499 1.000000
Problem
I am trying to use this to draw a pink circle on OpenCV side using the x,y,z values returned from the pose.
According to the documentation in the code
T[12] = 60.233962 , T[13] = -307.641967 , T[14] = -964.580499
The coordinates obtained from the pose matrix are not giving the correct value.
How do we transform the pose matrix from ARToolkit into OpenCV Screen coordinates?
static void runDetection(Mat &input_mat)
Edit :- I {
ARUint8 *gARTImage = NULL;
gARTImage = (uchar *)input_mat.data;
float trackingTrans[3][4] = {0,0,0,0,0,0,0,0,0,0,0,0};
cv::Mat rot_vec = Mat::zeros(1,3,CV_64F), trn_vec = Mat::zeros(1,3,CV_64F) , rot_mat = Mat::zeros(3,3,CV_64F);
if(true){
// NFT results.
static int detectedPage = -2; // -2 Tracking not inited, -1 tracking inited OK, >= 0 tracking online on page.
int i, j, k;
// Grab a video frame.
if (gARTImage != NULL) {
gCallCountMarkerDetect++; // Increment ARToolKit FPS counter.
// Run marker detection on frame
if (threadHandle) {
// Perform NFT tracking.
float err;
int ret;
int pageNo;
if( detectedPage == -2 ) {
trackingInitStart( threadHandle, gARTImage );
detectedPage = -1;
}
if( detectedPage == -1 ) {
ret = trackingInitGetResult( threadHandle, trackingTrans, &pageNo);
if( ret == 1 ) {
if (pageNo >= 0 && pageNo < surfaceSetCount) {
ARLOGd("Detected page %d.\n", pageNo);
detectedPage = pageNo;
ar2SetInitTrans(surfaceSet[detectedPage], trackingTrans);
} else {
ARLOGe("Detected bad page %d.\n", pageNo);
detectedPage = -2;
}
} else if( ret < 0 ) {
ARLOGd("No page detected.\n");
detectedPage = -2;
}
}
if( detectedPage >= 0 && detectedPage < surfaceSetCount) {
if( ar2Tracking(ar2Handle, surfaceSet[detectedPage], gARTImage, trackingTrans, &err) < 0 ) {
ARLOGd("Tracking lost.\n");
detectedPage = -2;
} else {
ARLOGd("Tracked page %d (max %d).\n", detectedPage, surfaceSetCount - 1);
}
}
} else {
ARLOGe("Error: threadHandle\n");
detectedPage = -2;
}
// Update markers.
for (i = 0; i < markersNFTCount; i++) {
markersNFT[i].validPrev = markersNFT[i].valid;
if (markersNFT[i].pageNo >= 0 && markersNFT[i].pageNo == detectedPage) {
markersNFT[i].valid = TRUE;
for (j = 0; j < 3; j++)
for (k = 0; k < 4; k++){
markersNFT[i].trans[j][k] = trackingTrans[j][k];
}
}
else markersNFT[i].valid = FALSE;
if (markersNFT[i].valid) {
// Filter the pose estimate.
if (markersNFT[i].ftmi) {
if (arFilterTransMat(markersNFT[i].ftmi, markersNFT[i].trans, !markersNFT[i].validPrev) < 0) {
ARLOGe("arFilterTransMat error with marker %d.\n", i);
}
}
if (!markersNFT[i].validPrev) {
// Marker has become visible, tell any dependent objects.
// --->
}
// We have already read how a new pose, so set that.
arglCameraViewRH((const ARdouble (*)[4])markersNFT[i].trans, markersNFT[i].pose.T, VIEW_SCALEFACTOR);
//Print(markersNFT[i].pose.T);
//************Rotation Matrix************
rot_mat.at<float>(0,0) = trackingTrans[0][0];
rot_mat.at<float>(0,1) = trackingTrans[0][1];
rot_mat.at<float>(0,2) = trackingTrans[0][2];
rot_mat.at<float>(1,0) = trackingTrans[1][0];
rot_mat.at<float>(1,1)= trackingTrans[1][1];
rot_mat.at<float>(1,2) = trackingTrans[1][2];
rot_mat.at<float>(2,0) = trackingTrans[2][0];
rot_mat.at<float>(2,1) = trackingTrans[2][1];
rot_mat.at<float>(2,2) = trackingTrans[2][2];
Rodrigues(rot_mat, rot_vec);
//************Translation Matrix***********
trn_vec.at<float>(0,0) = markersNFT[i].pose.T[12];
trn_vec.at<float>(0,1) = markersNFT[i].pose.T[13];
trn_vec.at<float>(0,2) = markersNFT[i].pose.T[14];
//************Camera Matrix*****************
cv::Mat intrisicMat(3, 3, cv::DataType<double>::type); // Intrisic matrix
intrisicMat.at<double>(0, 0) = 674.171631;
intrisicMat.at<double>(1, 0) = 0;
intrisicMat.at<double>(2, 0) = 0;
intrisicMat.at<double>(0, 1) = 0;
intrisicMat.at<double>(1, 1) = 633.898087;
intrisicMat.at<double>(2, 1) = 0;
intrisicMat.at<double>(0, 2) = 318.297791;
intrisicMat.at<double>(1, 2) = 237.900467;
intrisicMat.at<double>(2, 2) = 1;
//************Dist Matrix**********************
cv::Mat distCoeffs(5, 1, cv::DataType<double>::type); // Distortion vector
distCoeffs.at<double>(0) = 0;
distCoeffs.at<double>(1) = 0;
distCoeffs.at<double>(2) = 0;
distCoeffs.at<double>(3) = 0;
distCoeffs.at<double>(4) = 0;
std::vector<cv::Point3d> inputPnt ;
std::vector<cv::Point2d> outPnt;
cv::Point3d pnt = Point3d(markersNFT[i].pose.T[12],markersNFT[i].pose.T[13],markersNFT[i].pose.T[14]);
inputPnt.push_back(pnt);
cv::projectPoints(inputPnt,rot_vec,trn_vec,intrisicMat,distCoeffs,outPnt);
// Convert from ARToolkit Image type to convert OpenCV Mat
IplImage* iplImg;
iplImg = cvCreateImage(cvSize(640, 480), IPL_DEPTH_8U,3);
// Get video frame from ARToolkit
iplImg->imageData = (char *)gARTImage;
input_mat = cv::cvarrToMat(iplImg);
cout<<outPnt<<"**********"<<endl;
//****Draw a Circle using the OpenGL matrix into Transformation , rotation and scaling matrix but the output is not working as expectedpose data****
cv::circle(input_mat,Point(outPnt[0].y,0),30,Scalar(255,255,255) ,4);
} else {
if (markersNFT[i].validPrev) {
// Marker has ceased to be visible, tell any dependent objects.
// --->
}
}
}
}
}
}
