kin2pcl_node.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id: kin2pcl.cpp 619 2012-04-16 13:47:28Z ihulik $
00005  *
00006  * Copyright (C) Brno University of Technology
00007  *
00008  * This file is part of software developed by dcgm-robotics@FIT group.
00009  *
00010  * Author: Rostislav Hulik (ihulik@fit.vutbr.cz)
00011  * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00012  * Date: 11.01.2012 (version 0.8)
00013  * 
00014  * This file is free software: you can redistribute it and/or modify
00015  * it under the terms of the GNU Lesser General Public License as published by
00016  * the Free Software Foundation, either version 3 of the License, or
00017  * (at your option) any later version.
00018  * 
00019  * This file is distributed in the hope that it will be useful,
00020  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022  * GNU Lesser General Public License for more details.
00023  * 
00024  * You should have received a copy of the GNU Lesser General Public License
00025  * along with this file.  If not, see <http://www.gnu.org/licenses/>.
00026  */
00027 
00034 #include <srs_env_model_percp/but_seg_utils/kin2pcl_node.h>
00035 #include <srs_env_model_percp/but_segmentation/normals.h>
00036 
00037 // CV <-> ROS bridge
00038 #include <cv_bridge/cv_bridge.h>
00039 
00040 //PCL
00041 #include <pcl/point_types.h>
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/filters/voxel_grid.h>
00044 
00045 // Message filters
00046 #include <message_filters/subscriber.h>
00047 #include <message_filters/synchronizer.h>
00048 #include <message_filters/sync_policies/approximate_time.h>
00049 
00050 using namespace std;
00051 using namespace cv;
00052 using namespace pcl;
00053 using namespace sensor_msgs;
00054 using namespace message_filters;
00055 using namespace but_plane_detector;
00056 
00057 namespace srs_env_model_percp
00058 {
00059 
00060         void callback( const sensor_msgs::ImageConstPtr& dep, const CameraInfoConstPtr& cam_info)
00061         {
00062                 ros::Time begin = ros::Time::now();
00063                 //  Debug info
00064                 std::cerr << "Recieved frame..." << std::endl;
00065                 std::cerr << "Cam info: fx:" << cam_info->K[0] << " fy:" << cam_info->K[4] << " cx:" << cam_info->K[2] <<" cy:" << cam_info->K[5] << std::endl;
00066                 std::cerr << "Depth image h:" << dep->height << " w:" << dep->width << " e:" << dep->encoding << " " << dep->step << endl;
00067 
00068                 //get image from message
00069                 cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(dep);
00070                 cv::Mat depth = cv_image->image;
00071 
00072                 Normals normal(depth, cam_info);
00073 
00074                 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00075 
00076                 for (int i = 0; i < normal.m_points.rows; ++i)
00077                 for (int j = 0; j < normal.m_points.cols; ++j)
00078                 {
00079                         Vec3f vector = normal.m_points.at<Vec3f>(i, j);
00080 
00081                         //pcl::Vec
00082                         cloud->push_back(pcl::PointXYZ(vector[0], vector[1], vector[2]));
00083                 }
00084 
00085                 pcl::VoxelGrid<pcl::PointXYZ> voxelgrid;
00086                 voxelgrid.setInputCloud(cloud);
00087                 voxelgrid.setLeafSize(0.05, 0.05, 0.05);
00088                 voxelgrid.filter(*cloud);
00089 
00090                 cloud->header.frame_id = OUTPUT_POINT_CLOUD_FRAMEID;
00091 
00092                 pub.publish(cloud);
00093 
00094 
00095                 ros::Time end = ros::Time::now();
00096                 std::cerr << "Computation time: " << (end-begin).nsec/1000000.0 << " ms." << std::endl;
00097                 std::cerr << "=========================================================" << endl;
00098         }
00099 }
00100 
00101 int main( int argc, char** argv )
00102 {
00103         using namespace srs_env_model_percp;
00104 
00105         ros::init(argc, argv, NODE_NAME);
00106         ros::NodeHandle n;
00107 
00108         // subscribe depth info
00109         message_filters::Subscriber<Image> depth_sub(n, INPUT_IMAGE_TOPIC, 1);
00110         message_filters::Subscriber<CameraInfo> info_sub_depth(n, INPUT_CAM_INFO_TOPIC, 1);
00111 
00112         pub = n.advertise<pcl::PointCloud<pcl::PointXYZ> > (OUTPUT_POINT_CLOUD_TOPIC, 1);
00113         // sync images
00114         typedef sync_policies::ApproximateTime<Image, CameraInfo> MySyncPolicy;
00115         Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), depth_sub, info_sub_depth);
00116         sync.registerCallback(boost::bind(&callback, _1, _2));
00117 
00118         std::cerr << "Kinect depth map to point cloud converter initialized and listening..." << std::endl;
00119         ros::spin();
00120 
00121         return 1;
00122 }


srs_env_model_percp
Author(s): Rostislav Hulik (ihulik@fit.vutbr.cz), Tomas Hodan, Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Sun Jan 5 2014 11:51:56