目录
- 直接法介绍
- 单层直接法
- 多层直接法
直接法介绍
这里所说的直接法不同于上一篇所说的LK光流法,LK光流是首先要选取特征点,如何利用特征点附近的光流进行路标的追踪与位姿的求解的,其本质还是先得到匹配好的点对,然后利用点对进行位姿估计。而直接法不同,其核心思想是直接优化位姿
R
,
t
R,t
R,t。
直接法也需要两个假设:
1.灰度不变假设,即
I
1
(
u
,
v
)
=
I
2
(
u
+
Δ
x
,
v
+
Δ
y
)
I_1(u,v)=I_2(u+\Delta x,v+\Delta y)
I1(u,v)=I2(u+Δx,v+Δy)
2.小范围灰度不变假设,即存在
W
×
W
W×W
W×W的窗口,使
I
1
(
u
+
w
,
v
+
w
)
=
I
2
(
u
+
Δ
x
+
w
,
v
+
Δ
y
+
w
)
I_1(u+w,v+w)=I_2(u+\Delta x+w,v+\Delta y+w)
I1(u+w,v+w)=I2(u+Δx+w,v+Δy+w)
同LK光流法一样,假设1是基础,假设2则是为了构建最小二乘法。
基本理论如下:
同LK相比,直接法舍弃了
Δ
x
,
Δ
y
\Delta x,\Delta y
Δx,Δy作为优化变量,而是直接利用变换矩阵(位姿)
T
T
T作为优化变量,因为相邻图像上的关键点坐标的移动本质是因为相机在运动。这里理论不会太过详细,后期会单独写SLAM的理论。
设
P
P
P点的像素坐标为
(
u
,
v
)
(u,v)
(u,v),相机坐标为
(
X
,
Y
,
Z
)
(X,Y,Z)
(X,Y,Z)。在已知路标的深度,相机内参的情况下,利用简单的缩放与平移可得到相机坐标。
如上图所示,图1中的
p
(
u
,
v
)
p(u,v)
p(u,v)变换到了图2后,坐标为
p
′
=
K
T
P
p^\prime=KTP
p′=KTP,其中
K
K
K是相机内参矩阵,是已知的,基于前面的假设,可以得到以下等式:
e
r
r
o
r
=
I
1
(
u
,
v
)
−
I
2
(
K
T
P
)
error=I_1(u,v)-I_2(KTP)
error=I1(u,v)−I2(KTP),这个形式不规范,能理解就行。
C
o
s
t
=
∑
j
=
1
w
∑
i
=
1
w
I
1
(
u
+
w
,
v
+
w
)
−
I
2
(
K
T
P
+
W
)
Cost=\sum_{j=1}^{w}\sum_{i=1}^{w}I_1(u+w,v+w)-I_2(KTP+W)
Cost=∑j=1w∑i=1wI1(u+w,v+w)−I2(KTP+W)
同理,等式1基于假设1,等式2基于假设2。至于求导过程可参考《视觉SLAM十四讲》,这里也不做过多介绍。
单层直接法
直接上代码,里面有注解与注意事项。
void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) {
int max_patch_size = 1; //w的大小
int cnt_good = 0;
Matrix6d hession = Matrix6d::Zero(); //H矩阵
Vector6d bias = Vector6d::Zero(); //b矩阵
//double lastcost = 0;
double cost_tmp = 0;
//前提是进行坐标变换,图1的像素坐标系到归一化坐标系
for (int i = range.start; i < range.end; i++)
{
Eigen::Vector3d point_ref = depth_ref[i]* Eigen::Vector3d((pix_ref[i][0] - cx) / fx, (pix_ref[i][1] - cy) / fy, 1);
//变换到P(X,Y,Z)
Eigen::Vector3d point_cur = T * point_ref;
//T*P
//检查d是否存在
if (point_cur[2]<0)
continue;
double X = point_cur[0];
double Y = point_cur[1];
double Z = point_cur[2];
//以上得到了位姿变换后相机坐标
double Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;
float u = fx * X / Z + cx;
float v = fy * Y / Z + cy;
//得到了位姿变换后的像素坐标(u,v)
//检查变换后的坐标是否越界
if (u<max_patch_size || u>img_estimate.cols - max_patch_size ||
v<max_patch_size || v>img_estimate.rows - max_patch_size)
continue;
projection[i] = Eigen::Vector2d(u, v);
cnt_good++;
for (int x = -max_patch_size; x <= max_patch_size; x++)
for (int y = -max_patch_size; y <= max_patch_size; y++)
{
//开始计算error和j矩阵
float error = GetPixelValue(img, pix_ref[i][0] + x, pix_ref[i][1] + y) - GetPixelValue(img_estimate, u + x, v + y);
//开始计算导数
matrix26d J_pixel_xi;
Eigen::Vector2d J_img_pixel;
J_pixel_xi(0, 0) = fx * Z_inv;
J_pixel_xi(0, 1) = 0;
J_pixel_xi(0, 2) = -fx * X * Z2_inv;
J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
J_pixel_xi(0, 5) = -fx * Y * Z_inv;
J_pixel_xi(1, 0) = 0;
J_pixel_xi(1, 1) = fy * Z_inv;
J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
J_pixel_xi(1, 5) = fy * X * Z_inv;
J_img_pixel = Eigen::Vector2d(
0.5*(GetPixelValue(img_estimate, u + x + 1, v + y) - GetPixelValue(img_estimate, u + x - 1, v + y)),
0.5*(GetPixelValue(img_estimate, u + x, v + y + 1) - GetPixelValue(img_estimate, u + x, v + y - 1))
);
Vector6d J = -1.0*(J_img_pixel.transpose()*J_pixel_xi).transpose();
hession += J * J.transpose();
bias += -error * J;
cost_tmp += error * error;
}
}
if (cnt_good) {
// set hessian, bias and cost
unique_lock<mutex> lck(hession_mutex);
H += hession;
b += bias;
cost += (cost_tmp / cnt_good);
}
}
接下来是Gauss-Newton的主体部分
for (int iter = 0; iter < iteration; iter++)
{
jacpbian.reset(); //初始化
//求解方程
cv::parallel_for_(cv::Range(0, pixels_ref.size()),
std::bind(&JacobianAccumulator::accumulate_jacobian, &jacpbian, std::placeholders::_1)); //并行求解
Matrix6d H = jacpbian.hession();
Vector6d b = jacpbian.bias();
double lastcost = 0, cost = 0;
Vector6d update = H.ldlt().solve(b);
T = Sophus::SE3d::exp(update)*T;
cost = jacpbian.cost_function();
//接下来考虑三个方面:更新量是否为无穷,增量方向,收敛误差
if (std::isnan(update[0]))
{
cout << "更新量为无穷" << endl;
break;
}
if (lastcost>cost)
{
cout << "增量方向错误" << endl;
break;
}
if (update.norm()<1e-3)
{
break;
}
lastcost = cost;
cout << "iteration: " << iter << ", cost: " << cost << endl;
}
注意:
要创建一个accumulate_jacobian类,这个类主要是用于计算导数,而高斯牛顿法在外部的函数中调用该类完成,体现了面向对象的编程思维。
多层直接法
多层直接法的核心是金字塔,分为三部分:
代码如下:
void multy_derect_method(
const Mat & img1,
const Mat & imag_estimate,
const Vecvector2d & px_ref,
const vector<double>& depth_ref,
Sophus::SE3d & T) {
//金字塔方法的步骤,1.定义金字塔参数,2.创建金字塔(图像+参数) 3.调用单层算法
//1.定义基本参数:层数,尺度,尺度数组
int pyramid_size = 4;
double scal = 0.5;
double pyramid_scals[] = { 1,0.5,0.25,0.125 };
//2.创建金字塔
vector<Mat> pyr_img, pyr_imgestimate;
for (int i = 0; i < pyramid_size; i++)
{
if (i == 0)
{
pyr_img.push_back(img1);
pyr_imgestimate.push_back(imag_estimate);
}
else
{
Mat img_tmp, imgestimate_tmp;
cv::resize(pyr_img[i - 1], img_tmp,
cv::Size(pyr_img[i - 1].cols*scal, pyr_img[i - 1].rows*scal));
cv::resize(pyr_imgestimate[i - 1], imgestimate_tmp,
cv::Size(pyr_imgestimate[i - 1].cols*scal, pyr_imgestimate[i - 1].rows*scal));
pyr_img.push_back(img_tmp);
pyr_imgestimate.push_back(imgestimate_tmp);
}//以上为图像金字塔
}
double fxG = fx, fyG = fy, cxG = cx, cyG = cy;
for (int level = pyramid_size - 1; level >= 0; level--)
{
fx = fxG * pyramid_scals[level];
fy = fyG * pyramid_scals[level];
cx = cxG * pyramid_scals[level];
cy = cyG * pyramid_scals[level];
Vecvector2d px_pyr_ref;
for (auto &kp : px_ref)
{
px_pyr_ref.push_back(pyramid_scals[level] * kp);
}
single_derect_method(pyr_img[level], pyr_imgestimate[level], px_pyr_ref, depth_ref, T);
}
}
注意:
1.金字塔的层数是从第0层开始的,这样比较符合编程的思维。
2.金字塔不仅仅指图像,只要是需要缩放的各类参数都要建立金字塔。
3.层与层直接的参数传递主要是位姿矩阵T,T传入的是引用,不需要初始化,定义时默认初始化为单位矩阵,每层求解后T均被保存下来。
效果如下:
单层法:
多层法:
实际上,理论与代码的实现有着巨大的鸿沟,代码中还有很多细节部分没有理解清楚。加油!
参考文献:
[1]《视觉SLAM十四讲》 高翔