64,652
社区成员
发帖
与我相关
我的任务
分享
int main(int argc, char** argv){
std::vector<PCD, Eigen::aligned_allocator<PCD> > data;
loadData(argc, argv, data);
if(data.empty()){
PCL_ERROR("Syntax is: %s <source.pcd> <target.pcd> - ", argv
- [0]);
PCL_ERROR(" - - multiple files can be added. The registration results of
- (i,i+1) will be registered against (i+2). etc");
return -1;
}
PCL_INFO("Loaded %d datasets.", (int)data.size());
PCLVisualizer = new pcl::visualization::PCLVisualizer(argc, argv, "Pairwise Incremental Registration example");
PCLVisualizer->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
PCLVisualizer->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>), source, target;
Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity(), pairTransform;
for(size_t i=1; i<data.size();i++){
source = data[i-1].cloud;
target = data[i].cloud;
showCloudsLeft(source, target);
pcl::PointCloud<pcl::PointXYZ>::Ptr temp(new pcl::PointCloud<pcl::PointXYZ>);
PCL_INFO("Aligning %s (%d) with %s (%d).\n", data[i-1].f_name.c_str(), source->points.size(), data[i].f_name.c_str(), target->points.size());
pairAlign(source, target, temp, pairTransform, true);
std::cout << "pair registration finished" << endl; // 这一句无法输出说明上一步函数pairAlign存在问题
pcl::transformPointCloud(*temp, *result, GlobalTransform);
GlobalTransform = GlobalTransform * pairTransform;
std::stringstream ss;
ss << i << ".pcd";
pcl::io::savePCDFile(ss.str(), *result, true);
}
}
void pairAlign (const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src, const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt, pcl::PointCloud<pcl::PointXYZ>::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false){
//
// Downsample for consistency and speed
// \note enable this for large datasets
pcl::PointCloud<pcl::PointXYZ>::Ptr src(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr tgt(new pcl::PointCloud<pcl::PointXYZ>);
pcl::VoxelGrid<pcl::PointXYZ> grid;
if(downsample){
grid.setLeafSize(0.1, 0.1, 0.1);
grid.setInputCloud(cloud_src);
grid.filter(*src);
grid.setInputCloud(cloud_tgt);
grid.filter(*tgt);
}else{
src = cloud_src;
tgt = cloud_tgt;
}
pcl::PointCloud<pcl::PointNormal>::Ptr points_with_normals_src(new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointNormal>::Ptr points_with_normals_tgt(new pcl::PointCloud<pcl::PointNormal>);
pcl::NormalEstimation<pcl::PointXYZ,pcl::PointNormal> norm_est; //点云法线估计对象
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>());
norm_est.setSearchMethod(kdtree);
norm_est.setKSearch(30);
norm_est.setInputCloud(src);
norm_est.compute(*points_with_normals_src);
pcl::copyPointCloud(*src, *points_with_normals_src);
norm_est.setInputCloud(tgt);
norm_est.compute(*points_with_normals_tgt);
pcl::copyPointCloud(*tgt, *points_with_normals_tgt);
// 这是一个自己创建的点表达类
MyPointRepresentation point_representation;
float alpha[4] = {1.0, 1.0, 1.0, 1.0};
point_representation.setRescaleValues(alpha);
pcl::IterativeClosestPointNonLinear<pcl::PointNormal, pcl::PointNormal> reg;
reg.setTransformationEpsilon(1e-6);
reg.setMaxCorrespondenceDistance(0.05);
reg.setPointRepresentation(boost::make_shared<const MyPointRepresentation>(point_representation));
reg.setInputSource(points_with_normals_src);
reg.setInputTarget(points_with_normals_tgt);
Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity(), prev, targetToSource;
pcl::PointCloud<pcl::PointNormal>::Ptr reg_result = points_with_normals_src;
reg.setMaximumIterations(10);
for(int i=0; i<6; i++){
PCL_INFO("Iteration Nr. %d.\n", i);
points_with_normals_src = reg_result;
// Estimate
reg.setInputSource(points_with_normals_src);
reg.align(*reg_result);
Ti = reg.getFinalTransformation() * Ti;
if( fabs((reg.getLastIncrementalTransformation()-prev).sum())<reg.getTransformationEpsilon() ){
reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance()-0.01);
}
prev = reg.getLastIncrementalTransformation();
// 显示函数,函数的定义也保存在这个cpp文件中,声明保存在头文件中
showCloudsRight(points_with_normals_tgt, points_with_normals_src);
}
std::cout << "has converged:" << reg.hasConverged() << " score: " << reg.getFitnessScore() << std::endl;
targetToSource = Ti.inverse();
pcl::transformPointCloud(*cloud_tgt, *output, targetToSource);
// display
PCLVisualizer->removePointCloud ("source");
PCLVisualizer->removePointCloud ("target");
PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_tgt_h (output, 0, 255, 0);
PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_src_h (cloud_src, 255, 0, 0);
PCLVisualizer->addPointCloud (output, cloud_tgt_h, "target", vp_2);
PCLVisualizer->addPointCloud (cloud_src, cloud_src_h, "source", vp_2);
PCL_INFO ("Press q to continue the registration.\n");
PCLVisualizer->spin();
PCLVisualizer->removePointCloud ("source");
PCLVisualizer->removePointCloud ("target");
*output += *cloud_src;
std::cout << output->points.size() << " points have been merged into the source point cloud." << endl;
final_transform = targetToSource;
std::cout << "the transform matrix is:\n" << final_transform << endl; // 这一句可以运行,不报错。
// 这就很奇怪了,程序运行到了最后出现错误,第一次遇到这种问题,检查了函数的声明定义,应该没声没问题啊,难道时pcl点云指针在这儿释放了不能返回?
}