ceres 学习笔记

介绍

Ceres 可以解决以下形式的边界约束鲁棒化非线性最小二乘问题

• 表达式${\rho }_i\left({\Vert f_i\left(x_{i_1},\dots ,x_{i_k}\right)\Vert }^2\right)$被称为ResidualBlock，
• 其中$f_i(.)$是CostFunction
• $(x_{i_1},\dots ,x_{i_k})$这样的一组标量被称为ParameterBlock
• ${\rho }_i$是一个LossFunction,是标量函数，用于减少异常值对非线性最小二乘问题的解决方案的影响。（核函数）

基本流程

1. 构建代价函数结构体
2. 声明一个problem
3. 构造cost_function
4. 向problem中添加残差项
5. 配置求解器
6. 开始优化

例题1 helloword

首先，考虑寻找函数最小值的问题

1. 第一步编写一个函数来评估这个函数$f(x)=10-x$

struct CostFunctor{
    template<typename T>
    bool operator()(const T* const x, T* residual ) const{
        residual[0] = 10.0 - x[0];
        return true;
    } };

这里要注意的是，operator()是一个模板化方法，它假定其所有输入和输出都属于某种类型 T。此处使用模板允许 Ceres在仅需要残差值时调用 CostFunctor::operator ()

2. 一旦我们有了计算残差函数的方法，就可以使用它构建非线性最小二乘问题了。

int main(int argc, char **argv)
{
    google::InitGoogleLogging(argv[0]);
    // 要用初始值求解的变量
    double initial_x = 5.0;
    double x = initial_x;

    // 声明一个problem
    ceres::Problem problem;

    // 设置唯一的cost function(残差).cost function使用自动微分来获得导数(雅可比矩阵)
    ceres:: CostFunction* costFunction =  // 使用自动求导，模板参数: 误差类型，输出维度，输入维度，维度要与前面的struct一致
            new ceres::AutoDiffCostFunction<CostFunctor,1,1>(new CostFunctor);

    // 向问题中添加误差项
    problem.AddResidualBlock(costFunction, nullptr, &x);

    // 配置求解器
    ceres::Solver::Options options;
    // 增量方程如何求解
    options.linear_solver_type = ceres::DENSE_QR;
    // 输出到cout
    options.minimizer_progress_to_stdout = true;
    // 优化信息
    ceres::Solver::Summary summary;
    // 开始优化
    ceres::Solve(options,&problem,&summary);

    // 输出结果
    std::cout << summary.BriefReport() << std::endl;
    std::cout << "x : " << initial_x << " -> " << x << std::endl;

    return 0;

}

AutoDiffCostFunction将 CostFunctor作为输入，自动区分并给它一个CostFunction 接口。

3. 编译和运行结果

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.250000e+01    0.00e+00    5.00e+00   0.00e+00   0.00e+00  1.00e+04        0    2.11e-05    6.17e-05
   1  1.249750e-07    1.25e+01    5.00e-04   5.00e+00   1.00e+00  3.00e+04        1    6.98e-05    2.03e-04
   2  1.388518e-16    1.25e-07    1.67e-08   5.00e-04   1.00e+00  9.00e+04        1    8.23e-06    2.25e-04
Ceres Solver Report: Iterations: 3, Initial cost: 1.250000e+01, Final cost: 1.388518e-16, Termination: CONVERGENCE
x : 5 -> 10

4. CMakeLists :

CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
PROJECT(ceres)

SET(CMAKE_BUILD_TYPE Release)
SET(CMAKE_CXX_FLAGS "-std=c++14 -o3")

find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})

include_directories("/usr/include/eigen3")

ADD_EXECUTABLE(ceres_test1 ceres_test1.cpp)
TARGET_LINK_LIBRARIES(ceres_test1 ${CERES_LIBRARIES})

5. 完整代码：

#include 
#include "ceres/ceres.h"

struct CostFunctor{
    template<typename T>
    bool operator()(const T* const x, T* residual ) const{
        residual[0] = 10.0 - x[0];
        return true;
    }
};

