简介
在 [代码实践] 使用 PCA 进行平面和线特征拟合 中，我简单整理了一下如何使用 PCA 来进行线特征和平面特征的参数拟合。这种方法通常在我们已经获取了该特征对应的数据时使用，如果数据集中含有大量不属于该特征的数据，这些数据不同于噪声，称为外点（离群点），会对主成分分析造成障碍。因此，这篇博客整理如何使用 RANSAC 来进行含大量外点的鲁棒参数估计。
RANSAC
基本思想
关于 RANSAC (Random Sample consensus，随机样本一致性) 的原理推导网上有很多资源，这里不进行详细说明，主要过一下它的基本思想已经进行过程，RANSAC 基于以下假设：
- 给定数据中包含能够被一个参数模型描述的数据点（内点，inliers），这些数据点可以不是真值（包含噪声）
- 给定数据值包含不能被模型描述的离群点（outliers）
例如，针对三维点云数据，如果想进行地面拟合，那么点云中属于地面的点为内点，虽然由于噪声的原因，这些点不一定完美符合一个平面。而其他不属于地面（立杆，墙壁等）则属于外点。基于这个假设，RANSAC 的基本流程很简单，可以分为两步（获取随机样本，判断一致性）：
- （获取随机样本）从数据集中随机选取一个较小的子集，进行模型拟合，获得一组模型参数。注意，选取子集中数据点的数量至少要满足模型拟合的要求（对于直线至少要有两个点，对于平面至少要有三个点）
- （判断模型一致性）对整个数据集中所有点进行根据上一步计算出来的模型进行误差比较，如果误差小于一定阈值内则认为该点符合估计的模型
- 如果符合模型的点数超过当前找到的最优模型的内点数量，则更新模型和内点，迭代结束返回估计的模型和内点
从执行步骤中，不难看出 RANSAC 的几个特点：
- RANSAC 有可能在误差点较多时也能够拟合出正确的模型，因为本质上模型的拟合只取决于选取的一小部分数据子集，只要该子集中含有足够数量的内点即可（但内点被选中的概率随着外点的数量增加而减少）
- RANSAC 不保证一定能拟合出正确的模型，从执行过程中可以看出，（在原始算法内）不同次迭代之间的模型拟合没有任何联系，模型的估计只跟随机选取的子集有关系。理论上当迭代次数无限时，确实会选中包含大部分内点的子集，但在有限次迭代时不能够保证这一点（因为这个原因所以内点的数量不能过少）。
- 当数据集中有不止一组数据都能够满足给定参数模型，RANSAC 会随机拟合出其中一组参数，例如，当点云数据包含墙壁和地面，而我们想要拟合出平面时，RANSAC 会随机找到符合墙壁或者地面的参数组合，如果我们想要拟合出特定的组合，我们需要加额外的限制。
参数选择
在使用 RANSAC 时需要用户指定几个参数，分别为：
- 数据到模型的距离阈值
- 内点数量的阈值
- 迭代次数
上述参数中，前两个参数需要根据使用的场景（数据来源，数据误差水平等）来决定，迭代次数则可以根据以下方式来决定：
- 设迭代次数为 k
- 设用户期望 RANSAC 返回成功拟合模型的概率为 p
- 基于样本随机选择的前提下，每一次迭代选中一个数据点为内点的概率为： w = 内点数量 / 全部数据数量
根据 ransac 的思想，当一次随机样本子集中全部都为内点时，此时拟合的模型比较好：根据概率论知识有：
- 1 - p = (1 - w^n)^k
- k = log(1-p) / log(1 - w^k)
由于实际中，w 通常都是未知的，因此我们可以通过计算距离时获得的（估计）内点数量来更新迭代阈值。
RANSAC 实现
自己实现版本
根据原始的 RANSAC，实现如下：
template <class PointType>
bool RANSACWithPlane(boost::shared_ptr<pcl::PointCloud<PointType>>& cloud_ptr,
std::vector<int>& inliers,
Eigen::Vector4f& coefficients,
double threshold = 0.1) {
static float success_prob = 0.95f;
static int max_iterations = 100;
int iteration_thres = max_iterations;
auto cloud_map = cloud_ptr->getMatrixXfMap();
int points_size = cloud_ptr->points.size();
inliers.clear();
Eigen::Matrix<float, 1, Eigen::Dynamic> distances;
Eigen::Matrix<int, 1, Eigen::Dynamic> indices;
distances.resize(1, points_size);
indices.resize(1, points_size);
indices.array() = 0;
int best_inlier_count = 3;
int iteration = 0;
float log_prob = std::log(1.0 - success_prob);
float inverse_size = 1.0f / static_cast<float>(points_size);
while (iteration < iteration_thres) {
iteration++;
// randomly choose 3 points from point cloud
int idx1 = std::rand() % points_size;
int idx2 = std::rand() % points_size;
int idx3 = std::rand() % points_size;
const Eigen::Vector3f& p0 = cloud_map.template block<3, 1>(0, idx1);
const Eigen::Vector3f& p1 = cloud_map.template block<3, 1>(0, idx2);
const Eigen::Vector3f& p2 = cloud_map.template block<3, 1>(0, idx3);
// determine whether three points can form a plane
if (idx1 == idx2 || idx2 == idx3 || idx1 == idx3) continue;
Eigen::Vector3f p01 = p1 - p0;
Eigen::Vector3f p02 = p2 - p0;
Eigen::Vector3f scale = p01.array() / p02.array();
// determine if two line are parallel
if (scale(0) == scale(1) && scale(1) == scale(2)) continue;
// fit a plane: ax + by + cz + d = 0, normal = (a, b, c)
Eigen::Vector4f coeff;
auto normal = coeff.template block<3, 1>(0, 0);
normal = p01.cross(p02);
normal.normalize();
coeff(3) = -p0.transpose() * normal;
distances = coeff.transpose() * cloud_map;
int inliers_count = (distances.array().abs() < threshold).count();
if (inliers_count > best_inlier_count) {
indices = (distances.array().abs() < threshold).template cast<int>();
best_inlier_count = inliers_count;
coefficients = coeff;
float w = static_cast<float>(inliers_count) * inverse_size;
float prob_not_all_inliers =
std::max(std::numeric_limits<float>::epsilon(),
std::min(1.0f - std::numeric_limits<float>::epsilon(), 1.0f - w * w * w));
iteration_thres = log_prob / std::log(prob_not_all_inliers);
}
if (iteration > max_iterations) {
return false;
}
}
// updated inliers
for (int i = 0; i < points_size; ++i) {
if (indices(i) == 1) {
inliers.push_back(i);
}
}
return !inliers.empty();
}
基本上没有太多特殊处理，为了提高计算速度，作以下处理：
- 预先分配好大尺寸矩阵需要的空间
- 借助矩阵运算来减少计算整体点云距平面距离的时间
- 尽可能使用引用类型减少不必要的数据拷贝次数。
使用 0.2 为阈值，结果如下：
基于 PCL 实现
接下来看一下怎么使用 PCL 自带的实现来进行 RANSAC 处理。
// initialize a sample consensus model with plane
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(
new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud_ptr));
// use ransac to fit the model
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
ransac.setDistanceThreshold(.1);
ransac.computeModel();
// get inliers from fitting result
std::vector<int> inliers;
Eigen::VectorXf coefficients;
ransac.getInliers(inliers);
ransac.getModelCoefficients(coefficients);
将获得的内点以及法向量进行可视化，效果如下所示，可以看到 RANSAC 在点云里面比较好地找到了地面所在的点，估计出来的平面法向量效果也比较好。
PCL 内部的实现如下，可以看到整体过程大同小异：
template <typename PointT> bool
pcl::RandomSampleConsensus<PointT>::computeModel (int)
{
// Warn and exit if no threshold was set
if (threshold_ == std::numeric_limits<double>::max())
{
PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] No threshold set!\n");
return (false);
}
iterations_ = 0;
int n_best_inliers_count = -INT_MAX;
double k = 1.0;
std::vector<int> selection;
Eigen::VectorXf model_coefficients;
double log_probability = log (1.0 - probability_);
double one_over_indices = 1.0 / static_cast<double> (sac_model_->getIndices ()->size ());
int n_inliers_count = 0;
unsigned skipped_count = 0;
// supress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
const unsigned max_skip = max_iterations_ * 10;
// Iterate
while (iterations_ < k && skipped_count < max_skip)
{
// Get X samples which satisfy the model criteria
sac_model_->getSamples (iterations_, selection);
if (selection.empty ())
{
PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] No samples could be selected!\n");
break;
}
// Search for inliers in the point cloud for the current plane model M
if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
{
//++iterations_;
++skipped_count;
continue;
}
// Select the inliers that are within threshold_ from the model
//sac_model_->selectWithinDistance (model_coefficients, threshold_, inliers);
//if (inliers.empty () && k > 1.0)
// continue;
n_inliers_count = sac_model_->countWithinDistance (model_coefficients, threshold_);
// Better match ?
if (n_inliers_count > n_best_inliers_count)
{
n_best_inliers_count = n_inliers_count;
// Save the current model/inlier/coefficients selection as being the best so far
model_ = selection;
model_coefficients_ = model_coefficients;
// Compute the k parameter (k=log(z)/log(1-w^n))
double w = static_cast<double> (n_best_inliers_count) * one_over_indices;
double p_no_outliers = 1.0 - pow (w, static_cast<double> (selection.size ()));
p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by -Inf
p_no_outliers = (std::min) (1.0 - std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by 0.
k = log_probability / log (p_no_outliers);
}
++iterations_;
PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Trial %d out of %f: %d inliers (best is: %d so far).\n", iterations_, k, n_inliers_count, n_best_inliers_count);
if (iterations_ > max_iterations_)
{
PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
break;
}
}
PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_best_inliers_count);
if (model_.empty ())
{
inliers_.clear ();
return (false);
}
// Get the set of inliers that correspond to the best model found so far
sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
return (true);
}
关于 RANSAC 的一些思考
在使用 RANSAC 时，有一些思考列举如下：
- RANSAC 主要用来对外点进行滤波，那么对噪声应该如何处理？
- 噪声的影响主要可以通过距离阈值的来进行控制·
- 如果噪声，可以适当提高样本的数量，比如对平面可以选择超过 3 个点进行最小二乘拟合平面，也可以适当降低噪声的影响
- RANSAC 主要用于筛选一个模型（平面、线等）
- 因此传入的点云数据最好保证符合该模型的特征只有一个
- 如果在不能保证这一点的情况下，至少要保证要提取的特征是符合该模型的特征中最大的，例如在上述数据中地面占据的点数是最多的，因此如果要提取墙壁则很困难
- 除此之外，在添加距离阈值的同时，也可以基于其他先验知识对模型进行限制，如知道激光雷达相对地面的大概位置可以对法向量进行限制
- 在使用 RANSAC 避免想当然认为自己想要的特征符合该模型，例如在上述例子中，拐角的地面和激光雷达所在位置的地面已经不属于一个平面，此时进行 RANSAC 就会将较远处的墙壁也算作内点