ar_single.cpp
Go to the documentation of this file.
00001 /*
00002  *  Single Marker Pose Estimation using ARToolkit
00003  *  Copyright (C) 2013, I Heart Engineering
00004  *  Copyright (C) 2010, CCNY Robotics Lab
00005  *  William Morris <bill@iheartengineering.com>
00006  *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
00007  *  Gautier Dumonteil <gautier.dumonteil@gmail.com>
00008  *  http://www.iheartengineering.com
00009  *  http://robotics.ccny.cuny.edu
00010  *
00011  *  This program is free software: you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation, either version 3 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU General Public License for more details.
00020  *
00021  *  You should have received a copy of the GNU General Public License
00022  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00023  */
00024 
00025 #include "ar_pose/ar_single.h"
00026 
00027 int main (int argc, char **argv)
00028 {
00029   ros::init (argc, argv, "ar_single");
00030   ros::NodeHandle n;
00031   ar_pose::ARSinglePublisher arSingle(n);
00032   ros::spin();
00033   return 0;
00034 }
00035 
00036 namespace ar_pose
00037 {
00038   ARSinglePublisher::ARSinglePublisher (ros::NodeHandle & n):n_ (n), it_ (n_)
00039   {
00040     std::string local_path;
00041     std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
00042         std::string default_path = "data/patt.hiro";
00043     ros::NodeHandle n_param ("~");
00044     XmlRpc::XmlRpcValue xml_marker_center;
00045 
00046     ROS_INFO("Starting ArSinglePublisher");
00047 
00048     // **** get parameters
00049 
00050     if (!n_param.getParam("publish_tf", publishTf_))
00051       publishTf_ = true;
00052     ROS_INFO ("\tPublish transforms: %d", publishTf_);
00053 
00054     if (!n_param.getParam("publish_visual_markers", publishVisualMarkers_))
00055       publishVisualMarkers_ = true;
00056     ROS_INFO ("\tPublish visual markers: %d", publishVisualMarkers_);
00057 
00058     if (!n_param.getParam("threshold", threshold_))
00059       threshold_ = 100;
00060     ROS_INFO ("\tThreshold: %d", threshold_);
00061 
00062     if (!n_param.getParam("marker_width", markerWidth_))
00063       markerWidth_ = 80.0;
00064     ROS_INFO ("\tMarker Width: %.1f", markerWidth_);
00065 
00066     if (!n_param.getParam("reverse_transform", reverse_transform_))
00067       reverse_transform_ = false;
00068     ROS_INFO("\tReverse Transform: %d", reverse_transform_);
00069 
00070     if (!n_param.getParam("marker_frame", markerFrame_))
00071       markerFrame_ = "ar_marker";
00072     ROS_INFO ("\tMarker frame: %s", markerFrame_.c_str());
00073 
00074     // If mode=0, we use arGetTransMat instead of arGetTransMatCont
00075     // The arGetTransMatCont function uses information from the previous image
00076     // frame to reduce the jittering of the marker
00077     if (!n_param.getParam("use_history", useHistory_))
00078       useHistory_ = true;
00079     ROS_INFO("\tUse history: %d", useHistory_);
00080 
00081 //     n_param.param ("marker_pattern", local_path, std::string ("data/patt.hiro"));
00082 //     sprintf (pattern_filename_, "%s/%s", package_path.c_str (), local_path.c_str ());
00083 //     ROS_INFO ("\tMarker Pattern Filename: %s", pattern_filename_);
00084 
00085         //modifications to allow patterns to be loaded from outside the package
00086         n_param.param ("marker_pattern", local_path, default_path);
00087         if (local_path.compare(0,5,"data/") == 0){
00088           //to keep working on previous implementations, check if first 5 chars equal "data/"
00089           sprintf (pattern_filename_, "%s/%s", package_path.c_str (), local_path.c_str ());
00090         }
00091         else{
00092           //for new implementations, can pass a path outside the package_path
00093           sprintf (pattern_filename_, "%s", local_path.c_str ());
00094         }
00095 
00096     n_param.param ("marker_center_x", marker_center_[0], 0.0);
00097     n_param.param ("marker_center_y", marker_center_[1], 0.0);
00098     ROS_INFO ("\tMarker Center: (%.1f,%.1f)", marker_center_[0], marker_center_[1]);
00099 
00100     // **** subscribe
00101 
00102     ROS_INFO ("Subscribing to info topic");
00103     sub_ = n_.subscribe (cameraInfoTopic_, 1, &ARSinglePublisher::camInfoCallback, this);
00104     getCamInfo_ = false;
00105     
00106     // **** advertsie 
00107 
00108     arMarkerPub_   = n_.advertise<ar_pose::ARMarker>("ar_pose_marker", 0);
00109     if(publishVisualMarkers_){ 
00110                 rvizMarkerPub_ = n_.advertise<visualization_msgs::Marker>("visualization_marker", 0);
00111          }
00112   }
00113 
00114   ARSinglePublisher::~ARSinglePublisher (void)
00115   {
00116     //cvReleaseImage(&capture); //Don't know why but crash when release the image
00117     arVideoCapStop ();
00118     arVideoClose ();
00119   }
00120 
00121   void ARSinglePublisher::camInfoCallback (const sensor_msgs::CameraInfoConstPtr & cam_info)
00122   {
00123     if (!getCamInfo_)
00124     {
00125       cam_info_ = (*cam_info);
00126 
00127       cam_param_.xsize = cam_info_.width;
00128       cam_param_.ysize = cam_info_.height;
00129       
00130       cam_param_.mat[0][0] = cam_info_.P[0];
00131       cam_param_.mat[1][0] = cam_info_.P[4];
00132       cam_param_.mat[2][0] = cam_info_.P[8];
00133       cam_param_.mat[0][1] = cam_info_.P[1];
00134       cam_param_.mat[1][1] = cam_info_.P[5];
00135       cam_param_.mat[2][1] = cam_info_.P[9];
00136       cam_param_.mat[0][2] = cam_info_.P[2];
00137       cam_param_.mat[1][2] = cam_info_.P[6];
00138       cam_param_.mat[2][2] = cam_info_.P[10];
00139       cam_param_.mat[0][3] = cam_info_.P[3];
00140       cam_param_.mat[1][3] = cam_info_.P[7];
00141       cam_param_.mat[2][3] = cam_info_.P[11];
00142      
00143           cam_param_.dist_factor[0] = cam_info_.K[2];       // x0 = cX from openCV calibration
00144       cam_param_.dist_factor[1] = cam_info_.K[5];       // y0 = cY from openCV calibration
00145       if ( cam_info_.distortion_model == "plumb_bob" && cam_info_.D.size() == 5)
00146         cam_param_.dist_factor[2] = -100*cam_info_.D[0];// f = -100*k1 from CV. Note, we had to do mm^2 to m^2, hence 10^8->10^2
00147       else
00148         cam_param_.dist_factor[2] = 0;                  // We don't know the right value, so ignore it
00149 
00150       cam_param_.dist_factor[3] = 1.0;                  // scale factor, should probably be >1, but who cares...
00151              
00152       arInit();
00153 
00154       ROS_INFO ("Subscribing to image topic");
00155       cam_sub_ = it_.subscribe (cameraImageTopic_, 1, &ARSinglePublisher::getTransformationCallback, this);
00156       getCamInfo_ = true;
00157     }
00158   }
00159 
00160   void ARSinglePublisher::arInit ()
00161   {   
00162     arInitCparam (&cam_param_);
00163 
00164     ROS_INFO ("*** Camera Parameter ***");
00165     arParamDisp (&cam_param_);
00166 
00167     // load pattern file
00168     ROS_INFO ("Loading pattern");
00169     patt_id_ = arLoadPatt (pattern_filename_);
00170     if (patt_id_ < 0)
00171     {
00172       ROS_ERROR ("Pattern file load error: %s", pattern_filename_);
00173       ROS_BREAK ();
00174     }
00175 
00176     sz_ = cvSize (cam_param_.xsize, cam_param_.ysize);
00177 #if ROS_VERSION_MINIMUM(1, 9, 0)
00178 // FIXME: Why is this not in the object
00179     cv_bridge::CvImagePtr capture_;
00180 #else
00181 // DEPRECATED: Fuerte support ends when Hydro is released
00182     capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
00183 #endif
00184 
00185   }
00186 
00187   void ARSinglePublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
00188   {
00189     ARUint8 *dataPtr;
00190     ARMarkerInfo *marker_info;
00191     int marker_num;
00192     int i, k;
00193 
00194     /* Get the image from ROSTOPIC
00195      * NOTE: the dataPtr format is BGR because the ARToolKit library was
00196      * build with V4L, dataPtr format change according to the 
00197      * ARToolKit configure option (see config.h).*/
00198 #if ROS_VERSION_MINIMUM(1, 9, 0)
00199     try
00200     {
00201       capture_ = cv_bridge::toCvCopy (image_msg, sensor_msgs::image_encodings::BGR8);
00202     }
00203     catch (cv_bridge::Exception& e)
00204     {
00205       ROS_ERROR("cv_bridge exception: %s", e.what());
00206     }
00207     dataPtr = (ARUint8 *) ((IplImage) capture_->image).imageData;
00208 #else
00209     try
00210     {
00211       capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
00212     }
00213     catch (sensor_msgs::CvBridgeException & e)
00214     {
00215       ROS_ERROR("cv_bridge exception: %s", e.what());
00216     }
00217     dataPtr = (ARUint8 *) capture_->imageData;
00218 #endif
00219 
00220     // detect the markers in the video frame 
00221     if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
00222     {
00223       ROS_FATAL ("arDetectMarker failed");
00224       ROS_BREAK ();             // FIXME: I don't think this should be fatal... -Bill
00225     }
00226 
00227     // check for known patterns
00228     k = -1;
00229     for (i = 0; i < marker_num; i++)
00230     {
00231       if (marker_info[i].id == patt_id_)
00232       {
00233         ROS_DEBUG ("Found pattern: %d ", patt_id_);
00234 
00235         // make sure you have the best pattern (highest confidence factor)
00236         if (k == -1)
00237           k = i;
00238         else if (marker_info[k].cf < marker_info[i].cf)
00239           k = i;
00240       }
00241     }
00242 
00243     if (k != -1)
00244     {
00245       // **** get the transformation between the marker and the real camera
00246       double arQuat[4], arPos[3];
00247 
00248       if (!useHistory_ || contF == 0)
00249         arGetTransMat (&marker_info[k], marker_center_, markerWidth_, marker_trans_);
00250       else
00251         arGetTransMatCont (&marker_info[k], marker_trans_, marker_center_, markerWidth_, marker_trans_);
00252 
00253       contF = 1;
00254 
00255       //arUtilMatInv (marker_trans_, cam_trans);
00256       arUtilMat2QuatPos (marker_trans_, arQuat, arPos);
00257 
00258       // **** convert to ROS frame
00259 
00260       double quat[4], pos[3];
00261     
00262       pos[0] = arPos[0] * AR_TO_ROS;
00263       pos[1] = arPos[1] * AR_TO_ROS;
00264       pos[2] = arPos[2] * AR_TO_ROS;
00265 
00266       quat[0] = -arQuat[0];
00267       quat[1] = -arQuat[1];
00268       quat[2] = -arQuat[2];
00269       quat[3] = arQuat[3];
00270 
00271       ROS_DEBUG (" QUAT: Pos x: %3.5f  y: %3.5f  z: %3.5f", pos[0], pos[1], pos[2]);
00272       ROS_DEBUG ("     Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);
00273 
00274       // **** publish the marker
00275 
00276                   ar_pose_marker_.header.frame_id = image_msg->header.frame_id;
00277                   ar_pose_marker_.header.stamp    = image_msg->header.stamp;
00278                   ar_pose_marker_.id              = marker_info->id;
00279 
00280                   ar_pose_marker_.pose.pose.position.x = pos[0];
00281                   ar_pose_marker_.pose.pose.position.y = pos[1];
00282                   ar_pose_marker_.pose.pose.position.z = pos[2];
00283 
00284                   ar_pose_marker_.pose.pose.orientation.x = quat[0];
00285                   ar_pose_marker_.pose.pose.orientation.y = quat[1];
00286                   ar_pose_marker_.pose.pose.orientation.z = quat[2];
00287                   ar_pose_marker_.pose.pose.orientation.w = quat[3];
00288                 
00289                   ar_pose_marker_.confidence = marker_info->cf;
00290 
00291                   arMarkerPub_.publish(ar_pose_marker_);
00292                   ROS_DEBUG ("Published ar_single marker");
00293                 
00294       // **** publish transform between camera and marker
00295 
00296 #if ROS_VERSION_MINIMUM(1, 9, 0)
00297       tf::Quaternion rotation (quat[0], quat[1], quat[2], quat[3]);
00298       tf::Vector3 origin (pos[0], pos[1], pos[2]);
00299       tf::Transform t (rotation, origin);
00300 #else
00301 // DEPRECATED: Fuerte support ends when Hydro is released
00302       btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
00303       btVector3 origin (pos[0], pos[1], pos[2]);
00304       btTransform t (rotation, origin);
00305 #endif
00306 
00307 
00308       if(publishTf_)
00309       {
00310         if(reverse_transform_)
00311         {
00312           tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame_.c_str(), image_msg->header.frame_id);
00313           broadcaster_.sendTransform(markerToCam);
00314         } else {
00315           tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame_.c_str());
00316           broadcaster_.sendTransform(camToMarker);
00317         }
00318       }
00319 
00320       // **** publish visual marker
00321 
00322       if(publishVisualMarkers_)
00323       {
00324 #if ROS_VERSION_MINIMUM(1, 9, 0)
00325         tf::Vector3 markerOrigin (0, 0, 0.25 * markerWidth_ * AR_TO_ROS);
00326         tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
00327         tf::Transform markerPose = t * m; // marker pose in the camera frame
00328 #else
00329 // DEPRECATED: Fuerte support ends when Hydro is released
00330         btVector3 markerOrigin (0, 0, 0.25 * markerWidth_ * AR_TO_ROS);
00331         btTransform m (btQuaternion::getIdentity (), markerOrigin);
00332         btTransform markerPose = t * m; // marker pose in the camera frame
00333 #endif
00334 
00335       
00336         tf::poseTFToMsg(markerPose, rvizMarker_.pose);
00337 
00338                           rvizMarker_.header.frame_id = image_msg->header.frame_id;
00339                           rvizMarker_.header.stamp = image_msg->header.stamp;
00340                           rvizMarker_.id = 1;
00341 
00342                           rvizMarker_.scale.x = 1.0 * markerWidth_ * AR_TO_ROS;
00343                           rvizMarker_.scale.y = 1.0 * markerWidth_ * AR_TO_ROS;
00344                           rvizMarker_.scale.z = 0.5 * markerWidth_ * AR_TO_ROS;
00345                           rvizMarker_.ns = "basic_shapes";
00346                           rvizMarker_.type = visualization_msgs::Marker::CUBE;
00347                           rvizMarker_.action = visualization_msgs::Marker::ADD;
00348                           rvizMarker_.color.r = 0.0f;
00349                           rvizMarker_.color.g = 1.0f;
00350                           rvizMarker_.color.b = 0.0f;
00351                           rvizMarker_.color.a = 1.0;
00352                           rvizMarker_.lifetime = ros::Duration(1.0);
00353                         
00354                           rvizMarkerPub_.publish(rvizMarker_);
00355                           ROS_DEBUG ("Published visual marker");
00356       }
00357     }
00358     else
00359     {
00360       contF = 0;
00361       ROS_DEBUG ("Failed to locate marker");
00362     }
00363   }
00364 }                               // end namespace ar_pose
00365 


ar_pose
Author(s): Ivan Dryanovsk, Bill Morris , Gautier Dumonteil
autogenerated on Thu Aug 27 2015 12:24:37