int main(int argc, char **argv)
{
    google::InitGoogleLogging(argv[0]);
    // 要用初始值求解的变量
    double initial_x = 5.0;
    double x = initial_x;

    // 声明一个problem
    ceres::Problem problem;

    // 设置唯一的cost function(残差).cost function使用自动微分来获得导数(雅可比矩阵)
    ceres:: CostFunction* costFunction =  // 使用自动求导，模板参数: 误差类型，输出维度，输入维度，维度要与前面的struct一致
            new ceres::AutoDiffCostFunction<CostFunctor,1,1>(new CostFunctor);

    // 向问题中添加误差项
    problem.AddResidualBlock(costFunction, nullptr, &x);

    // 配置求解器
    ceres::Solver::Options options;
    // 增量方程如何求解
    options.linear_solver_type = ceres::DENSE_QR;
    // 输出到cout
    options.minimizer_progress_to_stdout = true;
    // 优化信息
    ceres::Solver::Summary summary;
    // 开始优化
    ceres::Solve(options,&problem,&summary);

    // 输出结果
    std::cout << summary.BriefReport() << std::endl;
    std::cout << "x : " << initial_x << " -> " << x << std::endl;

    return 0;
}

数值法求导

在某些情况下，像在Hello World中一样定义一个代价函数是不可能的。比如在求解残差值（residual）的时候调用了一个库函数，而这个库函数的内部算法你根本无法干预。在这种情况下数值微分算法就派上用场了。
比如对于 f(x)=10−x 对应函数体如下：

struct CostFunctor {
  bool operator()(const double* const x, double* residual) const {
    residual[0] = 10.0 - x[0];
    return true;
  }};

对比例题1： 没有使用模板类

struct CostFunctor{
    template<typename T>
    bool operator()(const T* const x, T* residual ) const{
        residual[0] = 10.0 - x[0];
        return true;
    } };

然后继续添加problem

ceres::CostFunction* cost_function =
  new ceres::NumericDiffCostFunction<CostFunctor, ceres::CENTRAL, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);

对比例题1： 没有使用自动求导而是构建一个NumericDiffCostFunction数值微分代价函数.同时在用Nummeric算法时需要额外给定一个参数ceres::CENTRAL 。这个参数告诉计算机如何计算导数。更多具体介绍可以参看NumericDiffCostFunction的Doc文档

ceres:: CostFunction* costFunction =  // 使用自动求导，模板参数: 误差类型，输出维度，输入维度，维度要与前面的struct一致
 new ceres::AutoDiffCostFunction<CostFunctor,1,1>(new CostFunctor);
problem.AddResidualBlock(costFunction, nullptr, &x);// 向问题中添加误差项

Ceres官方更加推荐自动微分算法，因为C++模板类使自动算法有更高的效率。数值微分算法通常来说计算更复杂，收敛更缓慢。

例题2:求解鲍威尔方程的最小值

我们定义参数块$x=[x_1,x_2,x_3,x_4]$,以及代价函数：

$F(x)$ 是关于上面四个残差值的方程。我们希望能找到一组x，使$\frac{1}{2}\Vert F(x){\Vert }^2$取得最小值。

1. 同样第一步依然是定义在这四个残差方程。

struct F1{
    template<typename T>
    bool operator()(const T* const x1, const T* const x2, T* residual)const{
        residual[0] = x1[0] + 10.0 * x2[0];
        return true;
    }
};

struct F2{
    template<typename T>
    bool operator()(const T* const x3, const T* const x4, T* residual) const{
        residual[0] = sqrt(5) * (x3[0] - x4[0]);
        return true;
    }
};

struct F3{
    template<typename T>
    bool operator()(const T* const x2, const T* const x3, T* residual) const {
        residual[0] = (x2[0] - 2.0 * x3[0]) * (x2[0] - 2.0 * x3[0]);
        return true;
    }
};

struct F4{
    template<typename T>
    bool operator()(const T* const x1,const T* const x4, T* residual) const{
        residual[0] = sqrt(10.0) * (x1[0] - x4[0]) * (x1[0] - x4[0]);
        return true;
    }
};

对比例题1，因为多了一个变量，所以要多加一个const T* const x4。

struct CostFunctor{
    template<typename T>
    bool operator()(const T* const x, T* residual ) const{
        residual[0] = 10.0 - x[0];
        return true;
    } };

2. 然后将各个残差块加入到problem中。

double x1 = 3.0, x2 = -1.0, x3 = 0.0, x4 = 1.0;
    ceres::Problem problem;
    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<F1,1,1,1>(new F1),NULL,&x1,&x2);
    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<F2,1,1,1>(new F2),NULL,&x3,&x4);
    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<F3,1,1,1>(new F3),NULL,&x2,&x3);
    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<F4,1,1,1>(new F4),NULL,&x1,&x4);

