$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id:$ 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 <but_segmentation/filtering.h> 00063 #include <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/extract_indices.h> 00082 #include <pcl/filters/voxel_grid.h> 00083 #include <pcl_ros/point_cloud.h> 00084 #include <pcl_ros/transforms.h> 00085 #include <pcl/ModelCoefficients.h> 00086 00087 #include <cstdlib> 00088 #include <cstdio> 00089 #include <cfloat> 00090 00091 #include <float.h> 00092 #include <sensor_msgs/Image.h> 00093 #include <sensor_msgs/CameraInfo.h> 00094 #include <visualization_msgs/Marker.h> 00095 #include <visualization_msgs/MarkerArray.h> 00096 #include <boost/math/quaternion.hpp> 00097 #include "srs_env_model_percp/but_plane_detector/plane_detector_params.h" 00098 #include <stdlib.h> 00099 #include <stdio.h> 00100 #include <float.h> 00101 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 tf::TransformListener *tfListener; 00115 ros::Publisher pub1; 00116 ros::Publisher pub2; 00117 ros::Publisher pub3; 00118 DynModelExporter *exporter = NULL; 00119 00120 sensor_msgs::CameraInfo cam_info_legacy; 00121 sensor_msgs::CameraInfoConstPtr cam_info_aux (&cam_info_legacy); 00122 SceneModel *model; 00123 00124 PlaneDetectorSettings settings; 00125 00129 void callbackpcl(const PointCloud2ConstPtr& cloud) 00130 { 00131 if (settings.param_ht_keeptrack == 0) 00132 { 00133 delete model; 00134 model = new SceneModel( settings.param_ht_maxdepth, 00135 settings.param_ht_minshift, 00136 settings.param_ht_maxshift, 00137 settings.param_ht_angle_res, 00138 settings.param_ht_shift_res, 00139 settings.param_ht_gauss_angle_res, 00140 settings.param_ht_gauss_shift_res, 00141 settings.param_ht_gauss_angle_sigma, 00142 settings.param_ht_gauss_shift_sigma, 00143 settings.param_ht_lvl1_gauss_angle_res, 00144 settings.param_ht_lvl1_gauss_shift_res, 00145 settings.param_ht_lvl1_gauss_angle_sigma, 00146 settings.param_ht_lvl1_gauss_shift_sigma); 00147 } 00148 00149 ++counter; 00150 00151 // make the pointcloud 00152 pcl::PointCloud<pcl::PointXYZ> pointcloud; 00153 pcl::fromROSMsg (*cloud, pointcloud); 00154 00155 // Control out 00156 std::cerr << "Recieved frame..." << std::endl; 00157 std::cerr << "Topic: " << pointcloud.header.frame_id << std::endl; 00158 std::cerr << "Width: " << pointcloud.width << " height: " << pointcloud.height << std::endl; 00159 std::cerr << "=========================================================" << endl; 00160 00162 // Kinect far clipping error override 00163 00164 for (unsigned int i = 0; i < pointcloud.size(); ++i) 00165 { 00166 // if point is farer than 7m, which is normal Kinect distance, put it into unwanted indices 00167 if (pointcloud[i].z > 7.0) 00168 { 00169 pointcloud[i].z = 0; 00170 } 00171 } 00172 00173 // transform to world 00174 tf::StampedTransform sensorToWorldTf; 00175 try { 00176 tfListener->waitForTransform(settings.param_output_frame, cloud->header.frame_id, cloud->header.stamp, ros::Duration(2.0)); 00177 tfListener->lookupTransform(settings.param_output_frame, cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf); 00178 } 00179 catch(tf::TransformException& ex){ 00180 std::cerr << "Transform error: " << ex.what() << ", quitting callback" << std::endl; 00181 return; 00182 } 00183 00184 // transform pointcloud from sensor frame to fixed robot frame 00185 Eigen::Matrix4f sensorToWorld; 00186 pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld); 00187 pcl::transformPointCloud(pointcloud, pointcloud, sensorToWorld); 00188 00189 // Compute normals on point cloud 00190 Normals normal(pointcloud); 00191 00192 // Add point cloud to current Hough space and recompute planes 00193 model->AddNext(normal); 00194 model->recomputePlanes( settings.param_search_minimum_current_space, 00195 settings.param_search_minimum_global_space, 00196 settings.param_search_maxima_search_blur, 00197 settings.param_search_maxima_search_neighborhood); 00198 00199 // TODO debug point cloud output 00201 pcl::PointCloud<pcl::PointXYZRGB> outcloud; 00202 outcloud.header.frame_id = pointcloud.header.frame_id; 00203 00204 // Majkl's note: The following line has been commented out and modified because 00205 // it's necessary to use aligned allocator when creating STL containers and 00206 // classes containing Eigen types. 00207 // std::vector<pcl::PointCloud<pcl::PointXYZ> > planecloud(model->planes.size()); 00208 00209 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; 00210 typedef std::vector<PointCloud, Eigen::aligned_allocator<PointCloud> > PlaneCloud; 00211 PlaneCloud planecloud(model->planes.size()); 00212 00213 for (int i = 0; i < normal.m_points.rows; ++i) 00214 for (int j = 0; j < normal.m_points.cols; ++j) 00215 { 00216 Vec3f point = normal.m_points.at<Vec3f>(i, j); 00217 cv::Vec4f localPlane = normal.m_planes.at<cv::Vec4f>(i, j); 00218 Plane<float> aaa(localPlane[0], localPlane[1], localPlane[2], localPlane[3]); 00219 double dist = DBL_MAX; 00220 int chosen = -1; 00221 // find the best plane 00222 for (unsigned int a = 0; a < model->planes.size(); ++a) 00223 { 00224 if (model->planes[a].distance(point) < dist && model->planes[a].distance(point) < settings.param_visualisation_distance && 00225 model->planes[a].isSimilar(aaa, settings.param_visualisation_plane_normal_dev, settings.param_visualisation_plane_shift_dev)) 00226 { 00227 dist = model->planes[a].distance(point); 00228 chosen = a; 00229 } 00230 } 00231 // if there is good plane, insert point into point cloud 00232 if (chosen > -1) 00233 { 00234 pcl::PointXYZRGB current; 00235 current.x = point[0]; 00236 current.y = point[1]; 00237 current.z = point[2]; 00238 // some not good debug info about planes 00239 current.r = abs(model->planes[chosen].a)*255; 00240 current.g = abs(model->planes[chosen].b)*255; 00241 current.b = abs(model->planes[chosen].d)*255; 00242 outcloud.push_back(current); 00243 00244 pcl::PointXYZ current2; 00245 current2.x = point[0]; 00246 current2.y = point[1]; 00247 current2.z = point[2]; 00248 planecloud[chosen].points.push_back(current2); 00249 00250 00251 } 00252 } 00253 pub1.publish(outcloud); 00254 00255 exporter->update(model->planes, normal); 00256 visualization_msgs::MarkerArray marker_array; 00257 for (unsigned int i = 0; i < exporter->displayed_planes.size(); ++i) 00258 { 00259 exporter->displayed_planes[i].marker.header.frame_id = settings.param_output_frame; 00260 exporter->displayed_planes[i].marker.header.stamp = pointcloud.header.stamp; 00261 marker_array.markers.push_back(exporter->displayed_planes[i].marker); 00262 } 00263 pub2.publish(marker_array); 00264 00265 // // todo visualisation of planes as marker array 00266 // //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00267 // visualization_msgs::MarkerArray marker_array; 00268 // for (unsigned int i = 0; i < planecloud.size(); ++i) 00269 // { 00270 // if (planecloud[i].points.size() > 20) 00271 // { 00272 // pcl::ModelCoefficientsPtr coefs(new pcl::ModelCoefficients()); 00273 // 00274 // coefs->values.push_back(model->planes[i].a); 00275 // coefs->values.push_back(model->planes[i].b); 00276 // coefs->values.push_back(model->planes[i].c); 00277 // coefs->values.push_back(model->planes[i].d); 00278 // 00279 // visualization_msgs::Marker marker; 00280 // DynModelExporter::createMarkerForConvexHull(planecloud[i], coefs, marker); 00281 // marker.header.frame_id = pointcloud.header.frame_id; 00282 // marker.header.stamp = pointcloud.header.stamp; 00283 // marker.id = i; 00284 // marker.ns = "Normals"; 00285 // marker.pose.position.x = 0.0; 00286 // marker.pose.position.y = 0.0; 00287 // marker.pose.position.z = 0.0; 00288 // marker.pose.orientation.x = 0.0; 00289 // marker.pose.orientation.y = 0.0; 00290 // marker.pose.orientation.z = 0.0; 00291 // marker.pose.orientation.w = 1.0; 00292 // marker.scale.x = 1; 00293 // marker.scale.y = 1; 00294 // marker.scale.z = 1; 00295 // marker.color.r = 0.5; 00296 // marker.color.g = 0.0; 00297 // marker.color.b = 0.0; 00298 // marker.color.a = 0.8; 00299 // marker_array.markers.push_back(marker); 00300 // } 00301 // } 00302 // pub2.publish(marker_array); 00303 00304 // Control out 00305 std::cerr << "DONE.... " << std::endl; 00306 std::cerr << "=========================================================" << endl<< endl; 00307 } 00308 00309 bool getParams(ros::NodeHandle nh) 00310 { 00311 nh.param(PARAM_NODE_INPUT, settings.param_node_input, PARAM_NODE_INPUT_DEFAULT); 00312 00313 nh.param(PARAM_NODE_OUTPUT_FRAME, settings.param_output_frame, PARAM_NODE_OUTPUT_FRAME_DEFAULT); 00314 nh.param(PARAM_NODE_ORIGINAL_FRAME, settings.param_original_frame, PARAM_NODE_ORIGINAL_FRAME_DEFAULT); 00315 00316 nh.param(PARAM_HT_KEEPTRACK, settings.param_ht_keeptrack, PARAM_HT_KEEPTRACK_DEFAULT); 00317 nh.param(PARAM_HT_MAXDEPTH, settings.param_ht_maxdepth, PARAM_HT_MAXDEPTH_DEFAULT); 00318 nh.param(PARAM_HT_MINSHIFT, settings.param_ht_minshift, PARAM_HT_MINSHIFT_DEFAULT); 00319 nh.param(PARAM_HT_MAXSHIFT, settings.param_ht_maxshift, PARAM_HT_MAXSHIFT_DEFAULT); 00320 nh.param(PARAM_HT_ANGLE_RES, settings.param_ht_angle_res, PARAM_HT_ANGLE_RES_DEFAULT); 00321 nh.param(PARAM_HT_SHIFT_RES, settings.param_ht_shift_res, PARAM_HT_SHIFT_RES_DEFAULT); 00322 nh.param(PARAM_HT_GAUSS_ANGLE_RES, settings.param_ht_gauss_angle_res, PARAM_HT_GAUSS_ANGLE_RES_DEFAULT); 00323 nh.param(PARAM_HT_GAUSS_SHIFT_RES, settings.param_ht_gauss_shift_res, PARAM_HT_GAUSS_SHIFT_RES_DEFAULT); 00324 nh.param(PARAM_HT_GAUSS_ANGLE_SIGMA, settings.param_ht_gauss_angle_sigma, PARAM_HT_GAUSS_ANGLE_SIGMA_DEFAULT); 00325 nh.param(PARAM_HT_GAUSS_SHIFT_SIGMA, settings.param_ht_gauss_shift_sigma, PARAM_HT_GAUSS_SHIFT_SIGMA_DEFAULT); 00326 nh.param(PARAM_HT_LVL1_GAUSS_ANGLE_RES, settings.param_ht_lvl1_gauss_angle_res, PARAM_HT_LVL1_GAUSS_ANGLE_RES_DEFAULT); 00327 nh.param(PARAM_HT_LVL1_GAUSS_SHIFT_RES, settings.param_ht_lvl1_gauss_shift_res, PARAM_HT_LVL1_GAUSS_SHIFT_RES_DEFAULT); 00328 nh.param(PARAM_HT_LVL1_GAUSS_ANGLE_SIGMA, settings.param_ht_lvl1_gauss_angle_sigma, PARAM_HT_LVL1_GAUSS_ANGLE_SIGMA_DEFAULT); 00329 nh.param(PARAM_HT_LVL1_GAUSS_SHIFT_SIGMA, settings.param_ht_lvl1_gauss_shift_sigma, PARAM_HT_LVL1_GAUSS_SHIFT_SIGMA_DEFAULT); 00330 00331 nh.param(PARAM_VISUALISATION_DISTANCE, settings.param_visualisation_distance, PARAM_VISUALISATION_DISTANCE_DEFAULT); 00332 nh.param(PARAM_VISUALISATION_PLANE_NORMAL_DEV, settings.param_visualisation_plane_normal_dev, PARAM_VISUALISATION_PLANE_NORMAL_DEV_DEFAULT); 00333 nh.param(PARAM_VISUALISATION_PLANE_SHIFT_DEV, settings.param_visualisation_plane_shift_dev, PARAM_VISUALISATION_PLANE_SHIFT_DEV_DEFAULT); 00334 nh.param(PARAM_VISUALISATION_MIN_COUNT, settings.param_visualisation_min_count, PARAM_VISUALISATION_MIN_COUNT_DEFAULT); 00335 nh.param(PARAM_VISUALISATION_MAX_POLY_SIZE, settings.param_visualisation_max_poly_size, PARAM_VISUALISATION_MAX_POLY_SIZE_DEFAULT); 00336 00337 nh.param(PARAM_SEARCH_MINIMUM_CURRENT_SPACE, settings.param_search_minimum_current_space, PARAM_SEARCH_MINIMUM_CURRENT_SPACE_DEFAULT); 00338 nh.param(PARAM_SEARCH_MINIMUM_GLOBAL_SPACE, settings.param_search_minimum_global_space, PARAM_SEARCH_MINIMUM_GLOBAL_SPACE_DEFAULT); 00339 nh.param(PARAM_SEARCH_MAXIMA_SEARCH_NEIGHBORHOOD, settings.param_search_maxima_search_neighborhood, PARAM_SEARCH_MAXIMA_SEARCH_NEIGHBORHOOD_DEFAULT); 00340 nh.param(PARAM_SEARCH_MAXIMA_SEARCH_BLUR, settings.param_search_maxima_search_blur, PARAM_SEARCH_MAXIMA_SEARCH_BLUR_DEFAULT); 00341 00342 return true; 00343 } 00344 } 00345 00349 int main( int argc, char** argv ) 00350 { 00351 using namespace srs_env_model_percp; 00352 00353 ros::init(argc, argv, "but_plane_detector"); 00354 ros::NodeHandle n; 00355 00356 // Private node handle to read private node parameters 00357 ros::NodeHandle private_nh("~"); 00358 getParams(private_nh); 00359 00360 // Init scene model 00361 model = new SceneModel( settings.param_ht_maxdepth, 00362 settings.param_ht_minshift, 00363 settings.param_ht_maxshift, 00364 settings.param_ht_angle_res, 00365 settings.param_ht_shift_res, 00366 settings.param_ht_gauss_angle_res, 00367 settings.param_ht_gauss_shift_res, 00368 settings.param_ht_gauss_angle_sigma, 00369 settings.param_ht_gauss_shift_sigma, 00370 settings.param_ht_lvl1_gauss_angle_res, 00371 settings.param_ht_lvl1_gauss_shift_res, 00372 settings.param_ht_lvl1_gauss_angle_sigma, 00373 settings.param_ht_lvl1_gauss_shift_sigma); 00374 00375 // Print out parameters 00376 std::cerr << std::endl; 00377 std::cerr << "Given HS parameters:" << std::endl; 00378 std::cerr << "====================" << std::endl; 00379 std::cerr << "Max point cloud z value: " << settings.param_ht_maxdepth << std::endl; 00380 std::cerr << "Minimal plane shift (d param) value: " << settings.param_ht_minshift << std::endl; 00381 std::cerr << "Maximal plane shift (d param) value: " << settings.param_ht_maxshift << std::endl; 00382 std::cerr << "Hough space angle resolution: " << settings.param_ht_angle_res << std::endl; 00383 std::cerr << "Hough space shift resolution: " << settings.param_ht_shift_res << std::endl; 00384 std::cerr << "Plane Gauss function angle resolution: " << settings.param_ht_gauss_angle_res << std::endl; 00385 std::cerr << "Plane Gauss function shift resolution: " << settings.param_ht_gauss_shift_res << std::endl; 00386 std::cerr << "Plane Gauss function angle sigma: " << settings.param_ht_gauss_angle_sigma << std::endl; 00387 std::cerr << "Plane Gauss function shift sigma: " << settings.param_ht_gauss_shift_sigma << std::endl; 00388 std::cerr << std::endl; 00389 std::cerr << "Given plane search parameters:" << std::endl; 00390 std::cerr << "==============================" << std::endl; 00391 std::cerr << "Minimum plane value in HS for current frame: " << settings.param_search_minimum_current_space << std::endl; 00392 std::cerr << "Minimum plane value in HS for global space: " << settings.param_search_minimum_global_space << std::endl; 00393 std::cerr << "Hough space local maximum neighborhood search: " << settings.param_search_maxima_search_neighborhood << std::endl; 00394 std::cerr << "Hough space local maximum search blur: " << settings.param_search_maxima_search_blur << std::endl; 00395 std::cerr << std::endl; 00396 std::cerr << "Given visualisation parameters:" << std::endl; 00397 std::cerr << "==============================" << std::endl; 00398 std::cerr << "Maximum point distance from plane: " << settings.param_visualisation_distance << std::endl; 00399 std::cerr << "Maximum point normal deviation from plane: " << settings.param_visualisation_plane_normal_dev << std::endl; 00400 std::cerr << "Maximum point plane shift (d param) from plane: " << settings.param_visualisation_plane_shift_dev << std::endl; 00401 std::cerr << "Minimum point count for visualised plane in current point cloud: " << settings.param_visualisation_min_count << std::endl; 00402 std::cerr << std::endl; 00403 00404 exporter = new DynModelExporter(&n, 00405 settings.param_original_frame, 00406 settings.param_output_frame, 00407 settings.param_visualisation_min_count, 00408 settings.param_visualisation_distance, 00409 settings.param_visualisation_plane_normal_dev, 00410 settings.param_visualisation_plane_shift_dev, 00411 settings.param_ht_keeptrack, 00412 settings.param_visualisation_max_poly_size); 00413 00414 // MESSAGES 00415 //message_filters::Subscriber<PointCloud2 > point_cloud(n, DET_INPUT_POINT_CLOUD_TOPIC, 1); 00416 00417 pub1 = n.advertise<pcl::PointCloud<pcl::PointXYZRGB> > (DET_OUTPUT_POINT_CLOUD_TOPIC, 1); 00418 pub2 = n.advertise<visualization_msgs::MarkerArray > (DET_OUTPUT_MARKER_TOPIC, 1); 00419 00420 // sync images 00421 tfListener = new tf::TransformListener(); 00422 message_filters::Subscriber<PointCloud2 > point_cloud(n, "/cam3d/depth/points", 1); 00423 transform_filter = new tf::MessageFilter<sensor_msgs::PointCloud2> (point_cloud, *tfListener, settings.param_output_frame, 1); 00424 transform_filter->registerCallback(boost::bind(&callbackpcl, _1)); 00425 00426 std::cerr << "Plane detector initialized and listening point clouds..." << std::endl; 00427 ros::spin(); 00428 00429 return 1; 00430 }