00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <cv_bridge/cv_bridge.h>
00037 #include <jsk_recognition_utils/pcl_ros_util.h>
00038 #include <jsk_topic_tools/rosparam_utils.h>
00039 #include <sensor_msgs/image_encodings.h>
00040 #include <sensor_msgs/point_cloud2_iterator.h>
00041
00042 #include "jsk_pcl_ros/depth_image_creator.h"
00043
00044 void jsk_pcl_ros::DepthImageCreator::onInit () {
00045 NODELET_INFO("[%s::onInit]", getName().c_str());
00046 ConnectionBasedNodelet::onInit();
00047 tf_listener_ = TfListenerSingleton::getInstance();
00048
00049 pnh_->param("scale_depth", scale_depth, 1.0);
00050 ROS_INFO("scale_depth : %f", scale_depth);
00051
00052
00053 pnh_->param("use_fixed_transform", use_fixed_transform, false);
00054 ROS_INFO("use_fixed_transform : %d", use_fixed_transform);
00055
00056 pnh_->param("use_service", use_service, false);
00057 ROS_INFO("use_service : %d", use_service);
00058
00059 pnh_->param("use_asynchronous", use_asynchronous, false);
00060 ROS_INFO("use_asynchronous : %d", use_asynchronous);
00061
00062 pnh_->param("use_approximate", use_approximate, false);
00063 ROS_INFO("use_approximate : %d", use_approximate);
00064
00065 pnh_->param("info_throttle", info_throttle_, 0);
00066 info_counter_ = 0;
00067 pnh_->param("max_queue_size", max_queue_size_, 3);
00068
00069
00070 pnh_->param("max_pub_queue_size", max_pub_queue_size_, max_queue_size_);
00071 pnh_->param("max_sub_queue_size", max_sub_queue_size_, max_queue_size_);
00072
00073 std::vector<double> trans_pos(3, 0);
00074 std::vector<double> trans_quat(4, 0); trans_quat[3] = 1.0;
00075 if (pnh_->hasParam("translation")) {
00076 jsk_topic_tools::readVectorParameter(*pnh_, "translation", trans_pos);
00077 }
00078 if (pnh_->hasParam("rotation")) {
00079 jsk_topic_tools::readVectorParameter(*pnh_, "rotation", trans_quat);
00080 }
00081 tf::Quaternion btq(trans_quat[0], trans_quat[1], trans_quat[2], trans_quat[3]);
00082 tf::Vector3 btp(trans_pos[0], trans_pos[1], trans_pos[2]);
00083 fixed_transform.setOrigin(btp);
00084 fixed_transform.setRotation(btq);
00085
00086 pub_depth_ = advertise<sensor_msgs::Image> (*pnh_, "output", max_pub_queue_size_);
00087 pub_image_ = advertise<sensor_msgs::Image> (*pnh_, "output_image", max_pub_queue_size_);
00088 pub_cloud_ = advertise<PointCloud>(*pnh_, "output_cloud", max_pub_queue_size_);
00089 pub_disp_image_ = advertise<stereo_msgs::DisparityImage> (*pnh_, "output_disp", max_pub_queue_size_);
00090
00091 if (use_service) {
00092 service_ = pnh_->advertiseService("set_point_cloud",
00093 &DepthImageCreator::service_cb, this);
00094 }
00095 onInitPostProcess();
00096 }
00097
00098 void jsk_pcl_ros::DepthImageCreator::subscribe() {
00099 if (!use_service) {
00100 if (use_asynchronous) {
00101 sub_as_info_ = pnh_->subscribe<sensor_msgs::CameraInfo> ("info", max_sub_queue_size_,
00102 &DepthImageCreator::callback_info, this);
00103 sub_as_cloud_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_sub_queue_size_,
00104 &DepthImageCreator::callback_cloud, this);
00105 } else {
00106 sub_info_.subscribe(*pnh_, "info", max_sub_queue_size_);
00107 sub_cloud_.subscribe(*pnh_, "input", max_sub_queue_size_);
00108
00109 if (use_approximate) {
00110 sync_inputs_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> > > (max_queue_size_);
00111 sync_inputs_a_->connectInput (sub_info_, sub_cloud_);
00112 sync_inputs_a_->registerCallback (bind (&DepthImageCreator::callback_sync, this, _1, _2));
00113 } else {
00114 sync_inputs_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> > > (max_queue_size_);
00115 sync_inputs_e_->connectInput (sub_info_, sub_cloud_);
00116 sync_inputs_e_->registerCallback (bind (&DepthImageCreator::callback_sync, this, _1, _2));
00117 }
00118 }
00119 } else {
00120
00121 sub_as_info_ = pnh_->subscribe<sensor_msgs::CameraInfo> ("info", max_sub_queue_size_,
00122 &DepthImageCreator::callback_info, this);
00123
00124 }
00125 }
00126
00127 void jsk_pcl_ros::DepthImageCreator::unsubscribe() {
00128 if (!use_service) {
00129 if (use_asynchronous) {
00130 sub_as_info_.shutdown();
00131 sub_as_cloud_.shutdown();
00132 }
00133 else {
00134 sub_info_.unsubscribe();
00135 sub_cloud_.unsubscribe();
00136 }
00137 } else {
00138
00139 sub_as_info_.shutdown();
00140 }
00141 }
00142
00143 bool jsk_pcl_ros::DepthImageCreator::service_cb (std_srvs::Empty::Request &req,
00144 std_srvs::Empty::Response &res) {
00145 return true;
00146 }
00147
00148 void jsk_pcl_ros::DepthImageCreator::callback_sync(const sensor_msgs::CameraInfoConstPtr& info,
00149 const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
00150 ROS_DEBUG("DepthImageCreator::callback_sync");
00151 publish_points(info, pcloud2);
00152 }
00153
00154 void jsk_pcl_ros::DepthImageCreator::callback_cloud(const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
00155 ROS_DEBUG("DepthImageCreator::callback_cloud");
00156 boost::mutex::scoped_lock lock(this->mutex_points);
00157 points_ptr_ = pcloud2;
00158 }
00159
00160 void jsk_pcl_ros::DepthImageCreator::callback_info(const sensor_msgs::CameraInfoConstPtr& info) {
00161 ROS_DEBUG("DepthImageCreator::callback_info");
00162 boost::mutex::scoped_lock lock(this->mutex_points);
00163 if( info_counter_++ >= info_throttle_ ) {
00164 info_counter_ = 0;
00165 } else {
00166 return;
00167 }
00168 publish_points(info, points_ptr_);
00169 }
00170
00171 void jsk_pcl_ros::DepthImageCreator::publish_points(const sensor_msgs::CameraInfoConstPtr& info,
00172 const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
00173 sensor_msgs::PointCloud2Ptr pcloud2_ptr(new sensor_msgs::PointCloud2(*pcloud2));
00174 if (!jsk_recognition_utils::hasField("rgb", *pcloud2_ptr)) {
00175 sensor_msgs::PointCloud2Modifier pc_mod(*pcloud2_ptr);
00176 pc_mod.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32,
00177 "y", 1, sensor_msgs::PointField::FLOAT32,
00178 "z", 1, sensor_msgs::PointField::FLOAT32,
00179 "rgb", 1, sensor_msgs::PointField::FLOAT32);
00180 }
00181
00182 ROS_DEBUG("DepthImageCreator::publish_points");
00183 if (!pcloud2_ptr) return;
00184 bool proc_cloud = true, proc_depth = true, proc_image = true, proc_disp = true;
00185 if ( pub_cloud_.getNumSubscribers()==0 ) {
00186 proc_cloud = false;
00187 }
00188 if ( pub_depth_.getNumSubscribers()==0 ) {
00189 proc_depth = false;
00190 }
00191 if ( pub_image_.getNumSubscribers()==0 ) {
00192 proc_image = false;
00193 }
00194 if ( pub_disp_image_.getNumSubscribers()==0 ) {
00195 proc_disp = false;
00196 }
00197 if( !proc_cloud && !proc_depth && !proc_image && !proc_disp) return;
00198
00199 int width = info->width;
00200 int height = info->height;
00201 float fx = info->P[0];
00202 float cx = info->P[2];
00203 float tx = info->P[3];
00204 float fy = info->P[5];
00205 float cy = info->P[6];
00206
00207 Eigen::Affine3f sensorPose;
00208 {
00209 tf::StampedTransform transform;
00210 if(use_fixed_transform) {
00211 transform = fixed_transform;
00212 } else {
00213 try {
00214 tf_listener_->waitForTransform(pcloud2_ptr->header.frame_id,
00215 info->header.frame_id,
00216 info->header.stamp,
00217 ros::Duration(0.001));
00218 tf_listener_->lookupTransform(pcloud2_ptr->header.frame_id,
00219 info->header.frame_id,
00220 info->header.stamp, transform);
00221 }
00222 catch ( std::runtime_error e ) {
00223 ROS_ERROR("%s",e.what());
00224 return;
00225 }
00226 }
00227 tf::Vector3 p = transform.getOrigin();
00228 tf::Quaternion q = transform.getRotation();
00229 sensorPose = (Eigen::Affine3f)Eigen::Translation3f(p.getX(), p.getY(), p.getZ());
00230 Eigen::Quaternion<float> rot(q.getW(), q.getX(), q.getY(), q.getZ());
00231 sensorPose = sensorPose * rot;
00232
00233 if (tx != 0.0) {
00234 Eigen::Affine3f trans = (Eigen::Affine3f)Eigen::Translation3f(-tx/fx , 0, 0);
00235 sensorPose = sensorPose * trans;
00236 }
00237 #if 0 // debug print
00238 ROS_INFO("%f %f %f %f %f %f %f %f %f, %f %f %f",
00239 sensorPose(0,0), sensorPose(0,1), sensorPose(0,2),
00240 sensorPose(1,0), sensorPose(1,1), sensorPose(1,2),
00241 sensorPose(2,0), sensorPose(2,1), sensorPose(2,2),
00242 sensorPose(0,3), sensorPose(1,3), sensorPose(2,3));
00243 #endif
00244 }
00245
00246 PointCloud pointCloud;
00247 pcl::RangeImagePlanar rangeImageP;
00248 {
00249
00250 PointCloud tpc;
00251 pcl::fromROSMsg(*pcloud2_ptr, tpc);
00252
00253 Eigen::Affine3f inv;
00254 #if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
00255 inv = sensorPose.inverse();
00256 pcl::transformPointCloud< Point > (tpc, pointCloud, inv);
00257 #else
00258 pcl::getInverse(sensorPose, inv);
00259 pcl::getTransformedPointCloud<PointCloud> (tpc, inv, pointCloud);
00260 #endif
00261
00262 Eigen::Affine3f dummytrans;
00263 dummytrans.setIdentity();
00264 rangeImageP.createFromPointCloudWithFixedSize (pointCloud,
00265 width/scale_depth, height/scale_depth,
00266 cx/scale_depth, cy/scale_depth,
00267 fx/scale_depth, fy/scale_depth,
00268 dummytrans);
00269 }
00270
00271
00272 cv::Mat color_mat = cv::Mat::zeros(rangeImageP.height, rangeImageP.width, CV_8UC3);
00273 cv::Mat depth_mat(rangeImageP.height, rangeImageP.width, CV_32FC1);
00274 depth_mat.setTo(std::numeric_limits<float>::quiet_NaN());
00275 for (size_t i=0; i<pointCloud.size(); i++) {
00276 Point pt = pointCloud[i];
00277
00278 int image_x;
00279 int image_y;
00280 rangeImageP.getImagePoint(pt.x, pt.y, pt.z, image_x, image_y);
00281
00282 if (!rangeImageP.isInImage(image_x, image_y)) {
00283 continue;
00284 }
00285
00286 pcl::PointWithRange pt_with_range = rangeImageP.getPoint(image_x, image_y);
00287 depth_mat.at<float>(image_y, image_x) = pt_with_range.z;
00288
00289 if (!std::isnan(pt.x) && !std::isnan(pt.y) && !std::isnan(pt.z)) {
00290 color_mat.at<cv::Vec3b>(image_y, image_x) = cv::Vec3b(pt.r, pt.g, pt.b);
00291 }
00292 }
00293
00294 if (scale_depth != 1.0) {
00295
00296 cv::resize(color_mat, color_mat, cv::Size(info->width, info->height));
00297 cv::resize(depth_mat, depth_mat, cv::Size(info->width, info->height));
00298 }
00299
00300 if (proc_image) {
00301 pub_image_.publish(cv_bridge::CvImage(info->header,
00302 sensor_msgs::image_encodings::RGB8,
00303 color_mat).toImageMsg());
00304 }
00305 if (proc_depth) {
00306 pub_depth_.publish(cv_bridge::CvImage(info->header,
00307 sensor_msgs::image_encodings::TYPE_32FC1,
00308 depth_mat).toImageMsg());
00309 }
00310
00311 if(proc_cloud || proc_disp) {
00312
00313 pcl::RangeImagePlanar rangeImagePP;
00314 rangeImagePP.setDepthImage ((float *)depth_mat.ptr(),
00315 width, height,
00316 cx, cy, fx, fy);
00317 #if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
00318 rangeImagePP.header = pcl_conversions::toPCL(info->header);
00319 #else
00320 rangeImagePP.header = info->header;
00321 #endif
00322 if (proc_cloud) {
00323 PointCloud cloud_out;
00324 cloud_out.header = rangeImagePP.header;
00325 cloud_out.resize(rangeImagePP.points.size());
00326 for (int y = 0; y < (int)rangeImagePP.height; y++ ) {
00327 for (int x = 0; x < (int)rangeImagePP.width; x++ ) {
00328 pcl::PointWithRange pt_from = rangeImagePP.points[rangeImagePP.width * y + x];
00329 cv::Vec3b rgb = color_mat.at<cv::Vec3b>(y, x);
00330 Point pt_to = cloud_out[rangeImagePP.width * y + x];
00331 pt_to.x = pt_from.x;
00332 pt_to.y = pt_from.y;
00333 pt_to.z = pt_from.z;
00334 pt_to.r = rgb[0];
00335 pt_to.g = rgb[1];
00336 pt_to.b = rgb[2];
00337 }
00338 }
00339 pub_cloud_.publish(boost::make_shared<PointCloud>(cloud_out));
00340 }
00341
00342 if(proc_disp) {
00343 stereo_msgs::DisparityImage disp;
00344 #if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
00345 disp.header = pcl_conversions::fromPCL(rangeImagePP.header);
00346 #else
00347 disp.header = rangeImagePP.header;
00348 #endif
00349 disp.image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00350 disp.image.height = rangeImagePP.height;
00351 disp.image.width = rangeImagePP.width;
00352 disp.image.step = disp.image.width * sizeof(float);
00353 disp.f = fx; disp.T = 0.075;
00354 disp.min_disparity = 0;
00355 disp.max_disparity = disp.T * disp.f / 0.3;
00356 disp.delta_d = 0.125;
00357
00358 disp.image.data.resize (disp.image.height * disp.image.step);
00359 float *data = reinterpret_cast<float*> (&disp.image.data[0]);
00360
00361 float normalization_factor = disp.f * disp.T;
00362 for (int y = 0; y < (int)rangeImagePP.height; y++ ) {
00363 for (int x = 0; x < (int)rangeImagePP.width; x++ ) {
00364 pcl::PointWithRange p = rangeImagePP.getPoint(x,y);
00365 data[y*disp.image.width+x] = normalization_factor / p.z;
00366 }
00367 }
00368 pub_disp_image_.publish(boost::make_shared<stereo_msgs::DisparityImage> (disp));
00369 }
00370 }
00371 }
00372
00373 #include <pluginlib/class_list_macros.h>
00374 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::DepthImageCreator, nodelet::Nodelet);