int main(int argc,char**argv)
{
signal(SIGINT,sigint_handler);
srand(time(NULL));
int r_ = rand()%255;int g_ = rand()%255;int b_ = rand()%255;
int i=0;
kinect::Kinect2Grabber obj;
kinect::FrameData data;
//std::vector<int> compress;
//compress.push_back(CV_IMWRITE_JPEG_QUALITY );
//compress.push_back(100);
//compress.push_back(CV_IMWRITE_PNG_COMPRESSION);
//compress.push_back(2);
cv::namedWindow( "registered",CV_WINDOW_AUTOSIZE );
//cv::namedWindow( "Depth",CV_WINDOW_AUTOSIZE );
//cv::namedWindow( "Ir",CV_WINDOW_AUTOSIZE );
data = obj.getRegisteredImage();
//data = obj.getFrameData();
pcl::PointCloud<PointT>::Ptr cloud_(new pcl::PointCloud<PointT>());
pcl::PointCloud<PointT>::Ptr cloud_segment(new pcl::PointCloud<PointT>());
cloud_->width = data.depth_.cols;
cloud_->height = data.depth_.rows;
cloud_->points.resize(data.depth_.rows*data.depth_.cols);
pcl::visualization::PCLVisualizer* viewer(new pcl::visualization::PCLVisualizer ("Kinect Cloud"));
//pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud_);
viewer->addPointCloud<PointT>(cloud_,"Kinect Cloud");
//pcl::visualization::PointCloudColorHandlerCustom <PointT> color_handle(cloud_segment,r_,g_,b_);
//viewer->addPointCloud<PointT>(cloud_segment,"Kinect Cloud");
viewer->setBackgroundColor (0, 0, 0);
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3, "Kinect Cloud");
//viewer->addCoordinateSystem (1.0);
viewer->initCameraParameters ();
/*
//segment
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<PointT> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud_);
*/
while(key!=99)
{
data = obj.getRegisteredImage();
//data = obj.getFrameData();
cv::imshow("registered",data.color_);
//cv::imshow("Depth",data.depth_);
//cv::imshow("Ir",data.IR_);
key = cv::waitKey(10);
libfreenect2::Freenect2Device::IrCameraParams params = obj.getIrParams();
std::cout<<"Camera centre : "<<params.cx<<" "<<params.cy<<std::endl;
std::cout<<"Focal parameters : "<<params.fx<<" "<<params.fy<<std::endl;
int size_r = data.color_.rows;
int size_c = data.color_.cols;
int index = 0;
#pragma omp parallel for
for(int i=0;i<size_r;i++)
{
for(int j=0;j<size_c;j++,index++)
{
float depth_pos = data.depth_.at<float>(i,j);
float pos_x = (float)((j-params.cx)*depth_pos)/params.fx;
float pos_y = (float)((i-params.cy)*depth_pos)/params.fy;
cloud_->points[index].x = pos_x;
cloud_->points[index].y = pos_y;
cloud_->points[index].z = depth_pos;
const cv::Vec3b tmp = data.color_.at<cv::Vec3b>(i,j);
cloud_->points[index].b = tmp.val[0];
cloud_->points[index].g = tmp.val[1];
cloud_->points[index].r = tmp.val[2];
}
}
/*
if(segment){
seg.segment (*inliers, *coefficients);
for(unsigned int i=0;i<inliers->indices.size();i++)
{
cloud_->points[inliers->indices[i]].r = 255;
cloud_->points[inliers->indices[i]].g = 0;
cloud_->points[inliers->indices[i]].b = 0;
}
}
*/
///////////////////////////////////////////////
//Multi Plane Segmentation
///////////////////////////////////////////////
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (cloud_);
ne.compute (*normal_cloud);
float* distance_map = ne.getDistanceMap ();
boost::shared_ptr<pcl::EdgeAwarePlaneComparator<PointT,pcl::Normal> > eapc(new pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal> ());
eapc->setDistanceMap (distance_map);
eapc->setDistanceThreshold (0.01f, false);
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
std::vector<pcl::ModelCoefficients> model_coefficients;
std::vector<pcl::PointIndices> inlier_indices;
pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
std::vector<pcl::PointIndices> label_indices;
std::vector<pcl::PointIndices> boundary_indices;
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud_);
mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
//////////////////view contours of segmented planes////////////////////////////
if(contours_only)
{
std::vector<PointT,Eigen::aligned_allocator<PointT> > c_points;
int index_s=0;
unsigned int size_region = regions.size();
long int cloud_size;
for(unsigned int i=0;i<size_region;i++)
{
cloud_size+=regions[i].getContour().size();
}
cloud_segment->points.resize(cloud_size);
for(unsigned int i=0;i<regions.size();i++)
{
c_points = regions[i].getContour();
r_ = rand()%255;g_ = rand()%255;b_ = rand()%255;
for(unsigned int j=0;j<c_points.size();j++,index_s++)
{
cloud_segment->points[index_s].x = c_points[j].x;
cloud_segment->points[index_s].y = c_points[j].y;
cloud_segment->points[index_s].z = c_points[j].z;
cloud_segment->points[index_s].r = r_;
cloud_segment->points[index_s].g = g_;
cloud_segment->points[index_s].b = b_;
}
}
}
//////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//Using boundary indices
///////////////////////////////////////////////////////////////////////////////
else{
/*
for(unsigned int i=0;i<boundary_indices.size();i++)
{
r_ = rand()%255;g_ = rand()%255;b_ = rand()%255;
for(unsigned int j=0;j<boundary_indices[i].indices.size();j++)
{
cloud_->points[boundary_indices[i].indices[j]].r = r_;
cloud_->points[boundary_indices[i].indices[j]].g = g_;
cloud_->points[boundary_indices[i].indices[j]].b = b_;
}
}
*/
for(unsigned int i=0;i<label_indices.size();i++)
{
r_ = rand()%255;g_ = rand()%255;b_ = rand()%255;
for(unsigned int j=0;j<label_indices[i].indices.size();j++)
{
cloud_->points[label_indices[i].indices[j]].r = r_;
cloud_->points[label_indices[i].indices[j]].g = g_;
cloud_->points[label_indices[i].indices[j]].b = b_;
}
}
}
//viewer->updatePointCloud(cloud_segment,"Kinect Cloud");
viewer->updatePointCloud(cloud_,"Kinect Cloud");
viewer->spinOnce (100);
if(print&&(key==99)){
cloud_->height = 1;
cloud_->width = cloud_->points.size();
pcl::io::savePCDFileASCII ("test_new.pcd",*cloud_);
print = false;
}
}
return 0;
}
void
cloud_cb_white (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
// Container for original & filtered data
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_segment (new pcl::PointCloud<pcl::PointXYZ>);
// Convert to PCL data type
pcl_conversions::toPCL(*cloud_msg, *cloud);
// Perform downsampling
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPtr);
sor.setLeafSize (0.01, 0.01, 0.01);
sor.filter (cloud_filtered);
// convert PointCloud2 to pcl::PointCloud<pcl::PointXYZ>
pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(cloud_filtered,*temp_cloud);
// segmenting area
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (temp_cloud);
// left - right
pass.setFilterFieldName ("x");
pass.setFilterLimits (-3, 3);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_segment);
pass.setInputCloud (cloud_segment);
// up - down
pass.setFilterFieldName ("y");
pass.setFilterLimits (-3, 3);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_segment);
pass.setInputCloud (cloud_segment);
// backward - forward
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.5, 4);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_segment);
// // Find centroid
float sum_x = 0;
float sum_y = 0;
float sum_z = 0;
if(cloud_segment->points.size() > 50) {
for(size_t i = 0; i < cloud_segment->points.size(); ++i) {
sum_x += cloud_segment->points[i].x;
sum_y += cloud_segment->points[i].y;
sum_z += cloud_segment->points[i].z;
}
sum_x = sum_x / cloud_segment->points.size();
sum_y = sum_y / cloud_segment->points.size();
sum_z = sum_z / cloud_segment->points.size();
geometry_msgs::PoseStamped sPose;
sPose.header.stamp = ros::Time::now();
sPose.pose.position.x = sum_z; // equals to z in pcl // backward - forward
sPose.pose.position.y = -(sum_x); // equals to -x in pcl // right - left
sPose.pose.position.z = -(sum_y); // equals to -y in pcl // down - up
sPose.pose.orientation.x = 0.0;
sPose.pose.orientation.y = 0.0;
sPose.pose.orientation.z = 0.0;
sPose.pose.orientation.w = 1.0;
cock_pose_white_pub.publish(sPose);
}
// Convert to ROS data type
sensor_msgs::PointCloud2 output;
//pcl_conversions::fromPCL(cloud_filtered, output);
pcl::toROSMsg(*cloud_segment, output);
// Publish the data
cloud_pub_white.publish (output);
}
