ceres-solver库使用示例

上一篇博客大致说明了下ceres-solver库的编译，然后形成了一个二次开发的库，下面就是用这个二次开发库来写一个简单（其实不太简单）的DEMO来演示ceres-solver库的强大。我们以求解一个非线性的方程（椭球方程）的系数为例子。下面是椭球方程的公式。

我们要求解的就是。为了演示，我通过程序生成了一个单位球上面的一系列坐标，也就是上面的abc均为1，偏移量均为0。为了验证ceres-solver是否可以完成这个工作。

首先我们编写下面的测试代码。

#include "glog/logging.h"
#include "ceres/ceres.h"
#include <vector>

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

struct EllipsoidResidual
{
    /*
     * x, y, z 分别为观测值
    */
    EllipsoidResidual(double x, double y, double z):_x(x), _y(y), _z(z){}

    /*
     *pEllipsoidParameters：-2分别为a、b、c,3-5分别为x0、y0、z0
    */
    template <typename T> bool operator () (const T * const pEllipsoidParameters,  T * residual) const
    {
        residual[0] = T(1.0) - T(_x - pEllipsoidParameters[3])*T(_x - pEllipsoidParameters[3])/T(pEllipsoidParameters[0]*pEllipsoidParameters[0]) -
            T(_y - pEllipsoidParameters[4])*T(_y - pEllipsoidParameters[4])/T(pEllipsoidParameters[1]*pEllipsoidParameters[1]) -
            T(_z - pEllipsoidParameters[5])*T(_z - pEllipsoidParameters[5])/T(pEllipsoidParameters[2]*pEllipsoidParameters[2]);
        return true;
    }
private:
    const double _x;
    const double _y;
    const double _z;
};

struct Point3D
{
    double x;
    double y;
    double z;
};

/*
 * 用于解算椭球参数的类
 * m_EllipsoidParameters：椭球参数的初始值
 * m_bInitParameters:椭球参数是否已经进行初始化
 * m_data：观测值
 * m_options：解算方式选项，可以进行设置
 * m_summary：解算的报告信息
 *
 * 使用方式：
 * （）设置椭球参数的初始值（m_EllipsoidParameters），并标记m_bInitParameters为true
 * （）设置观测值m_data(个数大于)
 * （）调用SolveParameters()函数
 * （）得到结果m_EllipsoidParameters与解算报告信息m_summary
*/
struct EllipsoidFittingSolver
{
    EllipsoidFittingSolver()
    {
        m_options;
        m_options.max_num_iterations = 25;
        m_options.linear_solver_type = ceres::DENSE_QR;
        m_options.minimizer_progress_to_stdout = true;
        m_bInitParameters = false;
    }

    bool SolveParameters()
    {
        if (m_data.size() < 6 || !m_bInitParameters)
        {
            return false;
        }

        Problem problem;
        const int nObservations = m_data.size();
        for (int i = 0; i < nObservations; ++i)
        {
            problem.AddResidualBlock(new AutoDiffCostFunction<EllipsoidResidual, 1, 6>(
                new EllipsoidResidual(m_data[i].x, m_data[i].y, m_data[i].z)),
                NULL,
                m_EllipsoidParameters);
        }

        Solve(m_options, &problem, &m_summary);
        return true;
    }

    bool                        m_bInitParameters;
    double                      m_EllipsoidParameters[6];
    std::vector<Point3D>        m_data;
    Solver::Options             m_options;
    Solver::Summary             m_summary;
};

double* readFile(const char* szFilename,int& nCount)
{
    if(!szFilename)return NULL;
    FILE* fid=fopen(szFilename,"rt");
    if(!fid)return NULL;

    nCount=0;
    fscanf(fid,"%d",&nCount);//读第一行数据，是行数，赋给nCount

    int nSize=sizeof(double)*3*nCount;//根据行数分配大小，要取列数据所以是*nCount
    double* dbData=(double*)malloc(nSize);
    if(!dbData)return NULL;
    memset(dbData,0,nSize);//将申请的内存空间初始化为

    double dbInvalid=0.;
    for(int i=0;i<nCount;i++)
    {
        double* dbTmp=dbData+3*i;
        fscanf(fid,"%lf %lf %lf",dbTmp,dbTmp+1,dbTmp+2);
    }
    fclose(fid);

    return dbData;
}

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

    EllipsoidFittingSolver efs;
    double dInit[6] = {0.5,0.5,0.5,0.5,0.5,0.5};
    memcpy(efs.m_EllipsoidParameters, dInit, sizeof(double)*6);
    efs.m_bInitParameters = true;

    const char* pszFilename = "testdata2.txt";
    fprintf(stderr,"ReadFile begins.\n");
    int nCount=-1;
    double* dbSrcData= readFile(pszFilename, nCount);
    if(nCount<0)
    {
        fprintf(stderr,"ReadFile Error!\n");
        return -1;
    }
    else
        fprintf(stderr,"ReadFile Completed.!\n");

    for (int i=0; i<nCount; i++)
    {
        Point3D pt;
        pt.x = dbSrcData[i*3+0];
        pt.y = dbSrcData[i*3+1];
        pt.z = dbSrcData[i*3+2];

        efs.m_data.push_back(pt);
    }

    free(dbSrcData);

    if(efs.SolveParameters())
    {
        fprintf(stdout,"Solver Success!\n");

        fprintf(stdout,"a=%lf b=%lf c=%lf\n",efs.m_EllipsoidParameters[0],efs.m_EllipsoidParameters[1],efs.m_EllipsoidParameters[2]);
        fprintf(stdout,"x0=%lf y0=%lf z0=%lf\n",efs.m_EllipsoidParameters[3],efs.m_EllipsoidParameters[4],efs.m_EllipsoidParameters[5]);
        fprintf(stdout,"%s\n",efs.m_summary.FullReport().c_str());
    }
    else
        fprintf(stdout,"Solver Failed!\n");

    return 0;
}

测试数据部分截图如下：

新建一个控制台应用程序，然后将第一篇的include和lib目录分别添加到工程中，同时配置引用的lib文件（RELEASE版本引用libglog.lib和ceres_r.lib，DEBUG版本引用libglog.lib和ceres_d.lib），然后编译即可。执行结果如下图所示。

红色区域为迭代过程，蓝色区域为计算的结果，最下面的为拟合报告。从蓝色的区域可以看出，拟合的abc的值为1，偏移量为0，和我们开始指定的球体方程一致。

从代码中可以看出，ceres-solver库不需要对非线性的方程进行求偏导数进行线性化然后再使用最小二乘求解系数。这大大方便了人们手动计算偏导数列系数矩阵，可以避免复杂的计算以及手工计算过程中出现的错误。

最后感谢@末末_happy提供的测试数据，感谢@AegeanSea87编写的测试代码。