public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactored matcher class

Browse Source
This commit is contained in:
Christian Forster
2014-04-29 21:43:24 +02:00
parent cdecc6325f
commit 44f87b860b
9 changed files with 192 additions and 196 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo/src/matcher.cpp
+99 -131
View File
@@ -28,20 +28,6 @@
#include <svo/feature_alignment.h>
namespace svo {
namespace matcher {
PatchData::PatchData(int patch_size) :
patch_size(patch_size)
{
patch = vk::aligned_mem::aligned_alloc<uint8_t>(patch_size*patch_size, 16);
patch_with_border = vk::aligned_mem::aligned_alloc<uint8_t>((patch_size+2)*(patch_size+2), 16);
}
PatchData::~PatchData()
{
free(patch);
free(patch_with_border);
}
namespace warp {
@@ -74,14 +60,14 @@ int getBestSearchLevel(
const int max_level)
{
// Compute patch level in other image
int level_cur = 0;
int search_level = 0;
double D = A_cur_ref.determinant();
while(D > 3.0 && level_cur < max_level)
while(D > 3.0 && search_level < max_level)
{
level_cur += 1;
search_level += 1;
D *= 0.25;
}
return level_cur;
return search_level;
}
void warpAffine(
@@ -89,12 +75,12 @@ void warpAffine(
const cv::Mat& img_ref,
const Vector2d& px_ref,
const int level_ref,
const int level_cur,
PatchData& ref_patch)
const int search_level,
const int halfpatch_size,
uint8_t* patch)
{
const int halfpatch_size = ref_patch.patch_size/2+1;
const int patch_size = halfpatch_size*2 ;
Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
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
@@ -102,64 +88,36 @@ void warpAffine(
}
// Perform the warp on a larger patch.
uint8_t* ref_patch_ptr = ref_patch.patch_with_border;
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, ++ref_patch_ptr)
for (int x=0; x<patch_size; ++x, ++patch_ptr)
{
Vector2f px_patch(x-halfpatch_size, y-halfpatch_size);
px_patch *= (1<<level_cur);
px_patch *= (1<<search_level);
const Vector2f px(A_ref_cur*px_patch + px_ref_pyr);
if (px[0]<0 || px[1]<0 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1)
*ref_patch_ptr = 0;
*patch_ptr = 0;
else
*ref_patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]);
*patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]);
}
}
// Remove border from large patch.
ref_patch_ptr = ref_patch.patch;
for(int y=1; y<g_patch_size+1; ++y, ref_patch_ptr += g_patch_size)
{
uint8_t* ref_patch_border_ptr = ref_patch.patch_with_border + y*(g_patch_size+2) + 1;
for(int x=0; x<g_patch_size; ++x)
ref_patch_ptr[x] = ref_patch_border_ptr[x];
}
}
} // namespace warp
bool findMatchDirect(
const Point& pt,
const Frame& cur_frame,
Vector2d& px_cur,
int& level_cur)
Matcher::Matcher()
{
Feature* ref_ftr;
if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr))
return false;
patch_ = vk::aligned_mem::aligned_alloc<uint8_t>(patch_size_*patch_size_, 16);
patch_with_border_ = vk::aligned_mem::aligned_alloc<uint8_t>((patch_size_+2)*(patch_size_+2), 16);
}
if(!ref_ftr->frame->cam_->isInFrame(
ref_ftr->px.cast<int>()/(1<<ref_ftr->level), g_halfpatch_size+2, ref_ftr->level))
return false;
// warp affine
PatchData ref_patch(g_patch_size);
Matrix2d A_cur_ref;
warp::getWarpMatrixAffine(
*ref_ftr->frame->cam_, *cur_frame.cam_, ref_ftr->px, ref_ftr->f,
(ref_ftr->frame->pos() - pt.pos_).norm(), cur_frame.T_f_w_ * ref_ftr->frame->T_f_w_.inverse(), ref_ftr->level, A_cur_ref);
level_cur = warp::getBestSearchLevel(A_cur_ref, Config::nPyrLevels()-1);
warp::warpAffine(
A_cur_ref, ref_ftr->frame->img_pyr_[ref_ftr->level], ref_ftr->px, ref_ftr->level, level_cur, ref_patch);
// px_cur should be set
Vector2d px_scaled(px_cur/(1<<level_cur));
bool success = feature_alignment::align2D(
cur_frame.img_pyr_[level_cur], ref_patch.patch_with_border, ref_patch.patch, 10, px_scaled);
px_cur = px_scaled * (1<<level_cur);
return success;
Matcher::~Matcher()
{
free(patch_);
free(patch_with_border_);
}
bool depthFromTriangulation(
@@ -177,20 +135,57 @@ bool depthFromTriangulation(
return true;
}
bool findEpipolarMatchDirect(
void Matcher::createPatchFromPatchWithBorder()
{
uint8_t* ref_patch_ptr = patch_;
for(int y=1; y<patch_size_+1; ++y, ref_patch_ptr += patch_size_)
{
uint8_t* ref_patch_border_ptr = patch_with_border_ + y*(patch_size_+2) + 1;
for(int x=0; x<patch_size_; ++x)
ref_patch_ptr[x] = ref_patch_border_ptr[x];
}
}
bool Matcher::findMatchDirect(
const Point& pt,
const Frame& cur_frame,
Vector2d& px_cur)
{
if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))
return false;
if(!ref_ftr_->frame->cam_->isInFrame(
ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level))
return false;
// warp affine
warp::getWarpMatrixAffine(
*ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f,
(ref_ftr_->frame->pos() - pt.pos_).norm(),
cur_frame.T_f_w_ * ref_ftr_->frame->T_f_w_.inverse(), ref_ftr_->level, A_cur_ref_);
search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_);
createPatchFromPatchWithBorder();
// px_cur should be set
Vector2d px_scaled(px_cur/(1<<search_level_));
bool success = feature_alignment::align2D(
cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
options_.align_max_iter, px_scaled);
px_cur = px_scaled * (1<<search_level_);
return success;
}
bool Matcher::findEpipolarMatchDirect(
const Frame& ref_frame,
const Frame& cur_frame,
const Feature& ref_ftr,
const double d_estimate,
const double d_min,
const double d_max,
const bool align_1d,
double& depth,
double& h_inv)
double& depth)
{
typedef vk::patch_score::ZMSSD<g_halfpatch_size> PatchScore;
PatchData ref_patch(g_patch_size);
SE3 T_cur_ref = cur_frame.T_f_w_ * ref_frame.T_f_w_.inverse();
int zmssd_best = PatchScore::threshold();
Vector2d uv_best;
@@ -201,64 +196,57 @@ bool findEpipolarMatchDirect(
Vector2d epi_dir = A - B;
// Compute affine warp matrix
Matrix2d A_cur_ref;
warp::getWarpMatrixAffine(
*ref_frame.cam_, *cur_frame.cam_, ref_ftr.px, ref_ftr.f,
d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref);
int level_cur = warp::getBestSearchLevel(A_cur_ref, Config::nPyrLevels()-1);
d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref_);
search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
// Find length of search range on epipolar line
Vector2d px_A(cur_frame.cam_->world2cam(A));
Vector2d px_B(cur_frame.cam_->world2cam(B));
double px_length = (px_A-px_B).norm() / (1<<level_cur);
#ifdef DEBUG_MATCHER_EPISEARCH
printf("px_length = %f\n", px_length);
cv::Mat img_rgb = cv::Mat(cur_frame.img_pyr_.at(level_cur).size(), CV_8UC3);
cv::cvtColor(cur_frame.img_pyr_.at(level_cur), img_rgb, CV_GRAY2RGB);
#endif
epi_length_ = (px_A-px_B).norm() / (1<<search_level_);
// Warp reference patch at ref_level
warp::warpAffine(
A_cur_ref, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px, ref_ftr.level, level_cur, ref_patch);
warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,
ref_ftr.level, search_level_, halfpatch_size_+1, patch_with_border_);
createPatchFromPatchWithBorder();
//printf("px_length = %f\n", px_length);
if(px_length < 2.0)
if(epi_length_ < 2.0)
{
Vector2d px = (px_A+px_B)/2.0;
Vector2d px_scaled(px/(1<<level_cur));
Vector2d px_scaled(px/(1<<search_level_));
bool res;
if(align_1d)
if(options_.align_1d)
res = feature_alignment::align1D(
cur_frame.img_pyr_[level_cur], (px_A-px_B).cast<float>().normalized(),
ref_patch.patch_with_border, ref_patch.patch, 10, px_scaled, h_inv);
cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
else
res = feature_alignment::align2D(
cur_frame.img_pyr_[level_cur], ref_patch.patch_with_border,
ref_patch.patch, 10, px_scaled);
cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
options_.align_max_iter, px_scaled);
if(res)
{
px = px_scaled*(1<<level_cur);
px = px_scaled*(1<<search_level_);
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px), depth))
return true;
}
return false;
}
size_t n_steps = px_length/0.7; // one step per pixel
size_t n_steps = epi_length_/0.7; // one step per pixel
Vector2d step = epi_dir/n_steps;
if(n_steps > 1000)
if(n_steps > options_.max_epi_search_steps)
{
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);
n_steps, epi_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);
PatchScore patch_score(patch_);
// now we sample along the epipolar line
Vector2d uv = B-step;
@@ -267,24 +255,22 @@ bool findEpipolarMatchDirect(
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
Vector2i pxi(px[0]/(1<<search_level_)+0.5,
px[1]/(1<<search_level_)+0.5); // +0.5 to round to closest int
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))
if(!cur_frame.cam_->isInFrame(pxi, patch_size_, search_level_))
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);
uint8_t* cur_patch_ptr = cur_frame.img_pyr_[search_level_].data
+ (pxi[1]-halfpatch_size_)*cur_frame.img_pyr_[search_level_].cols
+ (pxi[0]-halfpatch_size_);
int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[search_level_].cols);
if(zmssd < zmssd_best) {
zmssd_best = zmssd;
@@ -292,41 +278,24 @@ bool findEpipolarMatchDirect(
}
}
#ifdef DEBUG_MATCHER_EPISEARCH
if(zmssd_best < g_zmssd_thresh) {
Vector2d px(cur_frame.cam_->world2cam(uv_best));
Vector2i pxi(px[0]/(1<<level_cur)+0.5, px[1]/(1<<level_cur)+0.5);
cv::circle(img_rgb, cv::Point2f(px[0]/(1<<level_cur), px[1]/(1<<level_cur)), 8, cv::Scalar(255,0,0), 2);
}
cv::imshow("cur_img", img_rgb);
cv::Mat ref_img_rgb = cv::Mat(ref_frame.img_pyr_.at(ref_ftr.level).size(), CV_8UC3);
cv::cvtColor(ref_frame.img_pyr_.at(ref_ftr.level), ref_img_rgb, CV_GRAY2RGB);
cv::circle(ref_img_rgb, cv::Point2f(ref_ftr.px[0]/(1<<ref_ftr.level), ref_ftr.px[1]/(1<<ref_ftr.level)), 8, cv::Scalar(255,0,0), 2);
cv::imshow("ref_img", ref_img_rgb);
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);
cv::Mat cur_patch(g_patch_size,g_patch_size,CV_8U,cur_patch_ptr);
cv::imshow("cur_patch", cur_patch);
cv::waitKey(0);
#endif
if(zmssd_best < PatchScore::threshold())
{
// if(px_length < 4.0)
if(options_.subpix_refinement)
{
Vector2d px(cur_frame.cam_->world2cam(uv_best));
Vector2d px_scaled(px/(1<<level_cur));
Vector2d px_scaled(px/(1<<search_level_));
bool res;
if(align_1d)
if(options_.align_1d)
res = feature_alignment::align1D(
cur_frame.img_pyr_[level_cur], (px_A-px_B).cast<float>().normalized(),
ref_patch.patch_with_border, ref_patch.patch, 10, px_scaled, h_inv);
cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
else
res = feature_alignment::align2D(
cur_frame.img_pyr_[level_cur], ref_patch.patch_with_border,
ref_patch.patch, 10, px_scaled);
cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
options_.align_max_iter, px_scaled);
if(res)
{
px = px_scaled*(1<<level_cur);
px = px_scaled*(1<<search_level_);
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px), depth))
return true;
}
@@ -339,5 +308,4 @@ bool findEpipolarMatchDirect(
return false;
}
} // namespace matcher
} // namespace svo