void processImage(cv::Mat& image, cv::Mat& image_gray) {
// alternative way is to grab, then retrieve; allows for
// multiple grab when processing below frame rate - v4l keeps a
// number of frames buffered, which can lead to significant lag
// m_cap.grab();
// m_cap.retrieve(image);
// detect April tags (requires a gray scale image)
//cv::cvtColor(image, image_gray, CV_BGR2GRAY);
process_flag = true;
cout<<"process begin"<<endl;
double t = tic();
double t0;
if (m_timing) {
t0 = tic();
}
vector<AprilTags::TagDetection> detections = m_tagDetector->extractTags(image_gray);
if (m_timing) {
double dt = tic()-t0;
cout << "Extracting tags took " << dt << " seconds." << endl;
}
// print out each detection
cout << detections.size() << " tags detected:" << endl;
for (int i=0; i<detections.size(); i++) {
print_detection(detections[i]);
}
// show the current image including any detections
if (m_draw) {
for (int i=0; i<detections.size(); i++) {
// also highlight in the image
detections[i].draw(image_gray);
}
imshow(windowName, image_gray); // OpenCV call
}
// optionally send tag information to serial port (e.g. to Arduino)
if (m_arduino) {
if (detections.size() > 0) {
// only the first detected tag is sent out for now
Eigen::Vector3d translation;
Eigen::Matrix3d rotation;
detections[0].getRelativeTranslationRotation(m_tagSize, m_fx, m_fy, m_px, m_py,
translation, rotation);
m_serial.print(detections[0].id);
m_serial.print(",");
m_serial.print(translation(0));
m_serial.print(",");
m_serial.print(translation(1));
m_serial.print(",");
m_serial.print(translation(2));
m_serial.print("\n");
} else {
// no tag detected: tag ID = -1
m_serial.print("-1,0.0,0.0,0.0\n");
}
}
cout<<"process end:"<<endl;
process_flag = false;
}
