SVO-8-pose_optimizer–2021.2.1
pose_optimizer.h
#ifndef SVO_POSE_OPTIMIZER_H_
#define SVO_POSE_OPTIMIZER_H_
#include <svo/global.h>
namespace svo{
using namespace Eigen;
using namespace Sophus;
using namespace std;
typedef Matrix<double,6,6> Matrix6d;
typedef Matrix<double,2,6> Matrix26d;
typedef Matrix<double,6,1> Vector6d;
class Point;
namespace pose_optimizer{
void optimizeGaussNewton(const double reproj_thresh,
const size_t n_iter,
const bool verbose,
FramePtr& frame,
double& estimated_scale,
double& error_init,
double& error_final,
size_t& num_obs);
}
}
#endif
pose_optimizer.cpp
#include <stdexcept>
#include <svo_practice/pose_optimizer.h>
#include <svo_practice/frame.h>
#include <svo_practice/feature.h>
#include <svo_practice/point.h>
#include <vikit/robust_cost.h>
#include <vikit/math_utils.h>
namespace svo {
namespace pose_optimizer {
void optimizeGaussNewton(const double reproj_thresh, const size_t n_iter,
const bool verbose, FramePtr& frame,
double& estimated_scale, double& error_init,
double& error_final, size_t& num_obs)
{
double chi2(0.0);
vector<double> chi2_vec_init,chi2_vec_final;
vk::robust_cost::TukeyWeightFunction weight_function;
SE3 T_old(frame -> T_f_w_);
Matrix6d A;
Vector6d b;
std::vector<float> errors;
errors.reserve(frame -> fts_.size());
for(auto it = frame -> fts_.begin();it != frame -> fts_.end();++it){
if((*it) -> point == NULL) continue;
Vector2d e = vk::projected((*it) -> f) -
vk::projected(frame -> T_f_w_ * (*it) -> point -> pos_);
e *= 1.0 / (1 << (*it) -> level);
errors.push_back(e.norm());
}
if(errors.empty()) return;
vk::robust_cost::MADScaleEstimator scale_estimator;
estimated_scale = scale_estimator.compute(errors);
num_obs = errors.size();
chi2_vec_init.reserve(num_obs);
chi2_vec_final.reserve(num_obs);
double scale = estimated_scale;
for(size_t iter = 0;iter < n_iter;iter++){
if(iter == 5){
scale = 0.85 / frame -> cam_ -> errorMultiplier2();
}
b.setZero();
A.setZero();
double new_chi2(0.0);
for(auto it = frame -> fts_.begin();it != frame -> fts_.end();++it){
if((*it) -> point == NULL) continue;
Matrix26d J;
Vector3d xyz_f(frame -> T_f_w_ * (*it) -> point -> pos_);
Frame::jacobian_xyz2uv(xyz_f,J);
Vector2d e = vk::projected2d((*it)->f) - vk::projected2d(xyz_f);
double sqrt_inv_cov = 1.0 / (1 << (*it) -> level);
e *= sqrt_inv_cov;
if(iter == 0){
chi2_vec_init.push_back(e.squaredNorm());
}
J *= sqrt_inv_cov;
double weight = weight_function.value(e.norm() / scale);
A.noalias() += J.transpose() * J * weight;
b.noalias() -= J.transpose() * e * weight;
new_chi2 += e.squaredNorm() * weight;
}
const Vector6d dT(A.ldlt().solve(b));
if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0])){
if(verbose)
std::cout << "it " << iter
<< "\t FAILURE \t new_chi2" << new_chi2 << std::endl;
frame -> T_f_w_ = T_old;
break;
}
SE3 T_new = SE3::exp(dT) * frame -> T_f_w_;
T_old = frame -> T_f_w_;
frame -> T_f_w_ = T_new;
chi2 = new_chi2;
if(verbose)
std::cout << "it " << iter
<< "\t Success \t new_chi2 = " << new_chi2
<< "\t norm(dT) = " << vk::norm_max(dT) << std::endl;
if(vk::norm_max(dT) <= EPS)
break;
}
const double pixel_variance = 1.0;
frame -> Cov_ = pixel_variance * (A * std::pow(frame -> cam_ -> errorMultiplier2(),2)).inverse();
double reproj_thresh_scaled = reproj_thresh / frame -> cam_ -> errorMultiplier2();
size_t n_deleted_refs = 0;
for(Features::iterator it = frame -> fts_.begin();it != frame -> fts_.end();++it){
if((*it) -> point == NULL) continue;
Vector2d e = vk::projected2d((*it) -> f) -
vk::projected2d(frame -> T_f_w_ * (*it) -> point -> pos_);
double sqrt_inv_cov = 1.0 / (1 << (*it) -> level);
e *= sqrt_inv_cov;
chi2_vec_final.push_back(e.squaredNorm());
if(e.norm() > reproj_thresh_scaled){
(*it)->point = NULL;
++n_deleted_refs;
}
}
error_init = 0.0;
error_final = 0.0;
if(!chi2_vec_init.empty())
error_init = sqrt(vk::getMedian(chi2_vec_init))*frame -> cam_ -> errorMultiplier2();
if(!chi2_vec_final.empty())
error_final = sqrt(vk::getMedian(chi2_vec_final))*frame ->cam_ -> errorMultiplier2();
estimated_scale *= frame -> cam_ -> errorMultiplier2();
if(verbose)
std::cout << "n deleted obs = " << n_deleted_refs
<< "\t scale = " << estimated_scale
<< "\t error init = " << error_init
<< "\t error end = " << error_final << std::endl;
num_obs -= n_deleted_refs;
}
}
}