参考资料:
SVO详细解读--极品巧克力
SVO代码笔记(一)从头到尾
一步步完善视觉里程计7——mapping --冯兵
SVO代码分析
SVO深度解析(三)之深度滤波(建图部分)
什么是仿射变换矩阵
patch和computeScore的解释:
极线搜索匹配,通过8*8的patch块来描述像素的特征,两个patch块的匹配方式是ZMSSD(Zero Mean Sum of Squared Differences),类似于SSD(sum of squared difference),用于块儿匹配相似性打分(typedef vk::patch_score::ZMSSD<halfpatch_size_> PatchScore; )。
什么是仿射变换矩阵?
仿射变换(Affine Transformation)
Affine Transformation是一种二维坐标到二维坐标之间的线性变换,保持二维图形的“平直性”(译注:straightness,即变换后直线还是直线不会打弯,圆弧还是圆弧)和“平行性”(译注:parallelness,其实是指保二维图形间的相对位置关系不变,平行线还是平行线,相交直线的交角不变。)。
仿射变换可以通过一系列的原子变换的复合来实现,包括:平移(Translation)、缩放(Scale)、翻转(Flip)、旋转(Rotation)和剪切(Shear)。
为什么要计算仿射矩阵?
我们知道,同一张图像在经过旋转平移后,同一个场景在图像上的成像位置就发生了变换。在像素做匹配时,我们需要用像素周围的信息(一般是矩形窗口)来描述本像素的特征。设想一下,如果帧间图像发生了旋转,我们还用同样的窗口(当前帧的窗口坐标和种子所在帧窗口坐标一样)来描述搜索点,是不是不太合适。
如上图,描述p的窗口w1在后帧上投影点为p’,我们在匹配p和p’时,是用蓝色窗口来描述p’呢,还是用红色窗口?
答案肯定是红色窗口,那既然是红窗口,红色窗口怎么计算呢?
这就会用到仿射矩阵,SVO仿射矩阵是这样的计算的。
遗留问题:为什么有时候又不计算仿射变换矩阵呢???
应该是为了简化吧。
怎么进行仿射变换,如何得到reference patch?
计算窗口的思路是先利用三点法计算出tk和tn两时刻图像的仿射变换矩阵A_ref_cur,
然后再把窗口w1里的像素坐标逐一映射到图像tn里,这样映射的所有坐标就组成了窗口w2。
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 is locat at pyr [ref_level ]
px_patch *= (1<<search_level);// 1. patch tranform to level0, because A_ref_cur is only used to affine warp level0 patch
const Vector2f px(A_ref_cur*px_patch + px_ref_pyr); // 2. then, use A_ref_cur to affine warp the patch
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) interpolateMat_8u(img_ref, px[0], px[1]); // img_ref is the img at pyr[level]
}
}
}
}
size_t n_steps = px_length/0.7; // one step per pixel
Vector2d step = epi_dir/n_steps;
if(n_steps > 1000)
{
printf("WARNING: skip epipolar search: %zu evaluations, px_lenght=%f, d_min=%f, d_max=%f.\n",
n_steps, px_length, d_min, d_max);
return false;
}
// for matching, precompute sum and sum2 of warped reference patch
int pixel_sum = 0;
int pixel_sum_square = 0;
PatchScore patch_score(ref_patch.patch);
// now we sample along the epipolar line
Vector2d uv = B-step;
Vector2i last_checked_pxi(0,0);
++n_steps;
for(size_t i=0; i<n_steps; ++i, uv+=step)
{
Vector2d px(cur_frame.cam_->world2cam(uv));
Vector2i pxi(px[0]/(1<<level_cur)+0.5,
px[1]/(1<<level_cur)+0.5); // +0.5 to round to closest int
#ifdef DEBUG_MATCHER_EPISEARCH
cv::circle(img_rgb, cv::Point2f(pxi[0], pxi[1]), 1, cv::Scalar(0,0,255), 1);
#endif
if(pxi == last_checked_pxi)
continue;
last_checked_pxi = pxi;
// check if the patch is full within the new frame
if(!cur_frame.cam_->isInFrame(pxi, g_patch_size, level_cur))
continue;
// TODO interpolation would probably be a good idea
uint8_t* cur_patch_ptr = cur_frame.img_pyr_[level_cur].data + (pxi[1]-g_halfpatch_size)*cur_frame.img_pyr_[level_cur].cols + (pxi[0]-g_halfpatch_size);
int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[level_cur].cols);
if(zmssd < zmssd_best) {
zmssd_best = zmssd;
uv_best = uv;
}
}