SVO笔记 - Feature alignment
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）
参考：https://blog.csdn.net/wendox/article/details/52505971
公式：
H = ∑ x [ ∂ F ∂ p ] T [ ∂ F ∂ p ] ≡ ∑ x [ ∇ T ∂ W ∂ p ] T [ ∇ T ∂ W ∂ p ] H=\sum_{x} \left[ \frac{\partial F}{\partial \mathbf p} \right]^T \left[ \frac{\partial F}{\partial \mathbf p} \right]\\ \equiv \sum_{x} \left[ \nabla T \frac{\partial W}{\partial \mathbf p} \right]^T \left[ \nabla T \frac{\partial W}{\partial \mathbf p} \right] H=x∑[∂p∂F]T[∂p∂F]≡x∑[∇T∂p∂W]T[∇T∂p∂W]
Δ p = H − 1 ∑ x [ ∇ T ∂ W ∂ p ] T [ I ( W ) ( x ; p ) − T ( x ) ] \Delta\mathbf{p} = H^{-1} \sum_{\mathbf{x}}\left[ \nabla T \frac{\partial W}{\partial \mathbf p} \right]^T [I(\mathbf{W})(\mathbf{x};\mathbf{p})-\mathbf{T}(\mathbf{x})] Δp=H−1x∑[∇T∂p∂W]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;
}