SAC-IA算法实现

第一次尝试，记录自己的学习历程。
在学习点云配准的过程中，对于局部点云与全局点云的配准，发现SAC-IA配准算法的效果相对较好，所以自己尝试实现SAC-IA算法。当然其中基础部分、法线和FPFH特征描述子的计算还是直接调用pcl.

算法流程

首先简单介绍算法的实现流程：

1. 分别计算源点云和目标点云的FPFH特征描述子；
2. 基于FPFH特征描述子对两个点云中的点进行匹配；
3. 随机选择 n (n >= 3) 对匹配点；
4. 通过SVD求解该匹配情况下的旋转与位移；
5. 计算此时对应的误差；
6. 重复步骤３－５，直到满足条件，将最小误差对应的旋转和位移作为最终结果。
   可以看出上述步骤基本就是RANSAC算法的思想。下面结合代码详细分析每个步骤。

FPFH描述子的计算

这直接调用pcl，没啥好说的，直接上代码。所使用点云的分辨率为0.2m.

pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
norm_est.setSearchMethod(tree);
norm_est.setRadiusSearch(0.4f);
fpfh_est.setSearchMethod(tree);
fpfh_est.setRadiusSearch(0.4f);
// 源点云
pcl::PointCloud<pcl::Normal>::Ptr source_normal(new pcl::PointCloud<pcl::Normal>);
norm_est.setInputCloud(source);
norm_est.compute(*source_normal);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr source_fpfh(new pcl::PointCloud<pcl::FPFHSignature33>);
fpfh_est.setInputCloud(source);
fpfh_est.setInputNormals(source_normal);
fpfh_est.compute(*source_fpfh);
// 目标点云
pcl::PointCloud<pcl::Normal>::Ptr target_normal(new pcl::PointCloud<pcl::Normal>);
norm_est.setInputCloud(target);
norm_est.compute(*target_normal);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr target_fpfh(new pcl::PointCloud<pcl::FPFHSignature33>);
fpfh_est.setInputCloud(target);
fpfh_est.setInputNormals(target_normal);
fpfh_est.compute(*target_fpfh);

描述子匹配

该匹配的规则为每个源点云中的点匹配ｋ个目标点云中的点，即描述子的ｋ近邻。定义了如下的结构体用来保存匹配情况。

struct corrs{
	int source_index;　　　　　　　// 源点云中点的索引
	// double max_distance;
	vector<int> target_index;    // 目标点云中点的索引
};

在kd树的基础上实现的，效率高。将目标点云的FPFH描述子构建成 kd 树，再对源点云中的每个点的FPFH特征描述子进行近邻点查询。

pcl::search::KdTree<pcl::FPFHSignature33> fpfh_tree;
fpfh_tree.setInputCloud(target_fpfh);
vector<int> index(k);
vector<float> distance(k);
int len = source_fpfh->size();
vector<corrs> matches(len);

for(size_t i = 0; i < len; i++){
	fpfh_tree.nearestKSearch(*source_fpfh, i, k, index, distance);
	matches[i].source_index = i;
	matches[i].target_index = index;
	// matches[i].max_distance = distance[k - 1];
}

选择匹配点

匹配点随机选择，但也有些限制条件。随机选择源点云中的点时，要求这三个点之间有一定距离。再对选出来的三个点，在各自匹配的ｋ个目标点云点中随机选择一个作为匹配点，这样就能得到了三对匹配点。代码如下，

vector<int> sample_index;
int id = rand() % len;
sample_index.push_back(id);
// 随机选择源点云中的点
int cnt = 0;
while(sample_index.size() < sample_size){
	bool valid_sample = true;
	id = rand() % len;
	for(size_t i = 0; i < sample_index.size(); i++){
		float distance = point_distance(source->at(matches[id].source_index), source->at(matches[sample_index[i]].source_index));
		if(distance < min_distance){
			valid_sample = false;
			cnt++;
			break;
		}
	}
	if(valid_sample){
		sample_index.push_back(id);
		cnt = 0;
	}
	if(cnt > 100) break;      // 若循环100次还找不到符合要求的点，则直接退出，重新选点
}
if(sample_index.size() < sample_size) continue;
// 随机选择匹配点
vector<vector<int> > final_match;
final_match.push_back(sample_index);
final_match.push_back(sample_index);
for(size_t i = 0; i < sample_index.size(); i++){
	int id = rand() % k;
	final_match[1][i] = matches[sample_index[i]].target_index[id];
}

计算旋转与位移

根据选出的三对匹配点计算相应的旋转与位移，此处通过SVD求解旋转与位移。代码参考了《视觉SLAM十四讲》第７讲中的SVD方法。

vector<pcl::PointXYZ> pt1;
vector<pcl::PointXYZ> pt2;
for(size_t i = 0; i < sample_size; i++){
	pt1.push_back(target->at(final_match[1][i]));
	pt2.push_back(source->at(matches[final_match[0][i]].source_index));
}
// 计算匹配点的中心
pcl::PointXYZ p1(0, 0, 0);
pcl::PointXYZ p2(0, 0, 0);
for(size_t i = 0; i < sample_size; i++){
	p1.x += pt1[i].x; p1.y += pt1[i].y; p1.z += pt1[i].z;
	p2.x += pt2[i].x; p2.y += pt2[i].y; p2.z += pt2[i].z;
}
p1.x /= sample_size; p1.y /= sample_size; p1.z /= sample_size;
p2.x /= sample_size; p2.y /= sample_size; p2.z /= sample_size;

vector<pcl::PointXYZ> q1(sample_size), q2(sample_size);
for(size_t i = 0; i < sample_size; i++){
	q1[i].x = pt1[i].x - p1.x; q1[i].y = pt1[i].y - p1.y; q1[i].z = pt1[i].z - p1.z;
	q2[i].x = pt2[i].x - p2.x; q2[i].y = pt2[i].y - p2.y; q2[i].z = pt2[i].z - p2.z;
}

Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for(size_t i = 0; i < sample_size; i++)
	W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
// SVD
Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();

Eigen::Matrix3d R = U * V.transpose();
Eigen::Vector3d t = Eigen::Vector3d(p1.x, p1.y, p1.z) - R * Eigen::Vector3d(p2.x, p2.y, p2.z);
Eigen::Isometry3d T (R);
T.pretranslate (t);

误差计算

对于每次迭代得到的旋转与位移，需要计算相应的误差，根据误差选择较好的结果。

pcl::transformPointCloud(*source, *new_source, T.matrix());
float error = computeError(new_source, target);

其中computerError()函数的具体如下，与pcl中的应该有些差异。

float computeError(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target){
    float error = 0;
    int cnt = 0;
    pcl::KdTreeFLANN<pcl::PointXYZ> tree;
    tree.setInputCloud(target);

    vector<int> nn_index(1);
    vector<float> nn_distance(1);

    for(int i = 0; i < source->size(); ++i){
        tree.nearestKSearch(*source, i, 1, nn_index, nn_distance);
        if(nn_distance[0] > 1.0) continue;     // 对于距离太远的点，则将其排除误差，此处需要结合点云分辨率设定阈值
        error += nn_distance[0];
        cnt++;
    }

    return error / cnt;
}

实验结果

完整程序运行结果与直接调用pcl库存在一定差异，首先运行时间相对稍长一些，运行时间多出5%~15%，最终的误差也不一样。考虑到其中存在随机因素以及计算误差的函数存在差异，所以没有深究。以下图片展示了上述代码运行后得到的效果。