SVO学习日记-4--2021.1.25

SVO-3-Point.h–2021.1.25

Point.h

#ifndef SVO_POINT_H_
#define SVO_POINT_H_

#include <boost/noncopyable.hpp>
#include <svo/global.h>

namespace g2o {
  class VertexSBAPointXYZ;
}
typedef g2o::VertexSBAPointXYZ g2oPoint;

namespace svo {

class Feature;

typedef Matrix<double, 2, 3> Matrix23d;

class Point : boost::noncopyable
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  
  enum PointType {	//点类型
    TYPE_DELETED,	//删除点
    TYPE_CANDIDATE,	//候选点
    TYPE_UNKNOWN,	//未知点
    TYPE_GOOD		//好点
  };

  static int                  point_counter_;           //!< 创建点的数目，具有唯一的ID
  int                         id_;                      //!< 给个点唯一的ID
  Vector3d                    pos_;                     //!< 世界坐标系下点的3D位置
  Vector3d                    normal_;                  //!< 点的表面法线
  Matrix3d                    normal_information_;      //!< 估计法线不确定性的协方差的矩阵的逆
  bool                        normal_set_;              //!< 标记表面法线是否被估计
  list<Feature*>              obs_;                     //!< 记录观测到点的关键帧
  size_t                      n_obs_;                   //!< 观测次数，关键帧以及在帧间成功投影的帧
  g2oPoint*                   v_pt_;                    //!< 临时指针
  int                         last_published_ts_;       //!< 上一次发布的时间戳
  int                         last_projected_kf_id_;    //!< 投影标记，一个点不要被重复投影两次
  PointType                   type_;                    //!< 点的质量
  int                         n_failed_reproj_;         //!< 失败投影的次数，用于评估点的质量
  int                         n_succeeded_reproj_;      //!< 成功投影的次数，用于评估点的质量
  int                         last_structure_optim_;    //!< 上次优化点的时间戳

  Point(const Vector3d& pos);
  Point(const Vector3d& pos, Feature* ftr);
  ~Point();
  
   /// 添加一个参考帧
  void addFrameRef(Feature* ftr);

  /// 去除一个参考帧
  bool deleteFrameRef(Frame* frame); 
  
  ///初始化点的法线
  void initNormal();
  
   /// 检测地图点是否有个参考帧
  Feature* findFrameRef(Frame* frame);
  
    ///获得有个相同视角的帧
  bool getCloseViewObs(const Vector3d& pos, Feature*& obs) const;
  
    /// 获得观测的数目
  inline size_t nRefs() const { return obs_.size(); }
  
    /// 通过最小化重投影误差优化点的位置
  void optimize(const size_t n_iter);
  
  ///求得世界坐标对像素的jacobians
  inline static void jacobian_xyz2uv(
      const Vector3d& p_in_f,
      const Matrix3d& R_f_w,
      Matrix23d& point_jac)
  {
    const double z_inv = 1.0/p_in_f[2];
    const double z_inv_sq = z_inv*z_inv;
    point_jac(0, 0) = z_inv;
    point_jac(0, 1) = 0.0;
    point_jac(0, 2) = -p_in_f[0] * z_inv_sq;
    point_jac(1, 0) = 0.0;
    point_jac(1, 1) = z_inv;
    point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
    point_jac = - point_jac * R_f_w;
  }
};

} // namespace svo

#endif // SVO_POINT_H_

#endif

Point.cpp

#include <stdexcept>
#include <vikit/math_utils.h>
#include <svo/point.h>
#include <svo/frame.h>
#include <svo/feature.h>
 