对比例题1：(new F1)： 因为F1的变量有一个输出变量两个输入变量（先输出后输入），且它们的维度都是1维，所以对应1，1，1。

ceres:: CostFunction* costFunction =  // 使用自动求导，模板参数: 误差类型，输出维度，输入维度，维度要与前面的struct一致
            new ceres::AutoDiffCostFunction<CostFunctor,1,1>(new CostFunctor);

    // 向问题中添加误差项
    problem.AddResidualBlock(costFunction, nullptr, &x);

3. 完整代码：

#include 
#include 

struct F1{
    template<typename T>
    bool operator()(const T* const x1, const T* const x2, T* residual)const{
        residual[0] = x1[0] + 10.0 * x2[0];
        return true;
    }
};

struct F2{
    template<typename T>
    bool operator()(const T* const x3, const T* const x4, T* residual) const{
        residual[0] = sqrt(5) * (x3[0] - x4[0]);
        return true;
    }
};

struct F3{
    template<typename T>
    bool operator()(const T* const x2, const T* const x3, T* residual) const {
        residual[0] = (x2[0] - 2.0 * x3[0]) * (x2[0] - 2.0 * x3[0]);
        return true;
    }
};

struct F4{
    template<typename T>
    bool operator()(const T* const x1,const T* const x4, T* residual) const{
        residual[0] = sqrt(10.0) * (x1[0] - x4[0]) * (x1[0] - x4[0]);
        return true;
    }
};

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

    double x1 = 3.0, x2 = -1.0, x3 = 0.0, x4 = 1.0;
    ceres::Problem problem;
    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<F1,1,1,1>(new F1),NULL,&x1,&x2);
    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<F2,1,1,1>(new F2),NULL,&x3,&x4);
    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<F3,1,1,1>(new F3),NULL,&x2,&x3);
    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<F4,1,1,1>(new F4),NULL,&x1,&x4);

    // 配置求解器
    ceres::Solver::Options options;
    // 增量方程如何求解
    options.linear_solver_type = ceres::DENSE_QR;
    // 输出到cout
    options.minimizer_progress_to_stdout = true;
    // 优化信息
    ceres::Solver::Summary summary;
    ceres::Solve(options,&problem,&summary);

    std::cout << summary.BriefReport()  << std::endl;
    std::cout <<"x1 : " <<   3.0 << " -> " << x1 << std::endl;
    std::cout <<"x2 : " <<   -1.0 << " -> " << x2 << std::endl;
    std::cout <<"x3 : " <<   0.0 << " -> " << x3 << std::endl;
    std::cout <<"x4 : " <<   1.0 << " -> " << x4 << std::endl;
}

例题3 曲线拟合

本题所用的采样点根据$y=e^{0.3x+0.1}$生成，并且加入标准差为$\sigma$=0.2高斯噪声。这2n个数据，存入data[ ]当中。我们用下列带未知参数的方程来拟合这些采样点：

1. 同样定义一个用来计算残差的结构体(残差$=y-e^{mx+c}$)

struct ExpResidual{
    ExpResidual(double x,double y): _x(x),_y(y){};
    template<typename T>
    bool operator()(const T* const m, const T* const c, T* residual) const{
        // T() : 强制转换
        residual[0] = T(_y) - exp(m[0] * T(_x) + c[0]);
        return true;
    }

private:
    // 一个样本数据的观测值
    const double _x;
    const double _y;
};

对比例题1，因为对应每个采样点都要计算一个残差，所以我们在构建结构体的时候实现了其的赋值功能，保证在每次for循环在problem存放残差的同时对残差数值进行赋值。

struct CostFunctor{
    template<typename T>
    bool operator()(const T* const x, T* residual ) const{
        residual[0] = 10.0 - x[0];
        return true;
    } };

2. 然后通过for循环将各个残差块加入到problem中。

 double m =0.0,n=0.0;
    ceres::Problem problem;
    for(int i =0; i < N; ++i )
    {
        ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<ExpResidual,1,1,1>(
                new ExpResidual(data[2*i],data[2*i+1])
                );
        problem.AddResidualBlock(cost_function,
                                 // 使用核函数来对异常数据进行过滤，CauchyLoss是Ceres Solver附带的损失函数之一。 参数0.5指定了损失函数的规模。
                                 new ceres::CauchyLoss(0.5),
                                 &m,&n);
    }

