文章目录
前言
平面拟合是点云处理的常规操作，这里将pcl里随机采样一致性平面拟合的源码做一下分析，加深对原理以及代码的理解。
一、使用实例
随机采样一致性平面拟合实例在之前的文章里已经写过。
二、RandomSampleConsensus类
在RandomSampleConsensus类中，核心函数是computeModel。
三、computeModel函数
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;
std::size_t n_best_inliers_count = 0;
//返回编译器允许的 double 型数 最大值。
double k = std::numeric_limits<double>::max();
Indices selection;
//这里的model_coefficients 大小，已经在SampleConsensusModelPlane类的构造函数里定义了，在输入对应的model后，初始化时就直接获取了model_coefficients 的大小
Eigen::VectorXf model_coefficients (sac_model_->getModelSize ());
const double log_probability = std::log (1.0 - probability_);
//static_cast<double>:其他类型转换为double类型
const double one_over_indices = 1.0 / static_cast<double> (sac_model_->getIndices ()->size ());
unsigned skipped_count = 0;
// suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
//通过只允许对无效模型参数进行10次最大允许迭代来抑制无限循环！
const unsigned max_skip = max_iterations_ * 10;
int threads = threads_;
if (threads >= 0)
{
#if OPENMP_AVAILABLE_RANSAC
if (threads == 0)
{
threads = omp_get_num_procs();
PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Automatic number of threads requested, choosing %i threads.\n", threads);
}
#else
// Parallelization desired, but not available
PCL_WARN ("[pcl::RandomSampleConsensus::computeModel] Parallelization is requested, but OpenMP 3.1 is not available! Continuing without parallelization.\n");
threads = -1;
#endif
}
#if OPENMP_AVAILABLE_RANSAC
#pragma omp parallel if(threads > 0) num_threads(threads) shared(k, skipped_count, n_best_inliers_count) firstprivate(selection, model_coefficients) // would be nice to have a default(none)-clause here, but then some compilers complain about the shared const variables
#endif
{
#if OPENMP_AVAILABLE_RANSAC
if (omp_in_parallel())
#pragma omp master
PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Computing in parallel with up to %i threads.\n", omp_get_num_threads());
else
#endif
PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Computing not parallel.\n");
// Iterate
while (true) // infinite loop with four possible breaks
{
// Get X samples which satisfy the model criteria
#if OPENMP_AVAILABLE_RANSAC
#pragma omp critical(samples)
#endif
{
//选择样本时使用的随机数生成器不应并行调用
sac_model_->getSamples (iterations_, selection); // The random number generator used when choosing the samples should not be called in parallel
}
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)) // This function has to be thread-safe
{
//++iterations_;
unsigned skipped_count_tmp;
#if OPENMP_AVAILABLE_RANSAC
#pragma omp atomic capture
#endif
skipped_count_tmp = ++skipped_count;
//如果此次样本拟合失败，并且未达到最大迭代次数，则continue，进行下一次样本选择与拟合
//如果已经达到最大迭代次数，则直接break
if (skipped_count_tmp < max_skip)
continue;
else
break;
}
// Select the inliers that are within threshold_ from the model
//sac_model_->selectWithinDistance (model_coefficients, threshold_, inliers);
//if (inliers.empty () && k > 1.0)
// continue;
std::size_t n_inliers_count = sac_model_->countWithinDistance (model_coefficients, threshold_); // This functions has to be thread-safe. Most work is done here
//后面这一系列操作，判断筛选出来的内点是否最佳
std::size_t n_best_inliers_count_tmp;
#if OPENMP_AVAILABLE_RANSAC
#pragma omp atomic read
#endif
n_best_inliers_count_tmp = n_best_inliers_count;
if (n_inliers_count > n_best_inliers_count_tmp) // This condition is false most of the time, and the critical region is not entered, hopefully leading to more efficient concurrency
{
#if OPENMP_AVAILABLE_RANSAC
#pragma omp critical(update) // n_best_inliers_count, model_, model_coefficients_, k are shared and read/write must be protected
#endif
{
// Better match ?
if (n_inliers_count > n_best_inliers_count)
{
n_best_inliers_count = n_inliers_count; // This write and the previous read of n_best_inliers_count must be consecutive and must not be interrupted!
n_best_inliers_count_tmp = n_best_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=std::log(z)/std::log(1-w^n))
const double w = static_cast<double> (n_best_inliers_count) * one_over_indices;
double p_outliers = 1.0 - std::pow (w, static_cast<double> (selection.size ())); // Probability that selection is contaminated by at least one outlier
p_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_outliers); // Avoid division by -Inf
p_outliers = (std::min) (1.0 - std::numeric_limits<double>::epsilon (), p_outliers); // Avoid division by 0.
k = log_probability / std::log (p_outliers);
}
} // omp critical
}
int iterations_tmp;
double k_tmp;
#if OPENMP_AVAILABLE_RANSAC
#pragma omp atomic capture
#endif
iterations_tmp = ++iterations_;
#if OPENMP_AVAILABLE_RANSAC
#pragma omp atomic read
#endif
k_tmp = k;
#if OPENMP_AVAILABLE_RANSAC
PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Trial %d out of %f: %u inliers (best is: %u so far) (thread %d).\n", iterations_tmp, k_tmp, n_inliers_count, n_best_inliers_count_tmp, omp_get_thread_num());
#else
PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Trial %d out of %f: %u inliers (best is: %u so far).\n", iterations_tmp, k_tmp, n_inliers_count, n_best_inliers_count_tmp);
#endif
if (iterations_tmp > k_tmp)
break;
if (iterations_tmp > max_iterations_)
{
PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
break;
}
} // while
} // omp parallel
PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Model: %lu size, %u inliers.\n", model_.size (), n_best_inliers_count);
if (model_.empty ())
{
PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] RANSAC found no model.\n");
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);
}
1、getSamples 函数
该函数是样本选择器，随机选择样本，也即在输入的点云中随机选择3个
virtual void
getSamples (int &iterations, std::vector<int> &samples)
{
// We're assuming that indices_ have already been set in the constructor
//这里getSampleSize ()，也即样本数量的获取，实际上和model_coefficients 一样，已经在SampleConsensusModelPlane类的构造函数里定义了，并且是3，毕竟至少3个点才能构成一个平面
if (indices_->size () < getSampleSize ())
{
PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %lu unique points out of %lu!\n",
samples.size (), indices_->size ());
// one of these will make it stop :)
samples.clear ();
iterations = INT_MAX - 1;
return;
}
// Get a second point which is different than the first
samples.resize (getSampleSize ());
for (unsigned int iter = 0; iter < max_sample_checks_; ++iter)
{
// Choose the random indices
//std::numeric_limits<double>::epsilon ()：返回类型 T 的最小精度，这里是判断samples_radius_ 是否趋近于0
//随机取数的具体实现，有兴趣可以研究下
if (samples_radius_ < std::numeric_limits<double>::epsilon ())
SampleConsensusModel<PointT>::drawIndexSample (samples);
else
SampleConsensusModel<PointT>::drawIndexSampleRadius (samples);
// If it's a good sample, stop here
//方法就是拿第二个和第三个点减去第一个点，再比对这个两个差值
if (isSampleGood (samples))
{
PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] Selected %lu samples.\n", samples.size ());
return;
}
}
PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n", getSampleSize (), max_sample_checks_);
samples.clear ();
}
2、computeModelCoefficients函数
将1中选择的3个样本进行计算，计算出该样本下的平面参数方程，可以回顾下范数
template <typename PointT> bool
pcl::SampleConsensusModelPlane<PointT>::computeModelCoefficients (
const Indices &samples, Eigen::VectorXf &model_coefficients) const
{
// The checks are redundant with isSampleGood above, but since most of the
// computed values are also used to compute the model coefficients, this might
// be a situation where this duplication is acceptable.
if (samples.size () != sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
return (false);
}
//将pcl的数据类型，转换成eigen的数据类型，以方便计算向量或者矩阵
//这里是将pcl::PointXYZ类型转换成Eigen::Vector3f
pcl::Vector3fMapConst p0 = (*input_)[samples[0]].getVector3fMap ();
pcl::Vector3fMapConst p1 = (*input_)[samples[1]].getVector3fMap ();
pcl::Vector3fMapConst p2 = (*input_)[samples[2]].getVector3fMap ();
//p1减p0向量和p2减p0向量的叉乘，其结果，是垂直于这两个向量的另一个向量
//也即意味着，叉乘向量就是当前样本数据拟合成的平面的法线向量
const Eigen::Vector3f cross = (p1 - p0).cross(p2 - p0);
//这里stableNorm应该是求范数的意思
const float crossNorm = cross.stableNorm();
// Checking for collinearity here
//dummy_precision()函数返回的是一个表示最小可表示正数的值，这个值用于默认的数值精度设置，
//当需要默认精度时，可以使用这个函数来获取
//这里，实际意义就是想知道crossNorm 是否为零或者nan值（也即两向量共线或者平行时）
if (crossNorm < Eigen::NumTraits<float>::dummy_precision ())
{
PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Chosen samples are collinear!\n");
return (false);
}
// Compute the plane coefficients from the 3 given points in a straightforward manner
// calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
//model_coefficients在形参中是vectorxf形式，这里平面有四个参数，也即model_size_=4，，resize后，xf就是4f了
model_coefficients.resize (model_size_);
//这里除以范数，相当于归一化处理
model_coefficients.template head<3>() = cross / crossNorm;
// ... + d = 0
//对于平面参数：Ax + By + Cz + D = 0 -> D = -(Ax + By + Cz)
//因为前面求叉乘，是以p0为基准点的，因此，在求出法向量（也即ABC）后，将p0的值带入平面方程中，即可求出D
//假设p0 = (x0，y0,z0),也即D = -(Ax0 + By0 + Cz0)
//而 model_coefficients.template head<3>() = （A, B, C）(已经归一化后的值)
//因此，model_coefficients.template head<3>().dot(p0) = Ax0 + By0 + Cz0（向量点乘公式）
//最后，D = -1.0 * (Ax0 + By0 + Cz0)，这样，平面方程的四个参数都求出来了
model_coefficients[3] = -1.0f * (model_coefficients.template head<3>().dot (p0));
PCL_DEBUG ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Model is (%g,%g,%g,%g).\n",
model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3]);
return (true);
}
3、countWithinDistance函数
该函数作用是，计算出平面方程参数后，计算阈值范围内的内点数
template <typename PointT> std::size_t
pcl::SampleConsensusModelPlane<PointT>::countWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))//确定平面方程参数数量是4，因为是平面
{
PCL_ERROR ("[pcl::SampleConsensusModelPlane::countWithinDistance] Given model is invalid!\n");
return (0);
}
#if defined (__AVX__) && defined (__AVX2__)
return countWithinDistanceAVX (model_coefficients, threshold);
#elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
return countWithinDistanceSSE (model_coefficients, threshold);
#else
//计算点到当前样本拟合出来的平面方程的距离
return countWithinDistanceStandard (model_coefficients, threshold);
#endif
}
countWithinDistanceStandard 函数感觉有点问题，调用的和定义的形参数量不一样，这里是不是个bug？
template <typename PointT> std::size_t
pcl::SampleConsensusModelPlane<PointT>::countWithinDistanceStandard (
const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
{
std::size_t nr_p = 0;
// Iterate through the 3d points and calculate the distances from them to the plane
for (; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the plane normal as the dot product
// D = (P-A).N/|N|
//根据上面的公式（也即点到平面距离公式），|N|是归一化后的，因此|N|=1
//而分子=|Ax0 + By0 + Cz0 + D|，也即点(x0, y0 ,z0)，引入齐次坐标后：(x0, y0, z0, 1)，平面方程参数（A, B, C, D）
//根据向量点乘公式，这样分子|Ax0 + By0 + Cz0 + D| = |（A, B, C, D）.dot((x0, y0, z0, 1))|
Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
(*input_)[(*indices_)[i]].y,
(*input_)[(*indices_)[i]].z,
1.0f);
if (std::abs (model_coefficients.dot (pt)) < threshold)
{
nr_p++;
}
}
return (nr_p);
}
4、selectWithinDistance函数
该函数将内点筛选出来，和countWithinDistanceStandard 原理类似，就不多做解释了
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
{
PCL_ERROR ("[pcl::SampleConsensusModelPlane::selectWithinDistance] Given model is invalid!\n");
return;
}
inliers.clear ();
error_sqr_dists_.clear ();
inliers.reserve (indices_->size ());
error_sqr_dists_.reserve (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the plane
for (std::size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the plane normal as the dot product
// D = (P-A).N/|N|
Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
(*input_)[(*indices_)[i]].y,
(*input_)[(*indices_)[i]].z,
1.0f);
float distance = std::abs (model_coefficients.dot (pt));
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers.push_back ((*indices_)[i]);
error_sqr_dists_.push_back (static_cast<double> (distance));
}
}
}
总结
学习总结了一下pcl里随机采样一致性平面拟合的原理和代码结构，很有收获，后续有新的发现再更新。
扫一扫
3485
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
