vint8 class_interpreter::initialize()
{
initialize_directories();
input_file_pointer = 0;
// current_action_mode = vCopyString(graphical_module::mode_pixel_info_response);
// previous_action_mode = 0;
exit_request = 0;
// The goal here is to instantiate some template functions, so
// that I can call them while debugging.
vMatrix<uchar> junk1(1,1);
vMatrix<vint4> junk2(1,1);
vMatrix<float> junk3(1,1);
vMatrix<double> junk4(1,1);
vMatrix<vint8> junk5(1,1);
vint8 a = 0;
if (a)
{
junk1.Print(0);
junk1.PrintRange(1,1,1,1,0);
junk1.PrintInt(0);
junk1.PrintRangeInt(1,1,1,1,0);
junk1.PrintRangem(1,1,1,1,0);
junk1.PrintRangeIntm(1,1,1,1,0);
junk1.PrintTrans(0);
junk1.PrintRangeTrans(1,1,1,1,0);
junk1.PrintIntTrans(0);
junk1.PrintRangeIntTrans(1,1,1,1,0);
junk1.PrintRangemTrans(1,1,1,1,0);
junk1.PrintRangeIntmTrans(1,1,1,1,0);
junk1.WriteDebug("trash");
junk2.Print(0);
junk2.PrintRange(1,1,1,1,0);
junk2.PrintInt(0);
junk2.PrintRangeInt(1,1,1,1,0);
junk2.PrintRangem(1,1,1,1,0);
junk2.PrintRangeIntm(1,1,1,1,0);
junk2.PrintTrans(0);
junk2.PrintRangeTrans(1,1,1,1,0);
junk2.PrintIntTrans(0);
junk2.PrintRangeIntTrans(1,1,1,1,0);
junk2.PrintRangemTrans(1,1,1,1,0);
junk2.PrintRangeIntmTrans(1,1,1,1,0);
junk2.WriteDebug("trash");
junk3.Print(0);
junk3.PrintRange(1,1,1,1,0);
junk3.PrintInt(0);
junk3.PrintRangeInt(1,1,1,1,0);
junk3.PrintRangem(1,1,1,1,0);
junk3.PrintRangeIntm(1,1,1,1,0);
junk3.PrintTrans(0);
junk3.PrintRangeTrans(1,1,1,1,0);
junk3.PrintIntTrans(0);
junk3.PrintRangeIntTrans(1,1,1,1,0);
junk3.PrintRangemTrans(1,1,1,1,0);
junk3.PrintRangeIntmTrans(1,1,1,1,0);
junk3.WriteDebug("trash");
junk4.Print(0);
junk4.PrintRange(1,1,1,1,0);
junk4.PrintInt(0);
junk4.PrintRangeInt(1,1,1,1,0);
junk4.PrintRangem(1,1,1,1,0);
junk4.PrintRangeIntm(1,1,1,1,0);
junk4.PrintTrans(0);
junk4.PrintRangeTrans(1,1,1,1,0);
junk4.PrintIntTrans(0);
junk4.PrintRangeIntTrans(1,1,1,1,0);
junk4.PrintRangemTrans(1,1,1,1,0);
junk4.PrintRangeIntmTrans(1,1,1,1,0);
junk4.WriteDebug("trash");
junk5.Print(0);
junk5.PrintRange(1,1,1,1,0);
junk5.PrintInt(0);
junk5.PrintRangeInt(1,1,1,1,0);
junk5.PrintRangem(1,1,1,1,0);
junk5.PrintRangeIntm(1,1,1,1,0);
junk5.PrintTrans(0);
junk5.PrintRangeTrans(1,1,1,1,0);
junk5.PrintIntTrans(0);
junk5.PrintRangeIntTrans(1,1,1,1,0);
junk5.PrintRangemTrans(1,1,1,1,0);
junk5.PrintRangeIntmTrans(1,1,1,1,0);
junk5.WriteDebug("trash");
}
input_file_pointer = 0;
return 1;
}
void Tracker::run()
{
camera = cv::VideoCapture(camera_index);
if (force_width)
camera.set(CV_CAP_PROP_FRAME_WIDTH, force_width);
if (force_height)
camera.set(CV_CAP_PROP_FRAME_HEIGHT, force_height);
if (force_fps)
camera.set(CV_CAP_PROP_FPS, force_fps);
aruco::MarkerDetector detector;
detector.setDesiredSpeed(3);
detector.setThresholdParams(11, 6);
cv::Rect last_roi(65535, 65535, 0, 0);
cv::Mat color, color_, grayscale, rvec, tvec;
const double stateful_coeff = 0.81;
if (!camera.isOpened())
{
fprintf(stderr, "aruco tracker: can't open camera\n");
return;
}
while (!stop)
if(camera.read(color_))
break;
auto freq = cv::getTickFrequency();
auto last_time = cv::getTickCount();
int fps = 0;
int last_fps = 0;
cv::Point2f last_centroid;
while (!stop)
{
if (!camera.read(color_))
continue;
auto tm = cv::getTickCount();
color_.copyTo(color);
cv::cvtColor(color, grayscale, cv::COLOR_BGR2GRAY);
const int scale = frame.cols > 480 ? 2 : 1;
const float focal_length_w = 0.5 * grayscale.cols / tan(0.5 * fov * HT_PI / 180);
const float focal_length_h = 0.5 * grayscale.rows / tan(0.5 * fov * grayscale.rows / grayscale.cols * HT_PI / 180.0);
cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1);
intrinsics.at<float> (0, 0) = focal_length_w;
intrinsics.at<float> (1, 1) = focal_length_h;
intrinsics.at<float> (0, 2) = grayscale.cols/2;
intrinsics.at<float> (1, 2) = grayscale.rows/2;
cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1);
for (int i = 0; i < 5; i++)
dist_coeffs.at<float>(i) = 0;
std::vector< aruco::Marker > markers;
if (last_roi.width > 0 &&
(detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false),
markers.size() == 1 && markers[0].size() == 4))
{
detector.setMinMaxSize(std::max(0.2, 0.08 * grayscale.cols / last_roi.width),
std::min(1.0, 0.39 * grayscale.cols / last_roi.width));
auto& m = markers.at(0);
for (int i = 0; i < 4; i++)
{
auto& p = m.at(i);
p.x += last_roi.x;
p.y += last_roi.y;
}
}
else
{
detector.setMinMaxSize(0.09, 0.4);
detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false);
}
if (markers.size() == 1 && markers[0].size() == 4) {
const aruco::Marker& m = markers.at(0);
for (int i = 0; i < 4; i++)
cv::line(color, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), scale, 8);
}
auto time = cv::getTickCount();
if ((long) (time / freq) != (long) (last_time / freq))
{
last_fps = fps;
fps = 0;
last_time = time;
}
fps++;
char buf[128];
frame = color.clone();
::sprintf(buf, "Hz: %d", last_fps);
cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(0, 255, 0), scale);
::sprintf(buf, "Jiffies: %ld", (long) (10000 * (time - tm) / freq));
cv::putText(frame, buf, cv::Point(10, 54), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(80, 255, 0), scale);
if (markers.size() == 1 && markers[0].size() == 4) {
const aruco::Marker& m = markers.at(0);
const float size = 7;
cv::Mat obj_points(4,3,CV_32FC1);
obj_points.at<float>(1,0)=-size + headpos[0];
obj_points.at<float>(1,1)=-size + headpos[1];
obj_points.at<float>(1,2)=0 + headpos[2];
obj_points.at<float>(2,0)=size + headpos[0];
obj_points.at<float>(2,1)=-size + headpos[1];
obj_points.at<float>(2,2)=0 + headpos[2];
obj_points.at<float>(3,0)=size + headpos[0];
obj_points.at<float>(3,1)=size + headpos[1];
obj_points.at<float>(3,2)=0 + headpos[2];
obj_points.at<float>(0,0)=-size + headpos[0];
obj_points.at<float>(0,1)=size + headpos[1];
obj_points.at<float>(0,2)=0 + headpos[2];
last_roi = cv::Rect(65535, 65535, 0, 0);
for (int i = 0; i < 4; i++)
{
auto foo = m.at(i);
last_roi.x = std::min<int>(foo.x, last_roi.x);
last_roi.y = std::min<int>(foo.y, last_roi.y);
last_roi.width = std::max<int>(foo.x, last_roi.width);
last_roi.height = std::max<int>(foo.y, last_roi.height);
}
{
last_roi.width -= last_roi.x;
last_roi.height -= last_roi.y;
last_roi.x -= last_roi.width * stateful_coeff;
last_roi.y -= last_roi.height * stateful_coeff;
last_roi.width *= stateful_coeff * 3;
last_roi.height *= stateful_coeff * 3;
last_roi.x = std::max<int>(0, last_roi.x);
last_roi.y = std::max<int>(0, last_roi.y);
last_roi.width = std::min<int>(grayscale.cols - last_roi.x, last_roi.width);
last_roi.height = std::min<int>(grayscale.rows - last_roi.y, last_roi.height);
}
cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::ITERATIVE);
cv::Mat rotation_matrix = cv::Mat::zeros(3, 3, CV_64FC1);
cv::Mat junk1(3, 3, CV_64FC1), junk2(3, 3, CV_64FC1);
cv::Rodrigues(rvec, rotation_matrix);
{
const double beta = headpitch * HT_PI / 180;
double pitch[] = {
1, 0, 0,
0, cos(beta), -sin(beta),
0, sin(beta), cos(beta)
};
cv::Mat rot(3, 3, CV_64F, pitch);
cv::Mat tvec2 = rot * tvec;
rotation_matrix = rot * rotation_matrix;
cv::Vec3d euler = cv::RQDecomp3x3(rotation_matrix, junk1, junk2);
QMutexLocker lck(&mtx);
for (int i = 0; i < 3; i++)
pose[i] = tvec2.at<double>(i);
pose[Yaw] = euler[1];
pose[Pitch] = -euler[0];
pose[Roll] = euler[2];
}
std::vector<cv::Point2f> repr2;
std::vector<cv::Point3f> centroid;
centroid.push_back(cv::Point3f(0, 0, 0));
cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, repr2);
{
auto s = cv::Scalar(255, 0, 255);
cv::circle(frame, repr2.at(0), 4, s, -1);
}
last_centroid = repr2[0];
}
else
{
last_roi = cv::Rect(65535, 65535, 0, 0);
}
if (frame.rows > 0)
videoWidget->update_image(frame);
}
}