对比例题1：ExpResidual(data[2i],data[2i+1])：结构体增加了赋值函数，保证在每次for循环在problem存放残差的同时对残差数值进行赋值;
new ceres::CauchyLoss(0.5) ：设置了核函数，针对多数据拟合曲线。

ceres:: CostFunction* costFunction =  // 使用自动求导，模板参数: 误差类型，输出维度，输入维度，维度要与前面的struct一致
            new ceres::AutoDiffCostFunction<CostFunctor,1,1>(new CostFunctor);

    // 向问题中添加误差项
    problem.AddResidualBlock(costFunction, nullptr, &x);

3. 运行结果：

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  3.900273e+02    0.00e+00    3.18e+00   0.00e+00   0.00e+00  1.00e+04        0    3.41e-05    6.88e-05
   1  7.822299e+02   -3.92e+02    0.00e+00   6.66e-01  -4.49e+02  5.00e+03        1    2.95e-05    1.28e-04
   2  7.820699e+02   -3.92e+02    0.00e+00   6.65e-01  -4.49e+02  1.25e+03        1    1.47e-05    1.49e-04
   3  7.811116e+02   -3.91e+02    0.00e+00   6.64e-01  -4.48e+02  1.56e+02        1    1.36e-05    1.66e-04
   4  7.723189e+02   -3.82e+02    0.00e+00   6.54e-01  -4.38e+02  9.77e+00        1    1.31e-05    1.83e-04
   5  6.541501e+02   -2.64e+02    0.00e+00   5.28e-01  -3.09e+02  3.05e-01        1    1.30e-05    2.00e-04
   6  3.893149e+02    7.12e-01    8.87e+00   1.40e-01   1.88e+00  9.16e-01        1    3.41e-05    2.38e-04
   7  3.831437e+02    6.17e+00    1.91e+02   1.44e-01   6.39e+00  2.75e+00        1    3.10e-05    2.73e-04
   8  3.880161e+02   -4.87e+00    0.00e+00   8.31e-02  -7.02e-01  1.37e+00        1    1.30e-05    2.89e-04
   9  2.860887e+02    9.71e+01    1.38e+05   4.51e-02   1.66e+01  4.12e+00        1    3.18e-05    3.25e-04
  10  2.596611e+02    2.64e+01    2.92e+05   5.86e-03   3.01e+00  1.24e+01        1    3.03e-05    3.60e-04
  11  2.594955e+02    1.66e-01    6.11e+05   5.13e-05   1.43e+00  3.71e+01        1    2.98e-05    3.93e-04
  12  2.591315e+02    3.64e-01    2.12e+06   7.30e-05   1.92e+00  1.11e+02        1    3.02e-05    4.28e-04
  13  2.584629e+02    6.69e-01    2.79e+07   2.60e-05   4.30e+00  3.34e+02        1    2.97e-05    4.61e-04
  14  2.571779e+02    1.29e+00    4.77e+09   2.05e-07   1.03e+01  1.00e+03        1    2.99e-05    4.95e-04
Ceres Solver Report: Iterations: 15, Initial cost: 3.900273e+02, Final cost: 2.571779e+02, Termination: CONVERGENCE
m : 0 -> 0.29984
n : 0 -> 0.113263

4. 完整代码

#include 
#include 
#include 

struct ExpResidual{
    ExpResidual(double x,double y): _x(x),_y(y){};
    template<typename T>
    bool operator()(const T* const m, const T* const c, T* residual) const{
        // T() : 强制转换
        residual[0] = T(_y) - exp(m[0] * T(_x) + c[0]);
        return true;
    }

private:
    // 一个样本数据的观测值
    const double _x;
    const double _y;
};

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

    double _m = 0.3, _n = 0.1;  // 真实参数值
    int N = 100;                // 数据点个数
    double w_sigma = 1.0;       // 噪声sigma值
    cv::RNG rng;                // OpenCV随机数产生器

    double data[2*N];           // 数据容器
    for(int i =0; i<N;i++)      // 生成数据
    {
        double x = i;
        data[2*i] = x;
        data[2*i+1] = exp(0.3 * x + 0.1)+rng.gaussian(w_sigma);

    }

    double m =0.0,n=0.0;
    ceres::Problem problem;
    for(int i =0; i < N; ++i )
    {
        ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<ExpResidual,1,1,1>(
                new ExpResidual(data[2*i],data[2*i+1])
                );
        problem.AddResidualBlock(cost_function,
                                 // 使用核函数来对异常数据进行过滤，CauchyLoss是Ceres Solver附带的损失函数之一。 参数0.5指定了损失函数的规模。
                                 new ceres::CauchyLoss(0.5),
                                 &m,&n);
    }

    // 配置求解器
    ceres::Solver::Options options;
    // 增量方程如何求解
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    // 优化信息
    ceres::Solver::Summary summary;
    ceres::Solve(options,&problem,&summary);

    std::cout << summary.BriefReport() << std::endl;
    std::cout <<"m : " <<   0.0 << " -> " << m << std::endl;
    std::cout <<"n : " <<   0.0 << " -> " << n << std::endl;

}

