aruco.cpp
2.21 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
#include "aruco.h"
#include <aruco/cvdrawingutils.h>
#include <vector>
#include <iostream>
#include <cmath>
void ArucoDetector::setCamera(CvCamera cam)
{
cp = aruco::CameraParameters(cv::Mat(3,3,CV_32F,cam.matrix),cv::Mat(4,1,CV_32F,cam.distortion),cv::Size(cam.imgSize[0],cam.imgSize[1]) );
/* cp.CameraMatrix = cv::Mat(3,3,CV_32F,cam.matrix);
cp.Distorsion = cv::Mat(4,1,CV_32F,cam.distortion);
cp.CamSize = cv::Size(cam.imgSize[0],cam.imgSize[1]);*/
}
void ArucoDetector::setMarkerSize(float f)
{
size = f;
}
void ArucoDetector::setImage(IplImage *img)
{
cv::Mat image(img);
std::vector<aruco::Marker> markers;
try {
md.detect(image, markers, cp, size,false);
} catch (std::exception &ex)
{
std::cout << "Exception " << ex.what() << std::endl;
}
if (markers.size() > 0)
{
//markers[0].calculateExtrinsics(size, cp,false);
cv::Mat rot(3,3,CV_32FC1),jacob;
cv::Rodrigues(markers[0].Rvec,rot,jacob);
float r[9];
float t[3];
r[0]=rot.at<float>(0,0);
r[1]=rot.at<float>(0,1);
r[2]=rot.at<float>(0,2);
r[3]=-rot.at<float>(1,0);
r[4]=-rot.at<float>(1,1);
r[5]=-rot.at<float>(1,2);
r[6]=-rot.at<float>(2,0);
r[7]=-rot.at<float>(2,1);
r[8]=-rot.at<float>(2,2);
t[0]=markers[0].Tvec.ptr<float>(0)[0];
t[1]=-markers[0].Tvec.ptr<float>(0)[1];
t[2]=-markers[0].Tvec.ptr<float>(0)[2];
//std::cout << markers[0] << std::endl;
//cv::Mat reverse = (cv::Mat_<float>(3,3) << 0,1,0, 0,0,-1,-1,0,0) ;
//(Mat_<double>(3,3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
//rot = rot.inv();
//rot = reverse.t() * rot ;
/*float x = markers[0].Tvec.at<float>(0,0);
float y = markers[0].Tvec.at<float>(0,1);
markers[0].Tvec.at<float>(0,0) = y;
markers[0].Tvec.at<float>(0,1) = x;*/
emit sendPose(r,t);
/*std::cout << "Marker detected " << markers[0] << std::endl;
std::cout << "Rx" << rot.at<float>(0,0) << ", "
<< rot.at<float>(0,1) << ", "
<< rot.at<float>(0,2) << std::endl;*/
}
}