node.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 Delft Robotics B.V.
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
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         // initialize ros communication
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         // parameters
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         // create the calibration plane marker
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         // triangle 1
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         // triangle 2
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         // extract image
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         // extract pointcloud
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         // find calibration plate in image, and find isometry from camera to calibration plate
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         // get the current time for publishing stamps
00191         ros::Time now = sensor_msgs_cloud.header.stamp;
00192 
00193         // find transformation from tag frame to the frame to which we want to calibrate
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         // transform for target to camera frame
00214         Eigen::Isometry3d camera_to_target = tag_to_target * camera_to_tag;
00215 
00216         // clone image and draw detection
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         // transform the target (model) to the target frame to verify a correct calibration
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         // copy result to response
00225         tf::transformEigenToMsg(camera_to_target, transform);
00226 
00227         // publish result if necessary
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         // copy headers to publishing pointclouds
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         // publish calibration plane
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         // publish the debug information
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         // load the image
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         // load the pointcloud
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         // Use a specific callback queue for the data topics.
00311         ros::CallbackQueue queue;
00312 
00313         // Synchronize image and point cloud from topic
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         // Use a promise/future for communicating the data between threads.
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         // Run a background spinner for the callback queue.
00325         ros::AsyncSpinner spinner(1, &queue);
00326         spinner.start();
00327 
00328         // Wait for the future.
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         // Stop the spinner and copy the result to the request.
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         // Get all other calibration information from the service call request
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 


camera_pose_calibration
Author(s): Hans Gaiser , Jeff van Egmond , Maarten de Vries , Mihai Morariu , Ronald Ensing
autogenerated on Thu Jun 6 2019 20:34:08