给定一组测量的图像特征位置和对应关系,Bundle Adjustment 的目标是找到最小化重投影误差的 3D 点位置和相机参数。 该优化问题通常被表述为非线性最小二乘问题,其中误差是观察到的特征位置与相机图像平面上相应 3D 点的投影之间的差异的平方范数。 Ceres 为解决 Bundle Adjustment 问题提供了广泛的支持。
BAL 问题中的每个残差都取决于一个三维点和一个九参数相机。 定义相机的九个参数是:三个用于作为罗德里格斯轴角矢量的旋转,三个用于平移,一个用于焦距,两个用于径向畸变。 此相机型号的详细信息可以在 Bundler 主页和 BAL 主页中找到。
拿到一个数据集,首先要了解其含义,即每个数据所代表的意义。然后根据其不同的意义分类,思考如何通过代码的方式如何读入数据。
第二次记录:
已经了解到,数据集分为三部分,1 header 相关的个数数据 2 某个相机观察到的3D点以及对应的2D观察点的数据 3 相机参数(9个)以及3D点的坐标
(1)读入第一部分的个数
1 相机个数:int num_cameras_;
2 所观测的现实世界的位置,即3D点数量:int num_points_;
3 观测的2D点的数量:int num_observations_;
以上可以通过通过数据集的第一行得到。
int num_observations() const { return num_observations_; }
FscanfOrDie(fptr, "%d", &num_cameras_);
FscanfOrDie(fptr, "%d", &num_points_);
FscanfOrDie(fptr, "%d", &num_observations_);
(2)每个相机观察到的3D点的索引以及其对应的2D点的两个坐标(第二部分)
思考:只能按行读取数据,一次读取一行中的一个数据,其中一个隐含的条件是第二部分的行数与观测到的2D坐标点的个数相同,所以比较有用的便是 num_observations_
,其描述了循环读取数据的次数。
每一行中第一个数据表示第几个相机,第二个数据表示观测到的第几个3D点,那么由于 “第几个”,所以定义索引:
int* point_index_;
int* camera_index_;
第几个观察到的2D点:double* observations_;
point_index_ = new int[num_observations_];
camera_index_ = new int[num_observations_];
observations_ = new double[2 * 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);
}
}
(3)每个相机的参数以及3D点的坐标(第三部分)
如何读入这部分的数据?
1 首先确定这一部分总共有多少数据:int num_parameters_;
num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
2 通过 “第几个“确定位置
double* parameters_;
3 首先将读取到的参数看作整体,然后再分配:
for (int i = 0; i < num_parameters_; ++i) {
FscanfOrDie(fptr, "%lf", parameters_ + i);
}
double* mutable_cameras() { return parameters_; } //相机参数首地址
double* mutable_points() { return parameters_ + 9 * num_cameras_; } //3D点的首地址
double* mutable_camera_for_observation(int i) { //返回第i个相机
return mutable_cameras() + camera_index_[i] * 9;
}
double* mutable_point_for_observation(int i) { //返回第i个3D点
return mutable_points() + point_index_[i] * 3;
}
读取数据部分总结:1 了解数据文件格式 。2 明确目的,目的只有一个,即将数据读取入内存,那么首先写个读入函数,其次通过对数据集分类(3部分)用不同的方式读入。3 在读取的过程中需要用到的参数便自然而然的定义出来了。
我们从数据集中得到的数据分为两部分,一部分是相机与3D点,3D点的坐标是从世界坐标系测量得到的,由相机参数与3D点可以计算投影到像素平面上的像素值;另一部分是观测图像的本身的像素值,两个像素值构成最小二乘问题。
由公式:
P
c
=
R
P
w
+
t
P_c = RP_w + t
Pc=RPw+t
然后归一化,去畸变(径向):
x
d
i
s
=
x
(
1
+
k
1
r
2
+
k
2
r
4
)
x_{dis} = x (1+k_1r^2 + k_2r^4)
xdis=x(1+k1r2+k2r4),然后投影到像素平面
u
=
f
x
x
d
i
s
+
c
x
v
=
f
y
y
d
i
s
+
c
y
u = f_x x_{dis} + c_x\\ v = f_y y_{dis} + c_y
u=fxxdis+cxv=fyydis+cy
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;
}
本例中是调用ceres中的函数来求解 R P w RP_w RPw,在学习视觉Slam十四讲时作业代码中涉及自己编写求解代码,所以在此回顾。
(1)求旋转矩阵R:应用罗德里格斯公式
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
n
n
T
+
s
i
n
θ
n
Λ
R = cos\theta \; I + (1-cos\theta)nn^T + sin\theta \; n^{\Lambda}
R=cosθI+(1−cosθ)nnT+sinθnΛ
(2)
R
P
w
RP_w
RPw
R
P
w
=
P
w
c
o
s
θ
+
(
1
−
c
o
s
θ
)
n
(
n
P
w
)
+
s
i
n
θ
n
×
P
w
RP_w = P_wcos \theta \; + (1-cos \theta)n(nP_w)+ sin\theta \;n \times P_w
RPw=Pwcosθ+(1−cosθ)n(nPw)+sinθn×Pw
于是将一个大问题分解为求
n
n
n 与
P
w
P_w
Pw 的点乘以及
n
n
n 与
P
w
P_w
Pw 的叉乘。
出现了新问题:n 与 θ \theta θ 如何求解
(3)n 与 θ \theta θ
任意旋转都可以用一个旋转轴和一个旋转角来刻画,所以可以使用一个向量,其方向与旋转轴一致,而长度等于旋转角,这种向量称为旋转向量(角轴),用数学语言表示为 θ n \theta n θn,其中n为旋转轴且是一个单位长度的向量, θ \theta θ 为角度。
所以很显然 :
θ
n
⋅
θ
n
=
θ
2
\theta n \cdot \theta n = \theta^2
θn⋅θn=θ2
角轴已知,我们假设为 angle 所以可以求得
n
=
a
n
g
l
e
θ
n = \frac{angle}{\theta}
n=θangle
因为角 θ \theta θ 可能为 0,所以在编写代码时需要分类讨论。
突然发现十四讲的代码也是调用的ceres库的。。。
BALProblem bal_problem;
const double* observations = bal_problem.observations();
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
ceres::CostFunction* cost_function = SnavelyReprojectionError::Create(
observations[2 * i + 0], 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));
}
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";