int main(int argc, char** argv)
{
Options opts;
if (options(argc, argv, opts))
return 1;
// Load stuff
PCL_INFO("Loading src_pd: %s\n", opts.src_pcd_file.c_str());
PointCloud::Ptr src_cloud(new PointCloud());
if (pcl::io::loadPCDFile(opts.src_pcd_file, *src_cloud) < 0)
{
std::cout << "Error loading input point cloud " << opts.src_pcd_file << std::endl;
return -1;
}
// Do downsampling
PointCloud::Ptr out_cloud(new PointCloud());
pcl::VoxelGrid<PointT> voxel_grid;
voxel_grid.setInputCloud(src_cloud);
// FIXME Should be an argument
voxel_grid.setLeafSize(opts.leaf_size, opts.leaf_size, opts.leaf_size);
voxel_grid.filter(*out_cloud);
// Save downsampled cloud
pcl::io::savePCDFileBinary(opts.out_pcd_file, *out_cloud);
PCL_INFO("Downsampled from %d to %d points\n", src_cloud->size(), out_cloud->size());
return 0;
}
void Transformation::show(bool pcd){
IplImage * dummy = getMatchImg();//cvCreateImage(cvSize(100, 100), IPL_DEPTH_8U, 3);
cvShowImage("dummy", dummy);
if(pcd){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr src_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
*src_cloud = src->input->getCloud();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr dst_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
*dst_cloud = dst->input->getCloud();
Eigen::Matrix4f src_mat = src->input->gt.matrix();
Eigen::Matrix4f dst_mat = dst->input->gt.matrix();
float scaleval = 1;
Eigen::Matrix4f scale = Eigen::Matrix4f::Identity()*scaleval;
Eigen::Matrix4f true_mat = src_mat.inverse() * dst_mat;
Eigen::Matrix4f trans_mat = transformationMatrix.inverse();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
addToPCD(255,0 ,0, cloud, src_cloud, Eigen::Matrix4f::Identity());
addToPCD(0,255,0, cloud, dst_cloud, trans_mat);
if(!viewer){
viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->initCameraParameters ();
}
if(viewer->wasStopped ()){
viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->initCameraParameters ();
}
viewer->removePointCloud("sample cloud");
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.1, "sample cloud");
viewer->addCoordinateSystem (scaleval*0.2);
while (!viewer->wasStopped () && cvWaitKey(10) == -1){
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
viewer->close();
}else{cvWaitKey(0);}
cvReleaseImage( &dummy);
}
int main(int argc, char** argv)
{
Options opts;
if (options(argc, argv, opts))
return 1;
// Load stuff
PCL_INFO("Loading src_pd: %s\n", opts.src_pcd_file.c_str());
pcl::PointCloud<pcl::PointXYZI>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZI>());
if (pcl::io::loadPCDFile(opts.src_pcd_file, *src_cloud) < 0)
{
std::cout << "Error loading input point cloud " << opts.src_pcd_file << std::endl;
return -1;
}
// Do normal estimation
PointCloudWithNormals::Ptr out_cloud(new PointCloudWithNormals);
pcl::NormalEstimation<pcl::PointXYZI, pcl::PointXYZINormal> norm_est;
pcl::search::KdTree<pcl::PointXYZI>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZI>());
norm_est.setSearchMethod(tree);
norm_est.setKSearch(opts.k_search);
norm_est.setInputCloud(src_cloud);
// Only outputs the normals
norm_est.compute (*out_cloud);
// This copies over the XYZ coordinates
pcl::copyPointCloud (*src_cloud, *out_cloud);
// Save cloud with normals
pcl::io::savePCDFileBinary(opts.out_pcd_file, *out_cloud);
return 0;
}
int main(int argc, char** argv)
{
Options opts;
if (options(argc, argv, opts))
return 1;
// Load stuff
Eigen::MatrixXf ldr_data;
readLDRFile(opts.src_ldr_file, ldr_data);
std::cout << "rows: " << ldr_data.rows() << " cols: " << ldr_data.cols() << std::endl;
std::cout << ldr_data.block<10, 6>(0, 0) << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>());
ldr_to_cloud(ldr_data, *src_cloud);
std::cout << "Cloud size: " << src_cloud->size() << std::endl;
/*
PCL_INFO("Loading src_pcd: %s\n", opts.src_pcd_file.c_str());
pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>());
if (pcl::io::loadPCDFile(opts.src_pcd_file, *src_cloud) < 0)
{
std::cout << "Error loading input point cloud " << opts.src_pcd_file << std::endl;
return -1;
}
*/
// Do upsampling
pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls_upsampling;
// Set up KD-tree
// TODO Weight intensity
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
// Set parameters
// FIXME PARAMS
mls_upsampling.setInputCloud(src_cloud);
mls_upsampling.setComputeNormals (true);
mls_upsampling.setPolynomialFit (true);
mls_upsampling.setSearchMethod (tree);
mls_upsampling.setSearchRadius (0.25);
//mls_upsampling.setUpsamplingMethod (pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::VOXEL_GRID_DILATION);
//mls_upsampling.setUpsamplingMethod (pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::SAMPLE_LOCAL_PLANE);
mls_upsampling.setUpsamplingMethod (pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::NONE);
mls_upsampling.setUpsamplingRadius (0.25);
mls_upsampling.setUpsamplingStepSize (0.125);
//mls_upsampling.setDilationVoxelSize(0.25);
//mls_upsampling.setDilationIterations(1);
mls_upsampling.setPointDensity(1);
NormalCloud::Ptr out_cloud(new NormalCloud());
std::cout << "Running upsampling" << std::endl;
mls_upsampling.process(*out_cloud);
std::cout << "Finished upsampling" << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>());
pcl::copyPointCloud(*out_cloud, *out_cloud_xyz);
// TODO For each point in upsampled cloud, assign it the
// time, laser num, intensity of the closet point in
// the original cloud
// TODO Will have to read in original ldr file to do this
//pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2;
Eigen::MatrixXf out_matrix(out_cloud_xyz->size(), 6);
for (int k = 0; k < out_cloud_xyz->size(); k++)
{
pcl::PointXYZ pt = out_cloud_xyz->at(k);
// FIXME Fill in with intensity, laser
out_matrix.row(k) << (float)pt.x, (float)pt.y, (float)pt.z, 0.0f, 0.0f, 0.0f;
}
// Save upsampled cloud
if (opts.out_pcd_file != "")
pcl::io::savePCDFileBinary(opts.out_pcd_file, *out_cloud);
// Save ldr file
if (opts.out_ldr_file != "")
writeLDRFile(opts.out_ldr_file, out_matrix);
PCL_INFO("Upsampled from %d to %d points\n", src_cloud->size(), out_cloud->size());
return 0;
}
