ceres 点云平面拟合 实例

最近开始做非线性优化相关工作，需要用到类似ceres库的非线性优化库。

鉴于网络上的实例代码大部分是拷贝的官方的实例代码，为了让大家有更多的借鉴，在此记录点云平面拟合的代码，供大家参考

#include "ceres/ceres.h"
#include "glog/logging.h"

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

struct ExponentialResidual {
  ExponentialResidual(double x, double y, double z)
      : x_(x), y_(y), z_(z) {}

  template <typename T> bool operator()(const T* const a,
                                        const T* const b,
                                        const T* const c,
                                        const T* const d,
                                        T* residual) const {
    T temp = a[0] * x_ + b[0] * y_ + c[0] * z_ + d[0];
    residual[0] = temp * temp / (a[0] * a[0] + b[0] * b[0] + c[0] * c[0]);
    return true;
  }

 private:
  const double x_;
  const double y_;
  const double z_;
};

int main(int argc, char** argv)
{
    google::InitGoogleLogging(argv[0]);

    double a = -20;
    double b = 500;
    double c = 80;
    double d = -70;

    srand(100);

    // 10 * x + 5 * y + 2 * z + 1 = 0;

    std::vector<double> vecValue;

    for (int i = 0 ; i < 50; i++)
    {
        std::vector<double> x_row;
        std::vector<double> y_row;
        std::vector<double> z_row;
        for (int j = 0 ; j < 50; j++)
        {
            float random1 = (rand() % 500 / 1000.0f - 0.25);
            float random2 = (rand() % 500 / 1000.0f - 0.25);
            float random3 = (rand() % 500 / 1000.0f - 0.25);

            vecValue.push_back(i + random1);
            vecValue.push_back(j + random2);
            vecValue.push_back((-100 - 10 * i - 5 * j) / 2.0f + random3);
        }
    }

    Problem problem;
    for (int i = 0; i < vecValue.size() ; i = i + 3)
    {
      problem.AddResidualBlock(
          new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1, 1, 1>(
              new ExponentialResidual(
                  vecValue[i], vecValue[i + 1], vecValue[i + 2])),
          NULL,
          &a,
          &b,
          &c,
          &d);
    }

    Solver::Options options;
    options.max_num_iterations = 80;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;

    Solver::Summary summary;
    Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << "\n";
    std::cout << "Final   a: " << a << " b: " << b << " c: " << c << " d: " << d << "\n";

    getchar();

    return 0;
}