template <typename PointT> void
pcl::UnaryClassifier<PointT>::segment (pcl::PointCloud<pcl::PointXYZRGBL>::Ptr &out)
{
if (trained_features_.size () > 0)
{
// convert cloud into cloud with XYZ
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
convertCloud (input_cloud_, tmp_cloud);
// compute FPFH feature histograms for all point of the input point cloud
pcl::PointCloud<pcl::FPFHSignature33>::Ptr input_cloud_features (new pcl::PointCloud<pcl::FPFHSignature33>);
computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
// query the distances from the input data features to all trained features
std::vector<int> indices;
std::vector<float> distance;
queryFeatureDistances (trained_features_, input_cloud_features, indices, distance);
// assign a label to each point of the input point cloud
int n_feature_means = static_cast<int> (trained_features_[0]->points.size ());
convertCloud (input_cloud_, out);
assignLabels (indices, distance, n_feature_means, feature_threshold_, out);
//std::cout << "Assign labels - DONE" << std::endl;
}
else
PCL_ERROR ("no training features set \n");
}
//グリッド化
void make_elevation(object_map& m)
{
std::cout << "make_elevation" << std::endl;
double max_x = -10.0; double max_y = -10.0; double max_z = -10.0;
double min_x = 10.0; double min_y = 10.0; double min_z = 10.0;
int int_max_x = -100; int int_min_x = 100; int int_max_y = -100; int int_min_y = 100;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
*tmp_cloud = m.cloud_rgb;
for(int i=0;i<tmp_cloud->size();i++){
if(tmp_cloud->points[i].x > max_x) max_x = tmp_cloud->points[i].x;
if(tmp_cloud->points[i].x < min_x) min_x = tmp_cloud->points[i].x;
tmp_cloud->points[i].x = (int)(tmp_cloud->points[i].x*100);
if(tmp_cloud->points[i].y > max_y) max_y = tmp_cloud->points[i].y;
if(tmp_cloud->points[i].y < min_y) min_y = tmp_cloud->points[i].y;
tmp_cloud->points[i].y = (int)(tmp_cloud->points[i].y*100);
if(tmp_cloud->points[i].z > max_z) max_z = tmp_cloud->points[i].z;
if(tmp_cloud->points[i].z < min_z) min_z = tmp_cloud->points[i].z;
tmp_cloud->points[i].z = (int)(tmp_cloud->points[i].z*100);
}
m.max_x = max_x; m.min_x = min_x; m.max_y = max_y; m.min_y = min_y; m.max_z = max_z; m.min_z = min_z;
int_min_x = 100*min_x; int_min_y = 100*min_y;
int_max_x = 100*max_x; int_max_y = 100*max_y;
downsampling(*tmp_cloud, 1.0f);
m.size_x = int_max_x - int_min_x + 1;
m.size_y = int_max_y - int_min_y + 1;
// std::cout << m.min_x << " " << m.max_x << std::endl;
// std::cout << m.min_y << " " << m.max_y << std::endl;
// std::cout << m.size_x << " " << m.size_y << std::endl;
for(int i=0;i<MAP_X;i++){
for(int j=0;j<MAP_Y;j++){
m.map[i][j] = 0.0;
}
}
for(int i=0;i<tmp_cloud->size();i++){
if(tmp_cloud->points[i].z > m.map[(int)tmp_cloud->points[i].x-int_min_x][(int)tmp_cloud->points[i].y-int_min_y])
m.map[(int)tmp_cloud->points[i].x-int_min_x][(int)tmp_cloud->points[i].y-int_min_y] = tmp_cloud->points[i].z;
}
return;
}
template <typename PointT> void
pcl::UnaryClassifier<PointT>::train (pcl::PointCloud<pcl::FPFHSignature33>::Ptr &output)
{
// convert cloud into cloud with XYZ
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
convertCloud (input_cloud_, tmp_cloud);
// compute FPFH feature histograms for all point of the input point cloud
pcl::PointCloud<pcl::FPFHSignature33>::Ptr feature (new pcl::PointCloud<pcl::FPFHSignature33>);
computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
//PCL_INFO ("Number of input cloud features: %d\n", static_cast<int> (feature->points.size ()));
// use k-means to cluster the features
kmeansClustering (feature, output, cluster_size_);
}
void addToPCD(float r, float g, float b, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2, Matrix4f trans){
pcl::PointCloud<pcl::PointXYZRGB> cloud_trans;
pcl::transformPointCloud (*cloud2, cloud_trans, trans);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
*tmp_cloud = cloud_trans;
pcl::VoxelGrid<pcl::PointXYZRGB> sor;
sor.setLeafSize (0.001f, 0.001f, 0.001f);
sor.setInputCloud (tmp_cloud);
pcl::PointCloud<pcl::PointXYZRGB> voxelcloud_trans;
sor.filter(voxelcloud_trans);
for(unsigned int i = 0; i < voxelcloud_trans.points.size(); i++){
voxelcloud_trans.points.at(i).r = r;
voxelcloud_trans.points.at(i).g = g;
voxelcloud_trans.points.at(i).b = b;
}
*cloud1 += voxelcloud_trans;
}
void ConvexConnectedVoxels::segmentCloud(
const pcl::PointCloud<PointT>::Ptr cloud,
const std::vector<pcl::PointIndices> &indices,
std::vector<pcl::PointCloud<PointT>::Ptr> &cloud_clusters,
std::vector<pcl::PointCloud<pcl::Normal>::Ptr> &normal_clusters,
pcl::PointCloud<pcl::PointXYZ>::Ptr centroids)
{
boost::mutex::scoped_lock lock(this->mutex_);
pcl::ExtractIndices<PointT>::Ptr eifilter(
new pcl::ExtractIndices<PointT>);
eifilter->setInputCloud(cloud);
for (int i = 0; i < indices.size(); i++) {
pcl::PointIndices::Ptr index(new pcl::PointIndices());
index->indices = indices[i].indices;
eifilter->setIndices(index);
pcl::PointCloud<PointT>::Ptr tmp_cloud(
new pcl::PointCloud<PointT>);
eifilter->filter(*tmp_cloud);
if (tmp_cloud->width > 0) {
Eigen::Vector4f centroid;
pcl::compute3DCentroid<PointT, float>(*cloud, *index, centroid);
float ct_x = static_cast<float>(centroid[0]);
float ct_y = static_cast<float>(centroid[1]);
float ct_z = static_cast<float>(centroid[2]);
if (!isnan(ct_x) && !isnan(ct_y) && !isnan(ct_z)) {
pcl::PointCloud<pcl::Normal>::Ptr s_normal(
new pcl::PointCloud<pcl::Normal>);
this->estimatePointCloudNormals(
tmp_cloud, s_normal, 40, 0.05, false);
normal_clusters.push_back(s_normal);
centroids->push_back(pcl::PointXYZ(ct_x, ct_y, ct_z));
cloud_clusters.push_back(tmp_cloud);
}
}
}
}
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
// Updating parameters from the parameter server
ros::param::getCached("/PCL_object_clustering/cluster_tolerans",cluster_tolerans);
ros::param::getCached("/PCL_object_clustering/cluster_size/min",cluster_size_min);
ros::param::getCached("/PCL_object_clustering/cluster_size/max",cluster_size_max);
ros::param::getCached("/PCL_object_clustering/clusters_highest_point",clusters_highest_point);
// Converting from PointCloud2 msg to pcl::PointCloud
pcl::PCLPointCloud2 pcl_pc2;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_without_planes (new pcl::PointCloud<pcl::PointXYZ>);
pcl_conversions::toPCL(*cloud_msg,pcl_pc2);
pcl::fromPCLPointCloud2(pcl_pc2,*cloud_without_planes);
if(!(*cloud_without_planes).empty()){
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_without_planes);
// Getting indeces for each found cluster with parameters cluster_tolerans,
// cluster_size_min and cluster_size_max
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (cluster_tolerans);
ec.setMinClusterSize (cluster_size_min);
ec.setMaxClusterSize (cluster_size_max);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_without_planes);
ec.extract (cluster_indices);
/* For each cluster we store cluster in tmp_cloud, calculate its centroids.
If cluster highest point is less than clusters_highest_point we publish
centroid and append tmp_cloud to cloud_cluster. When we checked through all
possible centroids we publish cloud_cluster to visualize in rviz.*/
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit){
tmp_cloud->points.push_back (cloud_without_planes->points[*pit]);
}
pcl::PointXYZ min_point, max_point;
pcl::getMinMax3D(*tmp_cloud,min_point,max_point);
if(max_point.z < clusters_highest_point){
Eigen::Vector4f c;
pcl::compute3DCentroid(*tmp_cloud, c);
//std::cout << "Centroid is " << c << std::endl;
object_detecter_2d::object_loc object_loc_msg;
object_loc_msg.ID = 10;
object_loc_msg.point.x = c(0);
object_loc_msg.point.y = c(1);
object_loc_msg.point.z = c(2);
pub_centroid.publish(object_loc_msg);
(*cloud_cluster) = (*cloud_cluster)+(*tmp_cloud);
}
}
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
(*cloud_cluster).header.frame_id = (*cloud_without_planes).header.frame_id;
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud_cluster, output);
pub_debug_pcl.publish(output);
}
}
//***********************************************************
// segmentation
// セグメンテーションを行う．
//***********************************************************
void segmentation(pcl::PointCloud<pcl::PointXYZRGB>& cloud, double th, int c)
{
std::cout << "segmentation" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
for(int i=0;i<cloud.points.size();i++){
if((m_size.max_z <= cloud.points[i].z) && (cloud.points[i].z <= m_size.max_z+0.3)){
tmp_cloud->points.push_back(cloud.points[i]);
}
}
pcl::copyPointCloud(*tmp_cloud, cloud);
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud.makeShared ());
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (th);
ec.setMinClusterSize (200);
ec.setMaxClusterSize (300000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud.makeShared ());
ec.extract (cluster_indices);
std::cout << "クラスタリング開始" << std::endl;
int m = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZHSV>::Ptr cloud_cluster_hsv (new pcl::PointCloud<pcl::PointXYZHSV>);
pcl::PointXYZ g;
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++){
cloud_cluster_rgb->points.push_back (cloud.points[*pit]);
}
cloud_cluster_rgb->width = cloud_cluster_rgb->points.size ();
cloud_cluster_rgb->height = 1;
cloud_cluster_rgb->is_dense = true;
//クラスタ重心を求める
g = g_pos(*cloud_cluster_rgb);
if(((g.x < m_size.min_x) || (m_size.max_x < g.x)) ||
((g.y < m_size.min_y) || (m_size.max_y < g.y))){
continue;
}
m++;
if((m_size.max_z + 0.02 < g.z) && (g.z < m_size.max_z + 0.12)){
object_map map;
map.cloud_rgb = *cloud_cluster_rgb;
map.g = g;
map.tf = c;
make_elevation(map);
// RGB -> HSV
pcl::PointCloudXYZRGBtoXYZHSV(*cloud_cluster_rgb, *cloud_cluster_hsv);
for(int i=0;i<MAP_H;i++){
for(int j=0;j<MAP_S;j++){
map.histogram[i][j] = 0.000;
}
}
map.cloud_hsv = *cloud_cluster_hsv;
// H-Sヒストグラムの作成
for(int i=0;i<cloud_cluster_hsv->points.size();i++){
if(((int)(255*cloud_cluster_hsv->points[i].h)/((256/MAP_H)*360) >= 32) || ((int)(255*cloud_cluster_hsv->points[i].h)/((256/MAP_H)*360) < 0)) continue;
if(((int)(255*cloud_cluster_hsv->points[i].s)/(256/MAP_H) >= 32) || ((int)(255*cloud_cluster_hsv->points[i].s)/(256/MAP_H) < 0)) continue;
// 正規化のため,セグメントの点数で割る.
map.histogram[(int)(255*cloud_cluster_hsv->points[i].h)/((256/MAP_H)*360)][(int)(255*cloud_cluster_hsv->points[i].s)/(256/MAP_H)] += 1.000/(float)cloud_cluster_hsv->points.size();
}
Object_Map.push_back(map);
*tmp_rgb += *cloud_cluster_rgb;
}
}
pcl::copyPointCloud(*tmp_rgb, cloud);
return;
}
// Separate into separate clouds and publish polygons
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > // use jsk_recognition_msgs::PointsArray
separate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_rot, std_msgs::Header header) {
double x_pitch = 0.25, x_min = 1.0, x_max = 3.0; // 1.5~1.75 1.75~2.00 1.5~1.675
double y_min = -0.75, y_max = 0.75;
double z_min = -0.250, z_1 = 0.000, z_2 = 1.000, z_max = 1.750; // -0.3125, 2.0
pcl::PointXYZ pt_1, pt_2, pt_3, pt_4, pt_5, pt_6; // deprecate with polygon
// Divide large cloud
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cloud_vector;
// pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
// pcl::PointXYZ tmp_p;
jsk_recognition_msgs::PolygonArray polygon_array;
polygon_array.header = header;
for (int i = 0; i < (int)( (x_max - x_min) / x_pitch ); i++) {
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
geometry_msgs::PolygonStamped polygon;
visualization_msgs::Marker texts, line_strip; // TEXT_VIEW_FACING
texts.header = header;
texts.ns = "text"; // namespace + ID
texts.action = visualization_msgs::Marker::ADD;
texts.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
texts.pose.orientation.x = 0.0;
texts.pose.orientation.y = 0.0;
texts.pose.orientation.z = 0.0;
texts.pose.orientation.w = 1.0;
texts.scale.x = 0.125;
texts.scale.y = 0.125;
texts.scale.z = 0.125;
texts.color.r = 1.0f;
texts.color.g = 0.0f;
texts.color.b = 0.0f;
texts.color.a = 1.0;
geometry_msgs::Point32 tmp_p_up_0, tmp_p_up_1, tmp_p_up_2, tmp_p_down_0, tmp_p_down_1, tmp_p_down_2;
pcl::PointXYZ tmp_p;
double width_tmp, width_min_up = 2.000, width_min_down = 4.000;
double width_min_bottom = 0.500, width_min_top = 0.200;
for (pcl::PointCloud<pcl::PointXYZ>::const_iterator itr = cloud_xyz_rot->begin();
itr != cloud_xyz_rot->end(); itr++) {
if ( (x_min + i*x_pitch) < itr->x && itr->x < (x_min + (i+1)*x_pitch) ) {
if (y_min < itr->y && itr->y < y_max) {
if (z_min < itr->z && itr->z < z_max) {
// compare tmp_p and itr, and calculate width and points
if (itr != cloud_xyz_rot->begin()) { // skip at 1st time
if ( (tmp_p.y < 0 && 0 <= itr->y) || (itr->y < 0 && 0 <= tmp_p.y) ) {
if (itr->z < z_1) {
width_tmp = sqrt(pow(fabs(tmp_p.x - itr->x), 2)
+ pow(fabs(tmp_p.y - itr->y), 2)
+ pow(fabs(tmp_p.z - itr->z), 2));
if (width_min_bottom < width_tmp && width_tmp <= width_min_down) {
width_min_down = width_tmp; // create width_min array
tmp_p_down_0.x = tmp_p.x; tmp_p_down_0.y = tmp_p.y;
tmp_p_down_0.z = (tmp_p.z + itr->z) / 2;
tmp_p_down_1.x = itr->x; tmp_p_down_1.y = itr->y;
tmp_p_down_1.z = (tmp_p.z + itr->z) / 2;
tmp_p_down_2.x = tmp_p.x; // ignore adding sqrt
tmp_p_down_2.y = tmp_p.y + sqrt(pow(fabs(tmp_p.y - itr->y), 2)) / 2;
tmp_p_down_2.z = (tmp_p.z + itr->z) / 2;
}
}
if (z_2 < itr->z) {
width_tmp = sqrt(pow(fabs(tmp_p.x - itr->x), 2)
+ pow(fabs(tmp_p.y - itr->y), 2)
+ pow(fabs(tmp_p.z - itr->z), 2));
if (width_tmp <= width_min_down) {
width_min_up = width_tmp;
tmp_p_up_0.x = tmp_p.x; tmp_p_up_0.y = tmp_p.y;
tmp_p_up_0.z = (tmp_p.z + itr->z) / 2;
tmp_p_up_1.x = itr->x; tmp_p_up_1.y = itr->y;
tmp_p_up_1.z = (tmp_p.z + itr->z) / 2;
tmp_p_up_2.x = tmp_p.x; // ignore adding sqrt
tmp_p_up_2.y = tmp_p.y + sqrt(pow(fabs(tmp_p.y - itr->y), 2)) / 2;
tmp_p_up_2.z = (tmp_p.z + itr->z) / 2;
}
}
}
tmp_p.x = itr->x; tmp_p.y = itr->y; tmp_p.z = itr->z;
tmp_cloud->points.push_back(tmp_p);
}
}
}
}
// From tmp_cloud, get 4 points to publish marker
// Create polygon
}
cloud_vector.push_back(tmp_cloud);
tmp_p_up_0.x = x_min + i*x_pitch - x_pitch/2;
tmp_p_up_1.x = x_min + i*x_pitch - x_pitch/2;
tmp_p_down_0.x = x_min + i*x_pitch - x_pitch/2;
tmp_p_down_1.x = x_min + i*x_pitch - x_pitch/2;
if (tmp_p_up_0.y < tmp_p_up_1.y) {
polygon.polygon.points.push_back(tmp_p_up_0);
polygon.polygon.points.push_back(tmp_p_up_1);
}
if (tmp_p_up_0.y >= tmp_p_up_1.y) {
polygon.polygon.points.push_back(tmp_p_up_1);
polygon.polygon.points.push_back(tmp_p_up_0);
}
if (tmp_p_down_0.y < tmp_p_down_1.y) {
polygon.polygon.points.push_back(tmp_p_down_1);
polygon.polygon.points.push_back(tmp_p_down_0);
}
if (tmp_p_down_0.y >= tmp_p_down_1.y) {
polygon.polygon.points.push_back(tmp_p_down_0);
polygon.polygon.points.push_back(tmp_p_down_1);
}
polygon.header = header;
polygon_array.polygons.push_back(polygon);
std::cerr << "count:" << i << ", " << "size:" << cloud_vector.at(i)->size() << std::endl;
std::cerr << "width_min_up:" << width_min_up << std::endl;
std::cerr << "width_min_down:" << width_min_down << std::endl;
texts.id = 2*i;
texts.pose.position.x = tmp_p_up_0.x;
texts.pose.position.y = tmp_p_up_2.y;
texts.pose.position.z = tmp_p_up_2.z;
std::ostringstream strs;
strs << width_min_up;
std::string str = strs.str();
texts.text = str;
pub_marker.publish(texts);
texts.id = 2*i + 1;
texts.pose.position.x = tmp_p_down_0.x;
texts.pose.position.y = tmp_p_down_2.y;
texts.pose.position.z = tmp_p_down_2.z;
strs.str("");
strs.clear(std::stringstream::goodbit);
strs << width_min_down;
str = strs.str();
texts.text = str;
pub_marker.publish(texts);
}
pub_polygon_array.publish(polygon_array); // error
return cloud_vector;
}
