3-D 平面滤波 EVD RANSAC...我哪里出错了？
3-D Plane Filtering EVD RANSAC... where am I going wrong?
背景
对于计算机视觉任务，我的任务是实施 RANSAC 以将平面拟合到给定的一组点，并使用特征值分解通过共识模型过滤该输入点列表。
我花了几天时间尝试调整我的代码，以在输入的一组测试数据上实现正确的平面过滤行为。所有的算法迷，这个是给你的。
我的实现使用 ROS 数据结构 (Point32) 的向量作为输入，但这对手头的问题是透明的。
我做了什么
当我测试预期的平面过滤行为时（正确消除异常值 >95-99% 的时间），我在我的实现中看到我只消除异常值并提取测试点云的主平面 ~30 -40% 的时间。其他时候，我过滤了一个~有点~符合预期模型的平面，但在共识模型中留下了很多明显的异常值。这完全有效的事实表明我做对了一些事情，也做错了一些事情。
我已经调整了我的常量（距离阈值、最大迭代次数、估计的拟合点百分比）到伦敦和返回，我只看到共识模型中的微小差异。
实施（长）
const float RANSAC_ESTIMATED_FIT_POINTS = .80f; // % points estimated to fit the model
const size_t RANSAC_MAX_ITER = 500; // max RANSAC iterations
const size_t RANDOM_MAX_TRIES = 100; // max RANSAC random point tries per iteration
const float RANSAC_THRESHOLD = 0.0000001f; // threshold to determine what constitutes a close point to a plane
/*
Helper to randomly select an item from a STL container, from Whosebug.
*/
template <typename I>
I random_element(I begin, I end)
{
const unsigned long n = std::distance(begin, end);
const unsigned long divisor = ((long)RAND_MAX + 1) / n;
unsigned long k;
do { k = std::rand() / divisor; } while (k >= n);
std::advance(begin, k);
return begin;
}
bool run_RANSAC(const std::vector<Point32> all_points,
Vector3f *out_p0, Vector3f *out_n,
std::vector<Point32> *out_inlier_points)
{
for (size_t iterations = 0; iterations < RANSAC_MAX_ITER; iterations ++)
{
Point32 p1,p2,p3;
Vector3f v1;
Vector3f v2;
Vector3f n_hat; // keep track of the current plane model
Vector3f P0;
std::vector<Point32> points_agree; // list of points that agree with model within
bool found = false;
// try RANDOM_MAX_TRIES times to get random 3 points
for (size_t tries = 0; tries < RANDOM_MAX_TRIES; tries ++) // try to get unique random points 100 times
{
// get 3 random points
p1 = *random_element(all_points.begin(), all_points.end());
p2 = *random_element(all_points.begin(), all_points.end());
p3 = *random_element(all_points.begin(), all_points.end());
v1 = Vector3f (p2.x - p1.x,
p2.y - p1.y,
p2.z - p1.z ); //Vector P1P2
v2 = Vector3f (p3.x - p1.x,
p3.y - p1.y,
p3.z - p1.z); //Vector P1P3
if (std::abs(v1.dot(v2)) != 1.f) // dot product != 1 means we've found 3 nonlinear points
{
found = true;
break;
}
} // end try random element loop
if (!found) // could not find 3 random nonlinear points in 100 tries, go to next iteration
{
ROS_ERROR("run_RANSAC(): Could not find 3 random nonlinear points in %ld tries, going on to iteration %ld", RANDOM_MAX_TRIES, iterations + 1);
continue;
}
// nonlinear random points exist past here
// fit a plane to p1, p2, p3
Vector3f n = v1.cross(v2); // calculate normal of plane
n_hat = n / n.norm();
P0 = Vector3f(p1.x, p1.y, p1.z);
// at some point, the original p0, p1, p2 will be iterated over and added to agreed points
// loop over all points, find points that are inliers to plane
for (std::vector<Point32>::const_iterator it = all_points.begin();
it != all_points.end(); it++)
{
Vector3f M (it->x - P0.x(),
it->y - P0.y(),
it->z - P0.z()); // M = (P - P0)
float d = M.dot(n_hat); // calculate distance
if (d <= RANSAC_THRESHOLD)
{ // add to inlier points list
points_agree.push_back(*it);
}
} // end points loop
ROS_DEBUG("run_RANSAC() POINTS AGREED: %li=%f, RANSAC_ESTIMATED_FIT_POINTS: %f", points_agree.size(),
(float) points_agree.size() / all_points.size(), RANSAC_ESTIMATED_FIT_POINTS);
if (((float) points_agree.size()) / all_points.size() > RANSAC_ESTIMATED_FIT_POINTS)
{ // if points agree / total points > estimated % points fitting
// fit to points_agree.size() points
size_t n = points_agree.size();
Vector3f sum(0.0f, 0.0f, 0.0f);
for (std::vector<Point32>::iterator iter = points_agree.begin();
iter != points_agree.end(); iter++)
{
sum += Vector3f(iter->x, iter->y, iter->z);
}
Vector3f centroid = sum / n; // calculate centroid
Eigen::MatrixXf M(points_agree.size(), 3);
for (size_t row = 0; row < points_agree.size(); row++)
{ // build distance vector matrix
Vector3f point(points_agree[row].x,
points_agree[row].y,
points_agree[row].z);
for (size_t col = 0; col < 3; col ++)
{
M(row, col) = point(col) - centroid(col);
}
}
Matrix3f covariance_matrix = M.transpose() * M;
Eigen::EigenSolver<Matrix3f> eigen_solver;
eigen_solver.compute(covariance_matrix);
Vector3f eigen_values = eigen_solver.eigenvalues().real();
Matrix3f eigen_vectors = eigen_solver.eigenvectors().real();
// find eigenvalue that is closest to 0
size_t idx;
// find minimum eigenvalue, get index
float closest_eval = eigen_values.cwiseAbs().minCoeff(&idx);
// find corresponding eigenvector
Vector3f closest_evec = eigen_vectors.col(idx);
std::stringstream logstr;
logstr << "Closest eigenvalue : " << closest_eval << std::endl <<
"Corresponding eigenvector : " << std::endl << closest_evec << std::endl <<
"Centroid : " << std::endl << centroid;
ROS_DEBUG("run_RANSAC(): %s", logstr.str().c_str());
Vector3f all_fitted_n_hat = closest_evec / closest_evec.norm();
// invoke copy constructors for outbound
*out_n = Vector3f(all_fitted_n_hat);
*out_p0 = Vector3f(centroid);
*out_inlier_points = std::vector<Point32>(points_agree);
ROS_DEBUG("run_RANSAC():: Success, total_size: %li, inlier_size: %li, %% agreement %f",
all_points.size(), out_inlier_points->size(), (float) out_inlier_points->size() / all_points.size());
return true;
}
} // end iterations loop
return false;
}
来自维基百科的伪代码供参考：
Given:
data – a set of observed data points
model – a model that can be fitted to data points
n – minimum number of data points required to fit the model
k – maximum number of iterations allowed in the algorithm
t – threshold value to determine when a data point fits a model
d – number of close data points required to assert that a model fits well to data
Return:
bestfit – model parameters which best fit the data (or nul if no good model is found)
iterations = 0
bestfit = nul
besterr = something really large
while iterations < k {
maybeinliers = n randomly selected values from data
maybemodel = model parameters fitted to maybeinliers
alsoinliers = empty set
for every point in data not in maybeinliers {
if point fits maybemodel with an error smaller than t
add point to alsoinliers
}
if the number of elements in alsoinliers is > d {
% this implies that we may have found a good model
% now test how good it is
bettermodel = model parameters fitted to all points in maybeinliers and alsoinliers
thiserr = a measure of how well model fits these points
if thiserr < besterr {
bestfit = bettermodel
besterr = thiserr
}
}
increment iterations
}
return bestfit
我的实现与维基百科伪代码之间的唯一区别如下：
thiserr = a measure of how well model fits these points
if thiserr < besterr {
bestfit = bettermodel
besterr = thiserr
}
我的猜测是，我需要做一些与比较 (closest_eval) 和一些标记值相关的事情，以获得与倾向于拟合模型的平面的法线相对应的预期最小特征值。然而，class 中并未涵盖这一点，我不知道从哪里开始找出问题所在。
呵呵，思考如何将问题呈现给其他人实际上可以解决我遇到的问题，这很有趣。
通过使用 std::numeric_limits::max() 起始最佳拟合特征值简单地实现它来解决。这是因为在 RANSAC 的任何 n-th 迭代中提取的最佳拟合平面不能保证是最佳拟合平面，并且每个组成点之间的共识可能存在巨大错误，因此我需要在每次迭代中收敛.糟糕。
thiserr = a measure of how well model fits these points
if thiserr < besterr {
bestfit = bettermodel
besterr = thiserr
}
背景
对于计算机视觉任务，我的任务是实施 RANSAC 以将平面拟合到给定的一组点，并使用特征值分解通过共识模型过滤该输入点列表。
我花了几天时间尝试调整我的代码，以在输入的一组测试数据上实现正确的平面过滤行为。所有的算法迷，这个是给你的。
我的实现使用 ROS 数据结构 (Point32) 的向量作为输入，但这对手头的问题是透明的。
我做了什么
当我测试预期的平面过滤行为时（正确消除异常值 >95-99% 的时间），我在我的实现中看到我只消除异常值并提取测试点云的主平面 ~30 -40% 的时间。其他时候，我过滤了一个~有点~符合预期模型的平面，但在共识模型中留下了很多明显的异常值。这完全有效的事实表明我做对了一些事情，也做错了一些事情。
我已经调整了我的常量（距离阈值、最大迭代次数、估计的拟合点百分比）到伦敦和返回，我只看到共识模型中的微小差异。
实施（长）
const float RANSAC_ESTIMATED_FIT_POINTS = .80f; // % points estimated to fit the model
const size_t RANSAC_MAX_ITER = 500; // max RANSAC iterations
const size_t RANDOM_MAX_TRIES = 100; // max RANSAC random point tries per iteration
const float RANSAC_THRESHOLD = 0.0000001f; // threshold to determine what constitutes a close point to a plane
/*
Helper to randomly select an item from a STL container, from Whosebug.
*/
template <typename I>
I random_element(I begin, I end)
{
const unsigned long n = std::distance(begin, end);
const unsigned long divisor = ((long)RAND_MAX + 1) / n;
unsigned long k;
do { k = std::rand() / divisor; } while (k >= n);
std::advance(begin, k);
return begin;
}
bool run_RANSAC(const std::vector<Point32> all_points,
Vector3f *out_p0, Vector3f *out_n,
std::vector<Point32> *out_inlier_points)
{
for (size_t iterations = 0; iterations < RANSAC_MAX_ITER; iterations ++)
{
Point32 p1,p2,p3;
Vector3f v1;
Vector3f v2;
Vector3f n_hat; // keep track of the current plane model
Vector3f P0;
std::vector<Point32> points_agree; // list of points that agree with model within
bool found = false;
// try RANDOM_MAX_TRIES times to get random 3 points
for (size_t tries = 0; tries < RANDOM_MAX_TRIES; tries ++) // try to get unique random points 100 times
{
// get 3 random points
p1 = *random_element(all_points.begin(), all_points.end());
p2 = *random_element(all_points.begin(), all_points.end());
p3 = *random_element(all_points.begin(), all_points.end());
v1 = Vector3f (p2.x - p1.x,
p2.y - p1.y,
p2.z - p1.z ); //Vector P1P2
v2 = Vector3f (p3.x - p1.x,
p3.y - p1.y,
p3.z - p1.z); //Vector P1P3
if (std::abs(v1.dot(v2)) != 1.f) // dot product != 1 means we've found 3 nonlinear points
{
found = true;
break;
}
} // end try random element loop
if (!found) // could not find 3 random nonlinear points in 100 tries, go to next iteration
{
ROS_ERROR("run_RANSAC(): Could not find 3 random nonlinear points in %ld tries, going on to iteration %ld", RANDOM_MAX_TRIES, iterations + 1);
continue;
}
// nonlinear random points exist past here
// fit a plane to p1, p2, p3
Vector3f n = v1.cross(v2); // calculate normal of plane
n_hat = n / n.norm();
P0 = Vector3f(p1.x, p1.y, p1.z);
// at some point, the original p0, p1, p2 will be iterated over and added to agreed points
// loop over all points, find points that are inliers to plane
for (std::vector<Point32>::const_iterator it = all_points.begin();
it != all_points.end(); it++)
{
Vector3f M (it->x - P0.x(),
it->y - P0.y(),
it->z - P0.z()); // M = (P - P0)
float d = M.dot(n_hat); // calculate distance
if (d <= RANSAC_THRESHOLD)
{ // add to inlier points list
points_agree.push_back(*it);
}
} // end points loop
ROS_DEBUG("run_RANSAC() POINTS AGREED: %li=%f, RANSAC_ESTIMATED_FIT_POINTS: %f", points_agree.size(),
(float) points_agree.size() / all_points.size(), RANSAC_ESTIMATED_FIT_POINTS);
if (((float) points_agree.size()) / all_points.size() > RANSAC_ESTIMATED_FIT_POINTS)
{ // if points agree / total points > estimated % points fitting
// fit to points_agree.size() points
size_t n = points_agree.size();
Vector3f sum(0.0f, 0.0f, 0.0f);
for (std::vector<Point32>::iterator iter = points_agree.begin();
iter != points_agree.end(); iter++)
{
sum += Vector3f(iter->x, iter->y, iter->z);
}
Vector3f centroid = sum / n; // calculate centroid
Eigen::MatrixXf M(points_agree.size(), 3);
for (size_t row = 0; row < points_agree.size(); row++)
{ // build distance vector matrix
Vector3f point(points_agree[row].x,
points_agree[row].y,
points_agree[row].z);
for (size_t col = 0; col < 3; col ++)
{
M(row, col) = point(col) - centroid(col);
}
}
Matrix3f covariance_matrix = M.transpose() * M;
Eigen::EigenSolver<Matrix3f> eigen_solver;
eigen_solver.compute(covariance_matrix);
Vector3f eigen_values = eigen_solver.eigenvalues().real();
Matrix3f eigen_vectors = eigen_solver.eigenvectors().real();
// find eigenvalue that is closest to 0
size_t idx;
// find minimum eigenvalue, get index
float closest_eval = eigen_values.cwiseAbs().minCoeff(&idx);
// find corresponding eigenvector
Vector3f closest_evec = eigen_vectors.col(idx);
std::stringstream logstr;
logstr << "Closest eigenvalue : " << closest_eval << std::endl <<
"Corresponding eigenvector : " << std::endl << closest_evec << std::endl <<
"Centroid : " << std::endl << centroid;
ROS_DEBUG("run_RANSAC(): %s", logstr.str().c_str());
Vector3f all_fitted_n_hat = closest_evec / closest_evec.norm();
// invoke copy constructors for outbound
*out_n = Vector3f(all_fitted_n_hat);
*out_p0 = Vector3f(centroid);
*out_inlier_points = std::vector<Point32>(points_agree);
ROS_DEBUG("run_RANSAC():: Success, total_size: %li, inlier_size: %li, %% agreement %f",
all_points.size(), out_inlier_points->size(), (float) out_inlier_points->size() / all_points.size());
return true;
}
} // end iterations loop
return false;
}
来自维基百科的伪代码供参考：
Given:
data – a set of observed data points
model – a model that can be fitted to data points
n – minimum number of data points required to fit the model
k – maximum number of iterations allowed in the algorithm
t – threshold value to determine when a data point fits a model
d – number of close data points required to assert that a model fits well to data
Return:
bestfit – model parameters which best fit the data (or nul if no good model is found)
iterations = 0
bestfit = nul
besterr = something really large
while iterations < k {
maybeinliers = n randomly selected values from data
maybemodel = model parameters fitted to maybeinliers
alsoinliers = empty set
for every point in data not in maybeinliers {
if point fits maybemodel with an error smaller than t
add point to alsoinliers
}
if the number of elements in alsoinliers is > d {
% this implies that we may have found a good model
% now test how good it is
bettermodel = model parameters fitted to all points in maybeinliers and alsoinliers
thiserr = a measure of how well model fits these points
if thiserr < besterr {
bestfit = bettermodel
besterr = thiserr
}
}
increment iterations
}
return bestfit
我的实现与维基百科伪代码之间的唯一区别如下：
thiserr = a measure of how well model fits these points
if thiserr < besterr {
bestfit = bettermodel
besterr = thiserr
}
我的猜测是，我需要做一些与比较 (closest_eval) 和一些标记值相关的事情，以获得与倾向于拟合模型的平面的法线相对应的预期最小特征值。然而，class 中并未涵盖这一点，我不知道从哪里开始找出问题所在。
呵呵，思考如何将问题呈现给其他人实际上可以解决我遇到的问题，这很有趣。
通过使用 std::numeric_limits::max() 起始最佳拟合特征值简单地实现它来解决。这是因为在 RANSAC 的任何 n-th 迭代中提取的最佳拟合平面不能保证是最佳拟合平面，并且每个组成点之间的共识可能存在巨大错误，因此我需要在每次迭代中收敛.糟糕。
thiserr = a measure of how well model fits these points
if thiserr < besterr {
bestfit = bettermodel
besterr = thiserr
}