SVO-7-sparse_img_align–2021.1.31
sparse_img_align.h
#ifndef SVO_SPARSE_IMG_ALIGN_H_
#define SVO_SPARSE_IMG_ALIGN_H_
#include <vikit/nlls_solver.h>
#include <vikit/performance_monitor.h>
#include <svo/global.h>
namespace vk{
class AbstractCamera;
}
namespace svo{
class Feature;
class SparseImgAlign : public vk::NLLSSolver<6,SE3>{
static const int patch_halfsize_ = 2;
static const int patch_size_ = 2 * patch_halfsize_;
static const int patch_area_ = patch_size_ * patch_size_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
cv::Mat resimg_;
SparseImgAlign(int n_levels,
int min_levels,
int n_iter,
Method method,
bool display,
bool verbose);
size_t run(FramePtr ref_frame,FramePtr cur_frame);
Matrix<double,6,6> getFisherInformation();
protected:
FramePtr ref_frame_;
FramePtr cur_frame_;
int level_;
bool display_;
int max_level_;
int min_level_;
Matrix<double,6,Dynamic,ColMajor> jacobian_cache_;
bool have_ref_patch_cache_;
cv::Mat ref_patch_caches_;
std::vector<bool> visible_fts_;
void precomputeReferencePatches();
virtual double computeResiduals(const SE3& model,
bool linearize_system,
bool compute_weight_scale = false);
virtual int solve();
virtual void update (const ModelType& old_model,ModelType& new_model);
virtual void startIteration();
virtual void finishIteration();
};
}
#endif
sparse_img_align.cpp
#include <algorithm>
#include <svo_practice/sparse_img_align.h>
#include <svo_practice/frame.h>
#include <svo_practice/feature.h>
#include <svo/config.h>
#include <svo_practice/point.h>
#include <vikit/abstract_camera.h>
#include <vikit/vision.h>
#include <vikit/math_utils.h>
namespace svo{
svo::SparseImgAlign::SparseImgAlign(int max_levels, int min_levels,
int n_iter, Method method, bool display,
bool verbose) :
display_(display),max_levels_(max_levels),min_levels_(min_levels){
n_iter_ = n_iter;
n_iter_init_ = n_iter_;
method_ = method;
verbose_ = verbose;
eps_ = 0.000001;
}
size_t svo::SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
{
reset();
if(ref_frame -> fts_.empty()){
SVO_WARN_STREAM("SparseImgAlign: no features to track!");
return 0;
}
ref_frame_ = ref_frame;
cur_frame_ = cur_frame;
ref_patch_cache_ = cv::Mat(ref_frame_ -> fts_.size(),patch_area_,CV_32F);
jacobian_cache_.resize(Eigen::NoChange,ref_patch_cache_.rows * patch_area_);
visible_fts_.resize(ref_patch_cache_.rows,false);
SE3 T_cur_from_ref(cur_frame_->T_f_w_*ref_frame_->T_f_w_.inverse());
for(level_ = max_levels_;level_ >= min_levels_;--level_){
mu_ = 0.1;
jacobian_cache_.setZero();
have_ref_patch_cache_ = false;
if(verbose_){
printf("\nPYRAMID LEVEL %i\n---------------\n", level_);
optimize(T_cur_from_ref);
}
}
cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_ -> T_f_w_;
return n_meas / patch_area_;
}
Matrix<double,6,6> SparseImgAlign::getFisherInformation(){
double sigma_i_sq = 5e-4*255*255;
Matrix<double,6,6> I = H_ / sigma_i_sq;
return I;
}
void SparseImgAlign::precomputeReferencePatches()
{
const int border = patch_halfsize_ + 1;
const cv::Mat& ref_img = ref_frame_ -> img_pyr_.at(level_);
const int stride = ref_img.cols;
const float scale = 1.0f / (1 << level_);
const Vector3d ref_pos = ref_frame_ -> pos();
const double focal_length = ref_frame_-> cam_ -> errorMultiplier2();
size_t feature_counter = 0;
std::vector<bool>::iterator visiblity_it = visible_fts_.begin();
for(auto it = ref_frame_-> fts_.begin(),ite = ref_frame_->fts_.end();
it != ite; ++it, ++feature_counter, ++visiblity_it){
const float u_ref = (*it)-> px[0] * scale;
const float v_ref = (*it)-> px[1] * scale;
const int u_ref_i = floorf(u_ref);
const int v_ref_i = floorf(v_ref);
if((*it)->point==NULL || u_ref_i-border<0 || v_ref_i-border<0 ||
u_ref_i+border >= ref_img.cols || v_ref_i+border >= ref_img.rows){
continue;
*visiblity_it = true;
const double depth(((*it) -> point -> pos_ - ref_pos).norm());
const Vector3d xyz_ref((*it) -> f * depth);
Matrix<double,2,6> frame_jac;
Frame::jacobian_xyz2uv(xyz_ref,frame_jac);
const float subpix_u_ref = u_ref - u_ref_i;
const float subpix_v_ref = v_ref - v_ref_i;
const float w_ref_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref);
const float w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref);
const float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref;
const float w_ref_br = subpix_u_ref * subpix_v_ref;
size_t pixel_counter = 0;
float* cache_ptr = reinterpret_cast<float*>(ref_patch_caches_.data) +
patch_area_ * feature_counter;
for(int y = 0;y < patch_size_;++y){
uint8_t* ref_img_ptr = (uint8_t*) ref_img.data + (v_ref_i + y - patch_halfsize_)*
stride + (u_ref_i - patch_halfsize_);
for(int x = 0;x < patch_size_;++x,++ref_img_ptr,++cache_ptr,++pixel_counter){
*cache_ptr=w_ref_tl*ref_img_ptr[0]+w_ref_tr*ref_img_ptr[1]+w_ref_bl*ref_img_ptr[stride]+w_ref_br*ref_img_ptr[stride+1];
float dx = 0.5f * ((w_ref_tl * ref_img_ptr[1] + w_ref_tr * ref_img_ptr[2] + w_ref_bl * ref_img_ptr[stride + 1] + w_ref_br * ref_img_ptr[stride + 2]) -
(w_ref_tl * ref_img_ptr[-1]+ w_ref_tr * ref_img_ptr[0] + w_ref_bl * ref_img_ptr[stride - 1] + w_ref_br * ref_img_ptr[stride]));
float dy = 0.5f * ((w_ref_tl * ref_img_ptr[stride] + w_ref_tr * ref_img_ptr[1 + stride] + w_ref_bl * ref_img_ptr[stride * 2] + w_ref_br * ref_img_ptr[stride * 2 + 1]) -
(w_ref_tl * ref_img_ptr[-stride]+ w_ref_tr * ref_img_ptr[1 - stride] + w_ref_bl * ref_img_ptr[0] + w_ref_br * ref_img_ptr[1]));
jacobian_cache_.col(feature_counter * patch_area_ + pixel_counter) = (dx * frame_jac.row(0) + dy * frame_jac.row(1)) * (focal_length / (1 << level_));
}
}
have_ref_patch_cache_ = true;
}
double SparseImgAlign::computeResiduals(const SE3& T_cur_from_ref,bool linearize_system,bool compute_weight_scale){
const cv::Mat& cur_img = cur_frame_ -> img_pyr_.at(level_);
if(linearize_system && display_){
resimg_ = cv::Mat(cur_img.size(),CV_32F,cv::Scalar(0));
}
if(have_ref_patch_cache_ == false){
precomputeReferencePatches();
}
std::vector<float> errors;
if(compute_weight_scale)
errors.reserve(visible_fts_.size());
const int stride = cur_img.cols;
const int border = patch_halfsize_ + 1;
const float scale = 1.0f / (1 << level_);
const Vector3d ref_pos(ref_frame_ -> pos());
float chi2 = 0.0;
size_t feature_counter = 0;
std::vector<bool>::iterator visiblity_it = visible_fts_.begin();
for(auto it = ref_frame_ -> fts_.begin();it != ref_frame_ -> fts_.end();++it,++feature_counter,++visiblity_it){
if(!*visiblity_it) continue;
const double depth = ((*it)-> point -> pos_ - ref_pos).norm();
const Vector3d xyz_ref((*it) -> f * depth);
const Vector3d xyz_cur(T_cur_from_ref * xyz_ref);
const Vector2f uv_cur_pyr(cur_frame_->cam_-> world2cam(xyz_cur).cast<float>()* scale);
const float u_cur = uv_cur_pyr[0];
const float v_cur = uv_cur_pyr[1];
const int u_cur_i = floorf(u_cur);
const int v_cur_i = floorf(v_cur);
for(u_cur_i<0 || v_ref_i<0 || u_cur_i-border<0 || v_cur_i-border<0 ||
u_cur_i+border>=cur_img.cols || v_cur_i>=cur_img.rows)
continue;
const float subpix_u_cur = u_cur - u_cur_i;
const float subpix_v_cur = v_cur - v_cur_i;
const float w_cur_tl = (1.0 - subpix_u_cur) * (1.0 - subpix_v_cur);
const float w_cur_tr = subpix_u_cur * (1.0 - subpix_v_cur);
const float w_cur_bl = (1.0 - subpix_u_cur) * subpix_v_cur;
const float w_cur_br = subpix_u_cur * subpix_v_cur;
float* ref_patch_cache_ptr = reinterpret_cast<float*>(ref_patch_caches_.data)
+ patch_area_ * feature_counter;
size_t pixel_counter = 0;
for(int y = 0;y < patch_size_;++y){
uint8_t* cur_img_ptr = (uint8_t*) cur_img.data + (v_cur_i+y-patch_halfsize_)*stride +
(u_cur_i - patch_halfsize_);
for(int x = 0;x < patch_size_;++x,++pixel_counter,++cur_img_ptr,++ref_patch_cache_ptr){
const float intensity_cur = w_cur_tl * cur_img_ptr[0] + w_cur_tr * cur_img_ptr[1] +
w_cur_bl * cur_img_ptr[stride] + w_cur_br * cur_img_ptr[stride + 1];
const float res = intensity_cur - (*ref_patch_cache_ptr);
if(compute_weight_scale) errors.push_back(fabsf(res));
float weight = 1.0;
if(use_weights_){
weight = weight_function_ -> value(res / scale_);
}
chi2 += res * res * weight;
n_meas_++;
if(linearize_system){
const Vector6d J(jacobian_cache_.col(feature_counter * patch_area_ + pixel_counter))'
H_.noalias() += J*J,transpose() * weight;
Jres_.noalias() -= J*res*weight;
if(display_){
resimg_.at<float>((int) v_cur + y - patch_halfsize_,(int) u_cur + x - patch_halfsize_) = res / 255;
}
}
}
}
if(compute_weight_scale && iter == 0){
scale_ = scale_estimator_ -> compute(errors);
}
return chi2 / n_meas_;
}
int SparseImgAlign::solve(){
x_ = H_.ldlt().solve(Jres);
if(bool) std::isnan((double) x_[0]))
return 0;
return 1;
}
void SparseImgAlign::update(const ModelType& T_curold_from_ref,
ModelType& T_curnew_from_ref){
T_curnew_from_ref = T_curold_from_ref * SE3::exp(-x_);
}
void SparseImgAlign::startIteration(){}
void SparseImgAlign::finishIteration(){
if(display_){
cv::nameWindow("residuals",CV_WINDOW_AUTOSIZE);
cv::imshow("residuals",resimg_ * 10);
cv::waitKey(0);
}
}
}