SVO - Feature alignment
主要计算集中在 Reprojector
中。
// map reprojection & feature alignment
SVO_START_TIMER("reproject");
reprojector_.reprojectMap(new_frame_, overlap_kfs_);
SVO_STOP_TIMER("reproject");
Reprojector::reprojectMap()
map_.getCloseKeyframes(frame, close_kfs);
[kf_loop]
frame->isVisible();
[kf_loop]
[fts_loop]
reprojectPoint(frame, (*it_ftr)->point);
frame->cam_->isInFrame(px.cast<int>(), 8)
---
[cell_loop]
reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame);
Map Reprojection
将
map
中的 3D 点投影到图片中,并找到对应的feature
。
每个grid
一个 3D 点。既保证均匀分布,又减小计算量。
// 通过 isVisible() 判断 map 点是否在视野内;找到与当前帧视场重叠的关键帧;
map_.getCloseKeyframes(frame, close_kfs);
//close_kfs < 关键帧,与 cur 距离>
// sort(); 按照 当前帧 与 关键帧 的**位移**,对找到的 关键帧 排序;
for(; options_.max_n_kfs ;){
//将 close_kfs 中前 max_n_kfs 个关键帧取出;
overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));
...
//重投影到像素坐标系,判断其是否 isInFrame();
reprojectPoint(frame, (*it_ftr)->point)
}
// reproject candidates 略
Feature Align
reprojectPoint()
中,为cell
和pt
建立指针链接;
SVO_START_TIMER("feature_align");
for(size_t i=0; i<grid_.cells.size(); ++i)
{
if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
++n_matches_;
if(n_matches_ > (size_t) Config::maxFts())
break;
}
SVO_STOP_TIMER("feature_align");
reprojectCell();
found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
//找到视角偏差最小的 kf, 返回对应的 &ftr; [closest observation angle]
pt.getCloseViewObs(cur_frame.pos(), ref_ftr_);
//判断 patch 是否在视野内;排除边缘点;
ref_ftr_->frame->cam_->isInFrame(像素坐标, halfpatch_size_+2, ref_ftr_->level)
// 计算放射矩阵
warp::getWarpMatrixAffine(...);
//找到 cur 与 ref 缩放比例一致的 level;
warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
//得到 ref 中平行四边形状的 patch 图像;
warp::warpAffine(...);
//双线性插值函数
vk::interpolateMat_8u(img_ref, px[0], px[1]);
feature_alignment::align2D();
Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);
frame->addFeature(new_feature);
new_feature->point = it->pt;
warp::getWarpMatrixAffine();
将 ref
中特征点对应的 5x5
half_patch
经 T
转换到 cur
,通过 half_patch
上三点,计算 Affine
矩阵。
参考:https://blog.csdn.net/xbcreal/article/details/52549629
getBestSearchLevel()
找到 cur
与 ref
patch
缩放比例一致的 level
。
A = [ c o s θ − s i n θ s i n θ c o s θ ] [ s 0 0 s ] A = \begin{bmatrix}cos\theta & -sin\theta\\ sin\theta & cos\theta \end{bmatrix} \begin{bmatrix}s & 0 \\ 0 & s \end{bmatrix} A=[cosθsinθ−sinθcosθ][s00s]
d e t ( A ) = s 2 det(A) = s^2 det(A)=s2
int getBestSearchLevel(
const Matrix2d& A_cur_ref,
const int max_level)
{
// Compute patch level in other image
int search_level = 0;
double D = A_cur_ref.determinant();
while(D > 3.0 && search_level < max_level)
{
search_level += 1;
D *= 0.25;
}
return search_level;
}
warp::warpAffine();
线性插值,仿射变换;
patch
在 cur
中为正方形,在 ref
中为平行四边形;
patch_ptr
中输出 ref patch
;
uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16)));
void warpAffine(
const Matrix2d& A_cur_ref,
const cv::Mat& img_ref,
const Vector2d& px_ref,
const int level_ref,
const int search_level,
const int halfpatch_size,
uint8_t* patch)
{
const int patch_size = halfpatch_size*2 ;
const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
if(isnan(A_ref_cur(0,0)))
{
printf("Affine warp is NaN, probably camera has no translation\n"); // TODO
return;
}
// Perform the warp on a larger patch.
uint8_t* patch_ptr = patch;
const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
for (int y=0; y<patch_size; ++y)
{
for (int x=0; x<patch_size; ++x, ++patch_ptr)
{
Vector2f px_patch(x-halfpatch_size, y-halfpatch_size);
px_patch *= (1<<search_level); // cur img;
//此处假设 search_level 与 level_ref 的缩放比例一致,金字塔 2 倍缩放是否影响?
const Vector2f px(A_ref_cur*px_patch + px_ref_pyr); // ref img;
if (px[0]<0 || px[1]<0 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1)
*patch_ptr = 0;
else
*patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]);
}
}
}
feature_alignment::align2D();
- 计算代价函数对像素坐标的雅可比矩阵及海森矩阵;
- 高斯牛顿迭代特征点位置;
- 采用 LK 光流法(Inverse Compositional Image Alignment)
- 参考:http://www.cs.cmu.edu/~16385/ [Lecture 25]
- 优化变量中加入光照模型
photometric model
- svo 1 代码中仅优化了
mean_diff
, - svo 2 中添加了
alpha
,但是对Hessian
矩阵的近似误差较大,不建议独立使用,代码中有些奇怪的负号,看代码有点费劲。 - 参考:
- Direct Image Registration With Gain and Bias, Adrien Bartoli
- Active Exposure Control for Robust Visual Odometry in HDR Environments, Zichao Zhang
- svo 1 代码中仅优化了
公式:
min
Δ
p
,
α
,
β
∑
x
[
α
T
(
W
(
x
,
0
)
)
+
β
−
I
(
W
(
x
,
p
+
Δ
p
)
)
]
2
T
a
y
l
o
r
≈
min
Δ
p
,
Δ
α
,
Δ
β
∑
x
[
(
α
+
Δ
α
)
(
T
(
W
(
x
,
0
)
)
+
∇
T
∂
W
∂
p
Δ
p
)
+
β
+
Δ
β
−
I
(
W
(
x
,
p
)
)
]
2
=
min
Δ
p
,
Δ
α
,
Δ
β
∑
x
[
α
∇
T
∂
W
∂
p
Δ
p
+
T
(
W
(
x
,
0
)
)
Δ
α
+
Δ
β
+
α
T
(
W
(
x
,
0
)
)
+
β
−
I
(
W
(
x
,
p
)
)
]
2
\begin{align} & \min_{\Delta p, \alpha, \beta} \sum_x [\alpha T(W(x,0)) + \beta - I(W(x,p+\Delta p))]^2 \\ {}_{Taylor} \approx & \min_{\Delta p, \Delta \alpha, \Delta \beta} \sum_x [(\alpha + \Delta \alpha) (T(W(x,0)) + \nabla T \frac{\partial W}{\partial \mathbf p} \Delta \mathbf p)+ \beta + \Delta \beta - I(W(x,p))]^2 \\ = & \min_{\Delta p, \Delta \alpha, \Delta \beta} \sum_x [ \alpha \nabla T \frac{\partial W}{\partial \mathbf p} \Delta \mathbf p + T(W(x,0)) \Delta \alpha + \Delta \beta + \alpha T(W(x,0))+ \beta - I(W(x,p))]^2 \end{align}
Taylor≈=Δp,α,βminx∑[αT(W(x,0))+β−I(W(x,p+Δp))]2Δp,Δα,Δβminx∑[(α+Δα)(T(W(x,0))+∇T∂p∂WΔp)+β+Δβ−I(W(x,p))]2Δp,Δα,Δβminx∑[α∇T∂p∂WΔp+T(W(x,0))Δα+Δβ+αT(W(x,0))+β−I(W(x,p))]2
Δ p = H − 1 ∑ x [ ∇ T ∂ W ∂ p 1 T ( W ( x , 0 ) ) ] T [ I ( W ) ( x ; p ) − α T ( x ) − β ] \Delta\mathbf{p} = H^{-1} \sum_{\mathbf{x}} \begin{bmatrix} \nabla T \frac{\partial W}{\partial \mathbf p } \\ 1 \\ T(W(x,0)) \end{bmatrix}^T [I(\mathbf{W})(\mathbf{x};\mathbf{p})- \alpha \mathbf{T}(\mathbf{x}) - \beta] Δp=H−1x∑ ∇T∂p∂W1T(W(x,0)) T[I(W)(x;p)−αT(x)−β]
bool align2D(
const cv::Mat& cur_img,
uint8_t* ref_patch_with_border,
uint8_t* ref_patch,
const int n_iter,
Vector2d& cur_px_estimate,
bool no_simd)
{
const int halfpatch_size_ = 4;
const int patch_size_ = 8;
const int patch_area_ = 64;
bool converged=false;
// compute derivative of template and prepare inverse compositional
float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_];
float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_];
Matrix3f H; H.setZero();
// compute gradient and hessian
const int ref_step = patch_size_+2;
float* it_dx = ref_patch_dx;
float* it_dy = ref_patch_dy;
for(int y=0; y<patch_size_; ++y)
{
uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
for(int x=0; x<patch_size_; ++x, ++it, ++it_dx, ++it_dy)
{
Vector3f J;
J[0] = 0.5 * (it[1] - it[-1]);
J[1] = 0.5 * (it[ref_step] - it[-ref_step]);
J[2] = 1;
*it_dx = J[0];
*it_dy = J[1];
H += J*J.transpose();
// 矩阵相乘,列向量 x 行向量;
}
}
Matrix3f Hinv = H.inverse();
float mean_diff = 0;
// Compute pixel location in new image:
float u = cur_px_estimate.x();
float v = cur_px_estimate.y();
// termination condition
const float min_update_squared = 0.03*0.03;
const int cur_step = cur_img.step.p[0];
// float chi2 = 0;
Vector3f update; update.setZero();
for(int iter = 0; iter<n_iter; ++iter)
{
int u_r = floor(u);
int v_r = floor(v);
if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_)
break;
if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
return false;
// compute interpolation weights
float subpix_x = u-u_r;
float subpix_y = v-v_r;
float wTL = (1.0-subpix_x)*(1.0-subpix_y);
float wTR = subpix_x * (1.0-subpix_y);
float wBL = (1.0-subpix_x)*subpix_y;
float wBR = subpix_x * subpix_y;
// loop through search_patch, interpolate
uint8_t* it_ref = ref_patch;
float* it_ref_dx = ref_patch_dx;
float* it_ref_dy = ref_patch_dy;
// float new_chi2 = 0.0;
Vector3f Jres; Jres.setZero();
for(int y=0; y<patch_size_; ++y)
{
uint8_t* it = (uint8_t*) cur_img.data + (v_r+y-halfpatch_size_)*cur_step + u_r-halfpatch_size_;
for(int x=0; x<patch_size_; ++x, ++it, ++it_ref, ++it_ref_dx, ++it_ref_dy)
{
float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1];
// T - cur
float res = search_pixel - *it_ref + mean_diff;
Jres[0] -= res*(*it_ref_dx);
Jres[1] -= res*(*it_ref_dy);
Jres[2] -= res;
// new_chi2 += res*res;
}
}
update = Hinv * Jres;
u += update[0];
v += update[1];
mean_diff += update[2];
#if SUBPIX_VERBOSE
cout << "Iter " << iter << ":"
<< "\t u=" << u << ", v=" << v
<< "\t update = " << update[0] << ", " << update[1]
// << "\t new chi2 = " << new_chi2 << endl;
#endif
if(update[0]*update[0]+update[1]*update[1] < min_update_squared)
{
#if SUBPIX_VERBOSE
cout << "converged." << endl;
#endif
converged=true;
break;
}
}
cur_px_estimate << u, v;
return converged;
}