plane_detector_node.cpp
Go to the documentation of this file.
00001 /***********************************B*******************************************
00002  * \file
00003  *
00004  * $Id: detector.cpp 777 2012-05-11 11:23:17Z 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 
00037 #include <ros/ros.h>
00038 #include <image_transport/image_transport.h>
00039 #include <cv_bridge/cv_bridge.h>
00040 
00041 // OpenCV 2
00042 #include <opencv2/highgui/highgui.hpp>
00043 #include <opencv2/imgproc/imgproc_c.h>
00044 
00045 #include <sensor_msgs/Image.h>
00046 #include <sensor_msgs/CameraInfo.h>
00047 #include <visualization_msgs/Marker.h>
00048 #include <visualization_msgs/MarkerArray.h>
00049 #include <boost/math/quaternion.hpp>
00050 
00051 #include <tf/transform_listener.h>
00052 #include <tf/message_filter.h>
00053 #include <tf/tfMessage.h>
00054 
00055 #include <message_filters/subscriber.h>
00056 #include <message_filters/time_synchronizer.h>
00057 #include <message_filters/synchronizer.h>
00058 #include <message_filters/sync_policies/approximate_time.h>
00059 
00060 #include <srs_env_model_percp/ClearPlanes.h>
00061 
00062 #include <srs_env_model_percp/but_segmentation/filtering.h>
00063 #include <srs_env_model_percp/but_segmentation/normals.h>
00064 
00065 #include <srs_env_model_percp/but_plane_detector/scene_model.h>
00066 #include <srs_env_model_percp/but_plane_detector/dyn_model_exporter.h>
00067 #include <srs_env_model_percp/but_plane_detector/plane_detector_params.h>
00068 
00069 #include <srs_env_model_percp/topics_list.h>
00070 #include <srs_env_model_percp/services_list.h>
00071 
00072 // PCL
00073 #include <pcl/point_types.h>
00074 #include <pcl/point_cloud.h>
00075 #include <pcl/io/pcd_io.h>
00076 
00077 #include <pcl/kdtree/kdtree_flann.h>
00078 #include <pcl/features/normal_3d.h>
00079 #include <pcl/surface/gp3.h>
00080 #include <pcl/io/vtk_io.h>
00081 #include <pcl/filters/voxel_grid.h>
00082 #include <pcl_ros/point_cloud.h>
00083 #include <pcl_ros/transforms.h>
00084 #include <pcl/ModelCoefficients.h>
00085 
00086 #include <cstdlib>
00087 #include <cstdio>
00088 #include <cfloat>
00089 
00090 #include <float.h>
00091 #include <sensor_msgs/Image.h>
00092 #include <sensor_msgs/CameraInfo.h>
00093 
00094 #include <boost/math/quaternion.hpp>
00095 #include "srs_env_model_percp/but_plane_detector/plane_detector_params.h"
00096 #include <stdlib.h>
00097 #include <stdio.h>
00098 #include <float.h>
00099 
00100 #include <srs_env_model_percp/LoadSave.h>
00101 #include <srs_env_model_percp/ResetPlanes.h>
00102 using namespace std;
00103 using namespace cv;
00104 using namespace pcl;
00105 using namespace sensor_msgs;
00106 using namespace message_filters;
00107 using namespace but_plane_detector;
00108 
00109 namespace srs_env_model_percp
00110 {
00111     int counter = 0;
00112     sensor_msgs::PointCloud2 cloud_msg;
00113     tf::MessageFilter<sensor_msgs::PointCloud2> *transform_filter;
00114 
00115     tf::MessageFilter<Image> *transform_filterDepth;
00116     tf::MessageFilter<CameraInfo> *transform_filterCam;
00117     tf::MessageFilter<Image> *transform_filterRGB;
00118     tf::TransformListener *tfListener;
00119     tf::TransformListener *tfListenerCam;
00120     tf::TransformListener *tfListenerRGB;
00121     ros::Publisher pub1;
00122     ros::Publisher pub2;
00123     ros::Publisher pub3;
00124     DynModelExporter *exporter = NULL;
00125 
00126     sensor_msgs::CameraInfo cam_info_legacy;
00127     sensor_msgs::CameraInfoConstPtr cam_info_aux (&cam_info_legacy);
00128     SceneModel *model;
00129 
00130     PlaneDetectorSettings settings;
00131 
00132     ros::NodeHandle *n = NULL;
00133 
00137     void callbackpcl(const sensor_msgs::PointCloud2ConstPtr& cloud);
00138     void callbackkinect( const sensor_msgs::ImageConstPtr& dep, const sensor_msgs::CameraInfoConstPtr& cam_info);
00139     bool clear(srs_env_model_percp::ClearPlanes::Request &req,srs_env_model_percp::ClearPlanes::Response &res);
00140     bool getParams(ros::NodeHandle nh);
00141 
00142     void getPlanes(Normals &normal, pcl::PointCloud<pcl::PointXYZ> &pointcloud, const sensor_msgs::ImageConstPtr* rgb = NULL)
00143     {
00144                 // Add point cloud to current Hough space and recompute planes
00145                 model->AddNext(normal, settings.param_ht_min_smooth);
00146                 model->recomputePlanes( settings.param_search_minimum_current_space,
00147                                                                 settings.param_search_minimum_global_space,
00148                                                                 settings.param_search_maxima_search_blur,
00149                                                                 settings.param_search_maxima_search_neighborhood,
00150                                                                 settings.param_ht_step_substraction);
00151 
00152                 // update current sent planes
00153                 if (rgb && settings.param_visualisation_color == "mean_color")
00154                 {
00155                         cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(*rgb);
00156                         cv::Mat rgb_mat = cv_image->image;
00157                         exporter->update(model->planes, normal, settings.param_visualisation_color, rgb_mat);
00158                 }
00159                 else
00160                 {
00161                         exporter->update(model->planes, normal, settings.param_visualisation_color);
00162                 }
00163 
00164                 std::cerr << "Going to publish markers..." << std::endl;
00165 
00166                 visualization_msgs::MarkerArray marker_array;
00167         cob_3d_mapping_msgs::ShapeArray shape_array;
00168         exporter->getMarkerArray(marker_array, settings.param_output_frame);
00169         exporter->getShapeArray(shape_array, settings.param_output_frame);
00170         std::cerr << "Total no of sent planes:  " << marker_array.markers.size() << std::endl;
00171         pub2.publish(marker_array);
00172         pub3.publish(shape_array);
00173     }
00174 
00175     void pcl_proc(const PointCloud2ConstPtr& cloud, const sensor_msgs::ImageConstPtr* rgb = NULL)
00176     {
00177         ++counter;
00178                 ros::Time begin = ros::Time::now();
00179 
00180                 // make the pointcloud
00181                 pcl::PointCloud<pcl::PointXYZ> pointcloud;
00182                 pcl::fromROSMsg (*cloud, pointcloud);
00183 
00184                 // Control out
00185                 std::cerr << "Recieved frame..." << std::endl;
00186                 std::cerr << "Topic: " << pointcloud.header.frame_id << std::endl;
00187                 std::cerr << "Width: " << pointcloud.width << " height: " << pointcloud.height << std::endl;
00188                 std::cerr << "=========================================================" << endl;
00189 
00190                 // get indices of points to be deleted (max depth)
00191                 std::vector<unsigned int> zero_indices;
00192                 for (unsigned int i = 0; i < pointcloud.points.size(); ++i)
00193                 {
00194                         if (pointcloud.points[i].z > settings.param_ht_maxdepth)
00195                                 zero_indices.push_back(i);
00196                 }
00197 
00198                 // transform to world
00199                 tf::StampedTransform sensorToWorldTf;
00200                 try {
00201                         if( tfListener->waitForTransform(settings.param_output_frame, pointcloud.header.frame_id, pointcloud.header.stamp, ros::Duration(5.0)) )
00202                         {
00203                                 tfListener->lookupTransform(settings.param_output_frame, pointcloud.header.frame_id, pointcloud.header.stamp, sensorToWorldTf);
00204                         }
00205                         else
00206                         {
00207                                 std::cerr << "Cannot lookup transform: quitting callback" << std::endl;
00208                                 return;
00209                         }
00210                 }
00211                 catch(tf::TransformException& ex){
00212                         std::cerr << "Transform error: " << ex.what() << ", quitting callback" << std::endl;
00213                         return;
00214                 }
00215 
00216                 // transform pointcloud from sensor frame to fixed robot frame
00217                 Eigen::Matrix4f sensorToWorld;
00218                 pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld);
00219                 pcl::transformPointCloud(pointcloud, pointcloud, sensorToWorld);
00220 
00221         for (unsigned int i = 0; i < pointcloud.points.size(); ++i)
00222         {
00223                 if (pointcloud.points[i].z > settings.param_ht_maxheight || pointcloud.points[i].z < settings.param_ht_minheight)
00224                         zero_indices.push_back(i);
00225         }
00226 
00227         // set all zero indices to point(0,0,0)
00228         for (unsigned int i = 0; i < zero_indices.size(); ++i)
00229         {
00230                 pointcloud.points[zero_indices[i]].x = 0.0;
00231                 pointcloud.points[zero_indices[i]].y = 0.0;
00232                 pointcloud.points[zero_indices[i]].z = 0.0;
00233         }
00234 
00235         // Compute normals on point cloud
00236         Normals normal(pointcloud,
00237                                            settings.param_normal_type,
00238                                            settings.param_normal_neighborhood,
00239                                            settings.param_normal_threshold,
00240                                            settings.param_normal_outlierThreshold,
00241                                            settings.param_normal_iter);
00242 
00243         // get planes from point cloud
00244         getPlanes(normal, pointcloud, rgb);
00245 
00246         // Control out
00247         ros::Time end = ros::Time::now();
00248         std::cerr << "DONE.... Computation time: " << (end-begin).nsec/1000000.0 << " ms." << std::endl;
00249         std::cerr << "=========================================================" << endl<< endl;
00250     }
00254         void callbackpcl(const PointCloud2ConstPtr& cloud)
00255         {
00256                 pcl_proc(cloud);
00257         }
00258 
00259         void callbackpcl_rgb(const PointCloud2ConstPtr& cloud, const sensor_msgs::ImageConstPtr& rgb)
00260         {
00261                 ROS_INFO("Data received");
00262                 pcl_proc(cloud, &rgb);
00263         }
00264 
00265         void kinect_proc( const sensor_msgs::ImageConstPtr& dep, const CameraInfoConstPtr& cam_info, const sensor_msgs::ImageConstPtr* rgb = NULL)
00266         {
00267                 ++counter;
00268                 ros::Time begin = ros::Time::now();
00269 
00270                 //  Debug info
00271                 std::cerr << "Recieved frame..." << std::endl;
00272                 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;
00273                 std::cerr << "Depth image h:" << dep->height << " w:" << dep->width << " e:" << dep->encoding << " " << dep->step << endl;
00274                 std::cerr << "=========================================================" << endl;
00275 
00276                 //get image from message
00277                 cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(dep);
00278                 cv::Mat depth = cv_image->image;
00279 
00280                 pcl::PointCloud<PointXYZ> pointcloud;
00281                 pointcloud.resize(depth.rows * depth.cols);
00282                 pointcloud.width = depth.cols;
00283                 pointcloud.height = depth.rows;
00284                 pointcloud.header = dep->header;
00285                 // get indices of points to be deleted (max depth)
00286                 std::vector<unsigned int> zero_indices;
00287 
00288                 unsigned short aux;
00289                 for (int i = 0; i < depth.rows; ++i)
00290                         for (int j = 0; j < depth.cols; ++j)
00291                         {
00292                                 pcl::PointXYZ point(0.0, 0.0, 0.0);
00293                                 if ((aux = depth.at<unsigned short>(i, j)) != 0)
00294                                 {
00295                                         point.z = aux/1000.0;
00296                                         point.x = ( (j - cam_info->K[2]) * point.z / cam_info->K[0] );
00297                                         point.y = ( (i - cam_info->K[5]) * point.z / cam_info->K[4] );
00298 
00299                                         if (point.z > settings.param_ht_maxdepth)
00300                                                 zero_indices.push_back(i);
00301                                 }
00302                                 else
00303                                         zero_indices.push_back(i);
00304 
00305                                 pointcloud[i * depth.rows + j] = point;
00306                         }
00307 
00308                 // transform to world
00309                 tf::StampedTransform sensorToWorldTf;
00310                 try {
00311                         tfListener->waitForTransform(settings.param_output_frame, pointcloud.header.frame_id, pointcloud.header.stamp, ros::Duration(5.0));
00312                         tfListener->lookupTransform(settings.param_output_frame, pointcloud.header.frame_id, pointcloud.header.stamp, sensorToWorldTf);
00313                 }
00314                 catch(tf::TransformException& ex){
00315                         std::cerr << "Transform error: " << ex.what() << ", quitting callback" << std::endl;
00316                         return;
00317                 }
00318 
00319                 // transform pointcloud from sensor frame to fixed robot frame
00320                 Eigen::Matrix4f sensorToWorld;
00321                 pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld);
00322                 pcl::transformPointCloud(pointcloud, pointcloud, sensorToWorld);
00323 
00324 //              for (unsigned int i = 0; i < pointcloud.points.size(); ++i)
00325 //              {
00326 //                      if (pointcloud.points[i].z > settings.param_ht_maxheight || pointcloud.points[i].z < settings.param_ht_minheight)
00327 //                              zero_indices.push_back(i);
00328 //              }
00329 
00330                 // set all zero indices to point(0,0,0)
00331                 for (unsigned int i = 0; i < zero_indices.size(); ++i)
00332                 {
00333                         pointcloud.points[zero_indices[i]].x = 0.0;
00334                         pointcloud.points[zero_indices[i]].y = 0.0;
00335                         pointcloud.points[zero_indices[i]].z = 0.0;
00336                 }
00337 
00338                 Normals normal(pointcloud,
00339                                            settings.param_normal_type,
00340                                            settings.param_normal_neighborhood,
00341                                            settings.param_normal_threshold,
00342                                            settings.param_normal_outlierThreshold,
00343                                            settings.param_normal_iter);
00344                 getPlanes(normal, pointcloud, rgb);
00345 
00346                 // Control out
00347                 ros::Time end = ros::Time::now();
00348                 std::cerr << "DONE.... Computation time: " << (end-begin).nsec/1000000.0 << " ms." << std::endl;
00349                 std::cerr << "=========================================================" << endl<< endl;
00350         }
00351         void callbackkinect( const sensor_msgs::ImageConstPtr& dep, const CameraInfoConstPtr& cam_info)
00352         {
00353                 kinect_proc(dep, cam_info);
00354         }
00355 
00356         void callbackkinect_rgb( const sensor_msgs::ImageConstPtr& dep, const CameraInfoConstPtr& cam_info, const sensor_msgs::ImageConstPtr&rgb)
00357         {
00358                 kinect_proc(dep, cam_info, &rgb);
00359         }
00360 
00361 
00362         bool getParams(ros::NodeHandle nh)
00363         {
00364                 nh.param(PARAM_NODE_INPUT, settings.param_node_input, PARAM_NODE_INPUT_DEFAULT);
00365 
00366         nh.param(PARAM_NODE_OUTPUT_FRAME, settings.param_output_frame, PARAM_NODE_OUTPUT_FRAME_DEFAULT);
00367         nh.param(PARAM_NODE_ORIGINAL_FRAME, settings.param_original_frame, PARAM_NODE_ORIGINAL_FRAME_DEFAULT);
00368 
00369         nh.param(PARAM_HT_KEEPTRACK, settings.param_ht_keeptrack, PARAM_HT_KEEPTRACK_DEFAULT);
00370                 nh.param(PARAM_HT_MAXDEPTH, settings.param_ht_maxdepth, PARAM_HT_MAXDEPTH_DEFAULT);
00371                 nh.param(PARAM_HT_MAXHEIGHT, settings.param_ht_maxheight, PARAM_HT_MAXHEIGHT_DEFAULT);
00372                 nh.param(PARAM_HT_MINHEIGHT, settings.param_ht_minheight, PARAM_HT_MINHEIGHT_DEFAULT);
00373 
00374                 nh.param(PARAM_HT_MINSHIFT, settings.param_ht_minshift, PARAM_HT_MINSHIFT_DEFAULT);
00375                 nh.param(PARAM_HT_MAXSHIFT, settings.param_ht_maxshift, PARAM_HT_MAXSHIFT_DEFAULT);
00376                 nh.param(PARAM_HT_ANGLE_RES, settings.param_ht_angle_res, PARAM_HT_ANGLE_RES_DEFAULT);
00377                 nh.param(PARAM_HT_SHIFT_RES, settings.param_ht_shift_res, PARAM_HT_SHIFT_RES_DEFAULT);
00378                 nh.param(PARAM_HT_GAUSS_ANGLE_RES, settings.param_ht_gauss_angle_res, PARAM_HT_GAUSS_ANGLE_RES_DEFAULT);
00379                 nh.param(PARAM_HT_GAUSS_SHIFT_RES, settings.param_ht_gauss_shift_res, PARAM_HT_GAUSS_SHIFT_RES_DEFAULT);
00380                 nh.param(PARAM_HT_GAUSS_ANGLE_SIGMA, settings.param_ht_gauss_angle_sigma, PARAM_HT_GAUSS_ANGLE_SIGMA_DEFAULT);
00381                 nh.param(PARAM_HT_GAUSS_SHIFT_SIGMA, settings.param_ht_gauss_shift_sigma, PARAM_HT_GAUSS_SHIFT_SIGMA_DEFAULT);
00382                 nh.param(PARAM_HT_LVL1_GAUSS_ANGLE_RES, settings.param_ht_lvl1_gauss_angle_res, PARAM_HT_LVL1_GAUSS_ANGLE_RES_DEFAULT);
00383                 nh.param(PARAM_HT_LVL1_GAUSS_SHIFT_RES, settings.param_ht_lvl1_gauss_shift_res, PARAM_HT_LVL1_GAUSS_SHIFT_RES_DEFAULT);
00384                 nh.param(PARAM_HT_LVL1_GAUSS_ANGLE_SIGMA, settings.param_ht_lvl1_gauss_angle_sigma, PARAM_HT_LVL1_GAUSS_ANGLE_SIGMA_DEFAULT);
00385                 nh.param(PARAM_HT_LVL1_GAUSS_SHIFT_SIGMA, settings.param_ht_lvl1_gauss_shift_sigma, PARAM_HT_LVL1_GAUSS_SHIFT_SIGMA_DEFAULT);
00386                 nh.param(PARAM_HT_MIN_SMOOTH, settings.param_ht_min_smooth, PARAM_HT_MIN_SMOOTH_DEFAULT);
00387 
00388                 nh.param(PARAM_HT_PLANE_MERGE_ANGLE, settings.param_ht_plane_merge_angle, PARAM_HT_PLANE_MERGE_ANGLE_DEFAULT);
00389                 nh.param(PARAM_HT_PLANE_MERGE_SHIFT, settings.param_ht_plane_merge_shift, PARAM_HT_PLANE_MERGE_SHIFT_DEFAULT);
00390                 nh.param(PARAM_HT_STEP_SUBSTRACTION, settings.param_ht_step_substraction, PARAM_HT_STEP_SUBSTRACTION_DEFAULT);
00391 
00392                 nh.param(PARAM_VISUALISATION_DISTANCE, settings.param_visualisation_distance, PARAM_VISUALISATION_DISTANCE_DEFAULT);
00393                 nh.param(PARAM_VISUALISATION_PLANE_NORMAL_DEV, settings.param_visualisation_plane_normal_dev, PARAM_VISUALISATION_PLANE_NORMAL_DEV_DEFAULT);
00394                 nh.param(PARAM_VISUALISATION_PLANE_SHIFT_DEV, settings.param_visualisation_plane_shift_dev, PARAM_VISUALISATION_PLANE_SHIFT_DEV_DEFAULT);
00395         nh.param(PARAM_VISUALISATION_MIN_COUNT, settings.param_visualisation_min_count, PARAM_VISUALISATION_MIN_COUNT_DEFAULT);
00396         nh.param(PARAM_VISUALISATION_COLOR, settings.param_visualisation_color, PARAM_VISUALISATION_COLOR_DEFAULT);
00397         nh.param(PARAM_VISUALISATION_TTL, settings.param_visualisation_ttl, PARAM_VISUALISATION_TTL_DEFAULT);
00398         nh.param(PARAM_VISUALISATION_MAX_POLY_SIZE, settings.param_visualisation_max_poly_size, PARAM_VISUALISATION_MAX_POLY_SIZE_DEFAULT);
00399 
00400                 nh.param(PARAM_SEARCH_MINIMUM_CURRENT_SPACE, settings.param_search_minimum_current_space, PARAM_SEARCH_MINIMUM_CURRENT_SPACE_DEFAULT);
00401                 nh.param(PARAM_SEARCH_MINIMUM_GLOBAL_SPACE, settings.param_search_minimum_global_space, PARAM_SEARCH_MINIMUM_GLOBAL_SPACE_DEFAULT);
00402                 nh.param(PARAM_SEARCH_MAXIMA_SEARCH_NEIGHBORHOOD, settings.param_search_maxima_search_neighborhood, PARAM_SEARCH_MAXIMA_SEARCH_NEIGHBORHOOD_DEFAULT);
00403                 nh.param(PARAM_SEARCH_MAXIMA_SEARCH_BLUR, settings.param_search_maxima_search_blur, PARAM_SEARCH_MAXIMA_SEARCH_BLUR_DEFAULT);
00404 
00405                 nh.param(PARAM_NORMAL_ITER, settings.param_normal_iter, PARAM_NORMAL_ITER_DEFAULT);
00406                 nh.param(PARAM_NORMAL_NEIGHBORHOOD, settings.param_normal_neighborhood, PARAM_NORMAL_NEIGHBORHOOD_DEFAULT);
00407                 nh.param(PARAM_NORMAL_OUTLIERTHRESH, settings.param_normal_outlierThreshold, PARAM_NORMAL_OUTLIERTHRESH_DEFAULT);
00408                 nh.param(PARAM_NORMAL_THRESHOLD, settings.param_normal_threshold, PARAM_NORMAL_THRESHOLD_DEFAULT);
00409                 nh.param(PARAM_NORMAL_TYPE, settings.param_normal_type, PARAM_NORMAL_TYPE_DEFAULT);
00410 
00411 
00412                 return true;
00413         }
00414 
00415         bool onReset(srs_env_model_percp::ResetPlanes::Request &req, srs_env_model_percp::ResetPlanes::Response &res)
00416         {
00417                 std::cout << "Resetting plane settings..." << std::endl;
00418 
00419                 visualization_msgs::MarkerArray marker_array;
00420                 exporter->getMarkerArray(marker_array, settings.param_output_frame);
00421 
00422                 for (unsigned int i = 0; i < marker_array.markers.size(); ++i)
00423                         marker_array.markers[i].action = visualization_msgs::Marker::DELETE;
00424 
00425                 std::cerr << "Total no of deleted planes:  " << marker_array.markers.size() << std::endl;
00426                 pub2.publish(marker_array);
00427 
00428 
00429 
00430                 delete model;
00431                 model = new SceneModel( settings.param_ht_maxdepth,
00432                                 settings.param_ht_minshift,
00433                                 settings.param_ht_maxshift,
00434                                 settings.param_ht_angle_res,
00435                                 settings.param_ht_shift_res,
00436                                 settings.param_ht_gauss_angle_res,
00437                                 settings.param_ht_gauss_shift_res,
00438                                 settings.param_ht_gauss_angle_sigma,
00439                                 settings.param_ht_gauss_shift_sigma,
00440                                 settings.param_ht_lvl1_gauss_angle_res,
00441                                 settings.param_ht_lvl1_gauss_shift_res,
00442                                 settings.param_ht_lvl1_gauss_angle_sigma,
00443                                 settings.param_ht_lvl1_gauss_shift_sigma,
00444                                 settings.param_ht_plane_merge_angle,
00445                                 settings.param_ht_plane_merge_shift);
00446 
00447                 delete exporter;
00448                 exporter = new DynModelExporter(n,
00449                                                                            settings.param_original_frame,
00450                                                    settings.param_output_frame,
00451                                                    settings.param_visualisation_min_count,
00452                                                                            settings.param_visualisation_distance,
00453                                                                            settings.param_visualisation_plane_normal_dev,
00454                                                                            settings.param_visualisation_plane_shift_dev,
00455                                                                            settings.param_ht_keeptrack,
00456                                                                            settings.param_visualisation_ttl,
00457                                                                            settings.param_visualisation_max_poly_size);
00458 
00459                 res.message = "Hough space successfully reset.\n";
00460                 std::cout << "Hough space successfully reset." << std::endl;
00461                 return true;
00462         }
00463 
00464         bool onSave(srs_env_model_percp::LoadSave::Request &req, srs_env_model_percp::LoadSave::Response &res)
00465         {
00466                 std::cerr << "Trying to save planes..." << std::endl;
00467 
00468                 if( exporter )
00469                 {
00470                         exporter->xmlFileExport(req.filename);
00471                         res.all_ok = 1;
00472                         std::cerr << "Environment model successfuly saved into " << req.filename << "." << std::endl;
00473                         return true;
00474                 }
00475 
00476                 return false;
00477         }
00478 
00479         bool onLoad(srs_env_model_percp::LoadSave::Request &req, srs_env_model_percp::LoadSave::Response &res)
00480         {
00481                 std::cerr << "Trying to load planes..." << std::endl;
00482 
00483                 if( exporter )
00484                 {
00485                         visualization_msgs::MarkerArray marker_array;
00486                         exporter->getMarkerArray(marker_array, settings.param_output_frame);
00487 
00488                         for (unsigned int i = 0; i < marker_array.markers.size(); ++i)
00489                                 marker_array.markers[i].action = visualization_msgs::Marker::DELETE;
00490 
00491                         std::cerr << "Total no of deleted planes:  " << marker_array.markers.size() << std::endl;
00492                         pub2.publish(marker_array);
00493 
00494                         delete model;
00495                         model = new SceneModel( settings.param_ht_maxdepth,
00496                                                         settings.param_ht_minshift,
00497                                                         settings.param_ht_maxshift,
00498                                                         settings.param_ht_angle_res,
00499                                                         settings.param_ht_shift_res,
00500                                                         settings.param_ht_gauss_angle_res,
00501                                                         settings.param_ht_gauss_shift_res,
00502                                                         settings.param_ht_gauss_angle_sigma,
00503                                                         settings.param_ht_gauss_shift_sigma,
00504                                                         settings.param_ht_lvl1_gauss_angle_res,
00505                                                         settings.param_ht_lvl1_gauss_shift_res,
00506                                                         settings.param_ht_lvl1_gauss_angle_sigma,
00507                                                         settings.param_ht_lvl1_gauss_shift_sigma,
00508                                                         settings.param_ht_plane_merge_angle,
00509                                                         settings.param_ht_plane_merge_shift);
00510 
00511                         delete exporter;
00512                         exporter = new DynModelExporter(n,
00513                                                                                    settings.param_original_frame,
00514                                                                                    settings.param_output_frame,
00515                                                                                    settings.param_visualisation_min_count,
00516                                                                                    settings.param_visualisation_distance,
00517                                                                                    settings.param_visualisation_plane_normal_dev,
00518                                                                                    settings.param_visualisation_plane_shift_dev,
00519                                                                                    settings.param_ht_keeptrack,
00520                                                                                    settings.param_visualisation_ttl,
00521                                                                                    settings.param_visualisation_max_poly_size);
00522 
00523                         exporter->xmlFileImport(req.filename);
00524 
00525                         marker_array.markers.clear();
00526                         cob_3d_mapping_msgs::ShapeArray shape_array;
00527                         exporter->getMarkerArray(marker_array, settings.param_output_frame);
00528                         exporter->getShapeArray(shape_array, settings.param_output_frame);
00529                         std::cerr << "Total no of sent planes:  " << marker_array.markers.size() << std::endl;
00530                         pub2.publish(marker_array);
00531                         pub3.publish(shape_array);
00532 
00533                         res.all_ok = 1;
00534                         std::cerr << "Environment model successfuly loaded from " << req.filename << "." << std::endl;
00535                         return true;
00536                 }
00537 
00538                 return false;
00539         }
00540 
00541 }
00542 
00543 void spin() {
00544 
00545 //      ros::AsyncSpinner spinner(4);
00546 //      spinner.start();
00547 //      ros::waitForShutdown();
00548     ros::spin();
00549 
00550 }
00551 
00555 int main( int argc, char** argv )
00556 {
00557         using namespace srs_env_model_percp;
00558 
00559         ros::init(argc, argv, "but_plane_detector");
00560 
00561         n = new ros::NodeHandle();
00562         // Private node handle to read private node parameters
00563         ros::NodeHandle private_nh("~");
00564         getParams(private_nh);
00565 
00566         // Init scene model
00567         model = new SceneModel( settings.param_ht_maxdepth,
00568                                                         settings.param_ht_minshift,
00569                                                         settings.param_ht_maxshift,
00570                                                         settings.param_ht_angle_res,
00571                                                         settings.param_ht_shift_res,
00572                                                         settings.param_ht_gauss_angle_res,
00573                                                         settings.param_ht_gauss_shift_res,
00574                                                         settings.param_ht_gauss_angle_sigma,
00575                                                         settings.param_ht_gauss_shift_sigma,
00576                                                         settings.param_ht_lvl1_gauss_angle_res,
00577                                                         settings.param_ht_lvl1_gauss_shift_res,
00578                                                         settings.param_ht_lvl1_gauss_angle_sigma,
00579                                                         settings.param_ht_lvl1_gauss_shift_sigma,
00580                                                         settings.param_ht_plane_merge_angle,
00581                                                         settings.param_ht_plane_merge_shift);
00582 
00583         // Print out parameters
00584         std::cerr << std::endl;
00585         std::cerr << "Given HS parameters:" << std::endl;
00586         std::cerr << "====================" << std::endl;
00587         std::cerr << "Max point cloud z value:               " << settings.param_ht_maxdepth << std::endl;
00588         std::cerr << "Minimal plane shift (d param) value:   " << settings.param_ht_minshift << std::endl;
00589         std::cerr << "Maximal plane shift (d param) value:   " << settings.param_ht_maxshift << std::endl;
00590         std::cerr << "Hough space angle resolution:          " << settings.param_ht_angle_res << std::endl;
00591         std::cerr << "Hough space shift resolution:          " << settings.param_ht_shift_res << std::endl;
00592         std::cerr << "Plane Gauss function angle resolution: " << settings.param_ht_gauss_angle_res << std::endl;
00593         std::cerr << "Plane Gauss function shift resolution: " << settings.param_ht_gauss_shift_res << std::endl;
00594         std::cerr << "Plane Gauss function angle sigma:      " << settings.param_ht_gauss_angle_sigma << std::endl;
00595         std::cerr << "Plane Gauss function shift sigma:      " << settings.param_ht_gauss_shift_sigma << std::endl;
00596         std::cerr << std::endl;
00597         std::cerr << "Given plane search parameters:" << std::endl;
00598         std::cerr << "==============================" << std::endl;
00599         std::cerr << "Minimum plane value in HS for current frame:   " << settings.param_search_minimum_current_space << std::endl;
00600         std::cerr << "Minimum plane value in HS for global space:    " << settings.param_search_minimum_global_space << std::endl;
00601         std::cerr << "Hough space local maximum neighborhood search: " << settings.param_search_maxima_search_neighborhood << std::endl;
00602         std::cerr << "Hough space local maximum search blur:         " << settings.param_search_maxima_search_blur << std::endl;
00603         std::cerr << std::endl;
00604         std::cerr << "Given visualisation parameters:" << std::endl;
00605         std::cerr << "==============================" << std::endl;
00606         std::cerr << "Maximum point distance from plane:                               " << settings.param_visualisation_distance << std::endl;
00607         std::cerr << "Maximum point normal deviation from plane:                       " << settings.param_visualisation_plane_normal_dev << std::endl;
00608         std::cerr << "Maximum point plane shift (d param) from plane:                  " << settings.param_visualisation_plane_shift_dev << std::endl;
00609         std::cerr << "Minimum point count for visualised plane in current point cloud: " << settings.param_visualisation_min_count << std::endl;
00610         std::cerr << std::endl;
00611 
00612     ROS_INFO("Plane det. input: %s", settings.param_node_input.c_str());   
00613     ROS_INFO("Plane coloring: %s", settings.param_visualisation_color.c_str());   
00614 
00615         ros::ServiceServer service1 = n->advertiseService(DET_SERVICE_RESET_PLANES, onReset);
00616         ros::ServiceServer service2 = n->advertiseService(DET_SERVICE_SAVE_PLANES, onSave);
00617         ros::ServiceServer service3 = n->advertiseService(DET_SERVICE_LOAD_PLANES, onLoad);
00618 
00619         // if PCL input
00620         if (settings.param_node_input == PARAM_NODE_INPUT_PCL)
00621         {
00622                 exporter = new DynModelExporter(n,
00623                                                                                 settings.param_original_frame,
00624                                             settings.param_output_frame,
00625                                                                                 settings.param_visualisation_min_count,
00626                                                                                 settings.param_visualisation_distance,
00627                                                                                 settings.param_visualisation_plane_normal_dev,
00628                                                                                 settings.param_visualisation_plane_shift_dev,
00629                                                                                 settings.param_ht_keeptrack,
00630                                                                                 settings.param_visualisation_ttl,
00631                                                                                 settings.param_visualisation_max_poly_size);
00632 
00633                 // MESSAGES
00634                 message_filters::Subscriber<PointCloud2 > point_cloud(*n, DET_INPUT_POINT_CLOUD_TOPIC, 10);
00635 
00636                 pub1 = n->advertise<pcl::PointCloud<pcl::PointXYZRGB> > (DET_OUTPUT_POINT_CLOUD_TOPIC, 1);
00637                 pub2 = n->advertise<visualization_msgs::MarkerArray > (DET_OUTPUT_MARKER_TOPIC, 1);
00638                 pub3 = n->advertise<cob_3d_mapping_msgs::ShapeArray > (DET_OUTPUT_MARKER_SRS_TOPIC, 1);
00639 
00640                 if (settings.param_visualisation_color == "mean_color")
00641                 {
00642                         // sync images
00643                         tfListener = new tf::TransformListener();
00644                         transform_filter = new tf::MessageFilter<sensor_msgs::PointCloud2> (point_cloud, *tfListener, settings.param_output_frame, 10);
00645 
00646                         message_filters::Subscriber<Image> sub_rgb(*n, DET_INPUT_RGB_IMAGE_TOPIC, 10);
00647 
00648             // Majkl 2012/11/07: TimeSyncrhonizer doesn't work when playing data from a bag file
00649 //                      message_filters::TimeSynchronizer<PointCloud2, Image> sync(*transform_filter, sub_rgb, 10);
00650             typedef sync_policies::ApproximateTime<PointCloud2, Image> tSyncPolicy;
00651             Synchronizer<tSyncPolicy> sync(tSyncPolicy(10), *transform_filter, sub_rgb);
00652  
00653                         // register callback called when everything synchronized arrives
00654                         sync.registerCallback(boost::bind(&callbackpcl_rgb, _1, _2));
00655 
00656                         std::cerr << "Plane detector initialized and listening point clouds..." << std::endl;
00657                         spin();
00658 
00659                         return 1;
00660                 }
00661                 else
00662                 {
00663                         // sync images
00664                         tfListener = new tf::TransformListener();
00665                         transform_filter = new tf::MessageFilter<sensor_msgs::PointCloud2> (point_cloud, *tfListener, settings.param_output_frame, 10);
00666                         transform_filter->registerCallback(boost::bind(&callbackpcl, _1));
00667 
00668                         std::cerr << "Plane detector initialized and listening point clouds..." << std::endl;
00669                         spin();
00670 
00671                         return 1;
00672                 }
00673         }
00674 
00675         // if kinect input
00676         else if (settings.param_node_input == PARAM_NODE_INPUT_KINECT)
00677         {
00678                 exporter = new DynModelExporter(n,
00679                                                                                 settings.param_original_frame,
00680                                             settings.param_output_frame,
00681                                                                                 settings.param_visualisation_min_count,
00682                                                                                 settings.param_visualisation_distance,
00683                                                                                 settings.param_visualisation_plane_normal_dev,
00684                                                                                 settings.param_visualisation_plane_shift_dev,
00685                                                                                 settings.param_ht_keeptrack,
00686                                                                                 settings.param_visualisation_ttl,
00687                                                                                 settings.param_visualisation_max_poly_size);
00688 
00689                 // MESSAGES
00690                 message_filters::Subscriber<Image> depth_sub(*n, DET_INPUT_IMAGE_TOPIC, 10);
00691                 message_filters::Subscriber<CameraInfo> info_sub_depth(*n, DET_INPUT_CAM_INFO_TOPIC, 10);
00692 
00693                 tfListener = new tf::TransformListener();
00694                 transform_filterDepth = new tf::MessageFilter<Image> (depth_sub, *tfListener, settings.param_output_frame, 10);
00695 
00696                 tfListenerCam = new tf::TransformListener();
00697                 transform_filterCam = new tf::MessageFilter<CameraInfo> (info_sub_depth, *tfListenerCam, settings.param_output_frame, 10);
00698 
00699                 pub1 = n->advertise<pcl::PointCloud<pcl::PointXYZRGB> > (DET_OUTPUT_POINT_CLOUD_TOPIC, 1);
00700                 pub2 = n->advertise<visualization_msgs::MarkerArray > (DET_OUTPUT_MARKER_TOPIC, 1);
00701                 pub3 = n->advertise<cob_3d_mapping_msgs::ShapeArray > (DET_OUTPUT_MARKER_SRS_TOPIC, 1);
00702 
00703                 if (settings.param_visualisation_color == "mean_color")
00704                 {
00705                         message_filters::Subscriber<Image> sub_rgb(*n, DET_INPUT_RGB_IMAGE_TOPIC, 10);
00706 
00707                         tfListenerRGB = new tf::TransformListener();
00708                         transform_filterRGB = new tf::MessageFilter<Image> (sub_rgb, *tfListenerRGB, settings.param_output_frame, 10);
00709 
00710             // Majkl 2012/11/07: TimeSynchronizer doesn't work when playing data from a bag file
00711 //                      message_filters::TimeSynchronizer<Image, CameraInfo, Image> sync(*transform_filterDepth, *transform_filterCam, *transform_filterRGB, 10);
00712             typedef sync_policies::ApproximateTime<Image, CameraInfo, Image> tSyncPolicy;
00713             Synchronizer<tSyncPolicy> sync(tSyncPolicy(10), *transform_filterDepth, *transform_filterCam, *transform_filterRGB);
00714 
00715                         // register callback called when everything synchronized arrives
00716                         sync.registerCallback(boost::bind(&callbackkinect_rgb, _1, _2, _3));
00717 
00718                         std::cerr << "Plane detector initialized and listening depth images..." << std::endl;
00719                         spin();
00720 
00721                         return 1;
00722                 }
00723                 else
00724                 {
00725             // Majkl 2012/11/07: TimeSynchronizer doesn't work when playing data from a bag file
00726 //                      message_filters::TimeSynchronizer<Image, CameraInfo> sync(*transform_filterDepth, *transform_filterCam, 10);
00727             typedef sync_policies::ApproximateTime<Image, CameraInfo> tSyncPolicy;
00728             Synchronizer<tSyncPolicy> sync(tSyncPolicy(10), *transform_filterDepth, *transform_filterCam);
00729 
00730                         // register callback called when everything synchronized arrives
00731                         sync.registerCallback(boost::bind(&callbackkinect, _1, _2));
00732 
00733                         std::cerr << "Plane detector initialized and listening depth images..." << std::endl;
00734                         spin();
00735 
00736                         return 1;
00737                 }
00738         }
00739 }
00740 


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