Меня зовут Андрей Гладков, я разработчик в направлении беспилотных автомобилей. Сегодня я поделюсь с сообществом Хабра задачей, которая связана с важнейшим сенсором беспилотника лидаром, и с тем, как мы обрабатываем лидарные данные. Вы можете попробовать решить задачу самостоятельно на платформе Контест. Система проверит решение с помощью автотестов и сразу сообщит результат. Разбор и код решения в спойлерах ближе к концу поста. Тем, кто был на митапе в нашем цехе в прошлом году, задача покажется знакомой мы предлагали её в качестве входного билета, но публично никогда ей не делились.
Условие
Ограничение времени | 15 секунд |
Ограничение памяти | 64 МБ |
Ввод | стандартный ввод или input.txt |
Вывод | стандартный вывод или output.txt |
Измерения представляют собой множество из точек, имеющих координаты , и . Больше 50% точек принадлежат дороге. Моделью положения принадлежащих дороге точек в пространстве является плоскость с параметризацией
Точки, которые принадлежат дороге, отклоняются от модели не больше чем на заданную величину .
Необходимо найти параметры и соответствующей дороге плоскости. Число точек, отклоняющихся от модели не больше чем на , должно составлять не менее 50% от общего числа точек.
Формат ввода
Входные данные заданы в текстовом формате. Первая строка содержит фиксированный порог . Вторая строка содержит число точек . Следующие строк содержат координаты , и для каждой точки, разделителем является символ табуляции (формат строки
"x[TAB]y[TAB]z"
). Вещественные числа имеют не более 6
десятичных знаков.Формат вывода
Выведите параметры , , и соответствующей дороге плоскости. Числа разделяйте пробелами. Выведенные параметры должны задавать корректную плоскость.
Пример 1
Ввод | Вывод |
0.013200010-10010100 |
0.000000 0.000000 1.000000 0.000000 |
Пример 2
Ввод | Вывод |
0.013200310-10210102 |
-0.099504 0.000000 0.995037 -0.995037 |
Пример 3
Ввод | Вывод |
0.011020-100.22000.220100.215-100.151500.1515100.1510-100.110100.120181.715-151.2 |
-0.010000 0.000000 0.999950 0.000000 |
Где порешать
Попробовать свои силы можно на Контесте: https://contest.yandex.ru/contest/12698/enter/
Разбор и готовый код
Получается, для решения задачи нам нужен метод оценки параметров модели, устойчивый к большому количеству выбросов. Одним из таких методов является RANSAC (ссылка на Википедию). Метод итеративно перебирает гипотезы (наборы параметров модели), построенные по случайно выбранным точкам, и выбирает из гипотез лучшую.
Применительно к задаче шаг его итерации можно описать так:
- Берем случайные три точки, вычисляем по ним параметры плоскости (гипотезу).
- Оцениваем, насколько гипотеза хороша сколько точек с учетом заданного порога можно отнести к плоскости дороги, а сколько нужно признать выбросами.
- Обновляем лучшую гипотезу, если текущая оказалась лучше.
В базовом варианте шаг реализовывается довольно просто. Ниже пример кода. Пример не является продакшен-кодом и опирается на то, что входные данные соответствуют описанию задачи.
#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;}
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 и записывать в него идентификаторы троек.