IndividualMarkersNoKinect.cpp
Go to the documentation of this file.
00001 /*
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2012, Scott Niekum
00005  All rights reserved.
00006 
00007  Redistribution and use in source and binary forms, with or without
00008  modification, are permitted provided that the following conditions
00009  are met:
00010 
00011   * Redistributions of source code must retain the above copyright
00012     notice, this list of conditions and the following disclaimer.
00013   * Redistributions in binary form must reproduce the above
00014     copyright notice, this list of conditions and the following
00015     disclaimer in the documentation and/or other materials provided
00016     with the distribution.
00017   * Neither the name of the Willow Garage nor the names of its
00018     contributors may be used to endorse or promote products derived
00019     from this software without specific prior written permission.
00020 
00021  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  POSSIBILITY OF SUCH DAMAGE.
00033 
00034  author: Scott Niekum
00035 */
00036 
00037 
00038 #include "CvTestbed.h"
00039 #include "MarkerDetector.h"
00040 #include "Shared.h"
00041 #include <cv_bridge/CvBridge.h>
00042 #include <ar_track_alvar/AlvarMarker.h>
00043 #include <ar_track_alvar/AlvarMarkers.h>
00044 #include <tf/transform_listener.h>
00045 
00046 using namespace alvar;
00047 using namespace std;
00048 
00049 bool init=true;
00050 Camera *cam;
00051 IplImage *capture_;
00052 sensor_msgs::CvBridge bridge_;
00053 image_transport::Subscriber cam_sub_;
00054 ros::Publisher arMarkerPub_;
00055 ros::Publisher rvizMarkerPub_;
00056 ar_track_alvar::AlvarMarkers arPoseMarkers_;
00057 visualization_msgs::Marker rvizMarker_;
00058 tf::TransformListener *tf_listener;
00059 tf::TransformBroadcaster *tf_broadcaster;
00060 MarkerDetector<MarkerData> marker_detector;
00061 
00062 double marker_size;
00063 double max_new_marker_error;
00064 double max_track_error;
00065 std::string cam_image_topic; 
00066 std::string cam_info_topic; 
00067 std::string output_frame;
00068 
00069 void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg);
00070 
00071 
00072 void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
00073 {
00074         if(init){
00075                 CvSize sz_ = cvSize (cam->x_res, cam->y_res);
00076         capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
00077                 init = false;   
00078         }
00079 
00080         //If we've already gotten the cam info, then go ahead
00081         if(cam->getCamInfo_){
00082                 try{
00083                         tf::StampedTransform CamToOutput;
00084                         try{
00085                                         tf_listener->waitForTransform(output_frame, image_msg->header.frame_id, image_msg->header.stamp, ros::Duration(1.0));
00086                                         tf_listener->lookupTransform(output_frame, image_msg->header.frame_id, image_msg->header.stamp, CamToOutput);
00087                                 }
00088                         catch (tf::TransformException ex){
00089                                 ROS_ERROR("%s",ex.what());
00090                         }
00091 
00092                 capture_ = bridge_.imgMsgToCv (image_msg, "rgb8");
00093                         marker_detector.Detect(capture_, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true);
00094 
00095                         arPoseMarkers_.markers.clear ();
00096                         for (size_t i=0; i<marker_detector.markers->size(); i++) 
00097                         {
00098                                 //Get the pose relative to the camera
00099                         int id = (*(marker_detector.markers))[i].GetId(); 
00100                                 Pose p = (*(marker_detector.markers))[i].pose;
00101                                 double px = p.translation[0]/100.0;
00102                                 double py = p.translation[1]/100.0;
00103                                 double pz = p.translation[2]/100.0;
00104                                 double qx = p.quaternion[1];
00105                                 double qy = p.quaternion[2];
00106                                 double qz = p.quaternion[3];
00107                                 double qw = p.quaternion[0];
00108 
00109                                 btQuaternion rotation (qx,qy,qz,qw);
00110                         btVector3 origin (px,py,pz);
00111                         btTransform t (rotation, origin);
00112                                 btVector3 markerOrigin (0, 0, 0);
00113                                 btTransform m (btQuaternion::getIdentity (), markerOrigin);
00114                                 btTransform markerPose = t * m; // marker pose in the camera frame
00115 
00116                                 //Publish the transform from the camera to the marker           
00117                                 std::string markerFrame = "ar_marker_";
00118                                 std::stringstream out;
00119                                 out << id;
00120                                 std::string id_string = out.str();
00121                                 markerFrame += id_string;
00122                                 tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
00123                         tf_broadcaster->sendTransform(camToMarker);
00124                                 
00125                                 //Create the rviz visualization messages
00126                                 tf::poseTFToMsg (markerPose, rvizMarker_.pose);
00127                                 rvizMarker_.header.frame_id = image_msg->header.frame_id;
00128                                 rvizMarker_.header.stamp = image_msg->header.stamp;
00129                                 rvizMarker_.id = id;
00130 
00131                                 rvizMarker_.scale.x = 1.0 * marker_size/100.0;
00132                                 rvizMarker_.scale.y = 1.0 * marker_size/100.0;
00133                                 rvizMarker_.scale.z = 0.2 * marker_size/100.0;
00134                                 rvizMarker_.ns = "basic_shapes";
00135                                 rvizMarker_.type = visualization_msgs::Marker::CUBE;
00136                                 rvizMarker_.action = visualization_msgs::Marker::ADD;
00137                                 switch (id)
00138                                 {
00139                                   case 0:
00140                                     rvizMarker_.color.r = 0.0f;
00141                                     rvizMarker_.color.g = 0.0f;
00142                                     rvizMarker_.color.b = 1.0f;
00143                                     rvizMarker_.color.a = 1.0;
00144                                     break;
00145                                   case 1:
00146                                     rvizMarker_.color.r = 1.0f;
00147                                     rvizMarker_.color.g = 0.0f;
00148                                     rvizMarker_.color.b = 0.0f;
00149                                     rvizMarker_.color.a = 1.0;
00150                                     break;
00151                                   case 2:
00152                                     rvizMarker_.color.r = 0.0f;
00153                                     rvizMarker_.color.g = 1.0f;
00154                                     rvizMarker_.color.b = 0.0f;
00155                                     rvizMarker_.color.a = 1.0;
00156                                     break;
00157                                   case 3:
00158                                     rvizMarker_.color.r = 0.0f;
00159                                     rvizMarker_.color.g = 0.5f;
00160                                     rvizMarker_.color.b = 0.5f;
00161                                     rvizMarker_.color.a = 1.0;
00162                                     break;
00163                                   case 4:
00164                                     rvizMarker_.color.r = 0.5f;
00165                                     rvizMarker_.color.g = 0.5f;
00166                                     rvizMarker_.color.b = 0.0;
00167                                     rvizMarker_.color.a = 1.0;
00168                                     break;
00169                                   default:
00170                                     rvizMarker_.color.r = 0.5f;
00171                                     rvizMarker_.color.g = 0.0f;
00172                                     rvizMarker_.color.b = 0.5f;
00173                                     rvizMarker_.color.a = 1.0;
00174                                     break;
00175                                 }
00176                                 rvizMarker_.lifetime = ros::Duration (1.0);
00177                                 rvizMarkerPub_.publish (rvizMarker_);
00178 
00179                                 //Get the pose of the tag in the camera frame, then the output frame (usually torso)                            
00180                                 tf::Transform tagPoseOutput = CamToOutput * markerPose;
00181 
00182                                 //Create the pose marker messages
00183                                 ar_track_alvar::AlvarMarker ar_pose_marker;
00184                                 tf::poseTFToMsg (tagPoseOutput, ar_pose_marker.pose.pose);
00185                         ar_pose_marker.header.frame_id = output_frame;
00186                             ar_pose_marker.header.stamp = image_msg->header.stamp;
00187                             ar_pose_marker.id = id;
00188                             arPoseMarkers_.markers.push_back (ar_pose_marker);  
00189                         }
00190                         arMarkerPub_.publish (arPoseMarkers_);
00191                 }
00192         catch (sensor_msgs::CvBridgeException & e){
00193                 ROS_ERROR ("Could not convert from '%s' to 'rgb8'.", image_msg->encoding.c_str ());
00194         }
00195         }
00196 }
00197 
00198 
00199 int main(int argc, char *argv[])
00200 {
00201         ros::init (argc, argv, "marker_detect");
00202         ros::NodeHandle n;
00203         
00204         if(argc < 7){
00205                 std::cout << std::endl;
00206                 cout << "Not enough arguments provided." << endl;
00207                 cout << "Usage: ./individualMarkers <marker size in cm> <max new marker error> <max track error> <cam image topic> <cam info topic> <output frame>" << endl;
00208                 std::cout << std::endl;
00209                 return 0;
00210         }
00211 
00212         // Get params from command line
00213         marker_size = atof(argv[1]);
00214         max_new_marker_error = atof(argv[2]);
00215         max_track_error = atof(argv[3]);
00216         cam_image_topic = argv[4];
00217         cam_info_topic = argv[5];
00218     output_frame = argv[6];
00219         marker_detector.SetMarkerSize(marker_size);
00220 
00221         cam = new Camera(n, cam_info_topic);
00222         tf_listener = new tf::TransformListener(n);
00223         tf_broadcaster = new tf::TransformBroadcaster();
00224         arMarkerPub_ = n.advertise < ar_track_alvar::AlvarMarkers > ("ar_pose_marker", 0);
00225         rvizMarkerPub_ = n.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
00226         
00227         //Give tf a chance to catch up before the camera callback starts asking for transforms
00228         ros::Duration(1.0).sleep();
00229         ros::spinOnce();        
00230          
00231         ROS_INFO ("Subscribing to image topic");
00232         image_transport::ImageTransport it_(n);
00233         cam_sub_ = it_.subscribe (cam_image_topic, 1, &getCapCallback);
00234 
00235         ros::spin ();
00236 
00237     return 0;
00238 }


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Sat Dec 28 2013 16:46:15