НОВОСТИ Можно порешать: задача про лидарное облако от команды беспилотных автомобилей Яндекса

[NewsBot]

Меня зовут Андрей Гладков, я разработчик в направлении беспилотных автомобилей. Сегодня я поделюсь с сообществом Хабра задачей, которая связана с важнейшим сенсором беспилотника — лидаром, и с тем, как мы обрабатываем лидарные данные. Вы можете попробовать решить задачу самостоятельно на платформе Контест. Система проверит решение с помощью автотестов и сразу сообщит результат. Разбор и код решения — в спойлерах ближе к концу поста. Тем, кто был на митапе в нашем цехе в прошлом году, задача покажется знакомой — мы предлагали ее в качестве входного билета, но публично никогда ей не делились.

Условие

Ограничение времени	15 секунд
Ограничение памяти	64 МБ
Ввод	стандартный ввод или input.txt
Вывод	стандартный вывод или output.txt

Беспилотный автомобиль стоит на ровной асфальтовой площадке, на крыше автомобиля установлен лидар. Даны измерения, полученные лидаром за один период сканирования.

Измерения представляют собой множество из

точек, имеющих координаты

,

и

. Больше 50% точек принадлежат дороге. Моделью положения принадлежащих дороге точек в пространстве является плоскость с параметризацией

Точки, которые принадлежат дороге, отклоняются от модели не больше чем на заданную величину

.

Необходимо найти параметры

и

соответствующей дороге плоскости. Число точек, отклоняющихся от модели не больше чем на

, должно составлять не менее 50% от общего числа точек.

Формат ввода


Входные данные заданы в текстовом формате. Первая строка содержит фиксированный порог

. Вторая строка содержит число точек

. Следующие

строк содержат координаты

,

и

для каждой точки, разделителем является символ табуляции (формат строки "x

y

z"). Вещественные числа имеют не более 6 десятичных знаков.

Формат вывода


Выведите параметры

,

,

и

соответствующей дороге плоскости. Числа разделяйте пробелами. Выведенные параметры должны задавать корректную плоскость.

Пример 1

Ввод	Вывод
0.01 3 20 0 0 10 -10 0 10 10 0	0.000000 0.000000 1.000000 0.000000

Пример 2

Ввод	Вывод
0.01 3 20 0 3 10 -10 2 10 10 2	-0.099504 0.000000 0.995037 -0.995037

Пример 3

Ввод	Вывод
0.01 10 20 -10 0.2 20 0 0.2 20 10 0.2 15 -10 0.15 15 0 0.15 15 10 0.15 10 -10 0.1 10 10 0.1 20 18 1.7 15 -15 1.2	-0.010000 0.000000 0.999950 0.000000

Данные примеры — синтетические. В реальном облаке точек объектов значительно больше: поработайте над edge-кейсами.

Где порешать


Попробовать свои силы можно на Контесте:

You must be registered for see links

Разбор и готовый код


Разбор
Сначала давайте задумаемся над тем, что должны представлять из себя входные данные. Больше 50% точек, как сказано в условии, относятся к дороге, остальные, как мы можем догадаться, — к другим объектам, которые возвышаются над дорогой. Это могут быть другие автомобили, пешеходы, столбы и т. д. Их возвышение над дорогой может быть достаточно большим по сравнению с заданной величиной отклонения точек дороги

.

Получается, для решения задачи нам нужен метод оценки параметров модели, устойчивый к большому количеству выбросов. Одним из таких методов является RANSAC (

You must be registered for see links

на Википедию). Метод итеративно перебирает гипотезы (наборы параметров модели), построенные по случайно выбранным точкам, и выбирает из гипотез лучшую.

Применительно к задаче шаг его итерации можно описать так:

• Берем случайные три точки, вычисляем по ним параметры плоскости (гипотезу).
• Оцениваем, насколько гипотеза хороша — сколько точек с учетом заданного порога
  
  можно отнести к плоскости дороги, а сколько нужно признать «выбросами».
• Обновляем лучшую гипотезу, если текущая оказалась лучше.

В базовом варианте шаг реализовывается довольно просто. Ниже — пример кода. Пример не является продакшен-кодом и опирается на то, что входные данные соответствуют описанию задачи.

Код на C++

#include
#include
#include
#include
#include
#include
#include

struct Point3d {
double X = 0.0;
double Y = 0.0;
double Z = 0.0;
};

using PointCloud = std::vector
;

struct Plane {
double A = 0.0;
double B = 0.0;
double C = 0.0;
double D = 0.0;
};

bool CreatePlane(
Plane& plane,
const Point3d& p1,
const Point3d& p2,
const Point3d& p3) {
const double matrix[3][3] =
{{ 0, 0, 0}, // this row is not used
{p2.X - p1.X, p2.Y - p1.Y, p2.Z - p1.Z},
{p3.X - p1.X, p3.Y - p1.Y, p3.Z - p1.Z}};

auto getMatrixSignedMinor = [&matrix](size_t i, size_t j) {
const auto& m = matrix;
return (m[(i + 1) % 3][(j + 1) % 3] * m[(i + 2) % 3][(j + 2) % 3] -
m[(i + 2) % 3][(j + 1) % 3] * m[(i + 1) % 3][(j + 2) % 3]);
};

const double A = getMatrixSignedMinor(0, 0);
const double B = getMatrixSignedMinor(0, 1);
const double C = getMatrixSignedMinor(0, 2);
const double D = -(A * p1.X + B * p1.Y + C * p1.Z);

const double norm_coeff = std::sqrt(A * A + B * B + C * C);

const double kMinValidNormCoeff = 1.0e-6;
if (norm_coeff < kMinValidNormCoeff) {
return false;
}

plane.A = A / norm_coeff;
plane.B = B / norm_coeff;
plane.C = C / norm_coeff;
plane.D = D / norm_coeff;

return true;
}

double CalcDistance(const Plane& plane, const Point3d& point) {
// assume that the plane is valid
const auto numerator = std::abs(
plane.A * point.X + plane.B * point.Y + plane.C * point.Z + plane.D);
const auto denominator = std::sqrt(
plane.A * plane.A + plane.B * plane.B + plane.C * plane.C);
return numerator / denominator;
}

int CountInliers(
const Plane& plane,
const PointCloud& cloud,
double threshold) {
return std::count_if(cloud.begin(), cloud.end(),
[&plane, threshold](const Point3d& point) {
return CalcDistance(plane, point) best_number_of_inliers) {
best_plane = sample_plane;
best_number_of_inliers = number_of_inliers;
}
}
}
}

