视觉SLAM十四讲读书笔记ch10 ceres例子

该项目分为以下几个步骤以实现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

BAL数据集说明

第一行：

16 22106 83718

16个相机，22106个点，共进行83718次相机对点的观测

第2行到83719行：

6 18595 3.775000e+01 4.703003e+01

第6个相机观测18595个点，得到的相机的观测数据为3.775000e+01 4.703003e+01

第83720行到83720+16*9=83864

共16个相机的9纬参数：-R（3维），t（3维），f（焦距），k1,k2畸变参数

第83864到83864+3*22106=150182

22106个点的三维坐标

其含义，在提供该数据的官网找到

<num_cameras> <num_points> <num_observations>
<camera_index_1> <point_index_1> <x_1> <y_1>
… <camera_index_num_observations>
<point_index_num_observations> <x_num_observations>
<y_num_observations> <camera_1> …
<camera_num_cameras> <point_1> …
<point_num_points>

待估计变量是在所有的相机变量和所有的观测点变量。由于待估计变量被分为了q=2，即cameras和points两个类型。cameras的维度s_1=9，其中3个代表旋转，3个代表位移，三个代表内参。points的维度s_2=3，其代表路标点在三维空间中的位置。
由上面读取的文件我们知道，cameras数量为m,points数量为n，即是我们需要优化k=9m+3n个变量。但是对于每个误差项，我们与之相关的参数只有一个cameras和一个points。该最小二乘问题的误差项数量i=ob，即观测的数量。其维度r=2代表了路标点在图像中的像素坐标。

代码讲解

主函数

#include <iostream>
#include <fstream>
#include "ceres/ceres.h"

#include "SnavelyReprojectionError.h"
#include "common/BALProblem.h"
#include "common/BundleParams.h"


using namespace ceres;

//linear solver的选取，将参数传入params中，用下面的check检查传值 设置有无问题
void SetLinearSolver(ceres::Solver::Options* options, const BundleParams& params)
{
     // 设置LinearSolver，可选项为："sparse_schur, dense_schur, iterative_schur,   
    // sparse_normal_cholesky, ""dense_qr, dense_normal_cholesky and cgnr."
    CHECK(ceres::StringToLinearSolverType(params.linear_solver, &options->linear_solver_type));
    // 设置SparseLinearAlgebraLibraryType，可选项为："suite_sparse,cx_sparse"
    CHECK(ceres::StringToSparseLinearAlgebraLibraryType(params.sparse_linear_algebra_library, &options->sparse_linear_algebra_library_type));
    CHECK(ceres::StringToDenseLinearAlgebraLibraryType(params.dense_linear_algebra_library, &options->dense_linear_algebra_library_type));
    options->num_linear_solver_threads = params.num_threads;

}

//消元顺序设置
void SetOrdering(BALProblem* bal_problem, ceres::Solver::Options* options, const BundleParams& params)
{
    const int num_points = bal_problem->num_points();
    const int point_block_size = bal_problem->point_block_size();
    double* points = bal_problem->mutable_points();

    const int num_cameras = bal_problem->num_cameras();
    const int camera_block_size = bal_problem->camera_block_size();
    double* cameras = bal_problem->mutable_cameras();

    //这里如果设置为自动话直接return
    if (params.ordering == "automatic")
        return;

    //创建个ParameterBlockOrdering类型的对象，在下面按顺序把参数码进去，达到排序的目的，进行按序消元
    ceres::ParameterBlockOrdering* ordering = new ceres::ParameterBlockOrdering;

    // The points come before the cameras
    for(int i = 0; i < num_points; ++i)
       ordering->AddElementToGroup(points + point_block_size * i, 0); //注意0 
       
    
    for(int i = 0; i < num_cameras; ++i)
        ordering->AddElementToGroup(cameras + camera_block_size * i, 1); //注意1, 0在1之前先进行消元

    //最后就是用这句设置消元顺序
    options->linear_solver_ordering.reset(ordering);

}

void SetMinimizerOptions(Solver::Options* options, const BundleParams& params){
    //最大迭代次数：
    options->max_num_iterations = params.num_iterations;
    //标准输出端输出优化log日志
    options->minimizer_progress_to_stdout = true;
    //设置线程，加速雅克比矩阵计算
    options->num_threads = params.num_threads;
    // options->eta = params.eta;
    // options->max_solver_time_in_seconds = params.max_solver_time;
    
    //下降策略选取
    CHECK(StringToTrustRegionStrategyType(params.trust_region_strategy,
                                        &options->trust_region_strategy_type));
    
}

void SetSolverOptionsFromFlags(BALProblem* bal_problem,
                               const BundleParams& params, Solver::Options* options){
    这里其实可以堆一堆的优化选项设置，也就是列一堆options->，只不过根据设置的功能，又单分成了三个函数
    //书上P267的求解设置就是一堆options->
    //这里也可以发现，ceres的设置是比较简单的，定义个option对象，直接一通options->就好了。
    SetMinimizerOptions(options,params);
    SetLinearSolver(options,params);
    SetOrdering(bal_problem, options,params);
}

/**
 * 构建问题，主要是将优化数据和传入problem
 * @param bal_problem 数据
 * @param problem 要构建的优化问题，感觉ceres中的problem就类似于g2o中的optimizer，就是构建出优化问题。
 * @param params 优化所需参数
 */

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();
    double* cameras = bal_problem->mutable_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
        //定义problem->AddResidualBlock()函数中需要的cost_function
        cost_function = SnavelyReprojectionError::Create(observations[2*i + 0], observations[2*i + 1]);

        // If enabled use Huber's loss function. 
        //定义problem->AddResidualBlock()函数中需要的Huber核函数
        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.
        //定义problem->AddResidualBlock()函数中需要的待估计参数
        double* camera = cameras + camera_block_size * bal_problem->camera_index()[i];
        double* point = points + point_block_size * bal_problem->point_index()[i];

     	// 使用自动求导，模板参数：误差类型，输出维度r，s_1
        problem->AddResidualBlock(cost_function, loss_function, camera, point);
        /*
        cost_function   代价函数
        loss_function   核函数
        camera, point   待估计参数
        */
    }

}

//这个函数就是整个优化问题被单拉了出来。参数也很原始：待优化数据和优化参数
void SolveProblem(const char* filename, const BundleParams& params)
{
    //同样一开始，用BALProblem类对数据进行处理
    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..
    if(!params.initial_ply.empty()){
        bal_problem.WriteToPLYFile(params.initial_ply);
    }

    std::cout << "beginning problem..." << std::endl;
    
    // add some noise for the intial value
    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 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;
    //summary输出优化简报
    Solver::Summary summary;

    //真正的优化就这一句：Solve()函数，传入选项设置、目标函数、简报输出变量。
    Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << "\n";

    // 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! 
    }
}

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 <path for dataset>";
        return 1;
    }

    SolveProblem(params.input.c_str(), params);
 
    return 0;
}

误差函数

class SnavelyReprojectionError
{
public:
    SnavelyReprojectionError(double observation_x, double observation_y):observed_x(observation_x),observed_y(observation_y){}
//重载()以获得一个仿函数functor
template<typename T>
    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<SnavelyReprojectionError,2,9,3>(
            new SnavelyReprojectionError(observed_x,observed_y)));
    }


private:
    double observed_x;
    double observed_y;
};