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