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
00028
00029
00030
00031
00032
00033
00034 #include "jsk_pcl_ros/colorize_random_points_RF.h"
00035 #include <pluginlib/class_list_macros.h>
00036 #include <pcl/common/centroid.h>
00037
00038 namespace jsk_pcl_ros
00039 {
00040 void ColorizeMapRandomForest::onInit(void)
00041 {
00042 ConnectionBasedNodelet::onInit();
00043 pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "cloth_result", 1);
00044
00045 srand(time(NULL));
00046
00047 double tmp_radius, tmp_pass, tmp_pass2;
00048 int tmp_sum_num;
00049 tmp_radius = 0.03;
00050 tmp_pass = 0.03;
00051 tmp_pass2 = 0.06;
00052 tmp_sum_num = 100;
00053 if (!pnh_->getParam("rs", tmp_radius))
00054 {
00055 ROS_WARN("~rs is not specified, set 1");
00056 }
00057
00058 if (!pnh_->getParam("po", tmp_pass))
00059 {
00060 ROS_WARN("~po is not specified, set 1");
00061 }
00062
00063 if (!pnh_->getParam("po2", tmp_pass2))
00064 {
00065 ROS_WARN("~po is not specified, set 1");
00066 }
00067
00068 if (!pnh_->getParam("sum_num", tmp_sum_num))
00069 {
00070 ROS_WARN("~sum_num is not specified, set 1");
00071 }
00072
00073 radius_search_ = tmp_radius;
00074 pass_offset_ = tmp_pass;
00075 pass_offset2_ = tmp_pass2;
00076 sum_num_ = tmp_sum_num;
00077 onInitPostProcess();
00078 }
00079
00080 void ColorizeMapRandomForest::subscribe()
00081 {
00082 sub_input_ = pnh_->subscribe("input", 1, &ColorizeMapRandomForest::extract, this);
00083 }
00084
00085 void ColorizeMapRandomForest::unsubscribe()
00086 {
00087 sub_input_.shutdown();
00088 }
00089
00090 void ColorizeMapRandomForest::extract(const sensor_msgs::PointCloud2 pc)
00091 {
00092 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
00093
00094 std::vector<int> indices;
00095 pcl::fromROSMsg(pc, *cloud);
00096
00097 cloud->is_dense = false;
00098 pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
00099
00100 pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
00101 tree->setInputCloud (cloud);
00102
00103 pcl::PassThrough<pcl::PointXYZRGB> pass;
00104 pass.setInputCloud (cloud);
00105 pass.setFilterFieldName (std::string("z"));
00106 pass.setFilterLimits (0.0, 1.5);
00107 pass.filter (*cloud);
00108
00109 pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> ne;
00110 pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGB> ());
00111 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00112 ne.setInputCloud (cloud);
00113 ROS_INFO("normal estimate");
00114 ne.setSearchMethod (tree2);
00115 ne.setRadiusSearch (0.02);
00116 ne.compute (*cloud_normals);
00117
00118 for (int cloud_index = 0; cloud_index < cloud_normals->points.size(); cloud_index++){
00119 cloud_normals->points[cloud_index].x = cloud->points[cloud_index].x;
00120 cloud_normals->points[cloud_index].y = cloud->points[cloud_index].y;
00121 cloud_normals->points[cloud_index].z = cloud->points[cloud_index].z;
00122
00123 }
00124
00125 int result_counter=0, call_counter = 0;
00126 pcl::PointXYZRGBNormal max_pt,min_pt;
00127 pcl::getMinMax3D(*cloud_normals, min_pt, max_pt);
00128 for (int i = 0 ; i < 200; i++){
00129 ROS_INFO("fpfh %d / 200", i);
00130 double lucky = 0, lucky2 = 0;
00131 std::string axis("x"),other_axis("y");
00132 int rand_xy = rand()%2;
00133 if (rand_xy == 0){
00134 lucky = min_pt.x - pass_offset_ + (max_pt.x - min_pt.x - pass_offset_*2) * 1.0 * rand() / RAND_MAX;
00135 lucky2 = min_pt.y + (max_pt.y - min_pt.y) * 1.0 * rand() / RAND_MAX;
00136 }
00137 else {
00138 lucky = min_pt.y - pass_offset_ + (max_pt.y - min_pt.y - pass_offset_*2) * 1.0 * rand() / RAND_MAX;
00139 lucky2 = min_pt.x + (max_pt.x - min_pt.x) * 1.0 * rand() / RAND_MAX;
00140 axis = std::string("y");
00141 other_axis = std::string("x");
00142 }
00143
00144 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals_pass (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00145
00146 pcl::PassThrough<pcl::PointXYZRGBNormal> pass;
00147 pcl::IndicesPtr indices_x(new std::vector<int>());
00148 pass.setInputCloud (cloud_normals);
00149 pass.setFilterFieldName (axis);
00150 float small = std::min(lucky, lucky + pass_offset_);
00151 float large = std::max(lucky, lucky + pass_offset_);
00152 pass.setFilterLimits (small, large);
00153 pass.filter (*cloud_normals_pass);
00154 pass.setInputCloud (cloud_normals_pass);
00155 pass.setFilterFieldName (other_axis);
00156 float small2 = std::min(lucky2, lucky2 + pass_offset2_);
00157 float large2 = std::max(lucky2, lucky2 + pass_offset2_);
00158 pass.setFilterLimits (small2, large2);
00159 pass.filter (*cloud_normals_pass);
00160
00161 std::vector<int> tmp_indices;
00162 pcl::removeNaNFromPointCloud(*cloud_normals_pass, *cloud_normals_pass, tmp_indices);
00163 if(cloud_normals_pass->points.size() == 0){
00164 continue;
00165 }
00166 pcl::FPFHEstimationOMP<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal, pcl::FPFHSignature33> fpfh;
00167 pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree4 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
00168 pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33> ());
00169 fpfh.setNumberOfThreads(8);
00170 fpfh.setInputCloud (cloud_normals_pass);
00171 fpfh.setInputNormals (cloud_normals_pass);
00172 fpfh.setSearchMethod (tree4);
00173 fpfh.setRadiusSearch (radius_search_);
00174 fpfh.compute (*fpfhs);
00175 int target_id, max_value;
00176 if ((int)fpfhs->points.size() - sum_num_ - 1 < 1){
00177 target_id = 0;
00178 max_value = (int)fpfhs->points.size();
00179 }else{
00180 target_id = rand() % ((int)fpfhs->points.size() - sum_num_ - 1);
00181 max_value = target_id + sum_num_;
00182 }
00183 std::vector<double> result;
00184 bool success = true;
00185
00186 for(int index = 0; index < 33; index++){
00187 float sum_hist_points = 0;
00188 for(int kndex = target_id; kndex < max_value;kndex++)
00189 {
00190 if(pcl_isnan(fpfhs->points[kndex].histogram[index])){
00191 success = false;
00192 }else{
00193 sum_hist_points+=fpfhs->points[kndex].histogram[index];
00194 }
00195 }
00196 result.push_back( sum_hist_points/sum_num_ );
00197 }
00198
00199 bool cloth = false;
00200 if(success){
00201 call_counter++;
00202 ros::ServiceClient client = pnh_->serviceClient<ml_classifiers::ClassifyData>("/predict");
00203 ml_classifiers::ClassifyData srv;
00204 ml_classifiers::ClassDataPoint class_data_point;
00205 class_data_point.point = result;
00206 srv.request.data.push_back(class_data_point);
00207 if(client.call(srv))
00208 if (atoi(srv.response.classifications[0].c_str()) == 0)
00209 cloth = true;
00210 ROS_INFO("response result : %s", srv.response.classifications[0].c_str());
00211 }else{
00212 continue;
00213 }
00214
00215 Eigen::Vector4f c;
00216 pcl::compute3DCentroid (*cloud_normals_pass, c);
00217
00218 pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGBNormal> search_octree (0.05);
00219 search_octree.setInputCloud (cloud_normals);
00220 search_octree.addPointsFromInputCloud ();
00221
00222 pcl::PointXYZRGBNormal point_a;
00223 point_a.x = c[0];point_a.y = c[1];point_a.z = c[2];
00224 std::vector<int> k_indices;
00225 std::vector<float> k_sqr_distances;
00226 search_octree.radiusSearch(point_a, 0.05, k_indices, k_sqr_distances);
00227
00228 for(int index = 0; index < k_indices.size(); index++){
00229 int id = k_indices[index];
00230 if(cloth){
00231 cloud_normals->points[id].r = std::min(std::max(255.0 - sqrt(k_sqr_distances[index]) * 2000, 0.0) + cloud_normals->points[id].r, 255.0);
00232 }else{
00233 cloud_normals->points[id].b = std::min(std::max(255.0 - sqrt(k_sqr_distances[index]) * 2000, 0.0) + cloud_normals->points[id].b, 255.0);
00234 }
00235 }
00236 }
00237 sensor_msgs::PointCloud2 _pointcloud2;
00238 pcl::toROSMsg(*cloud_normals, _pointcloud2);
00239 _pointcloud2.header = pc.header;
00240 _pointcloud2.is_dense = false;
00241 pub_.publish(_pointcloud2);
00242 }
00243 }
00244
00245
00246 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::ColorizeMapRandomForest, nodelet::Nodelet);