int main (int argc, char** argv) { if (argc != 3) { ROS_INFO_STREAM("please provide a pointcloud file followed by a text file containing a transformation matrix as arguments"); exit(0); } pcl::PCDReader reader; pcl::PointCloud<PointType>::Ptr cloudIn (new pcl::PointCloud<PointType>); pcl::PointCloud<PointType>::Ptr cloudOut (new pcl::PointCloud<PointType>); pcl::PointCloud<PointType>::Ptr cloudOut_inv (new pcl::PointCloud<PointType>); Eigen::Matrix4f trafo, trafo_inv; reader.read (argv[1], *cloudIn); std::ifstream myfile; myfile.open (argv[2]); for (int row = 0; row < 4; row++) for (int col = 0; col < 4; col++) { myfile >> trafo (row, col); } trafo_inv = trafo.inverse(); ROS_INFO_STREAM("transform to be used: \n" << trafo); transformPointCloud (*cloudIn, *cloudOut, trafo); transformPointCloud (*cloudIn, *cloudOut_inv, trafo_inv); pcl::PCDWriter writer; writer.write ("output.pcd", *cloudOut, false); writer.write ("output_inverse.pcd", *cloudOut_inv, false); return (0); }
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud) { if (!viewer.wasStopped ()) { // stringstream to store compressed point cloud std::stringstream compressedData; // output pointcloud pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOut (new pcl::PointCloud<pcl::PointXYZRGBA> ()); // compress point cloud PointCloudEncoder->encodePointCloud (cloud, compressedData); // decompress point cloud PointCloudDecoder->decodePointCloud (compressedData, cloudOut); // show decompressed point cloud viewer.showCloud (cloudOut); } }
int main(int argc, char** argv) { if (argc != 3) { std::cerr << "please provide 2 point clouds as arguments: <source>.pcd <target>.pcd)" << std::endl; exit(0); } PointCloud::Ptr cloudSource(new PointCloud); PointCloud::Ptr cloudOut(new PointCloud); PointCloud::Ptr cloudTarget(new PointCloud); PointCloud::Ptr cloudSourceFiltered(new PointCloud); PointCloud::Ptr cloudTargetFiltered(new PointCloud); PointCloudNormal::Ptr cloudSourceNormal(new PointCloudNormal); PointCloudNormal::Ptr cloudTargetNormal(new PointCloudNormal); PointCloudNormal::Ptr cloudHandlesSource(new PointCloudNormal); PointCloudNormal::Ptr cloudHandlesTarget(new PointCloudNormal); PointCloudNormal::Ptr cloudSourceNormalNoNaNs(new PointCloudNormal); PointCloudNormal::Ptr cloudTargetNormalNoNaNs(new PointCloudNormal); pcl::PCDWriter writer; std::vector<int> indicesSource, indicesTarget; //Fill in the cloud data ROS_INFO("Reading files...."); pcl::PCDReader reader; reader.read(argv[1], *cloudSource); reader.read(argv[2], *cloudTarget); //pcl::copyPointCloud (*cloudSourceNormal, *cloudSource); //pcl::copyPointCloud (*cloudTargetNormal, *cloudTarget); normalEstimation(cloudSource, cloudSourceNormal, 0.03); normalEstimation(cloudTarget, cloudTargetNormal, 0.03); std::vector<int> sourceHandleClusters; std::vector<int> targetHandleClusters; ROS_INFO("Extracting handles...."); extractHandles(cloudSource, cloudSourceFiltered, cloudSourceNormal, sourceHandleClusters); extractHandles(cloudTarget, cloudTargetFiltered, cloudTargetNormal, targetHandleClusters); std::vector<int> removedPoints; removeNaNs(cloudSourceNormal, cloudSourceNormalNoNaNs, removedPoints); adjustIndicesFromRemovedPoints(sourceHandleClusters, removedPoints); removeNaNs(cloudTargetNormal, cloudTargetNormalNoNaNs, removedPoints); adjustIndicesFromRemovedPoints(targetHandleClusters, removedPoints); writer.write("handlesSource.pcd", *cloudSourceNormalNoNaNs, sourceHandleClusters, true); writer.write("handlesTarget.pcd", *cloudTargetNormalNoNaNs, targetHandleClusters, true); ROS_INFO("Calculating inital transformation from ICP SVD on handles...."); Eigen::Matrix4f guess; guess << 1, 0, 0, 0.0, 0, 1, 0, 0.5, 0, 0, 1, 0.33, 0, 0, 0, 1; pcl::IterativeClosestPoint<PointNormal, PointNormal> icp; // set source and target clouds from indices of pointclouds pcl::ExtractIndices<PointNormal> handlefilter; pcl::PointIndices::Ptr sourceHandleIndices (new pcl::PointIndices); sourceHandleIndices->indices = sourceHandleClusters; handlefilter.setIndices(sourceHandleIndices); handlefilter.setInputCloud(cloudSourceNormalNoNaNs); handlefilter.filter(*cloudHandlesSource); icp.setInputCloud(cloudHandlesSource); pcl::PointIndices::Ptr targetHandleIndices (new pcl::PointIndices); targetHandleIndices->indices = targetHandleClusters; handlefilter.setIndices(targetHandleIndices); handlefilter.setInputCloud(cloudTargetNormalNoNaNs); handlefilter.filter(*cloudHandlesTarget); icp.setInputTarget(cloudHandlesTarget); PointCloudNormal Final; icp.align(Final, guess); std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl; std::cout << icp.getFinalTransformation() << std::endl; ROS_INFO("Initialize transformation estimation object...."); boost::shared_ptr< TransformationEstimationJointOptimize<PointNormal, PointNormal > > transformationEstimation_(new TransformationEstimationJointOptimize<PointNormal, PointNormal>()); float denseCloudWeight = 1.0; float visualFeatureWeight = 0.0; float handleFeatureWeight = 0.25; transformationEstimation_->setWeights(denseCloudWeight, visualFeatureWeight, handleFeatureWeight); transformationEstimation_->setCorrespondecesDFP(indicesSource, indicesTarget); // Eigen::Matrix4f guess; // guess << 0.999203, 0.0337772, -0.0213298, 0.0110106, // -0.0349403, 0.99778, -0.0567293, -0.00746282, // 0.0193665, 0.0574294, 0.998162, 0.0141431, // 0, 0, 0, 1; // guess << 1, 0.2, 0.3, -0.3, // -0.2, 1, -0.2, 0.9, // -0.3, 0.2, 1, 0.4, // 0, 0, 0, 1; // guess << 1, 0, 0, 0.0, // 0, 1, 0, 0.5, // 0, 0, 1, 0.33, // 0, 0, 0, 1; // icp svd for handles node_1/node_91 // guess << 0.993523, 0.0152363, -0.112636, 0.138385, // -0.0264756, 0.994739, -0.0989777, 0.615225, // 0.110535, 0.101318, 0.988696, 0.347863, // 0, 0, 0, 1; // custom icp ROS_INFO("Initialize icp object...."); pcl::IterativeClosestPointJointOptimize<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> icpJointOptimize; //JointOptimize icpJointOptimize.setMaximumIterations (20); icpJointOptimize.setTransformationEpsilon (0); icpJointOptimize.setMaxCorrespondenceDistance(0.1); icpJointOptimize.setRANSACOutlierRejectionThreshold(0.03); icpJointOptimize.setEuclideanFitnessEpsilon (0); icpJointOptimize.setTransformationEstimation (transformationEstimation_); icpJointOptimize.setHandleSourceIndices(sourceHandleClusters); icpJointOptimize.setHandleTargetIndices(targetHandleClusters); icpJointOptimize.setInputCloud(cloudSourceNormalNoNaNs); icpJointOptimize.setInputTarget(cloudTargetNormalNoNaNs); ROS_INFO("Running ICP...."); PointCloudNormal::Ptr cloud_transformed( new PointCloudNormal); icpJointOptimize.align ( *cloud_transformed, icp.getFinalTransformation()); //init_tr ); std::cout << "[SIIMCloudMatch::runICPMatch] Has converged? = " << icpJointOptimize.hasConverged() << std::endl << " fitness score (SSD): " << icpJointOptimize.getFitnessScore (1000) << std::endl << icpJointOptimize.getFinalTransformation () << "\n"; transformPointCloud (*cloudSource, *cloudOut, icpJointOptimize.getFinalTransformation()); ROS_INFO("Writing output...."); writer.write("converged.pcd", *cloudOut, true); }