00001 #include "planar_removal.h"
00002
00003 using namespace Eigen;
00004
00005
00006 PlanarRemoval::PlanarRemoval() : it_(nh_) {
00007
00008
00009
00010
00011
00012
00013 pcl2_sub_ = this->nh_.subscribe<sensor_msgs::PointCloud2>("/planar_removal/pcl2/input", 1, &PlanarRemoval::pcl2_sub_callback, this);
00014
00015
00016 image_pub_ = it_.advertise("/planar_removal/image/dense", 1);
00017
00018
00019
00020 pcl2_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/planar_removal/pcl2/output", 15);
00021 pcl2_planes_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/planar_removal/pcl2/planes_output", 15);
00022
00023 coefficients_pub_ = nh_.advertise<pcl::ModelCoefficients> ("/planar_removal/ModelCoefficients/output", 1);
00024
00025
00026 dynamic_reconfigure::Server<iri_pcl_filters::IriPclFiltersParametersConfig>::CallbackType reconfig_callback_type = boost::bind(&PlanarRemoval::reconfigureCallback, this, _1, _2);
00027
00028 dyn_reconfig_srv.setCallback(reconfig_callback_type);
00029
00030
00031
00032
00033
00034
00035
00036 ROS_INFO("starting planar_removal");
00037
00038 }
00039
00040 void PlanarRemoval::reconfigureCallback(iri_pcl_filters::IriPclFiltersParametersConfig &config, uint32_t level)
00041 {
00042 ROS_INFO("Reconfigure request : distance th: %f lasting_cloud: %f #planes: %d ",
00043 config.distance_threshold, config.cloud_remaining_proportion_threshold, config.max_num_of_planes_removed);
00044
00045 mutex.enter();
00046 distance_threshold_ = config.distance_threshold;
00047 cloud_remaining_proportion_threshold_ = config.cloud_remaining_proportion_threshold;
00048 max_num_of_planes_removed_ = config.max_num_of_planes_removed;
00049 mutex.exit();
00050
00051 }
00052
00053 void PlanarRemoval::removePlanesXYZRGB(pcl::PointCloud<pcl::PointXYZRGB> cloud,pcl::PointCloud<pcl::PointXYZRGB>& cloud_filtered_rgb, pcl::PointCloud<pcl::PointXYZRGB>& cloud_filtered,pcl::PointCloud<pcl::PointXYZRGB>& cloud_planes){
00054
00055 pcl::PointCloud<pcl::PointXYZRGB> one_plane;
00056 pcl::PointCloud<pcl::PointXYZRGB> unstructured_cloud;
00057
00058 pcl::copyPointCloud(cloud, cloud_filtered_rgb);
00059 pcl::copyPointCloud(cloud, cloud_filtered);
00060
00061 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients ());
00062 pcl::PointIndices::Ptr inliers(new pcl::PointIndices ());
00063
00064 pcl::SACSegmentation<pcl::PointXYZRGB> seg;
00065
00066 double distance_threshold_aux;
00067 double cloud_remaining_proportion_threshold_aux;
00068 int max_num_of_planes_removed_aux;
00069
00070 mutex.enter();
00071 distance_threshold_aux = distance_threshold_;
00072 cloud_remaining_proportion_threshold_aux = cloud_remaining_proportion_threshold_;
00073 max_num_of_planes_removed_aux = max_num_of_planes_removed_;
00074 mutex.exit();
00075
00076
00077 cloud_planes.width = cloud.width;
00078 cloud_planes.height = cloud.height;
00079 cloud_planes.points.resize (one_plane.width * one_plane.height);
00080 cloud_planes.is_dense = cloud.is_dense;
00081 cloud_planes.header = cloud.header;
00082
00083
00084 seg.setOptimizeCoefficients (true);
00085
00086
00087 seg.setModelType (pcl::SACMODEL_PLANE);
00088 seg.setMethodType (pcl::SAC_RANSAC);
00089 seg.setDistanceThreshold (distance_threshold_aux);
00090
00091 seg.setInputCloud (cloud.makeShared ());
00092 seg.segment (*inliers, *coefficients);
00093
00094
00095 coefficients_pub_.publish (*coefficients);
00096
00097
00098 pcl::ExtractIndices<pcl::PointXYZRGB> extract;
00099
00100 int i=0, nr_points = (int) cloud.points.size ();
00101
00102 while (cloud.points.size () > cloud_remaining_proportion_threshold_aux * nr_points && i < max_num_of_planes_removed_aux)
00103 {
00104
00105 seg.setInputCloud (cloud.makeShared());
00106 seg.setMaxIterations (1000);
00107 seg.segment (*inliers, *coefficients);
00108
00109 if (inliers->indices.size () == 0) {
00110 ROS_INFO("Could not estimate a planar model for the given dataset.");
00111 break;
00112 }
00113
00114
00115 extract.setInputCloud (cloud.makeShared());
00116 extract.setIndices (inliers);
00117 extract.setNegative (false);
00118 extract.filter (one_plane);
00119
00120 cloud_planes += one_plane;
00121
00122
00123
00124
00125
00126 for(int j = 0; j < inliers->indices.size(); j++){
00127 uint8_t r = 0, g = 0, b = 0;
00128 uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
00129 cloud_filtered_rgb[inliers->indices[j]].rgb = *reinterpret_cast<float*>(&rgb);
00130 cloud_filtered[inliers->indices[j]].x = std::numeric_limits<float>::quiet_NaN();
00131 cloud_filtered[inliers->indices[j]].y = std::numeric_limits<float>::quiet_NaN();
00132 cloud_filtered[inliers->indices[j]].z = std::numeric_limits<float>::quiet_NaN();
00133 }
00134 i++;
00135 }
00136
00137 }
00138
00139
00140 void PlanarRemoval::pcl2_sub_callback(const sensor_msgs::PointCloud2::ConstPtr& pcl2_msg_) {
00141
00142
00143
00144
00145
00146
00147
00148 cv::Mat rgb_image;
00149 pcl::PointCloud<pcl::PointXYZRGB> pcl_xyzrgb_aux_rgb;
00150 pcl::PointCloud<pcl::PointXYZRGB> pcl_xyzrgb_aux_nan;
00151
00152 pcl::fromROSMsg(*pcl2_msg_, pcl_xyzrgb_);
00153
00154 removePlanesXYZRGB(pcl_xyzrgb_, pcl_xyzrgb_aux_rgb, pcl_xyzrgb_aux_nan, pcl_planes_xyzrgb_);
00155
00156
00157
00158 cv_image.header.stamp = ros::Time::now();
00159 cv_image.header.frame_id = "camera";
00160 cv_image.encoding = "bgr8";
00161
00162 rgb_image = cv::Mat::zeros(rgb_image.size(), CV_8UC3);
00163
00164 image_from_sparse_pcl2(pcl_xyzrgb_aux_rgb,rgb_image);
00165
00166 cv_image.image = rgb_image;
00167
00168 image_pub_.publish(cv_image.toImageMsg());
00169
00170 pcl::toROSMsg(pcl_xyzrgb_aux_nan, pcl2_filtered_msg_);
00171
00172 pcl2_pub_.publish(pcl2_filtered_msg_);
00173 pcl::toROSMsg(pcl_planes_xyzrgb_, pcl2_planes_msg_);
00174 pcl2_planes_pub_.publish(pcl2_planes_msg_);
00175
00176 }
00177
00178 void PlanarRemoval::image_from_sparse_pcl2(pcl::PointCloud<pcl::PointXYZRGB> cloud, cv::Mat& rgb_image){
00179
00180 pcl::PointCloud<pcl::PointXYZRGB>::iterator pt_iter_xyzrgb = cloud.begin();
00181
00182 rgb_image.create(cloud.height, cloud.width, CV_8UC3);
00183
00184 if(cloud.height != rgb_image.rows || cloud.width != rgb_image.cols){
00185 ROS_ERROR("Size mismatch between point cloud and rgb_image");
00186 ROS_INFO("xyzrgb size %d %d data size %d xyz %d %d data size %d rgb_image %d %d",pcl_xyzrgb_.height, pcl_xyzrgb_.width, pcl_xyzrgb_.size(),cloud.height, cloud.width, cloud.size(), rgb_image.rows, rgb_image.cols);
00187 } else {
00188 for (int rr = 0; rr < cloud.height; ++rr) {
00189 for (int cc = 0; cc < cloud.width; ++cc, ++pt_iter_xyzrgb) {
00190 pcl::PointXYZRGB &pt_xyzrgb = *pt_iter_xyzrgb;
00191 RGBValue color;
00192 color.float_value = pt_xyzrgb.rgb;
00193
00194 if(isnan(pt_iter_xyzrgb->x) && isnan(pt_iter_xyzrgb->y) && isnan(pt_iter_xyzrgb->z)){
00195 rgb_image.at<cv::Vec3b>(rr,cc)[0] = 0;
00196 rgb_image.at<cv::Vec3b>(rr,cc)[1] = 0;
00197 rgb_image.at<cv::Vec3b>(rr,cc)[2] = 0;
00198 }else{
00199 rgb_image.at<cv::Vec3b>(rr,cc)[0] = color.Blue;
00200 rgb_image.at<cv::Vec3b>(rr,cc)[1] = color.Green;
00201 rgb_image.at<cv::Vec3b>(rr,cc)[2] = color.Red;
00202 }
00203 }
00204 }
00205 }
00206 }
00207
00208 int main(int argc, char** argv)
00209 {
00210 ros::init(argc, argv, "planar_removal");
00211 PlanarRemoval planar_removal;
00212 ros::spin();
00213 return 0;
00214 }
00215