例题4 Bundle Adjustment

给定一系列测得的图像，包含特征点位置和对应关系。BA的目标就是，通过最小化重投影误差，确定三维空间点的位置和相机参数。这个优化问题通常被描述为非线性最小二乘法问题，要最小化的目标函数即为观测到的特征点位置与对应的三维点在相机成像平面上投影之差的L2平方模。Ceres例程使用了BAL数据集

需要注意的是，BAL数据集有其自身的特殊之处：

• BAL的相机内参模型由焦距f和畸变参数k1，k2给出。f类似于我们提到的fx，fy。由于照片像素基本是正方形，所以在很多实际场合中用一个值即可。此外，这个模型没有cx，cy。
• 因为BAL数据在投影时假设投影平面在相机光心之后，所以按照我们之前用的模型计算，需要在投影之后乘以系数-1

1. 第一步依然是定义一个残差模板，在本例题中残差也就是重投影误差。每个残差值与空间点位置（三个参数）和相机参数（9个参数）有关。这里相机是针孔相机模型，使用9个参数进行参数化:3个参数用于旋转，3个参数用于平移，1个参数用于焦距，2个参数用于径向畸变。

// 模板针孔相机模型。相机使用9个参数进行参数化:3个参数用于旋转，3个参数用于平移，1个参数用于焦距，2个参数用于径向畸变。
// 主点没有建模(即假设位于图像中心)。
struct ReprojectionError{
    // 读取空间点在成像平面上的位置
    ReprojectionError(double x, double y): observed_x(x),observed_y(y) {}

    template<typename T>
    bool operator()(const T* const camera,const T* const point, T* residuals) const{


        ///这一块主要实现将三维点投影到像素坐标系的过程*******************************************
        T p[3];
        // 旋转对齐相机坐标系（世界坐标系转到相机坐标系） // camera[0,1,2] are the angle-axis rotation 赋值给p
        ceres::AngleAxisRotatePoint(camera,point,p);
        // camera[3,4,5] are the translation.  // 相机坐标系下X，Y，Z
        p[0] += camera[3],p[1] += camera[4], p[2] += camera[5];

        // 计算distortion中心。相机坐标系为负z轴。
        // 相机坐标系 -> 归一化坐标系
        T xp = -p[0] / p[2];
        T yp = -p[1] / p[2];

        // camera[7],camera[8]应用于径向畸变
        const T& l1 = camera[7];
        const T& l2 = camera[8];
        T r2 = xp*xp + yp *yp;
        T distortion = T(1.0) + r2 *(l1+l2*r2); // 1+ k1*r + k2*r^2  十四讲5.10

        // 计算最终投影点位置
        const T& focal = camera[6];
        T predicted_x = focal * distortion * xp;       // 十四讲 5.13 u = f * xdis + (cx)
        T predicted_y = focal * distortion * yp;
        ///***********************************************************************************

        // 误差是预测位置和观测位置之间的差值。
        residuals[0] = predicted_x - T(observed_x);
        residuals[1] = predicted_y - T(observed_y);
        return true;}

     static ceres::CostFunction* Create(const double x,
                                       const double y) {
        // 此处的2，9，3：2是输出维度（残差），9和3是输入维度（相机的9个参数和三维点坐标3个参数）
        return (new ceres::AutoDiffCostFunction<ReprojectionError, 2, 9, 3>(
                new ReprojectionError(x, y)));
    }
private:
	const double observed_x;
    const double observed_y;

};

