void give_out (header *hd)
/***** give_out
print a value.
*****/
{ switch(hd->type)
{ case s_real : double_out(*realof(hd)); output("\n"); break;
case s_complex : complex_out(*realof(hd),*imagof(hd));
output("\n"); break;
case s_matrix : out_matrix(hd); break;
case s_cmatrix : out_cmatrix(hd); break;
case s_imatrix : out_imatrix(hd); break;
case s_string : output(stringof(hd)); output("\n"); break;
case s_interval : interval_out(*aof(hd),*bof(hd));
output("\n"); break;
default : output("?\n");
}
}
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;
}
