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();
}
