/* 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();
}
}