marker_detection.cpp

Go to the documentation of this file.

#include "marker_detection.h"
#include <ros/console.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

namespace aruco_marker_recognition {

MarkerDetection::MarkerDetection(double marker_size) : marker_size_(marker_size) {

}

std::vector<aruco::Marker> MarkerDetection::detect(const cv::Mat &image, CameraId id, const ArucoMarkerRecognitionConfig &config) {
    aruco::MarkerDetector mDetector;


    mDetector.setMinMaxSize(config.minSize, config.maxSize);
    switch (config.thresholdMethod) {
    case 0:
        mDetector.setThresholdMethod(aruco::MarkerDetector::FIXED_THRES);
        break;
    case 1:
        mDetector.setThresholdMethod(aruco::MarkerDetector::ADPT_THRES);
        break;
    case 2:
        mDetector.setThresholdMethod(aruco::MarkerDetector::CANNY);
        break;
    }
    mDetector.setThresholdParams(config.minThreshold, config.maxThreshold);

    std::vector<aruco::Marker> markers;
    if (id == CameraId::cam_left) {
        if (cam_params_left_.isValid()) {
            mDetector.detect(image, markers, cam_params_left_, marker_size_);

            if (config.showThresholdImage) {
                //cv::imshow("ThresholdImage", mDetector.getThresholdedImage());
                //cv::waitKey(10);
            }
            ROS_DEBUG_STREAM("Found " << markers.size() << " marker(s) in left camera image");
        } else {
            ROS_ERROR_STREAM("Left camera parameters (aruco) are not valid");
        }
    } else {
        if (cam_params_right_.isValid()) {
            mDetector.detect(image, markers, cam_params_right_, marker_size_);
            ROS_DEBUG_STREAM("Found " << markers.size() << " marker(s) in right camera image");
        } else {
            ROS_ERROR_STREAM("Right camera parameters (aruco) are not valid");
        }
    }

    return markers;
}

void MarkerDetection::setCameraParameters(const sensor_msgs::CameraInfo &cam_params_left, const sensor_msgs::CameraInfo &cam_params_right) {
    cv::Mat cameraMatrix(3, 3, CV_32FC1);
    cv::Mat distorsionCoeff(4, 1, CV_32FC1);
    //transform camera paramters from ros message to aruco representation (left)
    cv::Size size(cam_params_left.height, cam_params_left.width);
    for (int i=0; i<9; ++i) {
        cameraMatrix.at<float>(i%3, i-(i%3)*3) = cam_params_left.K[i];
    }
    if (cam_params_left.D.size() >= 4) {
        for (int i=0; i<4; ++i) {
            distorsionCoeff.at<float>(i, 0) = cam_params_left.D[i];
        }
    }
    cam_params_left_ = aruco::CameraParameters(cameraMatrix, distorsionCoeff, size);

    //transform camera paramters from ros message to aruco representation (right)
    size = cv::Size(cam_params_right.height, cam_params_right.width);
    for (int i=0; i<9; ++i) {
        cameraMatrix.at<float>(i%3, i-(i%3)*3) = cam_params_right.K[i];
    }
    if (cam_params_right.D.size() >= 4) {
        for (int i=0; i<4; ++i) {
            distorsionCoeff.at<float>(i, 0) = cam_params_right.D[i];
        }
    }
    cam_params_right_ = aruco::CameraParameters(cameraMatrix, distorsionCoeff, size);
}

}