aruco_marker_recognition.cpp
Go to the documentation of this file.
00001 
00018 #include "aruco_marker_recognition.h"
00019 #include <cv_bridge/cv_bridge.h>
00020 #include <camera_calibration_parsers/parse.h>
00021 #include <ros_uri/ros_uri.hpp>
00022 #include <Tracking/ICP.h>
00023 
00024 #include "opencv2/imgproc/imgproc.hpp"
00025 #include "opencv2/highgui/highgui.hpp"
00026 
00027 
00028 namespace aruco_marker_recognition {
00029 
00030 typedef boost::array< ::geometry_msgs::Point_<std::allocator<void> > , 8> BoundingBox;
00031 
00032 ArucoMarkerRecognition::ArucoMarkerRecognition() : nh_(NODE_NAME), marker_size_(DEFAULT_MARKER_SIZE), recognition_paused_(true)
00033 {
00034     ROS_DEBUG("Initializing recognition system");
00035     nh_.getParam("use_stereo", use_stereo_);
00036 
00037     nh_.getParam("image_left_topic", image_left_topic_);
00038     nh_.getParam("image_right_topic", image_right_topic_);
00039     nh_.getParam("image_left_cam_info_topic", image_left_cam_info_topic_);
00040     nh_.getParam("image_right_cam_info_topic", image_right_cam_info_topic_);
00041 
00042     // Set up dynamic reconfigure
00043     reconfigure_server_.setCallback(boost::bind(&ArucoMarkerRecognition::configCallback, this, _1, _2));
00044 
00045     nh_.getParam("marker_size", marker_size_);
00046     detector_ = MarkerDetection(marker_size_);
00047 
00048     ROS_DEBUG("Subscribe to image topics");
00049     image_left_sub_.subscribe(nh_, image_left_topic_, 1);
00050     image_left_param_sub_.subscribe(nh_, image_left_cam_info_topic_, 1);
00051 
00052     if (use_stereo_) {
00053         image_right_sub_.subscribe(nh_, image_right_topic_, 1);
00054         image_right_param_sub_.subscribe(nh_, image_right_cam_info_topic_, 1);
00055     } else {
00056         image_right_sub_.subscribe(nh_, image_left_topic_, 1);
00057         image_right_param_sub_.subscribe(nh_, image_left_cam_info_topic_, 1);
00058     }
00059 
00060     sync_policy_.reset(new ApproximateSync(ApproximatePolicy(20), image_left_sub_, image_right_sub_, image_left_param_sub_, image_right_param_sub_) );
00061     sync_policy_->registerCallback(boost::bind(&ArucoMarkerRecognition::imageCallback, this, _1, _2, _3, _4));
00062 
00063     ROS_DEBUG("Advertise services");
00064     get_recognizer_service_ = nh_.advertiseService(GET_RECOGNIZER_SERVICE_NAME, &ArucoMarkerRecognition::processGetRecognizerRequest, this);
00065     release_recognizer_service_ = nh_.advertiseService(RELEASE_RECOGNIZER_SERVICE_NAME, &ArucoMarkerRecognition::processReleaseRecognizerRequest, this);
00066 
00067     object_mesh_service_client_ = nh_.serviceClient<asr_object_database::ObjectMetaData>(OBJECT_DB_SERVICE_META_DATA);
00068 
00069     std::string left_markers_img_topic;
00070     std::string right_markers_img_topic;
00071     std::string asr_objects_topic;
00072     nh_.getParam("left_markers_img_topic", left_markers_img_topic);
00073     nh_.getParam("right_markers_img_topic", right_markers_img_topic);
00074     nh_.getParam("asr_objects_topic", asr_objects_topic);
00075     left_markers_img_pub_ = nh_.advertise<sensor_msgs::Image> (left_markers_img_topic, 10);
00076     if (use_stereo_) {
00077         right_markers_img_pub_ = nh_.advertise<sensor_msgs::Image> (right_markers_img_topic, 10);
00078     }
00079     asr_objects_pub_ = nh_.advertise<asr_msgs::AsrObject> (asr_objects_topic, 10);
00080 
00081     std::string vis_markers_topic;
00082     nh_.getParam("vis_markers_topic", vis_markers_topic);
00083     vis_markers_pub_ = nh_.advertise<visualization_msgs::Marker> (vis_markers_topic, 10);
00084 
00085     nh_.getParam("recognizer_name", recognizer_name_);
00086 
00087     ROS_DEBUG("Initializing completed");
00088 
00089     ROS_DEBUG("Recognition is paused");
00090 
00091 }
00092 
00093 bool ArucoMarkerRecognition::processGetRecognizerRequest(GetRecognizer::Request &req, GetRecognizer::Response &res) {
00094     ROS_DEBUG("Process get recognizer request");
00095     recognition_paused_ = false;
00096     ROS_DEBUG("Recognition is running");
00097     return true;
00098 }
00099 
00100 bool ArucoMarkerRecognition::processReleaseRecognizerRequest(ReleaseRecognizer::Request &req, ReleaseRecognizer::Response &res) {
00101     ROS_DEBUG("Process release recognizer request");
00102     recognition_paused_ = true;
00103     ROS_DEBUG("Recognition is paused");
00104     return true;
00105 }
00106 
00107 void ArucoMarkerRecognition::imageCallback(const sensor_msgs::ImageConstPtr& input_img_left, const sensor_msgs::ImageConstPtr& input_img_right, const sensor_msgs::CameraInfoConstPtr& cam_info_left, const sensor_msgs::CameraInfoConstPtr& cam_info_right) {
00108     if (!recognition_paused_) {
00109         ROS_DEBUG("Images received");
00110         cv_bridge::CvImagePtr cv_left;
00111         cv_bridge::CvImagePtr cv_right;
00112         try {
00113             cv_left = cv_bridge::toCvCopy(input_img_left, sensor_msgs::image_encodings::BGR8);
00114             cv_right = cv_bridge::toCvCopy(input_img_right, sensor_msgs::image_encodings::BGR8);
00115         } catch (cv_bridge::Exception& e) {
00116             ROS_ERROR("Cv_bridge exception: %s", e.what());
00117             return;
00118         }
00119         cv::Mat img;
00120         cv::GaussianBlur(cv_left->image, img, cv::Size(0, 0), config_.gaussianBlurSigma);
00121         cv::addWeighted(cv_left->image, config_.sharpenWeightImage, img, config_.sharpenWeightBlur, 0, cv_left->image);
00122 
00123         cv::GaussianBlur(cv_right->image, img, cv::Size(0, 0), config_.gaussianBlurSigma);
00124         cv::addWeighted(cv_right->image, config_.sharpenWeightImage, img, config_.sharpenWeightBlur, 0, cv_right->image);
00125 
00126         detector_.setCameraParameters(*cam_info_left, *cam_info_right);
00127 
00128         std::vector<aruco::Marker> left_markers = detector_.detect(cv_left->image, MarkerDetection::CameraId::cam_left, config_);
00129         std::vector<aruco::Marker> right_markers;
00130         std::map<int, geometry_msgs::Pose> idPoseMap;
00131         if (use_stereo_) {
00132             right_markers = detector_.detect(cv_right->image, MarkerDetection::CameraId::cam_right, config_);
00133 
00134             ivt_bridge::IvtStereoCalibration ivtCalib;
00135             ivtCalib.fromCameraInfo(cam_info_left, cam_info_right);
00136 
00137             idPoseMap = getMarkerPoses(left_markers, right_markers, ivtCalib);
00138         } else {
00139             idPoseMap = getMarkerPoses(left_markers);
00140         }
00141         std::string cam_left_frame_id = "/" + cam_info_left->header.frame_id;
00142 
00143         //publish marker visualizations
00144         int marker_id = 1;
00145         for (auto it = idPoseMap.begin(); it != idPoseMap.end(); ++it) {
00146             vis_markers_pub_.publish(createSquareMarker(marker_id, it->second, cam_left_frame_id));
00147             vis_markers_pub_.publish(createArrowMarker(marker_id, it->second, cam_left_frame_id));
00148             vis_markers_pub_.publish(createArrowMarker(marker_id, it->second, cam_left_frame_id, true));
00149             ++marker_id;
00150         }
00151 
00152         //publish marker mesh visualizations & asr-objects
00153         int id = 0;
00154         for (auto idPosePair : idPoseMap) {
00155             std::string object_type = "marker_" + std::to_string(idPosePair.first);
00156             asr_object_database::ObjectMetaData objectMetaData;
00157             objectMetaData.request.object_type = object_type;
00158             objectMetaData.request.recognizer = recognizer_name_;
00159             object_mesh_service_client_.call(objectMetaData);
00160 
00161             if (objectMetaData.response.is_valid) {
00162                 vis_markers_pub_.publish(createMeshMarker(id, idPosePair.second, objectMetaData.response.object_mesh_resource, cam_left_frame_id));
00163                 asr_objects_pub_.publish(createAsrObject(idPosePair.second, object_type, cam_left_frame_id, objectMetaData.response.object_mesh_resource));
00164                 id++;
00165             } else {
00166                 asr_objects_pub_.publish(createAsrObject(idPosePair.second, object_type, cam_left_frame_id));
00167             }
00168         }
00169 
00170 
00171         //publish images with marker overlay
00172         cv_left->image = drawMarkers(cv_left->image, left_markers);
00173         left_markers_img_pub_.publish(cv_left->toImageMsg());
00174 
00175         if (use_stereo_) {
00176             cv_right->image = drawMarkers(cv_right->image, right_markers);
00177             right_markers_img_pub_.publish(cv_right->toImageMsg());
00178         }
00179         ROS_DEBUG("--------------\n");
00180     }
00181 }
00182 
00183 void ArucoMarkerRecognition::configCallback(ArucoMarkerRecognitionConfig &config, uint32_t level) {
00184     this->config_ = config;
00185 }
00186 
00187 cv::Mat ArucoMarkerRecognition::drawMarkers(const cv::Mat &image, const std::vector<aruco::Marker> &markers) {
00188     cv::Mat output = image;
00189     for (unsigned int i = 0; i < markers.size(); i++) {
00190         markers[i].draw(output, cv::Scalar(0, 0, 255), 2);
00191     }
00192     return output;
00193 }
00194 
00195 std::map<int, geometry_msgs::Pose> ArucoMarkerRecognition::getMarkerPoses(const std::vector<aruco::Marker> &left_markers) {
00196     std::map<int, geometry_msgs::Pose> idPoseMap;
00197     for (aruco::Marker marker : left_markers) {
00198         geometry_msgs::Pose pose;
00199         pose.position.x = marker.Tvec.at<float>(0,0);
00200         pose.position.y = marker.Tvec.at<float>(1,0);
00201         pose.position.z = marker.Tvec.at<float>(2,0);
00202 
00203         cv::Mat rot_cv(3, 3, CV_32FC1);
00204         cv::Rodrigues(marker.Rvec, rot_cv);
00205 
00206         cv::Mat rotate_to_ros(3, 3, CV_32FC1);
00207         //  0 1  0
00208         //  0 0 -1
00209         // -1 0  0
00210         rotate_to_ros.at<float>(0,0) = 0.0;
00211         rotate_to_ros.at<float>(0,1) = 1.0;
00212         rotate_to_ros.at<float>(0,2) = 0.0;
00213         rotate_to_ros.at<float>(1,0) = 0.0;
00214         rotate_to_ros.at<float>(1,1) = 0.0;
00215         rotate_to_ros.at<float>(1,2) = -1.0;
00216         rotate_to_ros.at<float>(2,0) = -1.0;
00217         rotate_to_ros.at<float>(2,1) = 0.0;
00218         rotate_to_ros.at<float>(2,2) = 0.0;
00219         rot_cv = rot_cv*rotate_to_ros.t();
00220 
00221         float q4 = 0.5f * sqrt(1.0f + rot_cv.at<float>(0,0) + rot_cv.at<float>(1,1) + rot_cv.at<float>(2,2));
00222         float t = (1.0f / (4.0f * q4));
00223         float q1 = t * (rot_cv.at<float>(2,1) - rot_cv.at<float>(1,2));
00224         float q2 = t * (rot_cv.at<float>(0,2) - rot_cv.at<float>(2,0));
00225         float q3 = t * (rot_cv.at<float>(1,0) - rot_cv.at<float>(0,1));
00226 
00227         pose.orientation.x = q1;
00228         pose.orientation.y = q2;
00229         pose.orientation.z = q3;
00230         pose.orientation.w = q4;
00231 
00232         idPoseMap.insert(std::make_pair(marker.id, pose));
00233     }
00234     return idPoseMap;
00235 }
00236 
00237 std::map<int, geometry_msgs::Pose> ArucoMarkerRecognition::getMarkerPoses(const std::vector<aruco::Marker> &left_markers, const std::vector<aruco::Marker> &right_markers, const ivt_bridge::IvtStereoCalibration &ivtCalib) {
00238     std::map<int, geometry_msgs::Pose> idPoseMap;
00239     boost::shared_ptr<CStereoCalibration> stereoCalibPtr = ivtCalib.getStereoCalibration();
00240 
00241     for (unsigned int i = 0; i < left_markers.size(); i++) {
00242         int left_id = left_markers.at(i).id;
00243         for (unsigned int j = 0; j < right_markers.size(); j++) {
00244             if (right_markers.at(j).id == left_id) {
00245                 CVec3dArray world_marker_corners(4);
00246                 CVec2dArray image_marker_corners_left(4);
00247                 CVec3dArray camera_marker_corners_left(4);
00248 
00249                 for (int k = 0; k < 4; ++k) {
00250                     Vec2d img_point_left;
00251                     Vec3d world_point = {0.0f};
00252                     Vec2d left_point = { left_markers.at(i)[k].x, left_markers.at(i)[k].y };
00253                     Vec2d right_point = { right_markers.at(j)[k].x, right_markers.at(j)[k].y };
00254                     stereoCalibPtr->Calculate3DPoint(left_point, right_point, world_point, false, false);
00255                     world_marker_corners.AddElement(world_point);
00256 
00257                     stereoCalibPtr->GetLeftCalibration()->WorldToImageCoordinates(world_point, img_point_left);
00258                     image_marker_corners_left.AddElement(img_point_left);
00259                 }
00260 
00261                 Vec3d m0 = {0.0, 0.0, 0.0};
00262                 Vec3d m1 = {static_cast<float>(marker_size_ * 1000), 0.0, 0.0};
00263                 Vec3d m2 = {static_cast<float>(marker_size_ * 1000),static_cast<float>(marker_size_ * 1000), 0.0};
00264                 Vec3d m3 = {0.0, static_cast<float>(marker_size_ * 1000), 0.0};
00265                 for (int k = 0; k < 4; ++k) {
00266                     stereoCalibPtr->GetLeftCalibration()->WorldToCameraCoordinates(world_marker_corners[k], camera_marker_corners_left[k]);
00267 
00268                 }
00269                 Vec3d sourcePoints[4] = {m0, m1, m2, m3};
00270                 Vec3d targetPointsLeft[4] = {camera_marker_corners_left[0],camera_marker_corners_left[1],camera_marker_corners_left[2],camera_marker_corners_left[3]};
00271 
00272                 Mat3d rotation_left;
00273                 Vec3d translation_left;
00274                 CICP::CalculateOptimalTransformation(sourcePoints, targetPointsLeft, 4, rotation_left, translation_left);
00275 
00276                 //translate marker position from corner to center
00277                 const Vec3d x = { static_cast<float>(marker_size_ * 1000) / 2.0f, 0.0f, 0.0f };
00278                 Vec3d x2;
00279                 const Vec3d y = { 0.0f, static_cast<float>(marker_size_ * 1000) / 2.0f, 0.0f };
00280                 Vec3d y2;
00281 
00282                 Math3d::MulMatVec(rotation_left, x, x2);
00283                 Math3d::MulMatVec(rotation_left, y, y2);
00284 
00285                 Math3d::AddToVec(translation_left, x2);
00286                 Math3d::AddToVec(translation_left, y2);
00287 
00288                 //rotate marker in xz-plane, normal direction == -y
00289                 Mat3d B;
00290                 Vec3d axis;
00291                 axis.x = 1.0;
00292                 axis.y = 0.0;
00293                 axis.z = 0.0;
00294                 float theta = 0.5 * M_PI;
00295                 Math3d::SetRotationMat(B, axis, theta);
00296 
00297                 Mat3d C;
00298                 Math3d::MulMatMat(rotation_left, B, C);
00299 
00300                 geometry_msgs::Pose pose;
00301                 pose.position.x = translation_left.x / 1000;
00302                 pose.position.y = translation_left.y / 1000;
00303                 pose.position.z = translation_left.z / 1000;
00304 
00305                 float q4 = 0.5f * sqrt(1.0f + C.r1 + C.r5 + C.r9);
00306                 float t = (1.0f / (4.0f * q4));
00307                 float q1 = t * (C.r8 - C.r6);
00308                 float q2 = t * (C.r3 - C.r7);
00309                 float q3 = t * (C.r4 - C.r2);
00310 
00311                 pose.orientation.x = q1;
00312                 pose.orientation.y = q2;
00313                 pose.orientation.z = q3;
00314                 pose.orientation.w = q4;
00315 
00316                 idPoseMap.insert(std::make_pair(left_id, pose));
00317                 break;
00318             }
00319         }
00320     }
00321 
00322     return idPoseMap;
00323 }
00324 
00325 visualization_msgs::Marker ArucoMarkerRecognition::createSquareMarker(int id, const geometry_msgs::Pose &pose, const string &frame_id) {
00326     visualization_msgs::Marker square_marker;
00327     square_marker.header.frame_id = frame_id;
00328     square_marker.ns = "Found markers";
00329     square_marker.id = id;
00330     square_marker.action = visualization_msgs::Marker::ADD;
00331     square_marker.pose.position = pose.position;
00332     square_marker.pose.orientation = pose.orientation;
00333     square_marker.color.a = 1;
00334     square_marker.color.r = 1;
00335     square_marker.color.g = 0;
00336     square_marker.color.b = 0;
00337     square_marker.scale.x = marker_size_;
00338     square_marker.scale.y = 0.001;
00339     square_marker.scale.z = marker_size_;
00340     square_marker.type = visualization_msgs::Marker::CUBE;
00341     square_marker.lifetime = ros::Duration(2);
00342 
00343     return square_marker;
00344 }
00345 
00346 visualization_msgs::Marker ArucoMarkerRecognition::createArrowMarker(int id, const geometry_msgs::Pose &pose, const string &frame_id, bool isNsX) {
00347     visualization_msgs::Marker arrow_marker;
00348     arrow_marker.header.frame_id = frame_id;
00349     arrow_marker.ns = isNsX ? "Found markers (orientation x)" : "Found markers (orientation)";
00350     arrow_marker.id = id;
00351     arrow_marker.action = visualization_msgs::Marker::ADD;
00352     arrow_marker.pose.position = pose.position;
00353     arrow_marker.pose.orientation = pose.orientation;
00354     geometry_msgs::Point p0;
00355     p0.x = 0;
00356     p0.y = 0;
00357     p0.z = 0;
00358     geometry_msgs::Point p1;
00359     p1.x = isNsX ? 0.05 : 0;
00360     p1.y = isNsX ? 0 : -0.05;
00361     p1.z = 0;
00362     arrow_marker.points.push_back(p0);
00363     arrow_marker.points.push_back(p1);
00364     arrow_marker.color.a = 1;
00365     arrow_marker.color.r = isNsX ? 1 : 0; // marker color is red if ns = x
00366     arrow_marker.color.g = 0;
00367     arrow_marker.color.b = isNsX ? 0 : 1; // marker color is blue if ns != x
00368     arrow_marker.scale.x = 0.01;
00369     arrow_marker.scale.y = 0.02;
00370     arrow_marker.scale.z = 0.02;
00371     arrow_marker.type = visualization_msgs::Marker::ARROW;
00372     arrow_marker.lifetime = ros::Duration(2);
00373 
00374     return arrow_marker;
00375 }
00376 
00377 visualization_msgs::Marker ArucoMarkerRecognition::createMeshMarker(int id, const geometry_msgs::Pose &pose, const std::string &mesh_res, const string &frame_id) {
00378 
00379     visualization_msgs::Marker marker;
00380     marker.header.frame_id = frame_id;
00381     marker.ns = "Found markers (meshes)";
00382     marker.id = id;
00383     marker.action = visualization_msgs::Marker::ADD;
00384     marker.pose.position = pose.position;
00385     marker.pose.orientation = pose.orientation;
00386 
00387     marker.scale.x = marker.scale.y = marker.scale.z = 0.001;
00388     marker.color.r = marker.color.g = marker.color.b = 130.0f / 255.0f;
00389     marker.color.a = 1.0;
00390 
00391     marker.mesh_resource = mesh_res;
00392     marker.mesh_use_embedded_materials = false;
00393 
00394     marker.type = visualization_msgs::Marker::MESH_RESOURCE;
00395     marker.lifetime = ros::Duration(2);
00396 
00397     return marker;
00398 }
00399 
00400 asr_msgs::AsrObject ArucoMarkerRecognition::createAsrObject(const geometry_msgs::Pose &pose, const std::string &object_type, const string &frame_id, const std::string &mesh_res) {
00401     asr_msgs::AsrObject o;
00402 
00403     o.header.frame_id = frame_id;
00404     o.header.stamp = ros::Time::now();
00405     o.providedBy = recognizer_name_;
00406     geometry_msgs::PoseWithCovariance current_pose_with_c;
00407     current_pose_with_c.pose = pose;
00408     o.sampledPoses.push_back(current_pose_with_c);
00409 
00410     //Set bounding box as planar rectangle
00411     BoundingBox bounding_box;
00412 
00413     Vec3d minPoint;
00414     minPoint.x = minPoint.y = minPoint.z = 0.0;
00415     Vec3d maxPoint;
00416     maxPoint.x = maxPoint.z = marker_size_;
00417     maxPoint.y = 0.0;
00418 
00419     for (unsigned int z = 0; z < 2; z++) {
00420         for (unsigned int y = 0; y < 2; y++) {
00421             for (unsigned int x = 0; x < 2; x++) {
00422                 bounding_box[4*z+2*y+x].x = (1 - x) * minPoint.x + x * maxPoint.x;
00423                 bounding_box[4*z+2*y+x].y = (1 - y) * minPoint.y + y * maxPoint.y;
00424                 bounding_box[4*z+2*y+x].z = (1 - z) * minPoint.z + z * maxPoint.z;
00425             }
00426         }
00427     }
00428 
00429     o.boundingBox = bounding_box;
00430 
00431     o.color.r = o.color.g = o.color.b = 130.0f / 255.0f;
00432     o.color.a = 1.0;
00433     o.type = object_type;
00434     o.typeConfidence = 1.0f;
00435     o.identifier = "052052051100";
00436     o.meshResourcePath = mesh_res;
00437 
00438     return o;
00439 }
00440 
00441 
00442 }
00443 
00444 int main(int argc, char** argv) {
00445     ros::init(argc, argv, "asr_aruco_marker_recognition");
00446     ROS_INFO("Starting up ArucoMarkerRecognition.\n");
00447     aruco_marker_recognition::ArucoMarkerRecognition rec;
00448     ROS_DEBUG("ArucoMarkerRecognition is started up.\n");
00449     //Here we set the frame rate with which marker recognition shall work.
00450     unsigned int hertz = 5;
00451     ROS_DEBUG_STREAM("ArucoMarkerRecognition runs at " << hertz << " hertz.\n");
00452     ros::Rate r(hertz);
00453 
00454     while (ros::ok()) {
00455       ros::spinOnce();
00456       r.sleep();
00457     }
00458     return 0;
00459 }


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