namespace svo {

int Point::point_counter_ = 0;	//点的计数

/********************************
 * @ function: 管理三角化的3D点的类，就是MapPoint
 *              构造函数
 * @ param:    pos   该3D点坐标
 * 
 * @ note:
 *******************************/
Point::Point(const Vector3d& pos) :
  id_(point_counter_++), //3D点的ID
  pos_(pos), //3D点的世界坐标
  normal_set_(false), //是否估计表面法向量
  n_obs_(0), //能观测该点的帧数，包括关键帧和投影到的帧
  v_pt_(NULL), //作为g2o BA优化时的点的临时指针 
  last_published_ts_(0), //上一次的发布的时间戳
  last_projected_kf_id_(-1), //上一次投影的关键帧的ID，防止投影两次
  type_(TYPE_UNKNOWN), //3D点的质量（删除、候选、未知、好）
  n_failed_reproj_(0), //重投影失败的次数，用来衡量点的质量
  n_succeeded_reproj_(0), //重投影成功次数，同上
  last_structure_optim_(0) //上一次优化的时间戳
{}//构造函数

Point::Point(const Vector3d& pos, Feature* ftr) :
  id_(point_counter_++),
  pos_(pos),
  normal_set_(false),
  n_obs_(1),
  v_pt_(NULL),
  last_published_ts_(0),
  last_projected_kf_id_(-1),
  type_(TYPE_UNKNOWN),
  n_failed_reproj_(0),
  n_succeeded_reproj_(0),
  last_structure_optim_(0)
{
  obs_.push_front(ftr); //增加一个该点的观测帧
}

Point::~Point()
{}

//增加一个该点的观测帧，以特征的形式表示
void Point::addFrameRef(Feature* ftr)
{
  obs_.push_front(ftr);
  ++n_obs_;
}

//查找该点是否被帧frame观测到
Feature* Point::findFrameRef(Frame* frame)
{
  for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
    if((*it)->frame == frame)
      return *it;
  return NULL;    // no keyframe found
}

//删除该点的观测帧
bool Point::deleteFrameRef(Frame* frame)
{
  for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
  {
    if((*it)->frame == frame)
    {
      obs_.erase(it);
      return true;
    }
  }
  return false;
}

//计算该点的法向量（点到相机的方向），以及协方差
void Point::initNormal()
{
  assert(!obs_.empty());
  
//[***step 1***] 得到最近的关键帧上该点的特征点（投影）
  const Feature* ftr = obs_.back();	//链表最后一位，也就是最近的关键帧
  assert(ftr->frame != NULL);
  
//[***step 2***] 将该点在归一化平面上的坐标转到世界坐标下，作为法向量，方向由该点指向摄影机中心
  normal_ = ftr->frame->T_f_w_.rotation_matrix().transpose()*(-ftr->f);
//[***step 3***] 计算该法向量的协方差矩阵，为什么这么求？？？，3D点的坐标到归一化平面的距离越大协方差越小？？？ 
  normal_information_ = DiagonalMatrix<double,3,3>(pow(20/(pos_-ftr->frame->pos()).norm(),2), 1.0, 1.0);
  // 标志位置1
  normal_set_ = true;
}

bool Point::getCloseViewObs(const Vector3d& framepos,Feature*& ftr) const{
	//主要是为了获得有着相同视角的点的帧以及有着相同的金字塔层数
	//得到framepose与当前3D点之间的向量，并归一化
	Vector3d obs_dir(framepos - pos_);obs_dir.normalize();
	auto min_it = obs_.begin();
	double min_cos_angle = 0;
	
	//从当前点的可观测帧中找到夹角最小的
	for(auto it = obs_.begin(),ite = obs_.end();it != ite;++it){
		
		//得到每个观测帧到该点的向量并归一化
		Vector3d dir((*it) -> frame -> pos() - pos_); dir.normalize();
		
		//由于归一化可以求的夹角
		double cos_angle = obs_dir.dot(dir);
		
		//找寻最小的夹角
		if(cos_angle > min_cos_angle){
			min_cos_angle = cos_angle;
			min_it = it;
		}
	}
	
	//返回最近的feature(帧)
	ftr = *min_it;
	
	//大于60度则无用
	if(min_cos_angle < 0.5) return false;
	return true;
}

/********************************
 * @ function: 最小化投影的坐标，优化3D点的坐标
 * 
 * @ param: 
 * 
 * @ note:
 *******************************/
 void Point::optimize(const size_t n_iter){
	Vector3d old_point = pos_;	//待优化的量,3d坐标
	double chi2 = 0.0;
	Matrix3d A;
	Vector3d b;
	
	//计算残差
	for(size_t i = 0; i < n_iter;i++){
		A.setZero();	//将A，b都先置为0向量和矩阵
		b.setZero();
		double new_chi2 = 0.0;
		
		for(auto it = obs_.begin();it != obs_.end();++it){
			Matrix23d J;	//定义雅可比矩阵2x6
			const Vector3d p_in_f((*it) -> frame -> T_f_w_ * pos_);	//世界坐标系转化到相机坐标系
			Point::Jacobian_xyz2uv(p_in_f,(*it) -> frame -> T_f_w_.rotation_matrix(),J);//计算雅可比
			const Vector2d e(vk::project2d((*it) -> f) - vk::project2d(p_in_f));	//计算投影残差
			new_chi2 += e.squaredNorm();	//归一化
			A.noalias() += J.transpose() * J;	//进行了一个交换赋值的步骤
			b.noalias() -= J.transpose * e;
		}
		
		const Vector3d dp(A.ldlt().solve(b));	//用了直接线性变化法DLT
		
		if((*i > 0 && new_chi2 > chi2) || std::isnan((double)dp[0])){
			#ifdef POINT_OPTIMIZER_DEBUG
			cout <<"it " <<i << "\t Failure \t new_chi2 = " <<new_chi2 <<endl;
			#endif
			pos = old_point;	//回滚操作
			break;
		}
		
		//迭代优化
		Vector3d new_point = pos_ + dp;	//进行优化变量的叠加
		old_point = pos_;	//这几步为优化迭代的交换步骤
		pos_ = new_point;
		chi2 = new_chi2;
		
		#ifdef POINT_OPTIMIZER_DEBUG
		cout <<"it " << i <<"\t success \t new_chi2 = " >> new_chi2 << "\t norm(b) = " << vk::norm_max(b) <<endl;
		#endif
		
		//收敛时就停止
		if(vk::norm_max(dp) <= EPS) break;
	}
	#ifdef POINT_OPTIMIZER_DEBUG
	cout << endl;
	#endif
 }
 
}