3-D самолетная фильтрация EVD RANSAC … где я ошибаюсь?

Фон

Для задания по компьютерному зрению мне было поручено внедрить 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 stackoverflow.
*/
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) с некоторым значением дозорного для ожидаемого минимального собственного значения, соответствующего нормали для плоскостей, которые имеют тенденцию соответствовать модели. Однако это не было рассмотрено в классе, и я понятия не имею, с чего начать выяснять, что не так.