☀️ 🚵🏿 🍌 Peut être résolu: le problème du cloud lidar de l'équipe de véhicules sans pilote Yandex 🎥 🤸🏻 🌘

Je m'appelle Andrey Gladkov, je suis développeur dans le sens des véhicules sans pilote. Aujourd'hui, je vais partager avec la communauté Habr une tâche liée au capteur le plus important d'un drone - lidar, et comment nous traitons les données lidar. Vous pouvez essayer de résoudre le problème vous-même sur la plateforme du concours. Le système vérifiera la solution à l'aide de tests automatiques et rapportera immédiatement le résultat. Code d'analyse et de solution - dans les spoilers vers la fin du message. Pour ceux qui ont assisté à la rencontre dans notre atelier l'année dernière, la tâche semblera familière - nous l'avons offerte comme billet d'entrée, mais nous ne l'avons jamais partagée publiquement.

État

Limite de temps	15 secondes
Limitation de la mémoire	64 Mo
Contribution	entrée standard ou input.txt
Production	sortie standard ou output.txt

Un véhicule sans pilote se dresse sur une surface plate d'asphalte, avec un lidar installé sur le toit du véhicule. Les mesures obtenues par le lidar en une période de balayage sont données.

Les mesures sont un ensemble de

N

$N$ points avec coordonnées

x

$x$ ,

y

$y$ et

z

$z$ ... Plus de 50% des points appartiennent à la route. Le modèle de la position des points appartenant à la route dans l'espace est un plan avec paramétrage

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

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

Les points appartenant à la route ne s'écartent pas du modèle de plus d'une valeur donnée

p

$p$ ...

Rechercher des paramètres

A, B, C

$A, B, C$ et

D

$D$ le plan correspondant à la route. Le nombre de points s'écartant du modèle de pas plus de

p

$p$ , doit représenter au moins 50% du nombre total de points.

Format d'entrée

Les données d'entrée sont fournies au format texte. La première ligne contient un seuil fixe

p

$p$

(0.01 \leq p \leq 0.5)

$(0.01 ≤ p ≤ 0.5)$ ... La deuxième ligne contient le nombre de points

N

$N$

(3 \leq N \leq 25000)

$(3 ≤ N ≤ 25 000)$ ... Le suivant

N

$N$ les lignes contiennent des coordonnées

x

$x$ ,

y

$y$ et

z

$z$

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

$(−100 ≤ x, y, z ≤ 100)$ pour chaque point, le délimiteur est un caractère de tabulation (format chaîne "x[TAB]y[TAB]z"). Les nombres réels n'ont pas plus de 6 décimales.

Format de sortie

Paramètres de sortie

A

$A$ ,

B

$B$ ,

C

$C$ et

D

$D$ le plan correspondant à la route. Séparez les nombres avec des espaces. Les paramètres de sortie doivent spécifier le plan correct.

Exemple 1

Contribution	Production
0,01 3 20 0 0 10 -10 0 10 10 0	0,000000 0,000000 1,000000 0,000000

Exemple 2

Contribution	Production
0,01 3 20 0 3 10 -10 2 10 10 2	-0,099504 0,00000000 0,995037 -0,995037

Exemple 3

Contribution	Production
0,01 Dix 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

Ces exemples sont synthétiques. Il y a beaucoup plus d'objets dans un vrai nuage de points: travaillez sur des cas de bord.

Où décider

Vous pouvez vous essayer au concours

Analyse et code terminé

Analyse

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

p

$p$ .

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

:

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

. — . - , .

Code 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;
}

Commentaires sur le 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);

( ).

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 .

Peut être résolu: le problème du cloud lidar de l'équipe de véhicules sans pilote Yandex

État

Format d'entrée

Format de sortie

Exemple 1

Exemple 2

Exemple 3

Où décider

Analyse et code terminé

More articles: