ar_kinect.cpp
Go to the documentation of this file.
00001 /*
00002  *  Multi Marker Pose Estimation using ARToolkit & Kinect
00003  *  Copyright (C) 2010, CCNY Robotics Lab, 2011 ILS Robotics Lab
00004  *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
00005  *  William Morris <morris@ee.ccny.cuny.edu>
00006  *  Gautier Dumonteil <gautier.dumonteil@gmail.com>
00007  *  http://robotics.ccny.cuny.edu
00008  * 
00009  *  Michael Ferguson <ferguson@cs.albany.edu>
00010  *  http://robotics.ils.albany.edu
00011  *
00012  *  This program is free software: you can redistribute it and/or modify
00013  *  it under the terms of the GNU General Public License as published by
00014  *  the Free Software Foundation, either version 3 of the License, or
00015  *  (at your option) any later version.
00016  *
00017  *  This program is distributed in the hope that it will be useful,
00018  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00019  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00020  *  GNU General Public License for more details.
00021  *
00022  *  You should have received a copy of the GNU General Public License
00023  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00024  */
00025 
00026 #include <math.h>
00027 #include "ar_kinect/ar_kinect.h"
00028 #include "ar_kinect/object.h"
00029 
00030 
00031 int main (int argc, char **argv)
00032 {
00033   ros::init (argc, argv, "ar_kinect");
00034   ros::NodeHandle n;
00035   ar_pose::ARPublisher ar_kinect (n);
00036   ros::spin ();
00037   return 0;
00038 }
00039 
00040 namespace ar_pose
00041 {
00042   tf::Transform tfFromEigen(Eigen::Matrix4f trans)
00043   {
00044     tf::Matrix3x3 btm;
00045     btm.setValue(trans(0,0),trans(0,1),trans(0,2),
00046                trans(1,0),trans(1,1),trans(1,2),
00047                trans(2,0),trans(2,1),trans(2,2));
00048     tf::Transform ret;
00049     ret.setOrigin(tf::Vector3(trans(0,3),trans(1,3),trans(2,3)));
00050     ret.setBasis(btm);
00051     return ret;
00052   }
00053 
00054   pcl::PointXYZRGB makeRGBPoint( float x, float y, float z )
00055   {
00056     pcl::PointXYZRGB p;
00057     p.x = x;
00058     p.y = y; 
00059     p.z = z;
00060     return p;
00061   }
00062 
00063   ARPublisher::ARPublisher (ros::NodeHandle & n):n_ (n), configured_(false)
00064   {
00065     std::string path;
00066     std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
00067     ros::NodeHandle n_param ("~");
00068     XmlRpc::XmlRpcValue xml_marker_center;
00069 
00070     // **** get parameters
00071 
00072     if (!n_param.getParam ("publish_tf", publishTf_))
00073       publishTf_ = true;
00074     ROS_INFO ("\tPublish transforms: %d", publishTf_);
00075 
00076     if (!n_param.getParam ("publish_visual_markers", publishVisualMarkers_))
00077       publishVisualMarkers_ = true;
00078     ROS_INFO ("\tPublish visual markers: %d", publishVisualMarkers_);
00079 
00080     if (!n_param.getParam ("threshold", threshold_))
00081       threshold_ = 100;
00082     ROS_INFO ("\tThreshold: %d", threshold_);
00083 
00084     if (!n_param.getParam ("marker_pattern_list", path)){
00085       sprintf(pattern_filename_, "%s/data/objects_kinect", package_path.c_str());
00086     }else{
00087       sprintf(pattern_filename_, "%s", path.c_str());
00088     }    
00089     ROS_INFO ("Marker Pattern Filename: %s", pattern_filename_);
00090 
00091     if (!n_param.getParam ("marker_data_directory", path)){
00092       sprintf(data_directory_, "%s", package_path.c_str());
00093     }else{
00094       sprintf(data_directory_, "%s", path.c_str());
00095     }    
00096     ROS_INFO ("Marker Data Directory: %s", data_directory_);
00097 
00098     // **** subscribe
00099 
00100     configured_ = false;
00101     cloud_sub_ = n_.subscribe(cloudTopic_, 1, &ARPublisher::getTransformationCallback, this);
00102 
00103     // **** advertise 
00104 
00105     arMarkerPub_ = n_.advertise < ar_pose::ARMarkers > ("ar_pose_markers",0);
00106     if(publishVisualMarkers_)
00107     {
00108                 rvizMarkerPub_ = n_.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
00109     }
00110   }
00111 
00112   ARPublisher::~ARPublisher (void)
00113   {
00114     arVideoCapStop ();
00115     arVideoClose ();
00116   }
00117 
00118   /* 
00119    * Setup artoolkit
00120    */
00121   void ARPublisher::arInit ()
00122   {
00123     arInitCparam (&cam_param_);
00124     ROS_INFO ("*** Camera Parameter ***");
00125     arParamDisp (&cam_param_);
00126 
00127     // load in the object data - trained markers and associated bitmap files
00128     if ((object = ar_object::read_ObjData (pattern_filename_, data_directory_, &objectnum)) == NULL)
00129       ROS_BREAK ();
00130     ROS_DEBUG ("Objectfile num = %d", objectnum);
00131 
00132     sz_ = cvSize (cam_param_.xsize, cam_param_.ysize);
00133     capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
00134     configured_ = true;
00135   }
00136 
00137   /* 
00138    * One and only one callback, now takes cloud, does everything else needed. 
00139    */
00140   void ARPublisher::getTransformationCallback (const sensor_msgs::PointCloud2ConstPtr & msg)
00141   {
00142     sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
00143     ARUint8 *dataPtr;
00144     ARMarkerInfo *marker_info;
00145     int marker_num;
00146     int i, k, j;
00147 
00148     /* do we need to initialize? */
00149     if(!configured_)
00150     {
00151       if(msg->width == 0 || msg->height == 0)
00152       {
00153         ROS_ERROR ("Deformed cloud! Size = %d, %d.", msg->width, msg->height);
00154         return;
00155       }
00156 
00157       cam_param_.xsize = msg->width;
00158       cam_param_.ysize = msg->height;
00159 
00160       cam_param_.dist_factor[0] = msg->width/2;         // x0 = cX from openCV calibration
00161       cam_param_.dist_factor[1] = msg->height/2;        // y0 = cY from openCV calibration
00162       cam_param_.dist_factor[2] = 0;                    // f = -100*k1 from CV. Note, we had to do mm^2 to m^2, hence 10^8->10^2
00163       cam_param_.dist_factor[3] = 1.0;                  // scale factor, should probably be >1, but who cares...
00164       
00165       arInit ();
00166     }
00167 
00168     /* convert cloud to PCL */
00169     PointCloud cloud;
00170     pcl::fromROSMsg(*msg, cloud);
00171  
00172     /* get an OpenCV image from the cloud */
00173     pcl::toROSMsg (cloud, *image_msg);
00174 
00175     cv_bridge::CvImagePtr cv_ptr;
00176     try
00177     {
00178         cv_ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
00179     }
00180     catch (cv_bridge::Exception& e)
00181     {
00182       ROS_ERROR ("Could not convert from '%s' to 'bgr8'.", image_msg->encoding.c_str ());
00183     }
00184     dataPtr = (ARUint8 *) cv_ptr->image.ptr();
00185 
00186     /* detect the markers in the video frame */
00187     if (arDetectMarkerLite (dataPtr, threshold_, &marker_info, &marker_num) < 0)
00188     {
00189       argCleanup ();
00190       return;
00191     }
00192  
00193     arPoseMarkers_.markers.clear ();
00194     /* check for known patterns */
00195     for (i = 0; i < objectnum; i++)
00196     {
00197       k = -1;
00198       for (j = 0; j < marker_num; j++)
00199       {
00200         if (object[i].id == marker_info[j].id)
00201         {
00202           if (k == -1)
00203             k = j;
00204           else                  // make sure you have the best pattern (highest confidence factor)
00205           if (marker_info[k].cf < marker_info[j].cf)
00206             k = j;
00207         }
00208       }
00209       if (k == -1)
00210       {
00211         object[i].visible = 0;
00212         continue;
00213       }
00214       
00215       /* create a cloud for marker corners */
00216       int d = marker_info[k].dir;
00217       PointCloud marker;
00218       marker.push_back( cloud.at( (int)marker_info[k].vertex[(4-d)%4][0], (int)marker_info[k].vertex[(4-d)%4][1] ) ); // upper left
00219       marker.push_back( cloud.at( (int)marker_info[k].vertex[(5-d)%4][0], (int)marker_info[k].vertex[(5-d)%4][1] ) ); // upper right
00220       marker.push_back( cloud.at( (int)marker_info[k].vertex[(6-d)%4][0], (int)marker_info[k].vertex[(6-d)%4][1] ) ); // lower right
00221       marker.push_back( cloud.at( (int)marker_info[k].vertex[(7-d)%4][0], (int)marker_info[k].vertex[(7-d)%4][1] ) );
00222 
00223       /* create an ideal cloud */
00224       double w = object[i].marker_width;
00225       PointCloud ideal;
00226       ideal.push_back( makeRGBPoint(-w/2,w/2,0) );
00227       ideal.push_back( makeRGBPoint(w/2,w/2,0) );
00228       ideal.push_back( makeRGBPoint(w/2,-w/2,0) );
00229       ideal.push_back( makeRGBPoint(-w/2,-w/2,0) );
00230 
00231       /* get transformation */
00232       Eigen::Matrix4f t;
00233       TransformationEstimationSVD obj;
00234       obj.estimateRigidTransformation( marker, ideal, t );
00235 
00236       
00237       /* get final transformation */
00238       tf::Transform transform = tfFromEigen(t.inverse());
00239    
00240       // any(transform == nan)
00241       tf::Matrix3x3  m = transform.getBasis();
00242       tf::Vector3    p = transform.getOrigin();
00243       bool invalid = false;
00244       for(int i=0; i < 3; i++)
00245         for(int j=0; j < 3; j++)
00246           invalid = (invalid || isnan(m[i][j]) || fabs(m[i][j]) > 1.0);
00247 
00248       for(int i=0; i < 3; i++)
00249           invalid = (invalid || isnan(p[i]));
00250        
00251 
00252       if(invalid)
00253         continue; 
00254 
00255       /* publish the marker */
00256       ar_pose::ARMarker ar_pose_marker;
00257       ar_pose_marker.header.frame_id = msg->header.frame_id;
00258       ar_pose_marker.header.stamp = msg->header.stamp;
00259       ar_pose_marker.id = object[i].id;
00260 
00261       ar_pose_marker.pose.pose.position.x = transform.getOrigin().getX();
00262       ar_pose_marker.pose.pose.position.y = transform.getOrigin().getY();
00263       ar_pose_marker.pose.pose.position.z = transform.getOrigin().getZ();
00264 
00265       ar_pose_marker.pose.pose.orientation.x = transform.getRotation().getAxis().getX();
00266       ar_pose_marker.pose.pose.orientation.y = transform.getRotation().getAxis().getY();
00267       ar_pose_marker.pose.pose.orientation.z = transform.getRotation().getAxis().getZ();
00268       ar_pose_marker.pose.pose.orientation.w = transform.getRotation().getW();
00269 
00270       ar_pose_marker.confidence = marker_info->cf;
00271       arPoseMarkers_.markers.push_back (ar_pose_marker);
00272 
00273       /* publish transform */
00274       if (publishTf_)
00275       {
00276             broadcaster_.sendTransform(tf::StampedTransform(transform, msg->header.stamp, msg->header.frame_id, object[i].name));
00277       }
00278 
00279       /* publish visual marker */
00280 
00281       if (publishVisualMarkers_)
00282       {
00283         tf::Vector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
00284         tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
00285         tf::Transform markerPose = transform * m; // marker pose in the camera frame
00286 
00287         tf::poseTFToMsg (markerPose, rvizMarker_.pose);
00288 
00289         rvizMarker_.header.frame_id = msg->header.frame_id;
00290         rvizMarker_.header.stamp = msg->header.stamp;
00291         rvizMarker_.id = object[i].id;
00292 
00293         rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS;
00294         rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS;
00295         rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS;
00296         rvizMarker_.ns = "basic_shapes";
00297         rvizMarker_.type = visualization_msgs::Marker::CUBE;
00298         rvizMarker_.action = visualization_msgs::Marker::ADD;
00299         switch (i)
00300         {
00301           case 0:
00302             rvizMarker_.color.r = 0.0f;
00303             rvizMarker_.color.g = 0.0f;
00304             rvizMarker_.color.b = 1.0f;
00305             rvizMarker_.color.a = 1.0;
00306             break;
00307           case 1:
00308             rvizMarker_.color.r = 1.0f;
00309             rvizMarker_.color.g = 0.0f;
00310             rvizMarker_.color.b = 0.0f;
00311             rvizMarker_.color.a = 1.0;
00312             break;
00313           default:
00314             rvizMarker_.color.r = 0.0f;
00315             rvizMarker_.color.g = 1.0f;
00316             rvizMarker_.color.b = 0.0f;
00317             rvizMarker_.color.a = 1.0;
00318         }
00319         rvizMarker_.lifetime = ros::Duration ();
00320 
00321         rvizMarkerPub_.publish (rvizMarker_);
00322         ROS_DEBUG ("Published visual marker");
00323       }
00324     }
00325     arMarkerPub_.publish (arPoseMarkers_);
00326     ROS_DEBUG ("Published ar_multi markers");
00327   }
00328 
00329 }                               // end namespace ar_pose


ar_kinect
Author(s): Michael Ferguson
autogenerated on Sun Oct 5 2014 22:12:31