calibrate_kinect_checkerboard.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 #include <image_transport/image_transport.h>
00032 
00033 #include <cv_bridge/cv_bridge.h>
00034 #include <image_geometry/pinhole_camera_model.h>
00035 #include <tf/transform_listener.h>
00036 #include <tf/transform_broadcaster.h>
00037 
00038 #include <pcl/point_types.h>
00039 #include <pcl_ros/point_cloud.h>
00040 #include <pcl_ros/transforms.h>
00041 #include <pcl/registration/registration.h>
00042 
00043 #include <sensor_msgs/Image.h>
00044 #include <sensor_msgs/CameraInfo.h>
00045 #include <geometry_msgs/PointStamped.h>
00046 
00047 #include <turtlebot_kinect_arm_calibration/detect_calibration_pattern.h>
00048 
00049 using namespace std;
00050 using namespace Eigen;
00051 
00052 tf::Transform tfFromEigen(Eigen::Matrix4f trans)
00053 {
00054     btMatrix3x3 btm;
00055     btm.setValue(trans(0,0),trans(0,1),trans(0,2),
00056                  trans(1,0),trans(1,1),trans(1,2),
00057                  trans(2,0),trans(2,1),trans(2,2));
00058     btTransform ret;
00059     ret.setOrigin(btVector3(trans(0,3),trans(1,3),trans(2,3)));
00060     ret.setBasis(btm);
00061     return ret;
00062 }
00063 
00064 Eigen::Matrix4f EigenFromTF(tf::Transform trans)
00065 {
00066   Eigen::Matrix4f out;
00067   btQuaternion quat = trans.getRotation();
00068   btVector3 origin = trans.getOrigin();
00069   
00070   Eigen::Quaternionf quat_out(quat.w(), quat.x(), quat.y(), quat.z());
00071   Eigen::Vector3f origin_out(origin.x(), origin.y(), origin.z());
00072   
00073   out.topLeftCorner<3,3>() = quat_out.toRotationMatrix();
00074   out.topRightCorner<3,1>() = origin_out;
00075   out(3,3) = 1;
00076   
00077   return out;
00078 }
00079 
00080 
00081 class CalibrateKinectCheckerboard
00082 {
00083     // Nodes and publishers/subscribers
00084     ros::NodeHandle nh_;
00085     image_transport::ImageTransport it_;
00086     image_transport::Publisher pub_;
00087     ros::Publisher detector_pub_;
00088     ros::Publisher physical_pub_;
00089   
00090     // Image and camera info subscribers;
00091     ros::Subscriber image_sub_; 
00092     ros::Subscriber info_sub_;
00093 
00094     // Structures for interacting with ROS messages
00095     cv_bridge::CvImagePtr input_bridge_;
00096     cv_bridge::CvImagePtr output_bridge_;
00097     tf::TransformListener tf_listener_;
00098     tf::TransformBroadcaster tf_broadcaster_;
00099     image_geometry::PinholeCameraModel cam_model_;
00100     
00101     // Calibration objects
00102     PatternDetector pattern_detector_;
00103     
00104     // The optimized transform
00105     Eigen::Transform<float, 3, Eigen::Affine> transform_;
00106     
00107     // Visualization for markers
00108     pcl::PointCloud<pcl::PointXYZ> detector_points_;
00109     pcl::PointCloud<pcl::PointXYZ> ideal_points_;
00110     pcl::PointCloud<pcl::PointXYZ> image_points_;
00111     pcl::PointCloud<pcl::PointXYZ> physical_points_;
00112     
00113     // Have we calibrated the camera yet?
00114     bool calibrated;
00115     
00116     ros::Timer timer_;
00117     
00118     // Parameters
00119     std::string fixed_frame;
00120     std::string camera_frame;
00121     std::string target_frame;
00122     std::string tip_frame;
00123     std::string touch_frame;
00124     
00125     int checkerboard_width;
00126     int checkerboard_height;
00127     double checkerboard_grid;
00128     
00129     // Gripper tip position
00130     geometry_msgs::PointStamped gripper_tip;
00131 
00132 public:
00133   CalibrateKinectCheckerboard()
00134     : nh_("~"), it_(nh_), calibrated(false)
00135   {
00136     // Load parameters from the server.
00137     nh_.param<std::string>("fixed_frame", fixed_frame, "/base_link");
00138     nh_.param<std::string>("camera_frame", camera_frame, "/camera_link");
00139     nh_.param<std::string>("target_frame", target_frame, "/calibration_pattern");
00140     nh_.param<std::string>("tip_frame", tip_frame, "/gripper_link");
00141     
00142     nh_.param<int>("checkerboard_width", checkerboard_width, 6);
00143     nh_.param<int>("checkerboard_height", checkerboard_height, 7);
00144     nh_.param<double>("checkerboard_grid", checkerboard_grid, 0.027);
00145     
00146     // Set pattern detector sizes
00147     pattern_detector_.setPattern(cv::Size(checkerboard_width, checkerboard_height), checkerboard_grid, CHESSBOARD);
00148     
00149     transform_.translation().setZero();
00150     transform_.matrix().topLeftCorner<3, 3>() = Quaternionf().setIdentity().toRotationMatrix();
00151     
00152     // Create subscriptions
00153     info_sub_ = nh_.subscribe("/camera/rgb/camera_info", 1, &CalibrateKinectCheckerboard::infoCallback, this);
00154     
00155     // Also publishers
00156     pub_ = it_.advertise("calibration_pattern_out", 1);
00157     detector_pub_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZ> >("detector_cloud", 1);
00158     physical_pub_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZ> >("physical_points_cloud", 1);
00159     
00160     // Create ideal points
00161     ideal_points_.push_back( pcl::PointXYZ(0, 0, 0) );
00162     ideal_points_.push_back( pcl::PointXYZ((checkerboard_width-1)*checkerboard_grid, 0, 0) );
00163     ideal_points_.push_back( pcl::PointXYZ(0, (checkerboard_height-1)*checkerboard_grid, 0) );
00164     ideal_points_.push_back( pcl::PointXYZ((checkerboard_width-1)*checkerboard_grid, (checkerboard_height-1)*checkerboard_grid, 0) );
00165     
00166     // Create proper gripper tip point
00167     nh_.param<double>("gripper_tip_x", gripper_tip.point.x, 0.0);
00168     nh_.param<double>("gripper_tip_y", gripper_tip.point.y, 0.0);
00169     nh_.param<double>("gripper_tip_z", gripper_tip.point.z, 0.0);
00170     gripper_tip.header.frame_id = tip_frame;
00171     
00172     ROS_INFO("[calibrate] Initialized.");
00173   }
00174 
00175   void infoCallback(const sensor_msgs::CameraInfoConstPtr& info_msg)
00176   {
00177     if (calibrated)
00178       return;
00179     cam_model_.fromCameraInfo(info_msg);
00180     pattern_detector_.setCameraMatrices(cam_model_.intrinsicMatrix(), cam_model_.distortionCoeffs());
00181     
00182     calibrated = true;
00183     image_sub_ = nh_.subscribe("/camera/rgb/image_mono", 1, &CalibrateKinectCheckerboard::imageCallback, this);
00184     
00185     ROS_INFO("[calibrate] Got image info!");
00186   }
00187   
00188   void pointcloudCallback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& msg)
00189   {
00190     sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
00191     pcl::toROSMsg (*msg, *image_msg);
00192   
00193     imageCallback(image_msg);
00194   }
00195   
00196   void imageCallback(const sensor_msgs::ImageConstPtr& image_msg)
00197   {
00198     try
00199     {
00200       input_bridge_ = cv_bridge::toCvCopy(image_msg, "mono8");
00201       output_bridge_ = cv_bridge::toCvCopy(image_msg, "bgr8");
00202     }
00203     catch (cv_bridge::Exception& ex)
00204     {
00205       ROS_ERROR("[calibrate] Failed to convert image");
00206       return;
00207     }
00208   
00209     Eigen::Vector3f translation;
00210     Eigen::Quaternionf orientation;
00211     
00212     if (!pattern_detector_.detectPattern(input_bridge_->image, translation, orientation, output_bridge_->image))
00213     {
00214       ROS_INFO("[calibrate] Couldn't detect checkerboard, make sure it's visible in the image.");
00215       return;
00216     }
00217     
00218     tf::Transform target_transform;
00219     tf::StampedTransform base_transform;
00220     try
00221     {
00222       ros::Time acquisition_time = image_msg->header.stamp;
00223       ros::Duration timeout(1.0 / 30.0);
00224                                    
00225       target_transform.setOrigin( tf::Vector3(translation.x(), translation.y(), translation.z()) );
00226       target_transform.setRotation( tf::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w()) );
00227       tf_broadcaster_.sendTransform(tf::StampedTransform(target_transform, image_msg->header.stamp, image_msg->header.frame_id, target_frame));
00228     }
00229     catch (tf::TransformException& ex)
00230     {
00231       ROS_WARN("[calibrate] TF exception:\n%s", ex.what());
00232       return;
00233     }
00234     publishCloud(ideal_points_, target_transform, image_msg->header.frame_id);
00235     
00236     overlayPoints(ideal_points_, target_transform, output_bridge_);
00237     
00238     // Publish calibration image
00239     pub_.publish(output_bridge_->toImageMsg());
00240     
00241     pcl_ros::transformPointCloud(ideal_points_, image_points_, target_transform);
00242     
00243     cout << "Got an image callback!" << endl;
00244     
00245     calibrate(image_msg->header.frame_id);
00246     
00247     ros::shutdown();
00248   }
00249   
00250 
00251   void publishCloud(pcl::PointCloud<pcl::PointXYZ> detector_points, tf::Transform &transform, const std::string frame_id)
00252   {
00253     // Display to rviz
00254     pcl::PointCloud<pcl::PointXYZ> transformed_detector_points;
00255     
00256     pcl_ros::transformPointCloud(detector_points, transformed_detector_points, transform);
00257     
00258     transformed_detector_points.header.frame_id = frame_id;
00259     detector_pub_.publish(transformed_detector_points);
00260   }
00261   
00262   void overlayPoints(pcl::PointCloud<pcl::PointXYZ> detector_points, tf::Transform &transform, cv_bridge::CvImagePtr& image)
00263   {  
00264     // Overlay calibration points on the image
00265     pcl::PointCloud<pcl::PointXYZ> transformed_detector_points;
00266     
00267     pcl_ros::transformPointCloud(detector_points, transformed_detector_points, transform);
00268     
00269     int font_face = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;
00270     double font_scale = 1;
00271     int thickness = 2;
00272     int radius = 5;
00273    
00274     for (unsigned int i=0; i < transformed_detector_points.size(); i++)
00275     {
00276       pcl::PointXYZ pt = transformed_detector_points[i];
00277       cv::Point3d pt_cv(pt.x, pt.y, pt.z);
00278       cv::Point2d uv;
00279       uv = cam_model_.project3dToPixel(pt_cv);
00280 
00281       cv::circle(image->image, uv, radius, CV_RGB(255,0,0), -1);
00282       cv::Size text_size;
00283       int baseline = 0;
00284       std::stringstream out;
00285       out << i+1;
00286       
00287       text_size = cv::getTextSize(out.str(), font_face, font_scale, thickness, &baseline);
00288                             
00289       cv::Point origin = cvPoint(uv.x - text_size.width / 2,
00290                                uv.y - radius - baseline/* - thickness*/);
00291       cv::putText(image->image, out.str(), origin, font_face, font_scale, CV_RGB(255,0,0), thickness);
00292     }
00293   }
00294   
00295   bool calibrate(const std::string frame_id)
00296   {
00297     physical_points_.empty();
00298     physical_points_.header.frame_id = fixed_frame;
00299     cout << "Is the checkerboard correct? " << endl;
00300     cout << "Move edge of gripper to point 1 in image and press Enter. " << endl;
00301     cin.ignore();
00302     addPhysicalPoint();
00303     cout << "Move edge of gripper to point 2 in image and press Enter. " << endl;
00304     cin.ignore();
00305     addPhysicalPoint();
00306     cout << "Move edge of gripper to point 3 in image and press Enter. " << endl;
00307     cin.ignore();
00308     addPhysicalPoint();
00309     cout << "Move edge of gripper to point 4 in image and press Enter. " << endl;
00310     cin.ignore();
00311     addPhysicalPoint();
00312     
00313     Eigen::Matrix4f t;
00314     
00315     physical_pub_.publish(physical_points_);
00316     
00317     pcl::estimateRigidTransformationSVD( physical_points_, image_points_, t );
00318 
00319     // Output       
00320     tf::Transform transform = tfFromEigen(t), trans_full, camera_transform_unstamped;
00321     tf::StampedTransform camera_transform;
00322   
00323     cout << "Resulting transform (camera frame -> fixed frame): " << endl << t << endl << endl;
00324     
00325     try
00326     {
00327       tf_listener_.lookupTransform(frame_id, camera_frame, ros::Time(0), camera_transform);
00328     }
00329     catch (tf::TransformException& ex)
00330     {
00331       ROS_WARN("[calibrate] TF exception:\n%s", ex.what());
00332       return false;
00333     }
00334 
00335     camera_transform_unstamped = camera_transform;
00336     trans_full = camera_transform_unstamped.inverse()*transform;
00337     
00338     Eigen::Matrix4f t_full = EigenFromTF(trans_full);
00339     Eigen::Matrix4f t_full_inv = (Eigen::Transform<float,3,Affine>(t_full).inverse()).matrix();
00340     
00341     cout << "Resulting transform (fixed frame -> camera frame): " << endl << t_full << endl << endl;
00342     printStaticTransform(t_full_inv, fixed_frame, camera_frame);
00343 
00344     return true;
00345   }
00346   
00347   void printStaticTransform(Eigen::Matrix4f& transform, const std::string frame1, const std::string frame2)
00348   {
00349     Eigen::Quaternionf quat(transform.topLeftCorner<3,3>() );
00350     Eigen::Vector3f translation(transform.topRightCorner<3,1>() );
00351     
00352     cout << "Static transform publisher (use for external kinect): " << endl;
00353     
00354     cout << "rosrun tf static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms" << endl;
00355     cout << "rosrun tf static_transform_publisher " << translation.x() << " "
00356          << translation.y() << " " << translation.z() << " " 
00357          << quat.x() << " " << quat.y() << " " << quat.z() << " " << quat.w() << " "
00358          << frame1 << " " << frame2 << " 100" << endl << endl;
00359          
00360     tf::Transform temp_tf_trans = tfFromEigen(transform);
00361     
00362     double yaw, pitch, roll;
00363     
00364     std::string fixed_frame_urdf(fixed_frame);
00365     
00366     // If there's a leading '/' character, remove it, as xacro can't deal with 
00367     // extra characters in the link name.
00368     if (fixed_frame_urdf.size() > 0 && fixed_frame_urdf[0] == '/')
00369       fixed_frame_urdf.erase(0, 1);
00370     
00371     temp_tf_trans.getBasis().getEulerYPR(yaw, pitch, roll);
00372     
00373     cout << "URDF output (use for kinect on robot): " << endl;
00374     
00375     cout << "<?xml version=\"1.0\"?>\n<robot>\n" << 
00376           "\t<property name=\"turtlebot_calib_cam_x\" value=\"" << translation.x() << "\" />\n" <<
00377           "\t<property name=\"turtlebot_calib_cam_y\" value=\"" << translation.y() << "\" />\n" <<
00378           "\t<property name=\"turtlebot_calib_cam_z\" value=\"" << translation.z() << "\" />\n" <<
00379           "\t<property name=\"turtlebot_calib_cam_rr\" value=\"" << roll << "\" />\n" <<
00380           "\t<property name=\"turtlebot_calib_cam_rp\" value=\"" << pitch << "\" />\n" <<
00381           "\t<property name=\"turtlebot_calib_cam_ry\" value=\"" << yaw << "\" />\n" <<
00382           "\t<property name=\"turtlebot_kinect_frame_name\" value=\"" << fixed_frame_urdf << "\" />\n" <<
00383           "</robot>" << endl << endl;
00384   }
00385   
00386   void addPhysicalPoint()
00387   {
00388     geometry_msgs::PointStamped pt_out;
00389     
00390     try
00391     {
00392       tf_listener_.transformPoint(fixed_frame, gripper_tip, pt_out);
00393     }
00394     catch (tf::TransformException& ex)
00395     {
00396       ROS_WARN("[calibrate] TF exception:\n%s", ex.what());
00397       return;
00398     }
00399     
00400     physical_points_.push_back(pcl::PointXYZ(pt_out.point.x, pt_out.point.y, pt_out.point.z));
00401   }
00402 
00403   void convertIdealPointstoPointcloud()
00404   {
00405     detector_points_.points.resize(pattern_detector_.ideal_points.size());
00406     for (unsigned int i=0; i < pattern_detector_.ideal_points.size(); i++)
00407     {
00408       cv::Point3f pt = pattern_detector_.ideal_points[i];
00409       detector_points_[i].x = pt.x; detector_points_[i].y = pt.y; detector_points_[i].z = pt.z; 
00410     }
00411   }
00412   
00413 };
00414 
00415 
00416 int main(int argc, char** argv)
00417 {
00418   ros::init(argc, argv, "calibrate_kinect_arm");
00419   
00420   CalibrateKinectCheckerboard cal;
00421   ros::spin();
00422 }
00423 


turtlebot_kinect_arm_calibration
Author(s): Helen Oleynikova
autogenerated on Thu Dec 12 2013 12:33:33