Pode ser resolvido: o problema da nuvem lidar da equipe de veículos não tripulados Yandex





Meu nome é Andrey Gladkov, sou desenvolvedor na direção de veículos não tripulados. Hoje vou compartilhar com a comunidade Habr uma tarefa que está relacionada com o sensor mais importante de um drone - lidar, e como processamos dados lidar. Você pode tentar resolver o problema sozinho na plataforma do concurso. O sistema verificará a solução usando autotestes e relatará imediatamente o resultado. Análise e solução de código - em spoilers no final da postagem. Para aqueles que participaram de um encontro em nosso workshop no ano passado, a tarefa parecerá familiar - oferecemos como um ingresso de admissão, mas nunca a compartilhamos publicamente.



Doença

Limite de tempo 15 segundos
Limitação de memória 64 MB
Entrada entrada padrão ou input.txt
Resultado saída padrão ou output.txt
Um veículo não tripulado fica em uma superfície plana de asfalto com um lidar instalado no teto do veículo. As medidas obtidas pelo lidar para um período de varredura são fornecidas.



As medições são um conjunto deN pontos com coordenadas x, y e z... Mais de 50% dos pontos pertencem à estrada. O modelo da posição dos pontos pertencentes à estrada no espaço é um plano com parametrização

Ax+By+Cz+D=0.

Os pontos que pertencem à estrada se desviam do modelo em não mais do que um determinado valor p...



Parâmetros de localizaçãoA,B,C e Do plano correspondente à estrada. O número de pontos que se desviam do modelo em não mais do quep, deve ser pelo menos 50% do número total de pontos.



Formato de entrada



Os dados de entrada são fornecidos em formato de texto. A primeira linha contém um limite fixop (0.01p0.5)... A segunda linha contém o número de pontosN (3N25000)... Os seguintesN linhas contêm coordenadas x, y e z (100x,y,z100)para cada ponto, o delimitador é um caractere de tabulação (formato de string "x[TAB]y[TAB]z"). Os números reais não têm mais do que 6 casas decimais.



Formato de saída



Parâmetros de saída A, B, C e Do plano correspondente à estrada. Separe os números com espaços. Os parâmetros de saída devem especificar o plano correto.



Exemplo 1

Entrada Resultado
0,01
3
20 0 0
10 -10 0
10 10 0
0,000000 0,000000 1,0000,00 0,000000

Exemplo 2

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

Exemplo 3

Entrada Resultado
0,01
dez
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,9999950 0,000000
Esses exemplos são sintéticos. Existem muito mais objetos em uma nuvem de pontos reais: trabalhe em casos extremos.



Onde decidir



Você pode tentar sua sorte no Concurso



Análise e código finalizado



Análise
, . 50% , , , , , — , . , , . . 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;
}


Comentários 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);


( ). matrix xp1.X, yp1.Y zp1.Z. , x, y z, A, B C .



, . unordered_set .



All Articles