/* Convert AprilTags::TagDetection to duckietown_msgs::TagDetection */ duckietown_msgs::TagDetection toMsg (const AprilTags::TagDetection& detection) { duckietown_msgs::TagDetection msg; msg.good = detection.good; msg.id = detection.id; for (int i = 0; i < msg.p.size(); ++i){ msg.p.push_back(detection.p[i].first); msg.p.push_back(detection.p[i].second); } msg.cxy.push_back(detection.cxy.first); msg.cxy.push_back(detection.cxy.second); msg.observedPerimeter = detection.observedPerimeter; msg.homography.push_back(detection.homography(0,0)); msg.homography.push_back(detection.homography(0,1)); msg.homography.push_back(detection.homography(0,2)); msg.homography.push_back(detection.homography(1,0)); msg.homography.push_back(detection.homography(1,1)); msg.homography.push_back(detection.homography(1,2)); msg.homography.push_back(detection.homography(2,0)); msg.homography.push_back(detection.homography(2,1)); msg.homography.push_back(detection.homography(2,2)); msg.orientation = detection.getXYOrientation(); msg.hxy.push_back(detection.hxy.first); msg.hxy.push_back(detection.hxy.second); /* Extract focal length and principal point */ double fx = camera_info_.K[0]; double fy = camera_info_.K[4]; double px = camera_info_.K[2]; double py = camera_info_.K[5]; /* Extract relative translation rotation */ Eigen::Vector3d trans; Eigen::Matrix3d rot; detection.getRelativeTranslationRotation(tag_size_,fx,fy,px,py,trans,rot); msg.transform.translation.x = trans(0); msg.transform.translation.y = trans(1); msg.transform.translation.z = trans(2); tf::Matrix3x3 tf_matrix3x3; tf::matrixEigenToTF(rot,tf_matrix3x3); tf::Quaternion quat; tf_matrix3x3.getRotation(quat); msg.transform.rotation.x = quat.x(); msg.transform.rotation.y = quat.y(); msg.transform.rotation.z = quat.z(); msg.transform.rotation.w = quat.w(); return msg; }
void MultiAprilTagsTracker::imageDrawnCallback( const sensor_msgs::ImageConstPtr& msg) { cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy( msg, "bgr8" ); } catch( cv_bridge::Exception& e ) { ROS_ERROR( "cv_bridge exce[topm: %s", e.what() ); return; } if( tags.size() > 0) { multi_apriltags_tracker::april_tag_pos msg; for( unsigned int i=0; i<tags.size(); i++ ){ AprilTags::TagDetection tag = tags[i]; circle( cv_ptr->image, Point2f( tag.cxy.first, tag.cxy.second ), 2, Scalar(0,255,0), 4 ); line( cv_ptr->image, Point( tag.p[0].first, tag.p[0].second ), Point( tag.p[1].first, tag.p[1].second ), Scalar(0,255,0), 2 ); line( cv_ptr->image, Point( tag.p[1].first, tag.p[1].second ), Point( tag.p[2].first, tag.p[2].second ), Scalar(0,255,0), 2 ); line( cv_ptr->image, Point( tag.p[2].first, tag.p[2].second ), Point( tag.p[3].first, tag.p[3].second ), Scalar(0,255,0), 2 ); line( cv_ptr->image, Point( tag.p[3].first, tag.p[3].second ), Point( tag.p[0].first, tag.p[0].second ), Scalar(0,255,0), 2 ); double orientation_length = sqrt( pow(tag.p[0].first-tag.p[1].first,2) + pow(tag.p[0].second-tag.p[1].second,2) ); line( cv_ptr->image, Point( tag.cxy.first, tag.cxy.second ), Point( tag.cxy.first+orientation_length*cos(tag.getXYOrientation()), tag.cxy.second+orientation_length*sin(tag.getXYOrientation()) ), Scalar(0,255,0), 2 ); msg.id.push_back( tag.id ); geometry_msgs::Pose2D pose; pose.x = tag.cxy.first; pose.y = tag.cxy.second; pose.theta = convRadius( tag.getXYOrientation() ); msg.pose.push_back( pose ); } m_pos_pub.publish(msg); } cv::imshow(MULTI_APRIL_TAGS_TRACKER_VIEW, cv_ptr->image ); if( false == ros::ok() ) { ros::shutdown(); } }