Clever Geek Handbook

Can be solved: the problem about the lidar cloud from the team of unmanned vehicles Yandex





My name is Andrey Gladkov, I am a developer in the direction of unmanned vehicles. Today I will share with the Habr community a task that is related to the most important sensor of a drone - lidar, and how we process lidar data. You can try to solve the problem yourself on the Contest platform. The system will check the solution using autotests and immediately report the result. Parsing and solution code - in spoilers towards the end of the post. For those who attended the meetup in our workshop last year, the task will seem familiar - we offered it as an admission ticket, but we never shared it publicly.



Condition

Time limit 15 seconds
Memory limitation 64 MB
Input standard input or input.txt
Output standard output or output.txt
An unmanned vehicle stands on a flat asphalt surface with a lidar installed on the roof of the vehicle. The measurements obtained by the lidar for one scanning period are given.



Measurements are a set ofN points with coordinates x, y and z... More than 50% of points belong to the road. The model of the position of points belonging to the road in space is a plane with parameterization

A cdotx+B cdoty+C cdotz+D=0.

Points that belong to the road deviate from the model by no more than a given amount p...



Find parametersA,B,C and Dthe plane corresponding to the road. The number of points deviating from the model by no more thanp, should be at least 50% of the total number of points.



Input format



The input data is given in text format. The first line contains a fixed thresholdp (0.01≤p≤0.5)... The second line contains the number of pointsN (3≤N≤25,000)... The followingN lines contain coordinates x, y and z (−100≤x,y,z≤100)for each point, the delimiter is a tab character (string format "x[TAB]y[TAB]z"). Real numbers have no more than 6 decimal places.



Output format



Output parameters A, B, C and Dthe plane corresponding to the road. Separate numbers with spaces. The output parameters must specify the correct plane.



Example 1

Input Output 
0.01
3
20 0 0
10 -10 0
10 10 0
 
0.000000 0.000000 1.000000 0.000000

Example 2

Input Output 
0.01
3
20 0 3
10 -10 2
10 10 2
 
-0.099504 0.000000 0.995037 -0.995037

Example 3

Input Output 
0.01
ten
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
These examples are synthetic. There are much more objects in a real point cloud: work on edge cases.



Where to decide



You can try your hand at the Contest



Parsing and finished code



Parsing
, . 50% , , , , , — , . , , . . 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 Comments
, : 



  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 x−p1.X, y−p1.Y z−p1.Z. , x, y z, A, B C .



, . unordered_set .


More articles:


All Articles