🚻 💬 👩🏾‍🏫 Kann gelöst werden: das Problem um die Lidarwolke vom Team der unbemannten Fahrzeuge Yandex 🤪 💪🏽 👉🏿

Mein Name ist Andrey Gladkov, ich bin ein Entwickler in Richtung unbemannter Fahrzeuge. Heute werde ich der Habr-Community eine Aufgabe mitteilen, die sich auf den wichtigsten Sensor einer Drohne bezieht - Lidar und wie wir Lidar-Daten verarbeiten. Sie können versuchen, das Problem selbst auf der Wettbewerbsplattform zu lösen. Das System überprüft die Lösung mithilfe von Autotests und meldet das Ergebnis sofort. Analyse- und Lösungscode - in Spoilern gegen Ende des Beitrags. Für diejenigen, die letztes Jahr an dem Treffen in unserem Workshop teilgenommen haben, wird die Aufgabe vertraut erscheinen - wir haben sie als Eintrittskarte angeboten, aber nie öffentlich geteilt.

Bedingung

Zeitlimit	15 Sekunden
Speicherbeschränkung	64 MB
Eingang	Standardeingabe oder input.txt
Ausgabe	Standardausgabe oder output.txt

Ein unbemanntes Fahrzeug steht auf einer ebenen Asphaltoberfläche, auf deren Dach ein Lidar installiert ist. Die vom Lidar für eine Abtastperiode erhaltenen Messungen sind angegeben.

Abmessungen sind eine Reihe von

N

$N$ Punkte mit Koordinaten

x

$x$ ,

y

$y$ und

z

$z$ ... Mehr als 50% der Punkte gehören zur Straße. Das Modell der Position von Punkten, die zur Straße im Raum gehören, ist eine Ebene mit Parametrisierung

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

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

Punkte, die zur Straße gehören, weichen nicht mehr als einen bestimmten Betrag vom Modell ab

p

$p$ ...

Parameter finden

A, B, C

$A, B, C$ und

D

$D$ das der Straße entsprechende Flugzeug. Die Anzahl der vom Modell abweichenden Punkte beträgt höchstens

p

$p$ sollte mindestens 50% der Gesamtzahl der Punkte betragen.

Eingabeformat

Die Eingabedaten werden im Textformat angegeben. Die erste Zeile enthält einen festen Schwellenwert

p

$p$

(0.01 \leq p \leq 0.5)

$(0.01 ≤ p ≤ 0.5)$ ... Die zweite Zeile enthält die Anzahl der Punkte

N

$N$

(3 \leq N \leq 25000)

$(3 ≤ N ≤ 25 000)$ ... Folgende

N

$N$ Linien enthalten Koordinaten

x

$x$ ,

y

$y$ und

z

$z$

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

$(−100 ≤ x, y, z ≤ 100)$ Für jeden Punkt ist das Trennzeichen ein Tabulatorzeichen (Zeichenfolgenformat "x[TAB]y[TAB]z"). Reelle Zahlen haben nicht mehr als 6 Dezimalstellen.

Ausgabeformat

Ausgabeparameter

A

$A$ ,

B

$B$ ,

C

$C$ und

D

$D$ das der Straße entsprechende Flugzeug. Zahlen durch Leerzeichen trennen. Die Ausgabeparameter müssen die richtige Ebene angeben.

Beispiel 1

Eingang	Ausgabe
0,01 3 20 0 0 10 -10 0 10 10 0	0,000000 0,000000 1,000000 0,000000

Beispiel 2

Eingang	Ausgabe
0,01 3 20 0 3 10 -10 2 10 10 2	-0,099504 0,000000 0,995037 -0,995037

Beispiel 3

Eingang	Ausgabe
0,01 zehn 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

Diese Beispiele sind synthetisch. In einer realen Punktwolke gibt es viel mehr Objekte: Arbeiten Sie an Randfällen.

Wo soll man sich entscheiden?

Sie können sich beim Wettbewerb versuchen

Analyse und fertiger Code

Parsing

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

p

$p$ .

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

:

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

. — . - , .

C ++ - Code

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

Code Kommentare

, :

  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 .

Kann gelöst werden: das Problem um die Lidarwolke vom Team der unbemannten Fahrzeuge Yandex

Bedingung

Eingabeformat

Ausgabeformat

Beispiel 1

Beispiel 2

Beispiel 3

Wo soll man sich entscheiden?

Analyse und fertiger Code

More articles: