SVO学习日记-8--2021.2.1

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;
  
  //仅运动BA,对单一帧进行最小化重投影误差
  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;
      
      //鲁棒核函数选择Tukey权重函数
      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());//特征点的位置误差
      
      //计算frame上,每个特征点位置与3D点投影位置的误差,在单位平面上的误差
      for(auto it = frame -> fts_.begin();it != frame -> fts_.end();++it){
	if((*it) -> point == NULL) continue;
	
	//计算误差:特征点投影到单位平面的值-3D投影点到单位平面上的值
	Vector2d e = vk::projected((*it) -> f) - 
		     vk::projected(frame -> T_f_w_ * (*it) -> point -> pos_);
	
	//转换到对应的金字塔层,层数越高,占比重越小,因为高层的误差噪声很大
	e *= 1.0 / (1 << (*it) -> level);
	
	//xy坐标的平方和,像素距离,误差大小,来源于上面二者的误差
	errors.push_back(e.norm());
      }
      if(errors.empty()) return;
      
      //根据总的errors计算中位数绝对误差,误差的尺度
      //确定误差整体是比较大还是比较小
      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;	//把估计的标准差赋值给scale
      
      //迭代优化位姿
      for(size_t iter = 0;iter < n_iter;iter++){
	
	//第五次迭代的时候重新计算一下scale
	//之前计算的J不包括fx,所以尺度阈值里也要除掉fx
	//多次迭代后,误差的标准差将会减小,符合常规
	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;
	  
	  //首先计算位姿的雅可比矩阵,3D点到归一化平面的坐标到像素的雅可比
	  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;//配置的是最小二乘的右边项b
	  if(iter == 0){
	    
	    //存储误差的平方和到chi2_vec_init里
	    chi2_vec_init.push_back(e.squaredNorm());
	  }
	  
	  //配置最小二乘左边项A的部分
	  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;
	}
	
	//求解方法,DLT
	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;
      
      //之前的J里没有fx,因此这里要在A上乘以fx的平方,因为A = J^T * J
      frame -> Cov_ = pixel_variance * (A * std::pow(frame -> cam_ -> errorMultiplier2(),2)).inverse();
      
      //去除较大重投影误差的测量
      //同理阈值要除以fx
      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;
    }
  }
}