return best_plane;
}

Plane FindPlaneWithSimpleRansac(
const PointCloud& cloud,
double threshold,
size_t iterations) {
const size_t cloud_size = cloud.size();

// use uint64_t to calculate the number of all combinations
// for N (cloud_size);
const auto number_of_combinations =
cloud_size_ull * (cloud_size_ull - 1) * (cloud_size_ull - 2) / 6;

if (number_of_combinations distr(0, cloud_size - 1);

Plane best_plane;
int best_number_of_inliers = 0;

for (size_t i = 0; i < iterations; ++i) {
std::array indices;
do {
indices = {distr(gen), distr(gen), distr(gen)};
std::sort(indices.begin(), indices.end());
} while (indices[0] == indices[1] || indices[1] == indices[2]);

Plane sample_plane;
if (!CreatePlane(sample_plane,
cloud[indices[0]],
cloud[indices[1]],
cloud[indices[2]])) {
continue;
}

const int number_of_inliers = CountInliers(
sample_plane, cloud, threshold);
if (number_of_inliers > best_number_of_inliers) {
best_plane = sample_plane;
best_number_of_inliers = number_of_inliers;
}
}

return best_plane;
}

int main() {
double p = 0.0;
std::cin >> p;

size_t points_number = 0;
std::cin >> points_number;

PointCloud cloud;
cloud.reserve(points_number);
for (size_t i = 0; i < points_number; ++i) {
Point3d point;
std::cin >> point.X >> point.Y >> point.Z;
cloud.push_back(point);
}

const Plane plane = FindPlaneWithSimpleRansac(cloud, p, 100);

std::cout code>

Комментарии к коду
Посмотрим на кусок кода, представленного выше:


const double matrix[3][3] =
{{ 0, 0, 0}, // this row is not used
{p2.X - p1.X, p2.Y - p1.Y, p2.Z - p1.Z},
{p3.X - p1.X, p3.Y - p1.Y, p3.Z - p1.Z}};

auto getMatrixSignedMinor = [&matrix](size_t i, size_t j) {
const auto& m = matrix;
return (m[(i + 1) % 3][(j + 1) % 3] * m[(i + 2) % 3][(j + 2) % 3] -
m[(i + 2) % 3][(j + 1) % 3] * m[(i + 1) % 3][(j + 2) % 3]);
};

const double A = getMatrixSignedMinor(0, 0);
const double B = getMatrixSignedMinor(0, 1);
const double C = getMatrixSignedMinor(0, 2);
const double D = -(A * p1.X + B * p1.Y + C * p1.Z);

В этих строках выполняется вычисление параметров плоскости по трем точкам (

You must be registered for see links

на Википедию). В первой строке

должны стоять элементы

,

и

. Если вычислять определитель этой матрицы через алгебраические дополнения для первой строки, то дополнения войдут в итоговое выражение как коэффициенты при

,

и

, то есть будут как раз коэффициентами

,

и

плоскости.

При выборе случайной тройки индексов можно дополнительно учитывать, какие тройки уже встречались. Для этого можно завести unordered_set и записывать в него идентификаторы троек.