Меня зовут Андрей Гладков, я разработчик в направлении беспилотных автомобилей. Сегодня я поделюсь с сообществом Хабра задачей, которая связана с важнейшим сенсором беспилотника — лидаром, и с тем, как мы обрабатываем лидарные данные. Вы можете попробовать решить задачу самостоятельно на платформе Контест. Система проверит решение с помощью автотестов и сразу сообщит результат. Разбор и код решения — в спойлерах ближе к концу поста. Тем, кто был на митапе в нашем цехе в прошлом году, задача покажется знакомой — мы предлагали ее в качестве входного билета, но публично никогда ей не делились.
Условие
Ограничение времени | 15 секунд |
Ограничение памяти | 64 МБ |
Ввод | стандартный ввод или input.txt |
Вывод | стандартный вывод или output.txt |
Измерения представляют собой множество из точек, имеющих координаты , и . Больше 50% точек принадлежат дороге. Моделью положения принадлежащих дороге точек в пространстве является плоскость с параметризацией
Точки, которые принадлежат дороге, отклоняются от модели не больше чем на заданную величину .
Необходимо найти параметры и соответствующей дороге плоскости. Число точек, отклоняющихся от модели не больше чем на , должно составлять не менее 50% от общего числа точек.
Формат ввода
Входные данные заданы в текстовом формате. Первая строка содержит фиксированный порог . Вторая строка содержит число точек . Следующие строк содержат координаты , и для каждой точки, разделителем является символ табуляции (формат строки
"x[TAB]y[TAB]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 |
Где порешать
Попробовать свои силы можно на Контесте
Разбор и готовый код
Разбор
Сначала давайте задумаемся над тем, что должны представлять из себя входные данные. Больше 50% точек, как сказано в условии, относятся к дороге, остальные, как мы можем догадаться, — к другим объектам, которые возвышаются над дорогой. Это могут быть другие автомобили, пешеходы, столбы и т. д. Их возвышение над дорогой может быть достаточно большим по сравнению с заданной величиной отклонения точек дороги .
Получается, для решения задачи нам нужен метод оценки параметров модели, устойчивый к большому количеству выбросов. Одним из таких методов является RANSAC (ссылка на Википедию). Метод итеративно перебирает гипотезы (наборы параметров модели), построенные по случайно выбранным точкам, и выбирает из гипотез лучшую.
Применительно к задаче шаг его итерации можно описать так:
В базовом варианте шаг реализовывается довольно просто. Ниже — пример кода. Пример не является продакшен-кодом и опирается на то, что входные данные соответствуют описанию задачи.
Получается, для решения задачи нам нужен метод оценки параметров модели, устойчивый к большому количеству выбросов. Одним из таких методов является RANSAC (ссылка на Википедию). Метод итеративно перебирает гипотезы (наборы параметров модели), построенные по случайно выбранным точкам, и выбирает из гипотез лучшую.
Применительно к задаче шаг его итерации можно описать так:
- Берем случайные три точки, вычисляем по ним параметры плоскости (гипотезу).
- Оцениваем, насколько гипотеза хороша — сколько точек с учетом заданного порога можно отнести к плоскости дороги, а сколько нужно признать «выбросами».
- Обновляем лучшую гипотезу, если текущая оказалась лучше.
В базовом варианте шаг реализовывается довольно просто. Ниже — пример кода. Пример не является продакшен-кодом и опирается на то, что входные данные соответствуют описанию задачи.
Код на C++
#include <algorithm>
#include <array>
#include <cmath>
#include <cstdint>
#include <iostream>
#include <random>
#include <vector>
struct Point3d {
double X = 0.0;
double Y = 0.0;
double Z = 0.0;
};
using PointCloud = std::vector<Point3d>;
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) <= threshold; });
}
Plane FindPlaneWithFullSearch(const PointCloud& cloud, double threshold) {
const size_t cloud_size = cloud.size();
Plane best_plane;
int best_number_of_inliers = 0;
for (size_t i = 0; i < cloud_size - 2; ++i) {
for (size_t j = i + 1; j < cloud_size - 1; ++j) {
for (size_t k = j + 1; k < cloud_size; ++k) {
Plane sample_plane;
if (!CreatePlane(sample_plane, cloud[i], cloud[j], cloud[k])) {
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;
}
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 <= 25000 without overflow
const auto cloud_size_ull = static_cast<uint64_t>(cloud_size);
const auto number_of_combinations =
cloud_size_ull * (cloud_size_ull - 1) * (cloud_size_ull - 2) / 6;
if (number_of_combinations <= iterations) {
return FindPlaneWithFullSearch(cloud, threshold);
}
std::random_device rd;
std::mt19937 gen(rd());
std::uniform_int_distribution<size_t> distr(0, cloud_size - 1);
Plane best_plane;
int best_number_of_inliers = 0;
for (size_t i = 0; i < iterations; ++i) {
std::array<size_t, 3> 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 << plane.A << ' '
<< plane.B << ' '
<< plane.C << ' '
<< plane.D << std::endl;
return 0;
}
Комментарии к коду
Посмотрим на кусок кода, представленного выше:
В этих строках выполняется вычисление параметров плоскости по трем точкам (ссылка на Википедию). В первой строке должны стоять элементы , и . Если вычислять определитель этой матрицы через алгебраические дополнения для первой строки, то дополнения войдут в итоговое выражение как коэффициенты при , и , то есть будут как раз коэффициентами , и плоскости.
При выборе случайной тройки индексов можно дополнительно учитывать, какие тройки уже встречались. Для этого можно завести unordered_set и записывать в него идентификаторы троек.
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);
В этих строках выполняется вычисление параметров плоскости по трем точкам (ссылка на Википедию). В первой строке должны стоять элементы , и . Если вычислять определитель этой матрицы через алгебраические дополнения для первой строки, то дополнения войдут в итоговое выражение как коэффициенты при , и , то есть будут как раз коэффициентами , и плоскости.
При выборе случайной тройки индексов можно дополнительно учитывать, какие тройки уже встречались. Для этого можно завести unordered_set и записывать в него идентификаторы троек.