plane fitting method

                    Ax = 0 其中x = [a, b, c, d]，A为n个坐标点(xi, yi, zi)构成的矩阵， 1为增广的一列，使用svd矩阵分解可以很容易的求解待求的参数向量，如下所示:

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

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

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

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