00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00037 #include <ros/ros.h>
00038 #include <image_transport/image_transport.h>
00039 #include <cv_bridge/cv_bridge.h>
00040
00041
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
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
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
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
00181 pcl::PointCloud<pcl::PointXYZ> pointcloud;
00182 pcl::fromROSMsg (*cloud, pointcloud);
00183
00184
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
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
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
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
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
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
00244 getPlanes(normal, pointcloud, rgb);
00245
00246
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
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
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
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
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
00320 Eigen::Matrix4f sensorToWorld;
00321 pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld);
00322 pcl::transformPointCloud(pointcloud, pointcloud, sensorToWorld);
00323
00324
00325
00326
00327
00328
00329
00330
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
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
00546
00547
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
00563 ros::NodeHandle private_nh("~");
00564 getParams(private_nh);
00565
00566
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
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
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
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
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
00649
00650 typedef sync_policies::ApproximateTime<PointCloud2, Image> tSyncPolicy;
00651 Synchronizer<tSyncPolicy> sync(tSyncPolicy(10), *transform_filter, sub_rgb);
00652
00653
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
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
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
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
00711
00712 typedef sync_policies::ApproximateTime<Image, CameraInfo, Image> tSyncPolicy;
00713 Synchronizer<tSyncPolicy> sync(tSyncPolicy(10), *transform_filterDepth, *transform_filterCam, *transform_filterRGB);
00714
00715
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
00726
00727 typedef sync_policies::ApproximateTime<Image, CameraInfo> tSyncPolicy;
00728 Synchronizer<tSyncPolicy> sync(tSyncPolicy(10), *transform_filterDepth, *transform_filterCam);
00729
00730
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