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