🧚🏾 ♀️ 🤱🏾 可以解决：Yandex无人机团队关于激光雷达云的问题 👐🏾 👲🏾 ↔️

我的名字叫安德烈·格拉德科夫（Andrey Gladkov），我是无人驾驶汽车领域的开发商。今天，我将与Habr社区共享与无人机最重要的传感器-激光雷达以及我们如何处理激光雷达数据有关的任务。您可以尝试自己在竞赛平台上解决问题。系统将使用自动测试来检查解决方案，并立即报告结果。解析和解决方案代码-在发布即将结束时扰乱他人。对于去年参加我们研讨会的聚会的人来说，这项任务看起来很熟悉-我们将其作为入场券提供，但我们从未公开共享。

健康）状况

时限	15秒
内存限制	64兆字节
输入值	标准输入或input.txt
输出量	标准输出或output.txt

无人驾驶车辆站立在平坦的沥青路面上，并在车顶上安装了激光雷达。给出了激光雷达在一个扫描周期内获得的测量结果。

尺寸是一组

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)$ ... 第二行包含点数

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无人机团队关于激光雷达云的问题

健康）状况

输入格式

输出格式

例子1

例子2

例子3

在哪里决定

解析并完成代码

More articles: