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