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
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
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
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
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
00208
00209
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
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
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;
00366 arrow_marker.color.g = 0;
00367 arrow_marker.color.b = isNsX ? 0 : 1;
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
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
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 }