void main2(int argc, char* argv[]){
double m_voxel_leah_size = 0.005;
f1 = path_txt + argv[2] + ".txt";
fply1 = path_ply + argv[2] + ".ply";
fply2 = path_ply + argv[2] + "_new.ply";
fply3 = path_ply + argv[2] + "_filter_new.ply";
PCloud cloud;
PCloud cloud_f(new Cloud);
PCloud cloud_filter(new Cloud);
LoadCloudFromTXT(f1.data(), cloud);
start = ros::Time::now();
pcl::VoxelGrid<pcl::PointXYZRGB> grid;
grid.setLeafSize(m_voxel_leah_size,m_voxel_leah_size,m_voxel_leah_size);
grid.setInputCloud(cloud);
grid.filter(*cloud_f);
end = ros::Time::now();
double t1=(end-start).toSec();
ROS_INFO("Downsampling: %f",t1);
start = ros::Time::now();
pcl::PassThrough<pcl::PointXYZRGB> filter;
grid.setInputCloud(cloud_f);
grid.filter(*cloud_filter);
end = ros::Time::now();
double t2=(end-start).toSec();
ROS_INFO("filter: %f", t2);
ROS_INFO("Down+filter: %f", t1+t2);
PrintCloudToPLY(fply1.data(), cloud);
PrintCloudToPLY(fply2.data(), cloud_f);
PrintCloudToPLY(fply3.data(), cloud_filter);
std::cout <<"Number of points in cloud " << cloud->size() << std::endl;
std::cout <<"Number of points in cloud_f " << cloud_f->size() << std::endl;
std::cout <<"Number of points in cloud_filter " << cloud_filter->size() << std::endl;
}
/*call back function for subscriber*/
void preprocessCallback (const sensor_msgs::PointCloud2ConstPtr pc){
//void preprocessCallback (pcl::PCLPointCloud2::Ptr pc){
static pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_integrate (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_rev (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filter (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2::Ptr cloud_pc2(new pcl::PCLPointCloud2);
// static pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_integrate (new pcl::PointCloud<pcl::PointXYZ>);
// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_rev (new pcl::PointCloud<pcl::PointXYZ>);
// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filter (new pcl::PointCloud<pcl::PointXYZ>);
// pcl::PCLPointCloud2::Ptr cloud;
sensor_msgs::PointCloud2 cloud;
pcl_conversions::toPCL(*pc, *cloud_pc2);
if(cloud_pc2!=NULL)
pcl::fromPCLPointCloud2 (*cloud_pc2, *cloud_rev); //* convert from pcl/PCLPointCloud2 to pcl::PointCloud<T>
// pcl::fromROSMsg(pc,cloud_integrate);
// pcl::concatenatePointCloud(cloud_integrate, cloud_rev, cloud_integrate)
// static pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2());
*cloud_integrate = *cloud_integrate+ *cloud_rev;
msgcount++;
if(msgcount == 800) {// (2 topics, each with data rate at 40hz, integrate for 20s)
//build voxelization filter
ROS_INFO("20s (1600 point cloud) past!");
// pcl::VoxelGrid<pcl::PointXYZ> sor;
// sor.setInputCloud (cloud_integrate);
// sor.setLeafSize(0.05,0.05,0.05);
// sor.filter(*cloud_filter);
// pcl::toPCLPointCloud2(*cloud_filter, *cloud_pc2);
pcl::toPCLPointCloud2(*cloud_integrate, *cloud_pc2);
pcl_conversions::fromPCL(*cloud_pc2,cloud);
cloud.header.frame_id=pc->header.frame_id;
publisherPointer->publish(cloud);
// msgcount = 0;
}
}
