第十章ceres优化部分

common文件夹中的东西都是通用的，
主要的区别就在于有个SnavelyReprojectionError.h，里面定义了ceres需要的误差类。
还有就是ceresBundle.cpp主函数了。
这里主要看一下ceres的用法套路，尤其是problem构建和option设置这一些，相对来讲是比g2o要方便简单一些，通用性更高，消元相关主要是在option设置中，不过没怎么细看，回来还需再看一下。

先贴SnavelyReprojectionError.h

#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H

#include 
#include "ceres/ceres.h"


#include "common/tools/rotation.h"
#include "common/projection.h"

//定义误差类，这个类的作用就是在类中重载()运算符，写出误差的计算方式
class SnavelyReprojectionError
{
public:
    //构造函数，主要将观测坐标读进来
    SnavelyReprojectionError(double observation_x, double observation_y):observed_x(observation_x),observed_y(observation_y){}

    //这里定义个模板()函数，定义误差计算方式，这里也很简单，重投影误差，预测值减去观测值就得误差
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);
        residuals[0] = predictions[0] - T(observed_x);
        residuals[1] = predictions[1] - T(observed_y);

        return true;
    }

    /**
     * 这里用Create()函数生成ceresBundle.cpp中problem->AddResidualBlock()需要的第一个CostFunction*类型的参数.
     * 仔细观察return那一句发现：
     * SnavelyReprojectionError(observed_x,observed_y)就是调用了上方的重载()函数，结果就是个误差
     * ceres::AutoDiffCostFunction()是个模板函数，传入一堆的模板参数：误差类型、输出维度、camera输入维度、point输入维度
     * 那问题来了，整个这一句抽象为框架的话这样的：AutoDiffCostFunction(an_error)，
     * 看起来就是传入一个误差，然后用其构造AutoDiffCostFunction对象，而这个就是problem->AddResidualBlock()需要的CostFunction
     * @return 返回一个CostFunction*，一直没想出CostFunction的很贴切的名字，代价函数太直白不够形象，这里其实有点导函数的意思，但名字上又完全不相关
     */
    static ceres::CostFunction* Create(const double observed_x, const double observed_y)
    {
        return (new ceres::AutoDiffCostFunction2,9,3>(
                new SnavelyReprojectionError(observed_x,observed_y)));
    }


private:
    //私有成员就是观测的xy坐标了
    double observed_x;
    double observed_y;
};

#endif // SnavelyReprojection.h

然后ceresBundle.cpp

#include 
#include 
#include "ceres/ceres.h"

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


using namespace ceres;

void SetLinearSolver(ceres::Solver::Options* options, const BundleParams& params)
{
    //linear solver选取
    CHECK(ceres::StringToLinearSolverType(params.linear_solver, &options->linear_solver_type));
    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);

    for(int i = 0; i < num_cameras; ++i)
        ordering->AddElementToGroup(cameras + camera_block_size * i, 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)
    {

        // Each Residual block takes a point and a camera as input 
        // and outputs a 2 dimensional Residual
        //定义problem->AddResidualBlock()函数中需要的cost_function
        CostFunction* 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];

        //往问题中添加误差项：
        /**
         * 看一下函数定义：
         * cost_function： 中文怎么称呼，代价函数?
         * 多说几句这个cost_function，既不是误差，因为他是误差构造出来的。用法上瞅着有点像导数的意味，类似于g2o的雅克比矩阵？
         * loss_function： 损失函数，就是核函数
         * 紧接着的数组就是待优化参数了，由于各参数维度不同，所以类型为double*,有几个就写几个，这里两个，camera和point
         * ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0, double* x1);
         */
        problem->AddResidualBlock(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()函数，传入选项设置、目标函数、简报输出变量。
    ceres::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 ";
        return 1;
    }

    SolveProblem(params.input.c_str(), params);

    return 0;
}