$search
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