ar_multi.cpp
Go to the documentation of this file.
00001 /*
00002  *  Multi 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_multi.h"
00026 #include "ar_pose/object.h"
00027 
00028 int main (int argc, char **argv)
00029 {
00030   ros::init (argc, argv, "ar_multi");
00031   ros::NodeHandle n;
00032   ar_pose::ARMultiPublisher ar_multi (n);
00033   ros::spin ();
00034   return 0;
00035 }
00036 
00037 namespace ar_pose
00038 {
00039   ARMultiPublisher::ARMultiPublisher (ros::NodeHandle & n):n_ (n), it_ (n_)
00040   {
00041     std::string local_path;
00042     std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
00043         std::string default_path = "data/object_4x4";
00044     ros::NodeHandle n_param ("~");
00045     XmlRpc::XmlRpcValue xml_marker_center;
00046 
00047     // **** get parameters
00048 
00049     if (!n_param.getParam ("publish_tf", publishTf_))
00050       publishTf_ = true;
00051     ROS_INFO ("\tPublish transforms: %d", publishTf_);
00052 
00053     if (!n_param.getParam ("publish_visual_markers", publishVisualMarkers_))
00054       publishVisualMarkers_ = true;
00055     ROS_INFO ("\tPublish visual markers: %d", publishVisualMarkers_);
00056 
00057     if (!n_param.getParam ("threshold", threshold_))
00058       threshold_ = 100;
00059     ROS_INFO ("\tThreshold: %d", threshold_);
00060         
00061         //modifications to allow path list from outside the package
00062         n_param.param ("marker_pattern_list", local_path, default_path);
00063         if (local_path.compare(0,5,"data/") == 0){
00064           //according to previous implementations, check if first 5 chars equal "data/"
00065           sprintf (pattern_filename_, "%s/%s", package_path.c_str (), local_path.c_str ());
00066         }
00067         else{
00068           //for new implementations, can pass a path outside the package_path
00069           sprintf (pattern_filename_, "%s", local_path.c_str ());
00070         }
00071         ROS_INFO ("Marker Pattern Filename: %s", pattern_filename_);
00072         
00073     // **** subscribe
00074 
00075     ROS_INFO ("Subscribing to info topic");
00076     sub_ = n_.subscribe (cameraInfoTopic_, 1, &ARMultiPublisher::camInfoCallback, this);
00077     getCamInfo_ = false;
00078 
00079     // **** advertse 
00080 
00081     arMarkerPub_ = n_.advertise < ar_pose::ARMarkers > ("ar_pose_marker", 0);
00082     if(publishVisualMarkers_)
00083     {
00084       rvizMarkerPub_ = n_.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
00085     }
00086   }
00087 
00088   ARMultiPublisher::~ARMultiPublisher (void)
00089   {
00090     //cvReleaseImage(&capture_); //Don't know why but crash when release the image
00091     arVideoCapStop ();
00092     arVideoClose ();
00093   }
00094 
00095   void ARMultiPublisher::camInfoCallback (const sensor_msgs::CameraInfoConstPtr & cam_info)
00096   {
00097     if (!getCamInfo_)
00098     {
00099       cam_info_ = (*cam_info);
00100 
00101       cam_param_.xsize = cam_info_.width;
00102       cam_param_.ysize = cam_info_.height;
00103 
00104       cam_param_.mat[0][0] = cam_info_.P[0];
00105       cam_param_.mat[1][0] = cam_info_.P[4];
00106       cam_param_.mat[2][0] = cam_info_.P[8];
00107       cam_param_.mat[0][1] = cam_info_.P[1];
00108       cam_param_.mat[1][1] = cam_info_.P[5];
00109       cam_param_.mat[2][1] = cam_info_.P[9];
00110       cam_param_.mat[0][2] = cam_info_.P[2];
00111       cam_param_.mat[1][2] = cam_info_.P[6];
00112       cam_param_.mat[2][2] = cam_info_.P[10];
00113       cam_param_.mat[0][3] = cam_info_.P[3];
00114       cam_param_.mat[1][3] = cam_info_.P[7];
00115       cam_param_.mat[2][3] = cam_info_.P[11];
00116 
00117       cam_param_.dist_factor[0] = cam_info_.K[2];       // x0 = cX from openCV calibration
00118       cam_param_.dist_factor[1] = cam_info_.K[5];       // y0 = cY from openCV calibration
00119       if ( cam_info_.distortion_model == "plumb_bob" && cam_info_.D.size() == 5)
00120         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
00121       else
00122         cam_param_.dist_factor[2] = 0;                  // We don't know the right value, so ignore it
00123 
00124       cam_param_.dist_factor[3] = 1.0;                  // scale factor, should probably be >1, but who cares...
00125       
00126       arInit ();
00127 
00128       ROS_INFO ("Subscribing to image topic");
00129       cam_sub_ = it_.subscribe (cameraImageTopic_, 1, &ARMultiPublisher::getTransformationCallback, this);
00130       getCamInfo_ = true;
00131     }
00132   }
00133 
00134   void ARMultiPublisher::arInit ()
00135   {
00136     arInitCparam (&cam_param_);
00137     ROS_INFO ("*** Camera Parameter ***");
00138     arParamDisp (&cam_param_);
00139 
00140     // load in the object data - trained markers and associated bitmap files
00141     if ((object = ar_object::read_ObjData (pattern_filename_, &objectnum)) == NULL)
00142       ROS_BREAK ();
00143     ROS_DEBUG ("Objectfile num = %d", objectnum);
00144 
00145     sz_ = cvSize (cam_param_.xsize, cam_param_.ysize);
00146 #if ROS_VERSION_MINIMUM(1, 9, 0)
00147 // FIXME: Why is this not in the object
00148     cv_bridge::CvImagePtr capture_; 
00149 #else
00150 // DEPRECATED: Fuerte support ends when Hydro is released
00151     capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
00152 #endif
00153   }
00154 
00155   void ARMultiPublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
00156   {
00157     ARUint8 *dataPtr;
00158     ARMarkerInfo *marker_info;
00159     int marker_num;
00160     int i, k, j;
00161 
00162     /* Get the image from ROSTOPIC
00163      * NOTE: the dataPtr format is BGR because the ARToolKit library was
00164      * build with V4L, dataPtr format change according to the 
00165      * ARToolKit configure option (see config.h).*/
00166 #if ROS_VERSION_MINIMUM(1, 9, 0)
00167     try
00168     {
00169       capture_ = cv_bridge::toCvCopy (image_msg, sensor_msgs::image_encodings::BGR8);
00170     }
00171     catch (cv_bridge::Exception& e)
00172     {
00173       ROS_ERROR("cv_bridge exception: %s", e.what()); 
00174     }
00175     dataPtr = (ARUint8 *) ((IplImage) capture_->image).imageData;
00176 #else
00177     try
00178     {
00179       capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
00180     }
00181     catch (sensor_msgs::CvBridgeException & e)
00182     {
00183       ROS_ERROR("cv_bridge exception: %s", e.what()); 
00184     }
00185     dataPtr = (ARUint8 *) capture_->imageData;
00186 #endif
00187 
00188     // detect the markers in the video frame
00189     if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
00190     {
00191       argCleanup ();
00192       ROS_BREAK ();
00193     }
00194 
00195     arPoseMarkers_.markers.clear ();
00196     // check for known patterns
00197     for (i = 0; i < objectnum; i++)
00198     {
00199       k = -1;
00200       for (j = 0; j < marker_num; j++)
00201       {
00202         if (object[i].id == marker_info[j].id)
00203         {
00204           if (k == -1)
00205             k = j;
00206           else                  // make sure you have the best pattern (highest confidence factor)
00207           if (marker_info[k].cf < marker_info[j].cf)
00208             k = j;
00209         }
00210       }
00211       if (k == -1)
00212       {
00213         object[i].visible = 0;
00214         continue;
00215       }
00216 
00217       // calculate the transform for each marker
00218       if (object[i].visible == 0)
00219       {
00220         arGetTransMat (&marker_info[k], object[i].marker_center, object[i].marker_width, object[i].trans);
00221       }
00222       else
00223       {
00224         arGetTransMatCont (&marker_info[k], object[i].trans,
00225                            object[i].marker_center, object[i].marker_width, object[i].trans);
00226       }
00227       object[i].visible = 1;
00228 
00229       double arQuat[4], arPos[3];
00230 
00231       //arUtilMatInv (object[i].trans, cam_trans);
00232       arUtilMat2QuatPos (object[i].trans, arQuat, arPos);
00233 
00234       // **** convert to ROS frame
00235 
00236       double quat[4], pos[3];
00237 
00238       pos[0] = arPos[0] * AR_TO_ROS;
00239       pos[1] = arPos[1] * AR_TO_ROS;
00240       pos[2] = arPos[2] * AR_TO_ROS;
00241 
00242       quat[0] = -arQuat[0];
00243       quat[1] = -arQuat[1];
00244       quat[2] = -arQuat[2];
00245       quat[3] = arQuat[3];
00246 
00247       ROS_DEBUG (" Object num %i ------------------------------------------------", i);
00248       ROS_DEBUG (" QUAT: Pos x: %3.5f  y: %3.5f  z: %3.5f", pos[0], pos[1], pos[2]);
00249       ROS_DEBUG ("     Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);
00250 
00251       // **** publish the marker
00252 
00253       ar_pose::ARMarker ar_pose_marker;
00254       ar_pose_marker.header.frame_id = image_msg->header.frame_id;
00255       ar_pose_marker.header.stamp = image_msg->header.stamp;
00256       ar_pose_marker.id = object[i].id;
00257 
00258       ar_pose_marker.pose.pose.position.x = pos[0];
00259       ar_pose_marker.pose.pose.position.y = pos[1];
00260       ar_pose_marker.pose.pose.position.z = pos[2];
00261 
00262       ar_pose_marker.pose.pose.orientation.x = quat[0];
00263       ar_pose_marker.pose.pose.orientation.y = quat[1];
00264       ar_pose_marker.pose.pose.orientation.z = quat[2];
00265       ar_pose_marker.pose.pose.orientation.w = quat[3];
00266 
00267       ar_pose_marker.confidence = round(marker_info->cf * 100);
00268       arPoseMarkers_.markers.push_back (ar_pose_marker);
00269 
00270       // **** publish transform between camera and marker
00271 
00272 #if ROS_VERSION_MINIMUM(1, 9, 0)
00273       tf::Quaternion rotation (quat[0], quat[1], quat[2], quat[3]);
00274       tf::Vector3 origin (pos[0], pos[1], pos[2]);
00275       tf::Transform t (rotation, origin);
00276 #else
00277 // DEPRECATED: Fuerte support ends when Hydro is released
00278       btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
00279       btVector3 origin (pos[0], pos[1], pos[2]);
00280       btTransform t (rotation, origin);
00281 #endif
00282 
00283       if (publishTf_)
00284       {
00285         tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, object[i].name);
00286         broadcaster_.sendTransform(camToMarker);
00287       }
00288 
00289       // **** publish visual marker
00290 
00291       if (publishVisualMarkers_)
00292       {
00293 #if ROS_VERSION_MINIMUM(1, 9, 0)
00294         tf::Vector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
00295         tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
00296         tf::Transform markerPose = t * m; // marker pose in the camera frame 
00297 #else
00298 // DEPRECATED: Fuerte support ends when Hydro is released
00299         btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
00300         btTransform m (btQuaternion::getIdentity (), markerOrigin);
00301         btTransform markerPose = t * m; // marker pose in the camera frame
00302 #endif
00303 
00304         tf::poseTFToMsg (markerPose, rvizMarker_.pose);
00305 
00306         rvizMarker_.header.frame_id = image_msg->header.frame_id;
00307         rvizMarker_.header.stamp = image_msg->header.stamp;
00308         rvizMarker_.id = object[i].id;
00309 
00310         rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS;
00311         rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS;
00312         rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS;
00313         rvizMarker_.ns = "basic_shapes";
00314         rvizMarker_.type = visualization_msgs::Marker::CUBE;
00315         rvizMarker_.action = visualization_msgs::Marker::ADD;
00316         switch (i)
00317         {
00318           case 0:
00319             rvizMarker_.color.r = 0.0f;
00320             rvizMarker_.color.g = 0.0f;
00321             rvizMarker_.color.b = 1.0f;
00322             rvizMarker_.color.a = 1.0;
00323             break;
00324           case 1:
00325             rvizMarker_.color.r = 1.0f;
00326             rvizMarker_.color.g = 0.0f;
00327             rvizMarker_.color.b = 0.0f;
00328             rvizMarker_.color.a = 1.0;
00329             break;
00330           default:
00331             rvizMarker_.color.r = 0.0f;
00332             rvizMarker_.color.g = 1.0f;
00333             rvizMarker_.color.b = 0.0f;
00334             rvizMarker_.color.a = 1.0;
00335         }
00336         rvizMarker_.lifetime = ros::Duration (1.0);
00337 
00338         rvizMarkerPub_.publish(rvizMarker_);
00339         ROS_DEBUG ("Published visual marker");
00340       }
00341     }
00342     arMarkerPub_.publish(arPoseMarkers_);
00343     ROS_DEBUG ("Published ar_multi markers");
00344   }
00345 } // end namespace ar_pose


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