ros_detector.cpp
Go to the documentation of this file.
00001 #include <cv_bridge/cv_bridge.h>
00002 #include "checkerboard_pose_estimation/ros_detector.h"
00003 
00004 namespace checkerboard_pose_estimation {
00005 
00006 RosDetector::RosDetector(const std::string& name)
00007   : it_(nh_), name_(name)
00008 {
00009   // Advertise visualization topics
00010   display_pub_ = it_.advertise("display_image", 1);
00011   //marker_pub_ = nh_.advertise<visualization_msgs::Marker>("/visualization_marker", 1);
00012 }
00013 
00014 bool RosDetector::initFromParameters(const ros::NodeHandle& nh)
00015 {
00016   bool have_all_required = true;
00017 
00018   int width;
00019   if (!nh.getParam("board_width", width)) {
00020     ROS_ERROR("Board width [~board_width] unspecified");
00021     have_all_required = false;
00022   }
00023 
00024   int height;
00025   if (!nh.getParam("board_height", height)) {
00026     ROS_ERROR("Board height [~board_height] unspecified");
00027     have_all_required = false;
00028   }
00029 
00030   double square_size;
00031   if (!nh.getParam("square_size", square_size)) {
00032     ROS_ERROR("Square size [~square_size] unspecified");
00033     have_all_required = false;
00034   }
00035 
00036   bool subpixel;
00037   nh.param("subpixel_corners", subpixel, true);
00038 
00039   // Making origin side non-required since it's specified in action goal
00040   bool origin_on_right;
00041   nh.param("origin_on_right", origin_on_right, true);
00042 
00043   // Checkerboard frame (looking down on the board, prongs forward): (left, forward, down)
00044   tf::Transform& plug_in_board = target_in_detected_object_;
00045   double pos[3], ori[4];
00046   if (nh.getParam("plug_position_x", pos[0]) &&
00047       nh.getParam("plug_position_y", pos[1]) &&
00048       nh.getParam("plug_position_z", pos[2]) &&
00049       nh.getParam("plug_orientation_x", ori[0]) &&
00050       nh.getParam("plug_orientation_y", ori[1]) &&
00051       nh.getParam("plug_orientation_z", ori[2]) &&
00052       nh.getParam("plug_orientation_w", ori[3]) )
00053   {
00054     plug_in_board.getOrigin().setValue(pos[0], pos[1], pos[2]);
00055     plug_in_board.setRotation(tf::Quaternion(ori[0], ori[1], ori[2], ori[3]));
00056     ROS_DEBUG("plug_in_board: T(%f,%f,%f), R(%f,%f,%f,%f)",
00057               pos[0], pos[1], pos[2], ori[0], ori[1], ori[2], ori[3]);
00058   } else {
00059     ROS_WARN("Plug in board pose unspecified, setting it to identity");
00060     plug_in_board.setIdentity();
00061   }
00062 
00063   if (!have_all_required) return false;
00064 
00065   // Initialize detector
00066   detector_.setDimensions(width, height);
00067   detector_.setSubpixel(subpixel);
00068   detector_.setOriginSide(origin_on_right ? Detector::RIGHT : Detector::LEFT);
00069 
00070   // Initialize pose estimator with checkerboard object model
00071   pose_estimator_ = createCheckerboardEstimator(width, height, square_size);
00072 
00073   return true;
00074 }
00075 
00076 // detected object = checkerboard, target = plug
00077 bool RosDetector::detectObject(const sensor_msgs::ImageConstPtr& image_msg,
00078                                const sensor_msgs::CameraInfoConstPtr& info_msg,
00079                                const tf::Stamped<tf::Pose>& target_prior, const tf::Transformer& transformer,
00080                                tf::Stamped<tf::Pose>& target_pose)
00081 {
00082   // Convert image message
00083   cv_bridge::CvImageConstPtr cv_image = cv_bridge::toCvShare(image_msg, "mono8");
00084   cv::Mat image = cv_image->image;
00085 
00086   // Detect the checkerboard
00087   std::vector<cv::Point2f> corners;
00088   if (!detector_.detect(image, corners)) {
00089     ROS_DEBUG("%s: Failed to detect checkerboard", name_.c_str());
00090     publishDisplayImage(image, corners, false);
00092     return false;
00093   }
00094 
00095   // Estimate its pose
00096   cam_model_.fromCameraInfo(info_msg);
00097   tf::Stamped<tf::Pose> target_prior_in_camera;
00098   try {
00099     transformer.transformPose(cam_model_.tfFrame(), target_prior, target_prior_in_camera);
00100   }
00101   catch (tf::TransformException& ex) {
00102     ROS_WARN("%s: TF exception\n%s", name_.c_str(), ex.what());
00103     return false;
00104   }
00105   tf::Pose detected_object_prior = target_prior_in_camera * target_in_detected_object_.inverse();
00106   tf::Pose detected_object_pose = pose_estimator_.solveWithPrior(corners, cam_model_, detected_object_prior);
00107   target_pose = tf::Stamped<tf::Pose>(detected_object_pose * target_in_detected_object_,
00108                                       image_msg->header.stamp, cam_model_.tfFrame());
00109 
00110   // Publish visualization messages
00111   tf_broadcaster_.sendTransform(tf::StampedTransform(detected_object_prior, image_msg->header.stamp,
00112                                                      cam_model_.tfFrame(), "checkerboard_prior_frame"));
00113   tf_broadcaster_.sendTransform(tf::StampedTransform(target_prior, target_prior.stamp_,
00114                                                      target_prior.frame_id_, "plug_prior_frame"));
00115   tf_broadcaster_.sendTransform(tf::StampedTransform(detected_object_pose, image_msg->header.stamp,
00116                                                      cam_model_.tfFrame(), "checkerboard_frame"));
00117   tf_broadcaster_.sendTransform(tf::StampedTransform(target_pose, image_msg->header.stamp,
00118                                                      cam_model_.tfFrame(), "plug_frame"));
00119   publishDisplayImage(image, corners, true);
00120 
00121   return true;
00122 }
00123 
00124 void RosDetector::publishDisplayImage(const cv::Mat& source, const std::vector<cv::Point2f>& corners,
00125                                       bool success)
00126 {
00127   if (display_pub_.getNumSubscribers() == 0) return;
00128   detector_.getDisplayImage(source, corners, success, display_img_cv_);
00129   cv_bridge::CvImage cv_image(std_msgs::Header(), "bgr8", display_img_cv_);
00130   display_pub_.publish(*(cv_image.toImageMsg()));
00131 }
00132 
00133 } //namespace checkerboard_pose_estimation


checkerboard_pose_estimation
Author(s): Patrick Mihelich
autogenerated on Mon Dec 2 2013 13:22:26