ceres 点云平面拟合 实例

Posted 2021-03-06 stevenwind

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

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

#include <iostream>
#include <ceres/ceres.h>
#include <glog/logging.h>

struct PlaneFitResidual
{
PlaneFitResidual(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,
T* residual) const {

///< dist from point to plane as the residual
residual[0] =
(a[0] * x_ + b[0] * y_ + c[0] * z_ + 1.0) /
sqrt(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]);

///< set the initial parameter
double a = 100;
double b = 100;
double c = 100;

srand(100);

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

/// generate a plane dataset with noise
std::vector<double> vecPos;
for (int i = 0 ; i < 50 ; i++)
{
for (int j = 0 ; j < 50 ; j++)
{
double x = i;
double y = j;
double z = (-1 - 10.0 * x - 5.0 * y) / 2.0;

double random1 = rand() % 100 / 200.0 - 0.25;
double random2 = rand() % 100 / 200.0 - 0.25;
double random3 = rand() % 100 / 200.0 - 0.25;

vecPos.push_back(x);
vecPos.push_back(y);
vecPos.push_back(z + random3);
}
}

///< add data
ceres::Problem problem;
for (int i = 0 ; i < vecPos.size(); i += 3)
{
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<PlaneFitResidual, 1, 1, 1, 1>(
new PlaneFitResidual(vecPos[i], vecPos[i + 1], vecPos[i + 2])),
NULL, &a, &b, &c);
}

///< solve options
ceres::Solver::Options options;
options.max_num_iterations = 25;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;

///<< solve it
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

std::cout << summary.BriefReport() << std::endl;
std::cout << "Final a: " << a << " b: " << b << " c: " << c << std::endl;

getchar();

return 0;
}