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