对比例题3：
依然是在结构体中实现了赋值操作，保证在每次for循环在problem存放残差的同时对残差数值进行赋值;
中间的部分属于在结构体中实现了将三维点投影到像素坐标系的过程，与ceres本身无关。
static ceres::CostFunction* Create(const double x, const double y) 对比下面的例题1：相当于在结构体中实现了对于costfunction的定义，之后在主函数中只要cost_function = Create函数return值即可。

struct ExpResidual{
    ExpResidual(double x,double y): _x(x),_y(y){};
    template<typename T>
    bool operator()(const T* const m, const T* const c, T* residual) const{
        // T() : 强制转换
        residual[0] = T(_y) - exp(m[0] * T(_x) + c[0]);
        return true;
    }

private:
    // 一个样本数据的观测值
    const double _x;
    const double _y;
};

// 设置唯一的cost function(残差).cost function使用自动微分来获得导数(雅可比矩阵)
    ceres:: CostFunction* costFunction =  // 使用自动求导，模板参数: 误差类型，输出维度，输入维度，维度要与前面的struct一致
            new ceres::AutoDiffCostFunction<CostFunctor,1,1>(new CostFunctor);

2. 构建problem

 ceres::Problem problem;
    for(int i=0;i<bal_problem.num_observations();++i)
    {
        ceres::CostFunction* cost_function =
                ReprojectionError::Create(observations[2*i+0],observations[2*i+1]);
        problem.AddResidualBlock(cost_function, nullptr,bal_problem.mutable_camera_for_observation(i),bal_problem.mutable_point_for_observation(i));

    }

对比例题3：cost_function的实现部分在残差结构体中已经定义了，这里的cost_function直接用了Create
函数的返回值（ceres::CostFunction*）

 double m =0.0,n=0.0;
    ceres::Problem problem;
    for(int i =0; i < N; ++i )
    {
        ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<ExpResidual,1,1,1>(
                new ExpResidual(data[2*i],data[2*i+1])
                );
        problem.AddResidualBlock(cost_function,
                                 // 使用核函数来对异常数据进行过滤，CauchyLoss是Ceres Solver附带的损失函数之一。 参数0.5指定了损失函数的规模。
                                 new ceres::CauchyLoss(0.5),
                                 &m,&n); }

因为这是一个大规模稀疏矩阵，所以可以将Solver::Options::linear_solver_type设置成SPARSE_NORMAL_CHOLESKY。另外Ceres还提供了三个专门的解算器供使用。这里的样例代码用了其中最简单的一个解算器，即DENSE_SCHUR

options.linear_solver_type = ceres::DENSE_SCHUR;

3. 完整代码

#include 
#include 
// 旋转对齐时用到
#include 

//这个类用去读取BAL数据集相机、照片等相关信息的类，大致了解下
// Read a Bundle Adjustment in the Large dataset.
class BALProblem {
public:
    ~BALProblem() {
        delete[] point_index_;
        delete[] camera_index_;
        delete[] observations_;
        delete[] parameters_;
    }
    int num_observations()       const { return num_observations_;               }
    const double* observations() const { return observations_;                   }
    double* mutable_cameras()          { return parameters_;                     }
    double* mutable_points()           { return parameters_  + 9 * num_cameras_; }
    //每个相机对应的内参和外参
    double* mutable_camera_for_observation(int i) {
        return mutable_cameras() + camera_index_[i] * 9;
    }
    //对应数据点所在观测下的坐标
    double* mutable_point_for_observation(int i) {
        return mutable_points() + point_index_[i] * 3;
    }
    bool LoadFile(const char* filename) {
        FILE* fptr = fopen(filename, "r");
        if (fptr == NULL) {
            return false;
        };
        FscanfOrDie(fptr, "%d", &num_cameras_);
        FscanfOrDie(fptr, "%d", &num_points_);
        FscanfOrDie(fptr, "%d", &num_observations_);
        point_index_ = new int[num_observations_];
        camera_index_ = new int[num_observations_];
        observations_ = new double[2 * num_observations_];
        num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
        parameters_ = new double[num_parameters_];
        for (int i = 0; i < num_observations_; ++i) {
            FscanfOrDie(fptr, "%d", camera_index_ + i);
            FscanfOrDie(fptr, "%d", point_index_ + i);
            for (int j = 0; j < 2; ++j) {
                FscanfOrDie(fptr, "%lf", observations_ + 2*i + j);
            }
        }
        for (int i = 0; i < num_parameters_; ++i) {
            FscanfOrDie(fptr, "%lf", parameters_ + i);
        }
        return true;
    }
private:
    template<typename T>
    void FscanfOrDie(FILE *fptr, const char *format, T *value) {
        int num_scanned = fscanf(fptr, format, value);
        if (num_scanned != 1) {
            LOG(FATAL) << "Invalid UW data file.";
        }
    }
    int num_cameras_;
    int num_points_;
    int num_observations_;
    int num_parameters_;
    int* point_index_;
    int* camera_index_;
    double* observations_;
    double* parameters_;
};

// 模板针孔相机模型。相机使用9个参数进行参数化:3个参数用于旋转，3个参数用于平移，1个参数用于焦距，2个参数用于径向畸变。
// 主点没有建模(即假设位于图像中心)。
struct ReprojectionError{
    // 读取空间点在成像平面上的位置
    ReprojectionError(double x, double y): observed_x(x),observed_y(y) {}

    template<typename T>
    bool operator()(const T* const camera,const T* const point, T* residuals) const{


        ///这一块主要实现将三维点投影到像素坐标系的过程*******************************************
        T p[3];
        // 旋转对齐相机坐标系（世界坐标系转到相机坐标系） // camera[0,1,2] are the angle-axis rotation 赋值给p
        ceres::AngleAxisRotatePoint(camera,point,p);
        // camera[3,4,5] are the translation.  // 相机坐标系下X，Y，Z
        p[0] += camera[3],p[1] += camera[4], p[2] += camera[5];

        // 计算distortion中心。相机坐标系为负z轴。
        // 相机坐标系 -> 归一化坐标系
        T xp = -p[0] / p[2];
        T yp = -p[1] / p[2];

        // camera[7],camera[8]应用于径向畸变
        const T& l1 = camera[7];
        const T& l2 = camera[8];
        T r2 = xp*xp + yp *yp;
        T distortion = T(1.0) + r2 *(l1+l2*r2); // 1+ k1*r + k2*r^2  十四讲5.10

        // 计算最终投影点位置
        const T& focal = camera[6];
        T predicted_x = focal * distortion * xp;       // 十四讲 5.13 u = f * xdis + (cx)
        T predicted_y = focal * distortion * yp;
        ///***********************************************************************************

        // 误差是预测位置和观测位置之间的差值。
        residuals[0] = predicted_x - T(observed_x);
        residuals[1] = predicted_y - T(observed_y);
        return true;}

     static ceres::CostFunction* Create(const double x,
                                       const double y) {
         // 此处的2，9，3：2是输出维度（残差），9和3是输入维度（相机的9个参数和三维点坐标3个参数）
        return (new ceres::AutoDiffCostFunction<ReprojectionError, 2, 9, 3>(
                new ReprojectionError(x, y)));
    }
private:
    const double observed_x;
    const double observed_y;

};

int main(int argc, char** argv)
{
    if (argc != 2) {
        std::cerr << "usage: simple_bundle_adjuster \n";
        return 1;
    }
    BALProblem bal_problem;
    if (!bal_problem.LoadFile(argv[1])) {
        std::cerr << "ERROR: unable to open file " << argv[1] << "\n";
        return 1;
    }

    const double *observations = bal_problem.observations();

    google::InitGoogleLogging(argv[0]);

    ceres::Problem problem;
    for(int i=0;i<bal_problem.num_observations();++i)
    {
        ceres::CostFunction* cost_function =
                ReprojectionError::Create(observations[2*i+0],observations[2*i+1]);
        problem.AddResidualBlock(cost_function, nullptr,bal_problem.mutable_camera_for_observation(i),bal_problem.mutable_point_for_observation(i));

    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;
    ceres::Solve(options,&problem,&summary);
    std::cout << summary.BriefReport() << std::endl;
    return 0;

}

4. 十四讲对例题4的补充：

	// 生成一个LossFunction（核函数）
	ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);

例题5 复杂的曲线拟合

我们来思考一个相对复杂的曲线拟合问题。待确定参数方程如下：

现在给定一系列的对应数据点$\{x_i,y_i\}$。我们面临的问题是求解 $b1,b2,b3,b4$使下面的表达式取值最小：

根据高等数学的微分知识，我们可以算出$f$的一系列导数

