PCL实现SAC-IA点云初始配准

随机采样一致(ransac)算法是一种随机性的参数估计算法,利用Ransac可以实现采样一致性初始配准(SAC-IA)。采样一致性初始配准(SAC-IA)给出一个初始预估的刚性(Rigid)变换矩阵,为更精确的位姿估计算法(如ICP等)提供初始配准状态。

PCL点云库中封装了丰富的点云数据处理函数,利用PCL实现初始点云配准较为容易,于是博主就也实验了一下,效果还不错,就是时间耗费的问题,大概2000个点,迭代2000次,花了5s,实在是太慢了,代码托管在github,比较初级,欢迎指教。

代码

工程使用VS2013update5,pcl1.8.0

GitHub 仓库挂件 WordPress 插件

P-Chao / SAC-IA

Using PCL (Point Cloud Library) to Registration Point Cloud By SAC-IA, Initial Alignment

https://github.com/P-Chao/SAC-IA/releases

v1.0 只使用了SAC-IA 做初始配准

v2.0 后面还使用了ICP做更精细的配准,可能由于点云数据的关系,看不出什么效果

解释

首先对点云进行降采样,然后计算法向量,根据法向量和点云来计算特征,然后通过特征来进行初始点云配准。

在使用博主代码时,如果采用自己的点云数据,需要注意修改参数,各参数的含义看名字就应该能懂,工程架构直接看主函数就可以,比较基础。

const double FILTER_LIMIT = 1000.0;
const int MAX_SACIA_ITERATIONS = 2000;

const float VOXEL_GRID_SIZE = 3;
const double NORMALS_RADIUS = 20;
const double FEATURES_RADIUS = 50;
const double SAC_MAX_CORRESPONDENCE_DIST = 2000;
const double SAC_MIN_CORRESPONDENCE_DIST = 3;

int main(int argc, char *argv[]){
	time_t starttime = time(NULL);
	cout << "Loading clouds...\n";
	cout.flush();

	// open the clouds
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);
	
	pcl::io::loadPCDFile("pcd/a3.pcd", *cloud1);
	pcl::io::loadPCDFile("pcd/a4.pcd", *cloud2);

	// downsample the clouds
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1ds(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2ds(new pcl::PointCloud<pcl::PointXYZ>);

	voxelFilter(cloud1, cloud1ds, VOXEL_GRID_SIZE);
	voxelFilter(cloud2, cloud2ds, VOXEL_GRID_SIZE);

	// compute normals
	pcl::PointCloud<pcl::Normal>::Ptr normals1 = getNormals(cloud1ds, NORMALS_RADIUS);
	pcl::PointCloud<pcl::Normal>::Ptr normals2 = getNormals(cloud2ds, NORMALS_RADIUS);

	// compute local features
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr features1 = getFeatures(cloud1ds, normals1, FEATURES_RADIUS);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr features2 = getFeatures(cloud2ds, normals2, FEATURES_RADIUS);

	// 
	auto sac_ia = align(cloud1ds, cloud2ds, features1, features2, 
		MAX_SACIA_ITERATIONS, SAC_MIN_CORRESPONDENCE_DIST, SAC_MAX_CORRESPONDENCE_DIST);
	
	Eigen::Matrix4f init_transform = sac_ia.getFinalTransformation();
	pcl::transformPointCloud(*cloud2, *cloud2, init_transform);
	pcl::PointCloud<pcl::PointXYZ> final = *cloud1;
	final += *cloud2;

	cout << init_transform << endl;
	cout << "done. Time elapsed: " << time(NULL) - starttime << " seconds\n";
	cout.flush();

	viewPair(cloud1ds, cloud2ds, cloud1, cloud2);
	pcl::io::savePCDFile("result.pcd", final);
	//view(final);
	return 0;
}

结果

《PCL实现SAC-IA点云初始配准》有9条评论

  1. 博主,不知是否可以看一看你做的ICP配准代码?我是一个初学者,对很多东西不是很了解 谢谢你

    回复
  2. 您好,下载了您的代码v1.0,但是会显示如下报错,

    严重性 代码 说明 项目 文件 行 禁止显示状态
    错误 MSB4019 找不到导入的项目“C:\Program Files\PCL 1.8.0\PCL.Release.x64.props”。请确认 Import 声明“C:\Program Files\PCL 1.8.0\PCL.Release.x64.props”中的表达式正确,且文件位于磁盘上。

    找了下,您的代码里面好像没有PCL的属性表,这个我想用的话需要怎么做呀?麻烦您解答一下

    回复
  3. 严重性 代码 说明 项目 文件 行 禁止显示状态
    错误(活动) E0312 不存在用户定义的从 “boost::shared_ptr” 到 “const std::shared_ptr<const pcl::PointRepresentation>” 的适当转换 PointCloudRegistration C:\Users\cl\Desktop\current\Master’s Thesis\CodeReference\SAC-IA-2.0\PointCloudRegistration\registration.cpp 57

    您好,用了您的v2.0的代码,有这个报错,Google百度都没找到原因,烦请您看看

    回复

回复 PengChao 取消回复