5.Ceres官方教程-非线性最小二乘～Bundle Adjustment
编写Ceres的主要原因之一是我们需要解决大规模的Bundle Adjustment问题。参考文献《Multiview Geometry in Computer Vision》《Bundle Adjustment: A Modern Synthesis, Proceedings of the International Workshop on Vision Algorithms: Theory and Practice》
Bundle Adjustment可以参考
https://optsolution.github.io/archives/58892.html
给定一组测量的图像特征位置和对应关系，Bundle Adjustment的目标是找到使重投影误差最小的三维点位置和摄像机参数。
该优化问题通常表述为非线性最小二乘问题，其中误差为观测特征位置与相应三维点在摄像机图像平面上的投影差的L2范数的平方。Ceres广泛支持解决Bundle Adjustment问题。
让我们从BAL数据集解决一个问题
第一步通常是定义一个模板化的仿函数来计算重投影误差/残差。仿函数的结构类似于ExponentialResidual，因为每个图像观测都有一个该对象的实例。
BAL问题中的每个残差都依赖于一个三维点和九个相机参数。定义摄像机的9个参数是:3个旋转作为罗德里格斯轴-角向量，3个平移，1个焦距，2个径向畸变。
这款相机模型的详细信息可以在Bundler主页(http://www.cs.cornell.edu/~snavely/bundler/)和BAL主页(http://grail.cs.washington.edu/projects/bal/)上找到。
struct SnavelyReprojectionError {
// 读入两个参数，空间点在成像平面上的位置，并赋值。
SnavelyReprojectionError(double observed_x, double observed_y)
: observed_x(observed_x), observed_y(observed_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 p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation.
p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];
// Compute the center of distortion. The sign change comes from
// the camera model that Noah Snavely's Bundler assumes, whereby
// the camera coordinate system has a negative z axis.
T xp = - p[0] / p[2];
T yp = - p[1] / p[2];
// Apply second and fourth order radial distortion.
const T& l1 = camera[7];
const T& l2 = camera[8];
T r2 = xp*xp + yp*yp;
T distortion = 1.0 + r2 * (l1 + l2 * r2);
// Compute final projected point position.
const T& focal = camera[6];
T predicted_x = focal * distortion * xp;
T predicted_y = focal * distortion * yp;
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - T(observed_x);
residuals[1] = predicted_y - T(observed_y);
return true;
}
// Factory to hide the construction of the CostFunction object from
// the client code.
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)));
}
double observed_x;
double observed_y;
};
注意，与前面的例子不同，这是一个非平凡函数(non-trivial function)，计算它的解析雅可比矩阵有点麻烦。如果使用自动微分法就会容易很多。
函数AngleAxisRotatePoint()和其他操作旋转的函数可以在include/ceres/rotation.h中找到。给定这个仿函数，bundle adjustment问题可以构造如下:
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
ceres::CostFunction* cost_function =
SnavelyReprojectionError::Create(
bal_problem.observations()[2 * i + 0],
bal_problem.observations()[2 * i + 1]);
problem.AddResidualBlock(cost_function,
nullptr /* squared loss */,
bal_problem.mutable_camera_for_observation(i),
bal_problem.mutable_point_for_observation(i));
}
注意，bundle adjustment的问题构造与曲线拟合的例子非常相似——每次观测都向目标函数中添加一项。
因为这是一个大规模稀疏矩阵问题(至少对于DENSE_QR很大)，解决这个问题的一种方法是设置Solver::Options::linear_solver_type为SPARSE_NORMAL_CHOLESKY，并调用solve()
虽然这是一个合理的做法，bundle adjustment问题有一个特殊的稀疏结构，可以利用这个更有效地解决它们。Ceres为此任务提供了三个专门的求解器(统称为基于schur的求解器)。示例代码使用其中最简单的DENSE_SCHUR。
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.FullReport() << "\n";
针对更加复杂的BA问题，Ceres也提供了更加先进的特征，包括了各种各样的线性解算器、鲁棒的损失函数以及本地参数化。具体内容见代码文件examples/bundle_adjuster.cc。
#include <cmath>
#include <cstdio>
#include <iostream>
#include "ceres/ceres.h"
#include "ceres/rotation.h"
// 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_;
};
// Templated pinhole camera model for used with Ceres. The camera is
// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
// focal length and 2 for radial distortion. The principal point is not modeled
// (i.e. it is assumed be located at the image center).
struct SnavelyReprojectionError {
SnavelyReprojectionError(double observed_x, double observed_y)
: observed_x(observed_x), observed_y(observed_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 p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation.
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
// Compute the center of distortion. The sign change comes from
// the camera model that Noah Snavely's Bundler assumes, whereby
// the camera coordinate system has a negative z axis.
T xp = - p[0] / p[2];
T yp = - p[1] / p[2];
// Apply second and fourth order radial distortion.
const T& l1 = camera[7];
const T& l2 = camera[8];
T r2 = xp*xp + yp*yp;
T distortion = 1.0 + r2 * (l1 + l2 * r2);
// Compute final projected point position.
const T& focal = camera[6];
T predicted_x = focal * distortion * xp;
T predicted_y = focal * distortion * yp;
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - observed_x;
residuals[1] = predicted_y - observed_y;
return true;
}
// Factory to hide the construction of the CostFunction object from
// the client code.
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)));
}
double observed_x;
double observed_y;
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
if (argc != 2) {
std::cerr << "usage: simple_bundle_adjuster <bal_problem>\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();
// Create residuals for each observation in the bundle adjustment problem. The
// parameters for cameras and points are added automatically.
ceres::Problem problem;
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. Internally, the cost function stores the observed
// image location and compares the reprojection against the observation.
ceres::CostFunction* cost_function =
SnavelyReprojectionError::Create(observations[2 * i + 0],
observations[2 * i + 1]);
problem.AddResidualBlock(cost_function,
NULL /* squared loss */,
bal_problem.mutable_camera_for_observation(i),
bal_problem.mutable_point_for_observation(i));
}
// Make Ceres automatically detect the bundle structure. Note that the
// standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
// for standard bundle adjustment problems.
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.FullReport() << "\n";
return 0;
}
在终端中执行
$ ./ceres_test problem-49-7776-pre.txt
PS：problem-49-7776-pre.txt是在http://grail.cs.washington.edu/projects/bal/中下载的数据集
随便选择ladybug中的一个(http://grail.cs.washington.edu/projects/bal/ladybug.html)
结果如下
block_sparse_matrix.cc:81 Allocating values array with 6113856 bytes.
detect_structure.cc:113 Schur complement static structure <2,3,9>.
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 8.509125e+05 0.00e+00 8.57e+06 0.00e+00 0.00e+00 1.00e+04 0 2.18e+00 2.21e+00
detect_structure.cc:113 Schur complement static structure <2,3,9>.
1 4.648193e+04 8.04e+05 3.55e+06 2.10e+02 9.61e-01 3.00e+04 1 2.21e+00 4.42e+00
2 1.481752e+04 3.17e+04 4.47e+05 3.30e+02 9.60e-01 9.00e+04 1 2.20e+00 6.62e+00
3 1.346029e+04 1.36e+03 3.80e+04 5.21e+02 9.70e-01 2.70e+05 1 2.21e+00 8.83e+00
4 1.343304e+04 2.73e+01 4.68e+04 1.02e+03 3.56e-01 2.64e+05 1 2.21e+00 1.10e+01
5 1.338876e+04 4.43e+01 1.90e+04 1.23e+03 7.88e-01 3.26e+05 1 2.21e+00 1.32e+01
6 1.337551e+04 1.33e+01 1.40e+04 1.64e+03 6.47e-01 3.34e+05 1 2.20e+00 1.54e+01
7 1.336596e+04 9.54e+00 7.09e+03 1.78e+03 7.75e-01 4.00e+05 1 2.21e+00 1.77e+01
8 1.336049e+04 5.48e+00 5.24e+03 2.21e+03 7.44e-01 4.54e+05 1 2.20e+00 1.99e+01
9 1.335651e+04 3.98e+00 3.31e+03 2.59e+03 7.77e-01 5.46e+05 1 2.21e+00 2.21e+01
10 1.335360e+04 2.90e+00 2.36e+03 3.21e+03 7.75e-01 6.55e+05 1 2.20e+00 2.43e+01
11 1.335141e+04 2.19e+00 1.59e+03 3.94e+03 7.81e-01 7.97e+05 1 2.21e+00 2.65e+01
12 1.334976e+04 1.65e+00 1.07e+03 4.89e+03 7.83e-01 9.74e+05 1 2.21e+00 2.87e+01
13 1.334851e+04 1.25e+00 7.13e+02 6.08e+03 7.86e-01 1.20e+06 1 2.20e+00 3.09e+01
14 1.334756e+04 9.51e-01 4.78e+02 7.57e+03 7.88e-01 1.48e+06 1 2.20e+00 3.31e+01
15 1.334683e+04 7.28e-01 3.27e+02 9.46e+03 7.89e-01 1.83e+06 1 2.21e+00 3.53e+01
16 1.334627e+04 5.61e-01 2.29e+02 1.18e+04 7.91e-01 2.28e+06 1 2.20e+00 3.75e+01
17 1.334583e+04 4.35e-01 1.65e+02 1.49e+04 7.92e-01 2.85e+06 1 2.20e+00 3.97e+01
18 1.334549e+04 3.38e-01 1.21e+02 1.87e+04 7.93e-01 3.57e+06 1 2.20e+00 4.19e+01
19 1.334523e+04 2.65e-01 9.10e+01 2.35e+04 7.93e-01 4.48e+06 1 2.21e+00 4.41e+01
20 1.334502e+04 2.08e-01 6.97e+01 2.95e+04 7.94e-01 5.62e+06 1 2.22e+00 4.63e+01
21 1.334486e+04 1.64e-01 5.44e+01 3.72e+04 7.94e-01 7.05e+06 1 2.26e+00 4.86e+01
22 1.334473e+04 1.29e-01 4.32e+01 4.68e+04 7.94e-01 8.86e+06 1 2.27e+00 5.09e+01
23 1.334463e+04 1.01e-01 3.51e+01 5.26e+04 7.97e-01 1.12e+07 1 2.22e+00 5.31e+01
24 1.334455e+04 7.79e-02 2.61e+01 5.92e+04 8.00e-01 1.43e+07 1 2.21e+00 5.53e+01
25 1.334449e+04 6.09e-02 2.18e+01 7.05e+04 8.00e-01 1.82e+07 1 2.21e+00 5.75e+01
26 1.334444e+04 4.82e-02 2.02e+01 8.68e+04 7.99e-01 2.32e+07 1 2.21e+00 5.97e+01
27 1.334440e+04 3.81e-02 1.90e+01 9.83e+04 8.00e-01 2.96e+07 1 2.23e+00 6.19e+01
28 1.334437e+04 2.97e-02 1.97e+01 9.60e+04 8.03e-01 3.81e+07 1 2.23e+00 6.42e+01
29 1.334435e+04 2.32e-02 2.13e+01 1.04e+05 8.06e-01 4.93e+07 1 2.22e+00 6.64e+01
30 1.334433e+04 1.86e-02 2.36e+01 1.20e+05 8.05e-01 6.37e+07 1 2.21e+00 6.86e+01
31 1.334432e+04 1.42e-02 2.53e+01 1.33e+05 8.19e-01 8.62e+07 1 2.21e+00 7.08e+01
trust_region_minimizer.cc:707 Terminating: Function tolerance reached. |cost_change|/cost: 8.233139e-07 <= 1.000000e-06
Solver Summary (v 1.14.0-eigen-(3.3.7)-no_lapack-eigensparse-openmp-no_tbb-no_custom_blas)
Original Reduced
Parameter blocks 7825 7825
Parameters 23769 23769
Residual blocks 31843 31843
Residuals 63686 63686
Minimizer TRUST_REGION
Dense linear algebra library EIGEN
Trust region strategy LEVENBERG_MARQUARDT
Given Used
Linear solver DENSE_SCHUR DENSE_SCHUR
Threads 1 1
Linear solver ordering AUTOMATIC 7776,49
Schur structure 2,3,9 2,3,9
Cost:
Initial 8.509125e+05
Final 1.334432e+04
Change 8.375681e+05
Minimizer iterations 32
Successful steps 32
Unsuccessful steps 0
Time (in seconds):
Preprocessor 0.030042
Residual only evaluation 0.206193 (32)
Jacobian & residual evaluation 69.631664 (32)
Linear solver 0.840765 (32)
Minimizer 70.810021
Postprocessor 0.000816
Total 70.840879
Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 8.233139e-07 <= 1.000000e-06)
