# 1. Ceres 求解 BA

g2o 用 Edges 来保存每一个代价函数，但 Ceres 却是用 Problem 类型来构建最终的目标函数

#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H

#include <iostream>
#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<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);
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) {
return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
new SnavelyReprojectionError(observed_x, observed_y)));
}

private:
double observed_x;
double observed_y;
};

#endif // SnavelyReprojection.h


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();

// 构建points、cameras参数类型的优化变量
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
// 在这里构建代价函数costfunction每个误差块是以一个points和一个cameras为输入的
cost_function = SnavelyReprojectionError::Create(observations[2*i + 0], observations[2*i + 1]);

// 设置是否开启核函数
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];

}

}


Ceres采用额外的类型 ParameterBlockOrdering 来管理 schur 消元顺序

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();

if (params.ordering == "automatic")
return;

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

}


# 2. 求解

Ceres 除了能够在原始数组上操作以外，另一大优势在于它的优化设置非常便捷
g2o的设置全靠变量类型来选择不同的下降策略，以及选择稠密或者稀疏的线性方程组解法

Options 类型几乎集成了 Ceres 的所有求解方法设置和管理

void SetSolverOptionsFromFlags(BALProblem* bal_problem,
const BundleParams& params, Solver::Options* options) {

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->max_num_iterations = params.num_iterations;
options->minimizer_progress_to_stdout = true;
// 下降策略的选取
CHECK(StringToTrustRegionStrategyType(params.trust_region_strategy,
&options->trust_region_strategy_type));
// 设置变量排序
SetOrdering(bal_problem, options, params);
}


Ceres 的求解也很简单，和 视觉SLAM笔记（28） Ceres 的例子一样

Summary 类型用来负责函数求解每一次迭代的统计结果

options.gradient_tolerance = 1e-16;
options.function_tolerance = 1e-16;
Solver::Summary summary;
Solve(options, &problem, &summary);


// arg.param("initial_ply", initial_ply,"","Export the BAL file data as a PLY file.");
// arg.param("final_ply", final_ply, "", "Export the refined BAL file data as a PLY");
arg.param("initial_ply", initial_ply,"initial.ply","Export the BAL file data as a PLY file.");
arg.param("final_ply", final_ply, "final.ply", "Export the refined BAL file data as a PLY");


Ceres 库公开的 API 说明详细，同时源代码可读性也

《视觉SLAM十四讲》

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie