🌷 ⬛️ 🆑 Se puede resolver: el problema sobre la nube lidar del equipo de vehículos no tripulados Yandex ♉️ 🍯 🔠

Mi nombre es Andrey Gladkov, soy un desarrollador en la dirección de vehículos no tripulados. Hoy compartiré con la comunidad de Habr una tarea relacionada con el sensor más importante de un dron: el lidar, y cómo procesamos los datos del lidar. Puedes intentar resolver el problema tú mismo en la plataforma del concurso. El sistema verificará la solución mediante pruebas automáticas e informará inmediatamente el resultado. Código de análisis y solución: en spoilers hacia el final de la publicación. Para aquellos que asistieron a la reunión en nuestro taller el año pasado, la tarea les parecerá familiar: la ofrecimos como boleto de admisión, pero nunca la compartimos públicamente.

Condición

Límite de tiempo	15 segundos
Limitación de memoria	64 MB
Entrada	entrada estándar o input.txt
Salida	salida estándar o output.txt

Un vehículo no tripulado se encuentra sobre una superficie plana de asfalto, con un lidar instalado en el techo del vehículo. Se dan las medidas obtenidas por el lidar para un período de escaneo.

Las dimensiones son un conjunto de

N

$N$ puntos con coordenadas

x

$x$ ,

y

$y$ y

z

$z$ ... Más del 50% de los puntos pertenecen a la carretera. El modelo de la posición de los puntos pertenecientes a la carretera en el espacio es un plano con parametrización

A \cdot x + B \cdot y + C \cdot z + D = 0.

$A\cdot x+B \cdot y + C \cdot z + D=0.$

Los puntos que pertenecen a la carretera se desvían del modelo por no más de una cantidad determinada

p

$p$ ...

Encontrar parámetros

A, B, C

$A, B, C$ y

D

$D$ el plano correspondiente a la carretera. El número de puntos que se desvían del modelo en no más de

p

$p$ , debe ser al menos el 50% del número total de puntos.

Formato de entrada

Los datos de entrada se dan en formato de texto. La primera línea contiene un umbral fijo

p

$p$

(0.01 \leq p \leq 0.5)

$(0.01 ≤ p ≤ 0.5)$ ... La segunda línea contiene el número de puntos

N

$N$

(3 \leq N \leq 25000)

$(3 ≤ N ≤ 25 000)$ ... El seguimiento

N

$N$ las líneas contienen coordenadas

x

$x$ ,

y

$y$ y

z

$z$

(- 100 \leq x, y, z \leq 100)

$(−100 ≤ x, y, z ≤ 100)$ para cada punto, el delimitador es un carácter de tabulación (formato de cadena "x[TAB]y[TAB]z"). Los números reales no tienen más de 6 lugares decimales.

Formato de salida

Parámetros de salida

A

$A$ ,

B

$B$ ,

C

$C$ y

D

$D$ el plano correspondiente a la carretera. Números separados con espacios. Los parámetros de salida deben especificar el plano correcto.

Ejemplo 1

Entrada	Salida
0,01 3 20 0 0 10 -10 0 10 10 0	0,000000 0,000000 1,000000 0,000000

Ejemplo 2

Entrada	Salida
0,01 3 20 0 3 10 -10 2 10 10 2	-0,099504 0,000000 0,995037 -0,995037

Ejemplo 3

Entrada	Salida
0,01 diez 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

Estos ejemplos son sintéticos. Hay muchos más objetos en una nube de puntos real: trabaje en casos extremos.

Donde decidir

Puedes probar suerte en el Concurso

Análisis y código terminado

Analizando

, . 50% , , , , , — , . , , . .

p

$p$ .

, , . RANSAC ( ). ( ), , .

:

, ().
, — $p$ , «».
, .

. — . - , .

Código 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;
}

Comentarios de código

, :

  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);

( ).

m a t r i x

$matrix$

x - p 1. X

$x - p1.X$ ,

y - p 1. Y

$y - p1.Y$

z - p 1. Z

$z - p1.Z$ . ,

x

$x$ ,

y

$y$

z

$z$ ,

A

$A$ ,

B

$B$

C

$C$ .

, . unordered_set .

Se puede resolver: el problema sobre la nube lidar del equipo de vehículos no tripulados Yandex

Condición

Formato de entrada

Formato de salida

Ejemplo 1

Ejemplo 2

Ejemplo 3

Donde decidir

Análisis y código terminado

More articles: