✨ 🚹 🛶 解決できる：無人車両Yandexのチームからのlidarクラウドに関する問題 😽 🌛 🛋️

私の名前はアンドレイ・グラドコフです。私は無人車両の開発者です。今日は、ドローンの最も重要なセンサーであるリダーと、リダーデータの処理方法に関連するタスクをHabrコミュニティと共有します。コンテストプラットフォームで自分で問題の解決を試みることができます。システムは自動テストを使用してソリューションをチェックし、すぐに結果を報告します。解析とソリューションコード-投稿の終わりに向かってスポイラーで。昨年のワークショップでのミートアップに参加した人にとっては、タスクはおなじみのように思えます。入場券として提供しましたが、公開することはありませんでした。

状態

制限時間	15秒
メモリ制限	64 MB
入力	標準入力またはinput.txt
出力	標準出力またはoutput.txt

無人車両は、アスファルトの平らな面に立っており、車両の屋根にリダーが取り付けられています。1回のスキャン期間でリダーによって得られた測定値が示されています。

測定値は、

N

$N$ 座標を持つポイント

x

$x$ 、

y

$y$ そして

z

$z$ ..。ポイントの50％以上が道路に属しています。空間内の道路に属する点の位置のモデルは、パラメータ化された平面です

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

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

道路に属するポイントは、モデルから指定された量だけ逸脱します。

p

$p$ ..。

パラメータを見つける

A, B, C

$A, B, C$ そして

D

$D$ 道路に対応する飛行機。モデルからわずかに逸脱しているポイントの数

p

$p$ 、ポイントの総数の少なくとも50％である必要があります。

入力フォーマット

入力データはテキスト形式で提供されます。最初の行には固定しきい値が含まれています

p

$p$

(0.01 \leq p \leq 0.5)

$(0.01 ≤ p ≤ 0.5)$ ..。2行目にはポイント数が含まれています

N

$N$

(3 \leq N \leq 25000)

$(3 ≤ N ≤ 25 000)$ ..。以下

N

$N$ 線には座標が含まれています

x

$x$ 、

y

$y$ そして

z

$z$

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

$(−100 ≤ x, y, z ≤ 100)$ 各ポイントの区切り文字はタブ文字（文字列形式"x[TAB]y[TAB]z"）です。実数の小数点以下の桁数は6以下です。

出力フォーマット

出力パラメータ

A

$A$ 、

B

$B$ 、

C

$C$ そして

D

$D$ 道路に対応する飛行機。数字はスペースで区切ります。出力パラメータは正しい平面を指定する必要があります。

例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 十 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% , , , , , — , . , , . .

p

$p$ .

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

:

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

. — . - , .

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

コードコメント

, :

  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 .

解決できる：無人車両Yandexのチームからのlidarクラウドに関する問題

状態

入力フォーマット

出力フォーマット

例1

例2

例3

どこで決めるか

コードの解析と完成

More articles: