ceres快速教材及学习笔记（四）bundl adjustment，《视觉slam十四讲》第十讲ceres例子

0. 前言
本文是根据ceres官方教程内容ceres-solver官方教程链接，再结合自己理解的一个ceres快速学习笔记。
在博文ceres快速教材及学习笔记（一）hello,world!中，我们学习了

最小二乘问题数学模型；
弄清楚了各个参数的意义；
利用学习的最小二乘问题数学模型和ceres解决了一个最简单的最小二乘问题；

在博文ceres快速教材及学习笔记（二）曲线拟合，稍复杂的例子中，我们学会了怎样使用多个参数块，如何添加多个误差项。接下来在此基础上我们来求解更加复杂的问题，以处理如何使不同的误差项有不同代价函数以及参数块的情况。
在博文，ceres快速教材及学习笔记（三）鲍威尔方程Powell’s Function，中何使不同的误差项有不同代价函数以及参数块的情况。
接下来，在《视觉slam十四讲》第十讲ceres例子（源码在这里）中，我们将学会bundle adjustment。学会处理每个误差项输出维度不为一的情况

1.Bundle Adjustment
1.1准备工作
该项目分为以下几个步骤以实现bundle adjustment。

设置参数
读取ground truth 真实值
将真实值转化格式后输出initial.ply
基于真实值加入perturbation，也就是噪声，来模拟实际情况中获得的数据。
基于加了噪声的数据进行bundle adjustment。
输出BA之后的数据生成final.ply

在项目提供一个problem-16-22106-pre.txt文件：

16 22106 83718
0 0     -3.859900e+02 3.871200e+02
1 0     -3.844000e+01 4.921200e+02
2 0     -6.679200e+02 1.231100e+02
7 0     -5.991800e+02 4.079300e+02
12 0     -7.204300e+02 3.143400e+02
13 0     -1.151300e+02 5.548999e+01
0 1     3.838800e+02 -1.529999e+01

其含义，在提供该数据的官方网站上能找到：

  
   
...
   

...


...

翻译
假设相机数量为m，路标点数量为n，观测数量为ob，则

  
<观测1使用的相机序号> <观测1看到的路标点> <此次观测的路标点在图像中的位置x_1> <此次观测的路标点在图像中的位置y_1>
...
<观测ob使用的相机序号> <观测ob看到的路标点> <观测1看到的路标点x_ob> <观测1看到的路标点y_ob>
<相机1参数>
...
<相机m参数>
<路标点1参数>
...
<路标点n参数>

1.2问题思考

思考以下问题
首先，回答以下问题:（以下参数的各个含义点击这里）：

 

1.3.代码讲解

先来看main函数

int main(int argc, char** argv)
{    //设置参数
    BundleParams params(argc,argv);  // set the parameters here.
   
    google::InitGoogleLogging(argv[0]);
    std::cout << params.input << std::endl;
    if(params.input.empty()){
        std::cout << "Usage: bundle_adjuster -input ";
        return 1;
    }
	//求解BA
    SolveProblem(params.input.c_str(), params);
 
    return 0;
}

进入SolveProblem函数

void SolveProblem(const char* filename, const BundleParams& params)
{
    //读取文件
    BALProblem bal_problem(filename);

    // show some information here ...
    std::cout << "bal problem file loaded..." << std::endl;
    std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
              << bal_problem.num_points() << " points. " << std::endl;
    std::cout << "Forming " << bal_problem.num_observations() << " observatoins. " << std::endl;

    // store the initial 3D cloud points and camera pose..
    //储存初始的（未BA前的）3d点云位置以及相机位置
    if(!params.initial_ply.empty()){
        bal_problem.WriteToPLYFile(params.initial_ply);
    }

    std::cout << "beginning problem..." << std::endl;
    
    // add some noise for the intial value
    //给camera,points都加上噪音
    srand(params.random_seed);
    bal_problem.Normalize();
    bal_problem.Perturb(params.rotation_sigma, params.translation_sigma,
                        params.point_sigma);

    std::cout << "Normalization complete..." << std::endl;
    
    //开始构建最小二乘问题啦
    Problem problem;
    //具体构建的细节在这个函数里
    BuildProblem(&bal_problem, &problem, params);

    std::cout << "the problem is successfully build.." << std::endl;
   
   	//配置求解器
    Solver::Options options;
    SetSolverOptionsFromFlags(&bal_problem, params, &options);
    options.gradient_tolerance = 1e-16;
    options.function_tolerance = 1e-16;
    Solver::Summary summary;
    Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << "\n";
	
	//导出未BA后的3d点云位置以及相机位置
    // write the result into a .ply file.   
    if(!params.final_ply.empty()){
        bal_problem.WriteToPLYFile(params.final_ply);  
        // pay attention to this: ceres doesn't copy the value into optimizer, but implement on raw data! 
    }
}

接下来看看BuildProblem(&bal_problem, &problem, params);看他如何构建BA问题

void BuildProblem(BALProblem* bal_problem, Problem* problem, const BundleParams& params)
{
    const int point_block_size = bal_problem->point_block_size();
    const int camera_block_size = bal_problem->camera_block_size();
    double* points = bal_problem->mutable_points();//构建points参数类型的优化变量
    double* cameras = bal_problem->mutable_cameras();//构建cameras参数类型的优化变量

    // Observations is 2 * num_observations long array observations
    // [u_1, u_2, ... u_n], where each u_i is two dimensional, the x 
    // and y position of the observation. 
    const double* observations = bal_problem->observations();
	//有观测数量有多少就有多少误差项
    for(int i = 0; i < bal_problem->num_observations(); ++i){
        CostFunction* cost_function;

        // Each Residual block takes a point and a camera as input 
        // and outputs a 2 dimensional Residual
      	//在这里构建代价函数costfunction每个误差块是以一个points和一个cameras为输入的
        cost_function = SnavelyReprojectionError::Create(observations[2*i + 0], observations[2*i + 1]);

        // If enabled use Huber's loss function. 
        //设置是否开启核函数
        LossFunction* loss_function = params.robustify ? new HuberLoss(1.0) : NULL;

        // Each observatoin corresponds to a pair of a camera and a point 
        // which are identified by camera_index()[i] and point_index()[i]
        // respectively.
        double* camera = cameras + camera_block_size * bal_problem->camera_index()[i];
        double* point = points + point_block_size * bal_problem->point_index()[i];

     
        problem->AddResidualBlock(cost_function, loss_function, camera, point);
    }

}

接下来看看SnavelyReprojectionErrorcostfunction

class SnavelyReprojectionError
{
public:
    SnavelyReprojectionError(double observation_x, double observation_y):observed_x(observation_x),observed_y(observation_y){}
//重载()以获得一个仿函数functor
template
    bool operator()(const T* const camera,
                const T* const point,
                T* residuals)const{                  
        // camera[0,1,2] are the angle-axis rotation
        T predictions[2];
        CamProjectionWithDistortion(camera, point, predictions);
        //输出维度是2，代表着你在图像中观测到的路标点的像素坐标
        residuals[0] = predictions[0] - T(observed_x);
        residuals[1] = predictions[1] - T(observed_y);

        return true;
    }

    static ceres::CostFunction* Create(const double observed_x, const double observed_y){
    //在这里返回代价函数，这里是自动求导的，模版参数分别为<仿函数类型，误差项输出维度r,cameras维度s_1,points维度s_2>
        return (new ceres::AutoDiffCostFunction(
            new SnavelyReprojectionError(observed_x,observed_y)));
    }


private:
    double observed_x;
    double observed_y;
};

更多《计算机视觉与图形学》知识，可关注下方公众号：