marker_detection.cpp
Go to the documentation of this file.
00001 
00019 #include "marker_detection.h"
00020 #include <ros/console.h>
00021 #include <opencv2/core/core.hpp>
00022 #include <opencv2/highgui/highgui.hpp>
00023 
00024 namespace aruco_marker_recognition {
00025 
00026 MarkerDetection::MarkerDetection(double marker_size) : marker_size_(marker_size) {
00027 
00028 }
00029 
00030 std::vector<aruco::Marker> MarkerDetection::detect(const cv::Mat &image, CameraId id, const ArucoMarkerRecognitionConfig &config) {
00031     aruco::MarkerDetector mDetector;
00032 
00033 
00034     mDetector.setMinMaxSize(config.minSize, config.maxSize);
00035     switch (config.thresholdMethod) {
00036     case 0:
00037         mDetector.setThresholdMethod(aruco::MarkerDetector::FIXED_THRES);
00038         break;
00039     case 1:
00040         mDetector.setThresholdMethod(aruco::MarkerDetector::ADPT_THRES);
00041         break;
00042     case 2:
00043         mDetector.setThresholdMethod(aruco::MarkerDetector::CANNY);
00044         break;
00045     }
00046     mDetector.setThresholdParams(config.minThreshold, config.maxThreshold);
00047 
00048     std::vector<aruco::Marker> markers;
00049     if (id == CameraId::cam_left) {
00050         if (cam_params_left_.isValid()) {
00051             mDetector.detect(image, markers, cam_params_left_, marker_size_);
00052 
00053             if (config.showThresholdImage) {
00054                 //cv::imshow("ThresholdImage", mDetector.getThresholdedImage());
00055                 //cv::waitKey(10);
00056             }
00057             ROS_DEBUG_STREAM("Found " << markers.size() << " marker(s) in left camera image");
00058         } else {
00059             ROS_ERROR_STREAM("Left camera parameters (aruco) are not valid");
00060         }
00061     } else {
00062         if (cam_params_right_.isValid()) {
00063             mDetector.detect(image, markers, cam_params_right_, marker_size_);
00064             ROS_DEBUG_STREAM("Found " << markers.size() << " marker(s) in right camera image");
00065         } else {
00066             ROS_ERROR_STREAM("Right camera parameters (aruco) are not valid");
00067         }
00068     }
00069 
00070     return markers;
00071 }
00072 
00073 void MarkerDetection::setCameraParameters(const sensor_msgs::CameraInfo &cam_params_left, const sensor_msgs::CameraInfo &cam_params_right) {
00074     cv::Mat cameraMatrix(3, 3, CV_32FC1);
00075     cv::Mat distorsionCoeff(4, 1, CV_32FC1);
00076     //transform camera paramters from ros message to aruco representation (left)
00077     cv::Size size(cam_params_left.height, cam_params_left.width);
00078     for (int i=0; i<9; ++i) {
00079         cameraMatrix.at<float>(i%3, i-(i%3)*3) = cam_params_left.K[i];
00080     }
00081     if (cam_params_left.D.size() >= 4) {
00082         for (int i=0; i<4; ++i) {
00083             distorsionCoeff.at<float>(i, 0) = cam_params_left.D[i];
00084         }
00085     }
00086     cam_params_left_ = aruco::CameraParameters(cameraMatrix, distorsionCoeff, size);
00087 
00088     //transform camera paramters from ros message to aruco representation (right)
00089     size = cv::Size(cam_params_right.height, cam_params_right.width);
00090     for (int i=0; i<9; ++i) {
00091         cameraMatrix.at<float>(i%3, i-(i%3)*3) = cam_params_right.K[i];
00092     }
00093     if (cam_params_right.D.size() >= 4) {
00094         for (int i=0; i<4; ++i) {
00095             distorsionCoeff.at<float>(i, 0) = cam_params_right.D[i];
00096         }
00097     }
00098     cam_params_right_ = aruco::CameraParameters(cameraMatrix, distorsionCoeff, size);
00099 }
00100 
00101 }
00102 


asr_aruco_marker_recognition
Author(s): Allgeyer Tobias, Mei├čner Pascal, Qattan Mohamad
autogenerated on Thu Jun 6 2019 21:14:12