Plane Fitting Method

plane-fiitting

Plane equation: ax + by + cz + d = 0, where the coefficients a, b, c form the normal vector of the plane. We need at least three coordinate points to determine the coefficients of the equation, and we can construct the following matrix form:

                    Ax = 0 where x = [a, b, c, d], A is the matrix formed by n coordinate points (xi, yi, zi), and 1 is an augmented column. Using SVD matrix decomposition, we can easily solve for the desired parameter vector, as shown below:

Eigen::Vector4d GetPlaneCofficient(const Eigen::Vector3d& point1,
                                   const Eigen::Vector3d& point2,
                                   const Eigen::Vector3d& point3) {
  Eigen::Matrix<double, 3, 4> A;
  A << point1.x(), point1.y(), point1.z(), 1,
       point2.x(), point2.y(), point2.z(), 1,
       point3.x(), point3.y(), point3.z(), 1;
  const Eigen::JacobiSVD<Eigen::Matrix<double, 3, 4>> init_svd
      (A,Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Vector4d v = init_svd.matrixV().col(3);
  return v;
}

When we have a large number of plane points available as observations, we can first use a RANSAC scheme to filter out the inlier points, and then use all of the inlier points to optimize and solve for the plane parameters, as shown below:

void FindViaRansac(std::vector<Eigen::Vector3d>& road_points,
                   Eigen::Vector4d& plane_cofficient) {
  int best_inlier_count = 0;
  std::vector<int> best_inlier_indexs;
  Eigen::Vector4d best_plane_coff;
  for(int i = 0; i < max_iter_times; i++) {
    std::vector<int> random_index =
        create_random_array(3, 0, static_cast<int>(road_points.size() - 1));
    auto point1 = road_points[random_index[0]];
    auto point2 = road_points[random_index[1]];
    auto point3 = road_points[random_index[2]];
    Eigen::Vector4d plane_coff = GetPlaneCofficient(point1, point2, point3);
    int inlier_count = 0;
    std::vector<int> inlier_indexs;
    for(int j = 0; j < road_points.size(); j++) {
      if(PointToPlaneDis(road_points[j], plane_coff) < thres_dis_point_plane) {
        inlier_count++;
        inlier_indexs.push_back(j);
      }
    }
    if(inlier_count > best_inlier_count) {
      best_inlier_count = inlier_count;
      best_inlier_indexs = inlier_indexs;
      best_plane_coff = plane_coff;
    }
  }

  std::vector<Eigen::Vector3d> inlier_points(best_inlier_count);
  for(int i = 0; i < best_inlier_indexs.size(); i++) {
    inlier_points[i] = (road_points[best_inlier_indexs[i]]);
  }
}

Through random sampling, we obtain the plane parameters that yield the largest number of inliers. Using this set of parameters as the initial value, we then perform parameter estimation with an optimizer such as g2o/ceres. An inlier is defined by the criterion that the distance from the point to the plane should be smaller than a certain threshold.

The definitions of the vertex, edge, and residual in g2o/ceres are as follows:

#pragma once

#include "g2o/core/base_unary_edge.h"
#include "g2o/core/base_vertex.h"

#include <ceres/autodiff_cost_function.h>
#include <ceres/ceres.h>
#include <ceres/loss_function.h>

#include <Eigen/Core>

// Eigen::Vector4d is A, B, C, D
class VeretxPlane : public g2o::BaseVertex<4, Eigen::Vector4d> {
public:
  VeretxPlane() : g2o::BaseVertex<4, Eigen::Vector4d>() {}

  bool read(std::istream &is) { return false; };
  bool write(std::ostream &os) const { return false; };

  virtual void setToOriginImpl() {
    _estimate.setZero();
  }

  virtual void oplusImpl(const double *update_) {
    Eigen::Map<const Eigen::Vector4d> update(update_);
    setEstimate(update + estimate());
  }
};

class EdgePlanePoint : public g2o::BaseUnaryEdge<1, double, VeretxPlane> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgePlanePoint() = delete;

  EdgePlanePoint(const Eigen::Vector3d& plane_point) {
    plane_point_ = plane_point;
  }

  virtual void computeError() {
    const VeretxPlane* v = static_cast<VeretxPlane*>(_vertices[0]);
    Eigen::Vector4d plane_cofficients = v->estimate();
    _error(0, 0) = plane_cofficients[0] * plane_point_.x() +
                 plane_cofficients[1] * plane_point_.y() +
                 plane_cofficients[2] * plane_point_.z() +
                 plane_cofficients[3];
  }
  virtual void linearizeOplus() {
    Eigen::Matrix<double, 1, 4> jaco;
    jaco << plane_point_.x(), plane_point_.y(), plane_point_.z(), 1;
    _jacobianOplusXi = jaco;
  }

  // dummy read and write functions because we don't care...
  virtual bool read(std::istream &in) { return false; }
  virtual bool write(std::ostream &out) const { return false; }

public:
  Eigen::Vector3d plane_point_;
};

class PlanePointResidual {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  PlanePointResidual(Eigen::Vector3d plane_point) {
    plane_point_ = plane_point;
  }

  template <typename T>
  bool operator()(const T* a, const T* b, const T* c,
                  const T* d, T* residual) const {
    residual[0] = *a * T(plane_point_.x()) +
                  *b * T(plane_point_.y()) +
                  *c * T(plane_point_.z()) +
                  *d;
    return true;
  }

  static ceres::CostFunction* Create(Eigen::Vector3d plane_point) {
    return new ceres::AutoDiffCostFunction<PlanePointResidual, 1, 1, 1, 1, 1>(
          new PlanePointResidual(plane_point));
  }

  Eigen::Vector3d plane_point_;
};

Finally, we simply use the optimizer to solve for the parameters:

Eigen::Vector4d GetPlaneCofficientByCeres(
    const std::vector<Eigen::Vector3d>& road_points,
    Eigen::Vector4d init_plane_coff) {
  double a = init_plane_coff[0];
  double b = init_plane_coff[1];
  double c = init_plane_coff[2];
  double d = init_plane_coff[3];

  ceres::Problem problem;
  for(int i = 0; i < road_points.size(); i++) {
    ceres::CostFunction* cost_function =
      PlanePointResidual::Create(road_points[i]);
    problem.AddResidualBlock(cost_function, nullptr, &a, &b, &c, &d);
  }
  // Run
  ceres::Solver::Options options;
  options.max_num_iterations = 10;
  options.linear_solver_type = ceres::DENSE_QR;
  //options.logging_type = ceres::LoggingType::SILENT;
  options.minimizer_type = ceres::TRUST_REGION;
  options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  Eigen::Vector4d result(a, b, c, d);
  result /= result[3];
  return result;
}

plane-fiitting

Comments