• 对$b1$求导： $D_{1}f\left(b_1,b_2,b_3,b_4;x,y\right)=\frac{1}{{\left(1+e^{b_2-b_{3}x}\right)}^{1/b_4}}$
• 对$b2$求导： $D_{2}f\left(b_1,b_2,b_3,b_4;x,y\right)=\frac{-b_1{e}^{b_2-b_{3}x}}{b_4{\left(1+e^{b_2-b_{3}x}\right)}^{1/b_4+1}}$
• 对$b3$求导： $D_{3}f\left(b_1,b_2,b_3,b_4;x,y\right)=\frac{b_{1}x{e}^{b_2-b_{3}x}}{b_4{\left(1+e^{b_2-b_{3}x}\right)}^{1/b_4+1}}$
• 对$b4$求导： $D_{4}f\left(b_1,b_2,b_3,b_4;x,y\right)=\frac{b_1\log \left(1+e^{b_2-b_{3}x}\right)}{b_4^2{\left(1+e^{b_2-b_{3}x}\right)}^{1/b_4}}$

1. 构建一个残差类

class Analytic: public ceres::SizedCostFunction<1,4>{ //定义一个CostFunction或 SizedCostFunction（如果参数和残差在编译时就已知了）的子类。
public:
    Analytic(const double x,const double y):_x(x),_y(y){}
    virtual ~Analytic(){};
    virtual bool Evaluate(double const* const* parameters,
                          double * residual,
                          double ** jacobians) const{
        const double b1 = parameters[0][0];
        const double b2 = parameters[0][1];
        const double b3 = parameters[0][2];
        const double b4 = parameters[0][3];

        residual[0] = b1 * pow(1 + exp(b2 - b3 * _x),-1.0 / b4) - _y;
        if(!jacobians) return true;
        double * jacobian = jacobians[0];
        if(!jacobian) return true;

        jacobian[1] = -b1 * exp(b2 - b3 * _x) *
                      pow(1 + exp(b2 - b3 * _x), -1.0 / b4 - 1) / b4;
        jacobian[2] = _x * b1 * exp(b2 - b3 * _x) *
                      pow(1 + exp(b2 - b3 * _x), -1.0 / b4 - 1) / b4;
        jacobian[3] = b1 * log(1 + exp(b2 - b3 * _x)) *
                      pow(1 + exp(b2 - b3 * _x), -1.0 / b4) / (b4 * b4);
        return true;
    }

private:
    const double _x;
    const double _y;
};

对比例题3：

struct ExpResidual{
    ExpResidual(double x,double y): _x(x),_y(y){};
    template<typename T>
    bool operator()(const T* const m, const T* const c, T* residual) const{
        // T() : 强制转换
        residual[0] = T(_y) - exp(m[0] * T(_x) + c[0]);
        return true;
    }

private:
    // 一个样本数据的观测值
    const double _x;
    const double _y;
};

2. 什么时候应该使用analytical derivatives?

• 表达式很简单，例如大部分是线性的
• 计算机代数系统像 Maple , Mathematica, 或者SymPy可以被用来对目标函数进行符号化的微分。
• 式子中有一些代数结构可以实现比自动微分有更好的性能。
  也就是说, 获得在计算倒数之外的最大性能需要大量的工作.在沿着这条路径走下去之前，评估雅可比矩阵的计算花费是整个求解时间的一小部分是很有用的，，记住Amdahl法则是你的朋友。
• 没有其他的方法来计算这些导数，比如你想计算多项式的根的导数:
  $a_3(x,y)z^3+a_2(x,y)z^2+a_1(x,y{)}^z+a_0(x,y)=0$
  对于x,y.这需要用到逆函数理论。
• 你愿意亲自来做导数计算。

补充

示例: vins_mono
AddParameterBlock : 一般只有自定义自增加法时才会使用

for (int i = 0; i < WINDOW_SIZE + 1; i++)
    {
        // 由于位姿不满足正常的加法,因此需要自己定义
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        // 参数块
        // problem.AddParameterBlock(要添加的参数块,参数块的大小,自增的方式)
        problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
        // 速度 和 bias
        problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
    }

problem.SetParameterBlockConstant(参数地址)： 固定优化变量
例如下例子中固定了平移的优化变量：

problem.SetParameterBlockConstant(ext+4);

参考

[官方教程]
https://blog.csdn.net/qq_34935373/article/details/93494460
https://blog.csdn.net/wzheng92