00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "node.hpp"
00018
00019 #include <opencv2/opencv.hpp>
00020
00021 #include <pcl/io/pcd_io.h>
00022 #include <pcl_ros/point_cloud.h>
00023 #include <pcl_conversions/pcl_conversions.h>
00024 #include <pcl/common/transforms.h>
00025
00026 #include <eigen_conversions/eigen_msg.h>
00027 #include <tf_conversions/tf_eigen.h>
00028 #include <sensor_msgs/image_encodings.h>
00029 #include <cv_bridge/cv_bridge.h>
00030 #include <message_filters/subscriber.h>
00031 #include <message_filters/time_synchronizer.h>
00032
00033 #include <boost/bind.hpp>
00034
00035 #include <boost/thread/future.hpp>
00036
00037 namespace camera_pose_calibration {
00038
00039 namespace {
00040 template<typename T>
00041 T getParam(ros::NodeHandle & node, std::string const & name, T const & default_value) {
00042 T result;
00043 node.param(name, result, default_value);
00044 return result;
00045 }
00046
00048 char const * cloud_topic = "points_registered";
00049
00051 char const * image_topic = "image_color";
00052 }
00053
00054 CameraPoseCalibrationNode::CameraPoseCalibrationNode() :
00055 image_transport(node_handle),
00056 calibrated(false)
00057 {
00058
00059 cloud_publisher = node_handle.advertise<pcl::PointCloud<pcl::PointXYZ> >("cloud", 1, true);
00060 target_cloud_publisher = node_handle.advertise<pcl::PointCloud<pcl::PointXYZ> >("target", 1, true);
00061 transformed_target_cloud_publisher = node_handle.advertise<pcl::PointCloud<pcl::PointXYZ> >("transformed_target", 1, true);
00062 source_cloud_publisher = node_handle.advertise<pcl::PointCloud<pcl::PointXYZ> >("source", 1, true);
00063 projected_source_cloud_publisher = node_handle.advertise<pcl::PointCloud<pcl::PointXYZ> >("projected_source", 1, true);
00064 calibration_plane_marker_publisher = node_handle.advertise<visualization_msgs::Marker>("calibration_plane", 1, true);
00065 detected_pattern_publisher = image_transport.advertise("detected_pattern", 1, true);
00066
00067 calibrate_server_call = node_handle.advertiseService("calibrate_call", &CameraPoseCalibrationNode::onCalibrateCall, this);
00068 calibrate_server_topic = node_handle.advertiseService("calibrate_topic", &CameraPoseCalibrationNode::onCalibrateTopic, this);
00069 calibrate_server_file = node_handle.advertiseService("calibrate_file", &CameraPoseCalibrationNode::onCalibrateFile, this);
00070
00071
00072 publish_transform = getParam(node_handle, "publish_transform", false);
00073 publish_rate = getParam(node_handle, "publish_rate", 1);
00074
00075 if (publish_transform) {
00076 tf_timer = node_handle.createTimer(publish_rate, &CameraPoseCalibrationNode::onTfTimeout, this);
00077 }
00078 }
00079
00080 visualization_msgs::Marker CameraPoseCalibrationNode::createCalibrationPlaneMarker(
00081 pcl::PointCloud<pcl::PointXYZ>::ConstPtr points,
00082 uint8_t pattern_width,
00083 uint8_t pattern_height
00084 ) {
00085
00086 visualization_msgs::Marker calibration_plane;
00087 calibration_plane.header.frame_id = points->header.frame_id;
00088 calibration_plane.ns = "calibration";
00089 calibration_plane.type = visualization_msgs::Marker::TRIANGLE_LIST;
00090 calibration_plane.action = visualization_msgs::Marker::ADD;
00091 calibration_plane.color.r = 1.0;
00092 calibration_plane.color.g = 1.0;
00093 calibration_plane.color.a = 1.0;
00094 calibration_plane.scale.x = 1.0;
00095 calibration_plane.scale.y = 1.0;
00096 calibration_plane.scale.z = 1.0;
00097 calibration_plane.frame_locked = true;
00098 calibration_plane.id = 0;
00099
00100
00101 geometry_msgs::Point triangle_point;
00102 triangle_point.x = points->points[0].x;
00103 triangle_point.y = points->points[0].y;
00104 triangle_point.z = points->points[0].z;
00105 calibration_plane.points.push_back(triangle_point);
00106 triangle_point.x = points->points[(pattern_height - 1) * pattern_width].x;
00107 triangle_point.y = points->points[(pattern_height - 1) * pattern_width].y;
00108 triangle_point.z = points->points[(pattern_height - 1) * pattern_width].z;
00109 calibration_plane.points.push_back(triangle_point);
00110 triangle_point.x = points->points[pattern_width - 1].x;
00111 triangle_point.y = points->points[pattern_width - 1].y;
00112 triangle_point.z = points->points[pattern_width - 1].z;
00113 calibration_plane.points.push_back(triangle_point);
00114
00115
00116 triangle_point.x = points->points[pattern_height * pattern_width - 1].x;
00117 triangle_point.y = points->points[pattern_height * pattern_width - 1].y;
00118 triangle_point.z = points->points[pattern_height * pattern_width - 1].z;
00119 calibration_plane.points.push_back(triangle_point);
00120 triangle_point.x = points->points[pattern_width - 1].x;
00121 triangle_point.y = points->points[pattern_width - 1].y;
00122 triangle_point.z = points->points[pattern_width - 1].z;
00123 calibration_plane.points.push_back(triangle_point);
00124 triangle_point.x = points->points[(pattern_height - 1) * pattern_width].x;
00125 triangle_point.y = points->points[(pattern_height - 1) * pattern_width].y;
00126 triangle_point.z = points->points[(pattern_height - 1) * pattern_width].z;
00127 calibration_plane.points.push_back(triangle_point);
00128
00129 return calibration_plane;
00130 }
00131
00132 void CameraPoseCalibrationNode::onTfTimeout(ros::TimerEvent const &) {
00133 if (calibrated) {
00134 calibration_transform.stamp_ = ros::Time::now();
00135 transform_broadcaster.sendTransform(calibration_transform);
00136 }
00137 }
00138
00139 bool CameraPoseCalibrationNode::calibrate(
00140 sensor_msgs::Image const & sensor_msgs_image,
00141 sensor_msgs::PointCloud2 const & sensor_msgs_cloud,
00142 std::string const & tag_frame,
00143 std::string const & target_frame,
00144 double const & point_cloud_scale_x,
00145 double const & point_cloud_scale_y,
00146 camera_pose_calibration::PatternParameters const & pattern,
00147 geometry_msgs::Transform & transform
00148 ) {
00149 ROS_INFO_STREAM("Received calibration request from '" << sensor_msgs_cloud.header.frame_id << "' to '" << target_frame << "'.");
00150
00151
00152 cv_bridge::CvImagePtr cv_ptr;
00153 try {
00154 cv_ptr = cv_bridge::toCvCopy(sensor_msgs_image, sensor_msgs::image_encodings::BGR8);
00155 } catch (cv_bridge::Exception& e) {
00156 ROS_ERROR_STREAM("Failed to convert sensor_msgs/Image to cv::Mat. Error: " << e.what());
00157 return false;
00158 }
00159 cv::Mat image = cv_ptr->image;
00160
00161
00162 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00163 pcl::fromROSMsg(sensor_msgs_cloud, *cloud);
00164 if (sensor_msgs_cloud.header.frame_id == "") {
00165 ROS_ERROR_STREAM("Found empty frame_id in given pointcloud.");
00166 return false;
00167 }
00168
00169
00170 CalibrationInformation debug_information;
00171 Eigen::Isometry3d camera_to_tag;
00172 try {
00173 camera_to_tag = camera_pose_calibration::findCalibration(
00174 image,
00175 cloud,
00176 cv::Size(pattern.pattern_width, pattern.pattern_height),
00177 pattern.pattern_distance,
00178 pattern.neighbor_distance,
00179 pattern.valid_pattern_ratio_threshold,
00180 point_cloud_scale_x,
00181 point_cloud_scale_y,
00182 &debug_information
00183 );
00184 } catch (std::exception const & e) {
00185 ROS_ERROR_STREAM("Failed to find isometry. Error: " << e.what());
00186 detected_pattern_publisher.publish(cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg());
00187 return false;
00188 }
00189
00190
00191 ros::Time now = sensor_msgs_cloud.header.stamp;
00192
00193
00194 Eigen::Isometry3d tag_to_target;
00195 if (tag_frame == target_frame) {
00196 tag_to_target = Eigen::Isometry3d::Identity();
00197 } else {
00198 try {
00199 tf::StampedTransform transform;
00200 if (!transform_listener.waitForTransform(target_frame, tag_frame, now, ros::Duration(1.0))) {
00201 ROS_ERROR_STREAM("Failed to find transform from " << tag_frame << " to " << target_frame);
00202 return false;
00203 }
00204 transform_listener.lookupTransform(target_frame, tag_frame, now, transform);
00205
00206 tf::transformTFToEigen(transform, tag_to_target);
00207 } catch (tf::TransformException const & e) {
00208 ROS_ERROR_STREAM("Failed to find transform. Error: " << e.what());
00209 return false;
00210 }
00211 }
00212
00213
00214 Eigen::Isometry3d camera_to_target = tag_to_target * camera_to_tag;
00215
00216
00217 cv::Mat detected_pattern = image.clone();
00218 cv::drawChessboardCorners(detected_pattern, cv::Size(pattern.pattern_width, pattern.pattern_height), cv::Mat(debug_information.image_points), true);
00219
00220
00221 pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_target_cloud(new pcl::PointCloud<pcl::PointXYZ>(*debug_information.target_cloud));
00222 pcl::transformPointCloud(*debug_information.target_cloud, *transformed_target_cloud, Eigen::Affine3d(camera_to_tag.inverse()));
00223
00224
00225 tf::transformEigenToMsg(camera_to_target, transform);
00226
00227
00228 calibrated = true;
00229 tf::Transform tf_camera_to_target;
00230 transformEigenToTF(camera_to_target, tf_camera_to_target);
00231 calibration_transform = tf::StampedTransform(tf_camera_to_target, now, target_frame, cloud->header.frame_id);
00232 if (publish_transform) {
00233 transform_broadcaster.sendTransform(calibration_transform);
00234 }
00235
00236
00237 pcl_conversions::toPCL(now, cloud->header.stamp);
00238 transformed_target_cloud->header = cloud->header;
00239 debug_information.projected_source_cloud->header = cloud->header;
00240 debug_information.source_cloud->header = cloud->header;
00241 debug_information.target_cloud->header = cloud->header;
00242 debug_information.target_cloud->header.frame_id = tag_frame;
00243
00244
00245 visualization_msgs::Marker calibration_plane = createCalibrationPlaneMarker(debug_information.projected_source_cloud, pattern.pattern_width, pattern.pattern_height);
00246 calibration_plane.header.stamp = now;
00247 calibration_plane_marker_publisher.publish(calibration_plane);
00248
00249
00250 cloud_publisher.publish(cloud);
00251 target_cloud_publisher.publish(debug_information.target_cloud);
00252 transformed_target_cloud_publisher.publish(transformed_target_cloud);
00253 source_cloud_publisher.publish(debug_information.source_cloud);
00254 projected_source_cloud_publisher.publish(debug_information.projected_source_cloud);
00255 detected_pattern_publisher.publish(cv_bridge::CvImage(std_msgs::Header(), "bgr8", detected_pattern).toImageMsg());
00256
00257 ROS_INFO_STREAM(cloud->header.frame_id << " to " << tag_frame << " :\n" << camera_to_tag.matrix() );
00258 ROS_INFO_STREAM(cloud->header.frame_id << " to " << target_frame << " :\n" << camera_to_target.matrix() );
00259
00260 return true;
00261 }
00262
00263 bool CameraPoseCalibrationNode::onCalibrateFile(camera_pose_calibration::CalibrateFile::Request & req, camera_pose_calibration::CalibrateFile::Response & res) {
00264
00265 std::string image_path = req.image;
00266 std_msgs::Header header;
00267 header.frame_id = req.camera_frame;
00268 cv_bridge::CvImage image_msg(header, sensor_msgs::image_encodings::BGR8, cv::imread(image_path));
00269 if (!image_msg.image.data) {
00270 ROS_ERROR_STREAM("Failed to read image from " << image_path << ".");
00271 return false;
00272 }
00273
00274 sensor_msgs::Image image;
00275 image_msg.toImageMsg(image);
00276
00277
00278 std::string cloud_path = req.cloud;
00279 pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
00280 if (pcl::io::loadPCDFile(cloud_path, *pcl_cloud) == -1) {
00281 ROS_ERROR_STREAM("Failed to read pointcloud " << cloud_path << ".");
00282 return false;
00283 }
00284
00285 pcl_cloud->header.frame_id = req.camera_frame;
00286 pcl_conversions::toPCL(ros::Time::now(), pcl_cloud->header.stamp);
00287
00288 sensor_msgs::PointCloud2 cloud;
00289 pcl::toROSMsg(*pcl_cloud, cloud);
00290
00291 return calibrate(
00292 image,
00293 cloud,
00294 req.tag_frame,
00295 req.target_frame,
00296 req.point_cloud_scale_x,
00297 req.point_cloud_scale_y,
00298 req.pattern,
00299 res.transform
00300 );
00301 }
00302
00303 typedef std::pair<sensor_msgs::Image::ConstPtr, sensor_msgs::PointCloud2::ConstPtr> InputData;
00304
00305 void synchronizationCallback(boost::promise<InputData> & promise, sensor_msgs::Image::ConstPtr image, sensor_msgs::PointCloud2::ConstPtr cloud) {
00306 promise.set_value(InputData(image, cloud));
00307 }
00308
00309 bool CameraPoseCalibrationNode::onCalibrateTopic(camera_pose_calibration::CalibrateTopic::Request & req, camera_pose_calibration::CalibrateTopic::Response & res) {
00310
00311 ros::CallbackQueue queue;
00312
00313
00314 message_filters::Subscriber<sensor_msgs::Image> image_sub(node_handle, image_topic, 1, ros::TransportHints(), &queue);
00315 message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub(node_handle, cloud_topic, 1, ros::TransportHints(), &queue);
00316 message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::PointCloud2> sync(image_sub, cloud_sub, 10);
00317
00318
00319
00320 boost::promise<InputData> promise;
00321 boost::unique_future<InputData> future = promise.get_future();
00322 sync.registerCallback(boost::bind(&synchronizationCallback, boost::ref(promise), _1, _2));
00323
00324
00325 ros::AsyncSpinner spinner(1, &queue);
00326 spinner.start();
00327
00328
00329 while (true) {
00330 if (!node_handle.ok()) return false;
00331 if (future.wait_for(boost::chrono::milliseconds(100)) == boost::future_status::ready) break;
00332 }
00333
00334
00335 spinner.stop();
00336 InputData data = future.get();
00337 sensor_msgs::Image image = *data.first;
00338 sensor_msgs::PointCloud2 cloud = *data.second;
00339
00340 if (image.data.empty() || cloud.data.empty()) {
00341 ROS_ERROR_STREAM("No image and/or point cloud received.");
00342 return false;
00343 }
00344
00345
00346 return calibrate(
00347 image,
00348 cloud,
00349 req.tag_frame,
00350 req.target_frame,
00351 req.point_cloud_scale_x,
00352 req.point_cloud_scale_y,
00353 req.pattern,
00354 res.transform
00355 );
00356 }
00357
00359 bool CameraPoseCalibrationNode::onCalibrateCall(camera_pose_calibration::CalibrateCall::Request & req, camera_pose_calibration::CalibrateCall::Response & res) {
00360 return calibrate(
00361 req.image,
00362 req.cloud,
00363 req.tag_frame,
00364 req.target_frame,
00365 req.point_cloud_scale_x,
00366 req.point_cloud_scale_y,
00367 req.pattern,
00368 res.transform
00369 );
00370 }
00371
00372
00373 }
00374