int main(int argc, char** argv) { int scene_num = atoi(argv[2]); sensor_msgs::PointCloud2 cloud_blob; pcl::PointCloud<PointT> cloud; std::ofstream labelfile, nfeatfile, efeatfile; if (pcl::io::loadPCDFile(argv[1], cloud_blob) == -1) { ROS_ERROR("Couldn't read file test_pcd.pcd"); return (-1); } ROS_INFO("Loaded %d data points from test_pcd.pcd with the following fields: %s", (int) (cloud_blob.width * cloud_blob.height), pcl::getFieldsList(cloud_blob).c_str()); // convert to templated message type pcl::fromROSMsg(cloud_blob, cloud); pcl::PointCloud<PointT>::Ptr cloud_ptr(new pcl::PointCloud<PointT > (cloud)); pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT > ()); pcl::PointCloud<PointT>::Ptr cloud_seg(new pcl::PointCloud<PointT > ()); //pcl::PointCloud<PointXYZI>::Ptr cloud_seg (new pcl::PointCloud<PointXYZI> ()); std::vector<pcl::PointCloud<PointT> > segment_clouds; std::map<int,int> segment_num_index_map; pcl::PointIndices::Ptr segment_indices(new pcl::PointIndices()); // get segments std::set<int> myset; // find the max segment number int max_segment_num = 0; for (size_t i = 0; i < cloud.points.size(); ++i) { if ( cloud.points[i].label>0) { myset.insert(cloud.points[i].segment); } } set<int>::iterator it; SpectralProfile temp; //vector<SpectralProfile> spectralProfiles; for (it=myset.begin(); it!=myset.end(); it++) { apply_segment_filter_and_compute_HOG (*cloud_ptr,*cloud_seg,*it,temp); cerr<<*it<<"\t"<<cloud_seg->points[1].label<<"\t"<<cloud_seg->size()<<endl; //if (label!=0) cout << "segment: "<< seg << " label: " << label << " size: " << cloud_seg->points.size() << endl; } }
void getSegmentDistanceToBoundary( const pcl::PointCloud<PointT> &cloud , map<int,float> &segment_boundary_distance){ pcl::PointCloud<PointT>::Ptr cloud_rest(new pcl::PointCloud<PointT > ()); pcl::PointCloud<PointT>::Ptr cloud_cam(new pcl::PointCloud<PointT > ()); pcl::PointCloud<PointT>::Ptr cloud_seg(new pcl::PointCloud<PointT > ()); pcl::PointCloud<PointT>::Ptr cloud_ptr(new pcl::PointCloud<PointT > (cloud)); int cnt =0; // find all the camera indices// find the max segment number map<int,int> camera_indices; for (size_t i = 0; i < cloud.points.size(); ++i) { camera_indices[(int) cloud.points[i].cameraIndex] = 1; } // for every camera index .. apply filter anf get the point cloud for (map<int,int>::iterator it = camera_indices.begin(); it != camera_indices.end();it++) { int ci = (*it).first; apply_camera_filter(*cloud_ptr,*cloud_cam,ci); // find the segment list map<int,int> segments; for (size_t i = 0; i < cloud_cam->points.size(); ++i) { if( cloud_cam->points[i].label != 0) segments[(int) cloud_cam->points[i].segment] = 1; } for (map<int,int>::iterator it2 = segments.begin(); it2 != segments.end();it2++){ cnt++; int segnum = (*it2).first; apply_segment_filter(*cloud_cam,*cloud_seg,segnum); apply_notsegment_filter(*cloud_cam,*cloud_rest,segnum); float bdist = getDistanceToBoundary(*cloud_seg,*cloud_rest); map<int , float>::iterator segit = segment_boundary_distance.find(segnum); if(segit== segment_boundary_distance.end() || bdist> segment_boundary_distance[segnum] ) segment_boundary_distance[segnum] = bdist; // if(cnt == 1) outcloud = *cloud_seg; // else outcloud += *cloud_seg; } } //for(map<int,float>::iterator it = ) }
void PlaneSegmentationPclRgbAlgorithm::getBiggestPlaneInliersDownsampling(pcl::PointIndices::Ptr inliers, pcl::ModelCoefficients::Ptr coefficients, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_seg (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr ground_hull (new pcl::PointCloud<pcl::PointXYZ>); // downsampling pcl::VoxelGrid<pcl::PointXYZ> grid_objects_; grid_objects_.setLeafSize (pointcloud_downsample_size, pointcloud_downsample_size, pointcloud_downsample_size); grid_objects_.setDownsampleAllData (false); grid_objects_.setInputCloud (cloud); grid_objects_.filter (*cloud_downsampled); // segment plane if (choose_plane_by_distance) getNearestBigPlaneInliers(inliers, coefficients, cloud_downsampled); else getBiggestPlaneInliers(inliers, coefficients, cloud_downsampled); // check if the plane exists if (inliers->indices.size() == 0) { // if plane doesn't exist a black image will be returned ROS_WARN_STREAM("Plane segmentation: couldn't find plane."); return; } // copy inliers pcl::ProjectInliers<pcl::PointXYZ> proj; proj.setModelType(pcl::SACMODEL_PLANE); proj.setInputCloud(cloud_downsampled); proj.setModelCoefficients(coefficients); proj.setIndices (inliers); proj.filter(*cloud_seg); // remove NaN pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (cloud_seg); pass.filter (*cloud_seg); // get biggest cluster if (plane_clustering) cloud_seg = getBiggestClusterPC(cloud_seg); // Create a Convex Hull representation of the projected inliers pcl::ConvexHull<pcl::PointXYZ> chull; chull.setInputCloud(cloud_seg); chull.setDimension(2); chull.reconstruct(*ground_hull); // Extract only those outliers that lie inside the ground plane's convex hull pcl::PointIndices::Ptr object_indices (new pcl::PointIndices); pcl::ExtractPolygonalPrismData<pcl::PointXYZ> hull_limiter; hull_limiter.setInputCloud(cloud); hull_limiter.setInputPlanarHull(ground_hull); hull_limiter.setHeightLimits(plane_min_height, plane_max_height); hull_limiter.segment(*object_indices); *inliers = *object_indices; }
void PlaneSegmentationPclRgbAlgorithm::getNearestBigPlaneInliers(pcl::PointIndices::Ptr inliers, pcl::ModelCoefficients::Ptr coefficients, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_orig) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_seg (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointIndices::Ptr inliers_first (new pcl::PointIndices ()); pcl::PointIndices::Ptr inliers_second (new pcl::PointIndices ()); pcl::ModelCoefficients::Ptr coefficients_first (new pcl::ModelCoefficients); pcl::ModelCoefficients::Ptr coefficients_second (new pcl::ModelCoefficients); pcl::ExtractIndices<pcl::PointXYZ> extract; float dist1, dist2; pcl::PointXYZ p_orig = pcl::PointXYZ(0,0,0); ROS_DEBUG("Get nearest big plane."); // Get biggest plane getBiggestPlaneInliers(inliers_first, coefficients_first, cloud_orig); // check if first plane exists if (inliers_first->indices.size () == 0) // no plane return; //dist1 = euclideanDistance(cloud_orig->at(inliers_first->indices[0]), p_orig); dist1 = pointToPlaneDistance(p_orig, coefficients_first->values); // Remove plane from pointcloud extract.setInputCloud (cloud_orig); extract.setIndices (inliers_first); extract.setNegative (true); extract.filter (*cloud_seg); // Get second biggest plane getBiggestPlaneInliers(inliers_second, coefficients_second, cloud_seg); // check if the second plane exists if (inliers_second->indices.size () == 0) { ROS_INFO_STREAM("Plane segmentation: couldn't find a second plane. Choosing biggest one."); *inliers = *inliers_first; *coefficients = *coefficients_first; return; } //dist2 = euclideanDistance(cloud_seg->at(inliers_second->indices[0]), p_orig); dist2 = pointToPlaneDistance(p_orig, coefficients_second->values); ROS_DEBUG_STREAM("Dist 1 is "<<dist1); ROS_DEBUG_STREAM("Dist 2 is "<<dist2); // if first is nearest get its inliers if ( ( (dist1 < dist2) && choose_nearest_plane ) || ( (dist1 > dist2) && !choose_nearest_plane ) ) { ROS_DEBUG("Chose biggest plane."); *inliers = *inliers_first; *coefficients = *coefficients_first; } else { // else inliers would be of the second plane ROS_DEBUG("Chose second biggest plane."); *inliers = *inliers_second; *coefficients = *coefficients_second; *cloud_orig = *cloud_seg; } }
void point_cb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg){ pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); pcl::PCLPointCloud2 cloud_filtered; pcl_conversions::toPCL(*cloud_msg, *cloud); pcl::VoxelGrid<pcl::PCLPointCloud2> sor; sor.setInputCloud(cloudPtr); float leaf = 0.005; sor.setLeafSize(leaf, leaf, leaf); sor.filter(cloud_filtered); sensor_msgs::PointCloud2 sensor_pcl; pcl_conversions::moveFromPCL(cloud_filtered, sensor_pcl); //publish pcl data pub_voxel.publish(sensor_pcl); global_counter++; if((theta == 0.0 && y_offset == 0.0) || global_counter < 800 ){ // part for planar segmentation starts here .. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_seg(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_rotated(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_transformed(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>); sensor_msgs::PointCloud2 plane_sensor, plane_transf_sensor; //convert sen pcl::fromROSMsg(*cloud_msg, *cloud1); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::SACSegmentation<pcl::PointXYZ> seg; seg.setOptimizeCoefficients(true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.01); seg.setInputCloud(cloud1); seg.segment (*inliers, *coefficients); Eigen::Matrix<float, 1, 3> surface_normal; Eigen::Matrix<float, 1, 3> floor_normal; surface_normal[0] = coefficients->values[0]; surface_normal[1] = coefficients->values[1]; surface_normal[2] = coefficients->values[2]; std::cout << surface_normal[0] << "\n" << surface_normal[1] << "\n" << surface_normal[2]; floor_normal[0] = 0.0; floor_normal[1] = 1.0; floor_normal[2] = 0.0; theta = acos(surface_normal.dot(floor_normal)); //extract the inliers - copied from tutorials... pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(cloud1); extract.setIndices (inliers); extract.setNegative(true); extract.filter(*cloud_p); ROS_INFO("print cloud info %d", cloud_p->height); pcl::toROSMsg(*cloud_p, plane_sensor); pub_plane_simple.publish(plane_sensor); // anti rotate the point cloud by first finding the angle of rotation Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity(); transform_1.translation() << 0.0, 0.0, 0.0; transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX())); pcl::transformPointCloud(*cloud_p, *cloud_p_rotated, transform_1); double y_sum = 0; // int num_points = for (int i = 0; i < 20; i++){ y_sum = cloud_p_rotated->points[i].y; } y_offset = y_sum / 20; Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity(); transform_2.translation() << 0.0, -y_offset, 0.0; transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX())); pcl::transformPointCloud(*cloud_p, *cloud_p_transformed, transform_2); pcl::transformPointCloud(*cloud1, *cloud_transformed, transform_2); //now remove the y offset because of the camera rotation pcl::toROSMsg(*cloud_p_transformed, plane_transf_sensor); // pcl::toROSMsg(*cloud_transformed, plane_transf_sensor); // pcl::toROSMsg(*cloud1, plane_transf_sensor); pub_plane_transf.publish(plane_transf_sensor); } ras_msgs::Cam_transform cft; cft.theta = theta; cft.y_offset = y_offset; pub_ctf.publish(cft); // pub_tf.publish(); }