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

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

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

代码

工程使用VS2013update5，pcl1.8.0

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

结果