Extractor() {
tree.reset(new pcl::KdTreeFLANN<Point > ());
cloud.reset(new pcl::PointCloud<Point>);
cloud_filtered.reset(new pcl::PointCloud<Point>);
cloud_normals.reset(new pcl::PointCloud<pcl::Normal>);
coefficients_plane_.reset(new pcl::ModelCoefficients);
coefficients_cylinder_.reset(new pcl::ModelCoefficients);
inliers_plane_.reset(new pcl::PointIndices);
inliers_cylinder_.reset(new pcl::PointIndices);
// Filter Pass
pass.setFilterFieldName("z");
pass.setFilterLimits(-100, 100);
// VoxelGrid for Downsampling
LEAFSIZE = 0.01f;
vg.setLeafSize(LEAFSIZE, LEAFSIZE, LEAFSIZE);
// Any object < CUT_THRESHOLD will be abandoned.
//CUT_THRESHOLD = (int) (LEAFSIZE * LEAFSIZE * 7000000); // 700
CUT_THRESHOLD = 700; //for nonfiltering
// Clustering
cluster_.setClusterTolerance(0.06 * UNIT);
cluster_.setMinClusterSize(50.0);
cluster_.setSearchMethod(clusters_tree_);
// Normals
ne.setSearchMethod(tree);
ne.setKSearch(50); // 50 by default
// plane SAC
seg_plane.setOptimizeCoefficients(true);
seg_plane.setModelType(pcl::SACMODEL_NORMAL_PLANE);
seg_plane.setNormalDistanceWeight(0.1); // 0.1
seg_plane.setMethodType(pcl::SAC_RANSAC);
seg_plane.setMaxIterations(1000); // 10000
seg_plane.setDistanceThreshold(0.05); // 0.03
// cylinder SAC
seg_cylinder.setOptimizeCoefficients(true);
seg_cylinder.setModelType(pcl::SACMODEL_CYLINDER);
seg_cylinder.setMethodType(pcl::SAC_RANSAC);
seg_cylinder.setNormalDistanceWeight(0.1);
seg_cylinder.setMaxIterations(10000);
seg_cylinder.setDistanceThreshold(0.02); // 0.05
seg_cylinder.setRadiusLimits(0.02, 0.07); // [0, 0.1]
}
void initialize(bool toFilter) {
// A passthrough filter to remove spurious NaNs
pass.setInputCloud(cloud);
pass.filter(*cloud_filtered);
std::cout << "PointCloud after pass through has: " <<
cloud_filtered->points.size() << " data points." << std::endl;
// Downsample the dataset using a leaf size of 1cm
// After filtering the point cloud, all indices do not point to the
// original points. Therefore disable this if called from initializeFromPointCLoud
if (toFilter) {
vg.setInputCloud(cloud_filtered);
vg.filter(*cloud_filtered);
std::cout << "PointCloud after filtering has: " <<
cloud_filtered->points.size() << " data points." << std::endl;
}
// Estimate point normals
ne.setInputCloud(cloud_filtered);
ne.compute(*cloud_normals);
ROS_INFO("%lu normals estimated", cloud_normals->points.size());
}
void initialize(const std::string & pcd_name) {
// Read in the cloud data
reader.read(pcd_name, *cloud);
std::cerr << "PointCloud has: " << cloud->points.size() << " data points." << std::endl;
// pcl::io::loadPCDFile(pcd_name, *cloud);
// A passthrough filter to remove spurious NaNs
pass.setInputCloud(cloud);
pass.filter(*cloud_filtered);
std::cout << "PointCloud after filtering has: " <<
cloud_filtered->points.size() << " data points." << std::endl;
// Downsample the dataset using a leaf size of 1cm
vg.setInputCloud(cloud_filtered);
vg.filter(*cloud_filtered);
std::cout << "PointCloud after filtering has: " <<
cloud_filtered->points.size() << " data points." << std::endl;
// Estimate point normals
ne.setInputCloud(cloud_filtered);
ne.compute(*cloud_normals);
ROS_INFO("%lu normals estimated", cloud_normals->points.size());
}
void vad_cb(const sensor_msgs::PointCloud2ConstPtr& cloud) {
if ((cloud->width * cloud->height) == 0)
return;
//ROS_INFO ("Received %d data points in frame %s with the following fields: %s", (int)cloud->width * cloud->height, cloud->header.frame_id.c_str (), pcl::getFieldsList (*cloud).c_str ());
//std::cout << "fromROSMsg?" << std::endl;
pcl::fromROSMsg (*cloud, cloud_xyz_);
//std::cout << " fromROSMsg done." << std::endl;
t0 = my_clock();
if( limitPoint( cloud_xyz_, cloud_xyz, distance_th ) > 10 ){
//std::cout << " limit done." << std::endl;
std::cout << "compute normals and voxelize...." << std::endl;
//****************************************
//* compute normals
n3d.setInputCloud (cloud_xyz.makeShared());
n3d.setRadiusSearch (normals_radius_search);
normals_tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
n3d.setSearchMethod (normals_tree);
n3d.compute (cloud_normal);
pcl::concatenateFields (cloud_xyz, cloud_normal, cloud_xyz_normal);
t0_2 = my_clock();
//* voxelize
getVoxelGrid( grid, cloud_xyz_normal, cloud_downsampled, voxel_size );
std::cout << " ...done.." << std::endl;
const int pnum = cloud_downsampled.points.size();
float x_min = 10000000, y_min = 10000000, z_min = 10000000;
float x_max = -10000000, y_max = -10000000, z_max = -10000000;
for( int p=0; p<pnum; p++ ){
if( cloud_downsampled.points[ p ].x < x_min ) x_min = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y < y_min ) y_min = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z < z_min ) z_min = cloud_downsampled.points[ p ].z;
if( cloud_downsampled.points[ p ].x > x_max ) x_max = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y > y_max ) y_max = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z > z_max ) z_max = cloud_downsampled.points[ p ].z;
}
//std::cout << x_min << " " << y_min << " " << z_min << std::endl;
//std::cout << x_max << " " << y_max << " " << z_max << std::endl;
//std::cout << grid.getMinBoxCoordinates() << std::endl;
std::cout << "search start..." << std::endl;
//****************************************
//* object detection start
t1 = my_clock();
search_obj.cleanData();
search_obj.setGRSD( dim, grid, cloud_xyz_normal, cloud_downsampled, voxel_size, box_size );
t1_2 = my_clock();
if( ( search_obj.XYnum() != 0 ) && ( search_obj.Znum() != 0 ) )
search_obj.searchWithoutRotation();
t2 = my_clock();
//* object detection end
//****************************************
std::cout << " ...search done." << std::endl;
tAll += t2 - t0;
process_count++;
std::cout << "normal estimation :"<< t0_2 - t0 << " sec" << std::endl;
std::cout << "voxelize :"<< t1 - t0_2 << " sec" << std::endl;
std::cout << "feature extraction : "<< t1_2 - t1 << " sec" <<std::endl;
std::cout << "search : "<< t2 - t1_2 << " sec" <<std::endl;
std::cout << "all processes : "<< t2 - t0 << " sec" << std::endl;
std::cout << "AVERAGE : "<< tAll / process_count << " sec" << std::endl;
marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker_range", 1);
marker_array_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
visualization_msgs::MarkerArray marker_array_msg_;
//* show the limited space
visualization_msgs::Marker marker_;
marker_.header.frame_id = "openni_rgb_optical_frame";
marker_.header.stamp = ros::Time::now();
marker_.ns = "BoxEstimation";
marker_.id = -1;
marker_.type = visualization_msgs::Marker::CUBE;
marker_.action = visualization_msgs::Marker::ADD;
marker_.pose.position.x = (x_max+x_min)/2;
marker_.pose.position.y = (y_max+y_min)/2;
marker_.pose.position.z = (z_max+z_min)/2;
marker_.pose.orientation.x = 0;
marker_.pose.orientation.y = 0;
marker_.pose.orientation.z = 0;
marker_.pose.orientation.w = 1;
marker_.scale.x = x_max-x_min;
marker_.scale.y = x_max-x_min;
marker_.scale.z = x_max-x_min;
marker_.color.a = 0.1;
marker_.color.r = 1.0;
marker_.color.g = 0.0;
marker_.color.b = 0.0;
marker_.lifetime = ros::Duration();
marker_pub_.publish(marker_);
for( int q=0; q<rank_num; q++ ){
if( search_obj.maxDot( q ) < detect_th ) break;
std::cout << search_obj.maxX( q ) << " " << search_obj.maxY( q ) << " " << search_obj.maxZ( q ) << std::endl;
std::cout << "dot " << search_obj.maxDot( q ) << std::endl;
//if( (search_obj.maxX( q )!=0)||(search_obj.maxY( q )!=0)||(search_obj.maxZ( q )!=0) ){
//* publish marker
visualization_msgs::Marker marker_;
//marker_.header.frame_id = "base_link";
marker_.header.frame_id = "openni_rgb_optical_frame";
marker_.header.stamp = ros::Time::now();
marker_.ns = "BoxEstimation";
marker_.id = q;
marker_.type = visualization_msgs::Marker::CUBE;
marker_.action = visualization_msgs::Marker::ADD;
marker_.pose.position.x = search_obj.maxX( q ) * region_size + sliding_box_size_x/2 + x_min;
marker_.pose.position.y = search_obj.maxY( q ) * region_size + sliding_box_size_y/2 + y_min;
marker_.pose.position.z = search_obj.maxZ( q ) * region_size + sliding_box_size_z/2 + z_min;
marker_.pose.orientation.x = 0;
marker_.pose.orientation.y = 0;
marker_.pose.orientation.z = 0;
marker_.pose.orientation.w = 1;
marker_.scale.x = sliding_box_size_x;
marker_.scale.y = sliding_box_size_y;
marker_.scale.z = sliding_box_size_z;
marker_.color.a = 0.5;
marker_.color.r = 0.0;
marker_.color.g = 1.0;
marker_.color.b = 0.0;
marker_.lifetime = ros::Duration();
// std::cerr << "BOX MARKER COMPUTED, WITH FRAME " << marker_.header.frame_id << " POSITION: "
// << marker_.pose.position.x << " " << marker_.pose.position.y << " "
// << marker_.pose.position.z << std::endl;
// marker_pub_.publish (marker_);
// }
marker_array_msg_.markers.push_back(marker_);
}
//std::cerr << "MARKER ARRAY published with size: " << marker_array_msg_.markers.size() << std::endl;
marker_array_pub_.publish(marker_array_msg_);
}
std::cout << "Waiting msg..." << std::endl;
}