image_algos: image_algos: pcd_to_image_projector

00001 /* 
00002  * Copyright (c) 2010, Dejan Pangercic <dejan.pangercic@cs.tum.edu>, 
00003  Zoltan-Csaba Marton <marton@cs.tum.edu>, Nico Blodow <blodow@cs.tum.edu>
00004  * All rights reserved.
00005  * 
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  * 
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of Willow Garage, Inc. nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  * 
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 
00032 #include <image_algos/pcd_to_image_projector_algo.h>
00033 #include "highgui.h"  
00034 
00035 using namespace image_algos;
00036 
00037 void  PCDToImageProjector::init (ros::NodeHandle& nh)
00038 {
00039   nh_ = nh;
00040   nh_.param("origin", origin_, std::string("/RightEyeCalcOff"));
00041   nh_.param("child", child_, std::string("/map"));
00042   //nh_.param("input_image_topic", input_image_topic_, std::string("/cop/left/camera"));
00043   //nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/swissranger_test/cloud_sr"));
00044   //nh_.param("output_cluster_topic", output_cluster_topic_, std::string("output_cluster"));
00045   //nh_.param("output_image_topic", output_image_topic_, std::string("image_with_projected_cluster"));
00046   nh_.param("focal_length", focal_length_, 0.00641331974023884);
00047   nh_.param("proj_center_x", proj_center_x_, 833.248646581603);
00048   nh_.param("proj_center_y", proj_center_y_, 661.107370424523);
00049   nh_.param("pix_size_x", pix_size_x_, 7.43100103980579e-06);
00050   nh_.param("pix_size_y", pix_size_y_, 7.4e-06);
00051   //image_sub_ = it_.subscribe(input_image_topic_, 1, &PointCloudToImageProjector::image_cb, this);
00052   //cloud_sub_= nh_.subscribe (input_cloud_topic_, 10, &PointCloudToImageProjector::cloud_cb, this);
00053   //image_pub_ = it_.advertise(output_image_topic_, 1);
00054 }
00055 
00056 
00057 void  PCDToImageProjector::pre  ()
00058 {
00059   //  subimage_ = boost::shared_ptr <OutputType>(new OutputType ());
00060 }
00061 
00062 
00063 void  PCDToImageProjector::post ()
00064 {
00065   cvReleaseImage(&subimage_);
00066   //Nothing TODO
00067 }
00068 
00069 
00070 std::vector<std::string>  PCDToImageProjector::requires ()
00071 {
00072   return std::vector<std::string>();
00073 }
00074 
00075 
00076 std::vector<std::string>  PCDToImageProjector::provides ()
00077 {
00078   return std::vector<std::string>();
00079 }
00080 
00081 void PCDToImageProjector::project_3D_point(const double &x, const double &y, const double &z, int &row, int &column)
00082 {
00083   double temp1,temp2;
00084   if (focal_length_ > 0.0)
00085   {
00086     temp1 = focal_length_ * (x / z );
00087     temp2 = focal_length_ * (y / z );
00088   }
00089   else
00090   {
00091     temp1 = x;
00092     temp2 = y;
00093   }
00094   column = ( temp1 / pix_size_x_ ) + proj_center_x_;
00095   row = ( temp2 / pix_size_y_ ) + proj_center_y_;
00096 }
00097 
00098 
00099 std::string PCDToImageProjector::process (sensor_msgs::PointCloud &cloud_in, IplImage *image)
00100 {
00101   try
00102   {
00103     tf_listener_.transformPointCloud(origin_, cloud_in, cloud_in);
00104   }
00105   catch (tf::TransformException ex)
00106   {
00107     ROS_ERROR("[PCDToImageProjector:] %s",ex.what());
00108   }
00109   ROS_INFO("[PCDToImageProjector:] cloud transformed to %s", origin_.c_str());
00110   //create a sequence storage for projected points 
00111   CvMemStorage* stor = cvCreateMemStorage (0);
00112   CvSeq* seq = cvCreateSeq (CV_SEQ_ELTYPE_POINT, sizeof (CvSeq), sizeof (CvPoint), stor);
00113   if (cloud_in.points.size() == 0)
00114   {
00115     ROS_WARN("[PCDToImageProjector:] Point cloud points with size 0!!! Returning.");
00116     return std::string("[PCDToImageProjector:] Point cloud points with size 0!!! Returning.");
00117   }
00118   for (unsigned long i = 0; i < cloud_in.points.size(); i++)
00119   {
00120     cv::Point3d pt_cv(cloud_in.points[i].x, cloud_in.points[i].y, cloud_in.points[i].z);
00121     CvPoint uv;
00122     project_3D_point(pt_cv.x, pt_cv.y, pt_cv.z, uv.y, uv.x);
00123     if (uv.x < 0 || uv.x > image->width)
00124     {
00125       ROS_WARN("[PCDToImageProjector:] width out of bounds");
00126       continue;
00127     }
00128     if (uv.y < 0 || uv.y > image->height)
00129     {
00130       ROS_WARN("[PCDToImageProjector:] height out of bounds");
00131       continue;
00132     }
00133     //static const int RADIUS = 3;
00134     //cvCircle(image, uv, RADIUS, CV_RGB(255,0,0), -1);
00135     cvSeqPush( seq, &uv );
00136   }
00137   
00138   CvRect rect = cvBoundingRect(seq);
00139   if (rect.width == 0 || rect.height == 0)
00140   {
00141     ROS_WARN("[PCDToImageProjector:] Rectangle's width = height = 0. This is impossible. Skipping ROI extraction.");
00142     return std::string("[PCDToImageProjector:] Rectangle's width = height = 0. This is impossible. Skipping ROI extraction.");
00143   }
00144   CvPoint pt1, pt2;
00145   pt1.x = rect.x;
00146   //check boundary, increase ROI for 10%
00147   if ((rect.x + (rect.width + 0.1*rect.width)) > image->width)
00148   {
00149     rect.width =  image->width - rect.x;
00150   }
00151   else 
00152     rect.width = rect.width + 0.1 * rect.width;
00153   
00154   pt2.x = rect.x + rect.width;
00155   pt1.y = rect.y;
00156   
00157   //check boundary, increase ROI for 10%
00158   if ((rect.y + (rect.height + 0.1*rect.height)) > image->height)
00159   {
00160     rect.height =  image->height - rect.y;
00161   }
00162   else
00163     rect.height = rect.height + 0.1 * rect.height;
00164   pt2.y = rect.y+rect.height;
00165   
00166   //draw rectangle around the points
00167   //cvRectangle(image, pt1, pt2, CV_RGB(255,0,0), 3, 8, 0 );
00168   cvClearMemStorage( stor );
00169   
00170   //get subimage_, aka region of interest
00171   
00172   cvSetImageROI(image,rect);
00173   // sub-image
00174   subimage_ = cvCreateImage( cvSize(rect.width, rect.height), image->depth, image->nChannels );
00175   cvCopy(image, subimage_);
00176   cvResetImageROI(image); // release image ROI
00177   //roi_ = bridge_.cvToImgMsg(subimage_)
00178   //ROS_INFO("[PCDToImageProjector:] image published to %s", output_image_topic_.c_str());
00179   return std::string ("ok");
00180 }
00181 
00182 std::string PCDToImageProjector::process (geometry_msgs::Polygon &cloud_in, IplImage *image)
00183 {
00184   //create a sequence storage for projected points 
00185   CvMemStorage* stor = cvCreateMemStorage (0);
00186   CvSeq* seq = cvCreateSeq (CV_SEQ_ELTYPE_POINT, sizeof (CvSeq), sizeof (CvPoint), stor);
00187   if (cloud_in.points.size() == 0)
00188   {
00189     ROS_WARN("[PCDToImageProjector:] Point cloud points with size 0!!! Returning.");
00190     return std::string("[PCDToImageProjector:] Point cloud points with size 0!!! Returning.");
00191   }
00192   for (unsigned long i = 0; i < cloud_in.points.size(); i++)
00193   {
00194     //transform polygon to right camera frame
00195     geometry_msgs::PointStamped Pstamped_local;
00196     Pstamped_local.point.x = cloud_in.points[i].x;
00197     Pstamped_local.point.y = cloud_in.points[i].y;
00198     Pstamped_local.point.z = cloud_in.points[i].z;
00199     Pstamped_local.header.frame_id = child_;
00200     geometry_msgs::PointStamped Pstamped_global;
00201     try
00202     {
00203       tf_listener_.transformPoint(origin_, Pstamped_local, Pstamped_global);
00204       cloud_in.points[i].x = Pstamped_global.point.x;
00205       cloud_in.points[i].y = Pstamped_global.point.y;
00206       cloud_in.points[i].z = Pstamped_global.point.z;
00207     }
00208     catch (tf::TransformException ex)
00209     {
00210       ROS_ERROR("[PCDToImageProjector:] %s",ex.what());
00211       return ("PCDToImageProjector: failed polygon transform");
00212     }
00213 
00214     cv::Point3d pt_cv(cloud_in.points[i].x, cloud_in.points[i].y, cloud_in.points[i].z);
00215     CvPoint uv;
00216     project_3D_point(pt_cv.x, pt_cv.y, pt_cv.z, uv.y, uv.x);
00217     if (uv.x < 0 || uv.x > image->width)
00218     {
00219       ROS_WARN("[PCDToImageProjector:] width out of bounds");
00220       continue;
00221     }
00222     if (uv.y < 0 || uv.y > image->height)
00223     {
00224       ROS_WARN("[PCDToImageProjector:] height out of bounds");
00225       continue;
00226     }
00227     //static const int RADIUS = 3;
00228     //cvCircle(image, uv, RADIUS, CV_RGB(255,0,0), -1);
00229     cvSeqPush( seq, &uv );
00230   }
00231   CvRect rect = cvBoundingRect(seq);
00232   if (rect.width == 0 || rect.height == 0)
00233   {
00234     ROS_WARN("[PCDToImageProjector:] Rectangle's width = height = 0. This is impossible. Skipping ROI extraction.");
00235     return std::string("[PCDToImageProjector:] Rectangle's width = height = 0. This is impossible. Skipping ROI extraction.");
00236   }
00237   CvPoint pt1, pt2;
00238   pt1.x = rect.x;
00239   //check boundary, increase ROI for 10%
00240   if ((rect.x + (rect.width + 0.1*rect.width)) > image->width)
00241   {
00242     rect.width =  image->width - rect.x;
00243   }
00244   else 
00245     rect.width = rect.width + 0.1 * rect.width;
00246     
00247   pt2.x = rect.x + rect.width;
00248   pt1.y = rect.y;
00249     
00250   //check boundary, increase ROI for 10%
00251   if ((rect.y + (rect.height + 0.1*rect.height)) > image->height)
00252   {
00253     rect.height =  image->height - rect.y;
00254   }
00255   else
00256     rect.height = rect.height + 0.1 * rect.height;
00257   pt2.y = rect.y+rect.height;
00258     
00259   //draw rectangle around the points
00260   //cvRectangle(image, pt1, pt2, CV_RGB(255,0,0), 3, 8, 0 );
00261   cvClearMemStorage( stor );
00262     
00263   //get subimage_, aka region of interest
00264     
00265   cvSetImageROI(image,rect);
00266   // sub-image
00267   subimage_ = cvCreateImage( cvSize(rect.width, rect.height), image->depth, image->nChannels );
00268   cvCopy(image, subimage_);
00269   cvResetImageROI(image); // release image ROI
00270   //roi_ = bridge_.cvToImgMsg(subimage_)
00271   //ROS_INFO("[PCDToImageProjector:] image published to %s", output_image_topic_.c_str());
00272   return std::string ("ok");
00273 }
00274 
00275 
00276 PCDToImageProjector::OutputType PCDToImageProjector::output () 
00277 {
00278   return subimage_;
00279 };
00280 
00281 
00282 #ifdef CREATE_NODE
00283 int main (int argc, char* argv[])
00284 {
00285   return standalone_node < PCDToImageProjector> (argc, argv);
00286 }
00287 #endif
00288
pcd_to_image_projector_algo.cpp
