SVO学习日记-6--2021.1.29

SVO-6-map–2021.1.29

map.h

#ifndef SVO_MAP_H_
#define SVO_MAP_H_

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

namespace svo{
  class Point;
  class Feature;
  class Seed;
  
  //对于没有被分配到两个关键帧中已经收敛的3D点
  class MapPointCandidates{
  public:
    //候选点
    typedef pair<Point*,Feature*> PointCandidate;
    typedef pair<PointCandidate> PointCandidateList;
    
    //深度滤波运行在并行线程，同时填充候选链表
    //此互斥锁控制候选点的并发访问
    boost::mutex mut_;
    
    //从收敛的种子点中创建候选点
    //直到下一个关键帧的出现，这些点被用于重投影误差和位姿优化
    PointCandidateList camdidates_;
    list< Point* > trash_points_;
    
    MapPointCandidates();
    ~MapPointCandidates();
    
    //添加一个候选点
    void newCandidatePoint(Point* point,double depth_sigma2);
    
    //向帧上添加特征点，并从链表中删除候选点
    void addCandidatePointToFrame(FramePtr frame);
    
    //从后选链表中删除候选点
    bool deleteCandidatePoint(Point* point);
    
    //去除属于一个帧的所有候选
    void removeFrameCandidates(FramePtr frame);
    
    //重置候选链表，去除并删除所有点
    void reset();
    
    void deleteCandidate(PointCandidate& c);
    
    void emptyTrash();
  };
  
  //地图管理，保存在一个地图中的所有关键帧
  class Map : boost::noncopyable{
  public:
    
    //地图中的关键帧链表
    list<FramePtr > keyframes_;
    
    //一个删除点的机制
    list<Point* > trash_points_;
    MapPointCandidates point_candidates_;
    
    Map();
    ~Map();
    
    //重置地图，删除所有的关键帧并重置
    void reset();
    
    //删除地图中的点并去除所有的参考关键帧
    void safeDeletePoint(Point* pt);
    
    //删除点，将点移动到上述删除机制的双端队列里
    void deletePoint(Point* pt);
    
    //删除帧，将帧移动到删除机制的双端队列里
    bool safeDeleteFrame(FramePtr frame);
    
    //删除点与特征点之间的引用联系
    void removePtFrameRef(Frame* frame,Feature* ftr);
    
    //向地图中添加新的关键帧
    void addKeyframe(FramePtr new_keyframe);
    
    //给一个帧，返回所有视角重叠的关键帧
    void getCloseKeyframes(const FramePtr& frame,list<pair<FramePtr,double>>& close_kfs) const;
    
    //返回在空间中最接近并具有重叠视野的关键帧
    FramePtr getClosestKeyframes(const FramePtr& frame) const;
    
    //返回在空间中距离最远的关键帧
    FramePtr getFurthestKeyframe(const Vector3d& pos) const;
    
    //获得关键帧的id
    bool getKeyframeById(const int id,FramePtr& frame) const;
    
    //用旋转R，平移t和尺度s来转换整个地图
    void transform(const Matrix3d& R,const Vector3d* t,const double& s);
    
    //一个清除缓冲机制
    void emptyTrash();
    
    //返回最近插入地图的关键帧
    inline FramePtr lastKeyframe(){return keyframes_.back();}
    
    //返回地图中关键帧的数目
    inline size_t size() const {return keyframes_.size();}
  };
  
  //搜集函数的debug部分，主要作用是为了检查数据的一致性
  namespace map_debug{
    
    void mapStatistics(Map* map);
    void mapValidation(Map* map,int id);
    void frameValidation(Frame* frame,int id);
    void pointValidation(Point* point,int id);
  }
}

#endif
		

map.cpp

#include <set>
#include <svo_practice/map.h>
#include <svo_practice/point.h>
#include <svo_practice/frame.h>
#include <svo_practice/feature.h>
#include <boost/bind.hpp>

namespace svo{
  
svo::Map::Map() {}
svo::Map::~Map(){
  reset();
  SVO_INFO_STREAM("map destructed");
}

//重置---复位功能
void svo::Map::reset()
{
  keyframes_.clear();//删除地图中的关键帧
  point_candidates_.reset();//重置候选点
  emptyTrash();//清空KF，mp
}

//删除关键帧
bool svo::Map::safeDeleteFrame(FramePtr frame)
{
  bool found = false;
  
  //遍历地图中的关键帧
  for(auto it = keyframes_.begin(),ite = keyframes_.end();it != ite,++it){
    
    //从关键帧中找到这一帧
    if(*it == frame){
      
      //删除上面该帧中特征点与Point之间的联系
      std::for_each((*it) -> fts_.begin(),(*it) -> fts_.end(),[&](Feature* ftr){
	removePtFrameRef(it -> get(),ftr);
      });
      
      //清除这个关键帧
      keyframes_.erase(it);
      found = true;
      break;
    }
  }
  
  //删除属于这一帧的所有候选点
  point_candidates_.removeFrameCandidates(frame);
  
  if(found) return true;
  //没找到的话输出ERROR
  SVO_ERROR_STREAM("Tried to delete Keyframe in map which was not there.");
  return false;
}

//删除点和特征之间的引用关系
//参数：要删除的关键帧，帧frame与点point之间的特征点
//point与frame是通过ftr连接的
void svo::Map::removePtFrameRef(Frame* frame, svo::Feature* ftr)
{
  //先做一个判断，可能地图点在之前就被删除了
  if(ftr -> point == NULL) return;
  
  //切断feature与point之间的联系
  Point* pt = ftr -> point;
  ftr -> point = NULL;
  
  //该点point观测要是少于2的话，则删除
  if(pt -> obs_.size() <= 2){
    
    safeDeletePoint(pt);
    return;
  }
  
  //切断point与feature之间的联系，这里与上步不同的是互相指
  pt -> delateFrameRef(frame);//去除来自于地图点的引用
  
  //若ftr是keypoint则删除，keypoint要和3d点相连
  frame -> removeKeyPoint(ftr);
}

//安全删除点point,把point的obs_中的每一个都删除
void svo::Map::safeDeletePoint(svo::Point* pt)
{
  //删除所有关键帧中地图点的引用
  std::for_each(pt -> obs_.begin(),pt -> obs_.end(),[&](Feature* ftr){
    ftr -> point = NULL;//切断feature与point之间的联系
    
    //是关键点的话就删除
    ftr -> frame -> removeKeyPoint(ftr);
  });
  
  pt -> obs_.clear();
  
  //删除地图点
  deletePoint(pt);
}

void svo::Map::deletePoint(svo::Point* pt)
{
  //将地图点point的类型置为删除
  pt -> type_ = Point::TYPE_DELETED;
  
  //放入trash points
  trash_points_.push_back(pt);
}

//利用关键点找到接近frame中有共识关系的关键帧
//简单点说就是获取一堆与这一帧具有共视关系的帧，并记录起来
void svo::Map::getCloseKeyframes(const FramePtr& frame, 
				list< pair< FramePtr, double > >& close_kfs) const
{
  //检查对于这一帧，关键帧在视野中是否有重叠
  for(auto kf : keyframes_){	//得到关键帧
    for(auto keypoint : kf -> key_pts_){	//得到关键帧上的关键点
      if(keypoint == nullptr) continue;	//判断关键点有没有
      
      //有就返回他们之间关键帧的指针，以及他们之间的距离
      if(frame -> isVisible(keypoint -> point -> pos_)){
	close_kfs.push_back(std::make_pair(kf,
	  frame -> T_f_w_.translation() -
	     kf -> T_f_w_.translation()).norm()));
	     //这里取得都是world原点在frame之间的距离
	  break;
      }
    }
  }
}

//从上面记录的帧中找出最近的那一个
FramePtr svo::Map::getClosestKeyframes(const FramePtr& frame) const
{
  list< pair<FramePtr,double> > close_kfs;
  getCloseKeyframes(frame,close_kfs);
  
  //获得close_kfs,为空则返回
  if(close_kfs.empty()){return nullptr;}
  
  //对close_kfs的关键帧（具有共视关系）进行排序
  close_kfs.sort(boost::bind(&std::pair<FramePtr,double>::second,_1) < 
		 boost::bind(&std::pair<FramePtr,double>::second,_2));
  
  //判断最接近的是不是它本身，不是才返回，是才弹出
  if(close_kfs.front().first != frame) return close_kfs.front().first;
  
  close_kfs.pop_front();
  return close_kfs.front().first;
}

//找到离pos最远的关键帧
//Pos是指在world坐标系下到原点的距离
FramePtr svo::Map::getFurthestKeyframe(const Vector3d& pos) const
{
  FramePtr furthest_kf;//最远的帧
  double maxdist = 0.0;
  for(auto it = keyframes_.begin(),ite = keyframes_.end();it != ite;++it){
    double dist = ((*it) -> pos() - pos).norm();
    
    if(dist > maxdist){
      maxdist = dist;
      furthest_kf = *it;
    }
  }
  return furthest_kf;
}

//根据Id找frame
bool svo::Map::getKeyframeById(const int id, FramePtr& frame) const
{
  bool found = false;
  for(auto it = keyframes_.begin(),ite = keyframes_.end();it != ite;++it){
    if((*it) -> id_ == id){
      found = true;
      frame = *it;
      break;
    }
  }
  return found;
}

//对整个地图进行变换，R，t,s,
//分别计算的关键帧到世界坐标系下的Pos,然后计算了point的pos
void svo::Map::transform(const Matrix3d& R, const Vector3d* t, const double& s)
{
  //遍历地图里的关键帧
  for(auto it = keyframes_.begin(),ite = keyframes_.end();it != ite;++it){
    Vector3d pos = s * R * (*it) -> pos() + t;//获取每一个关键帧的pos
    Matrix3d rot = R * (*it) -> T_f_w_.rotation_matrix().inverse();//获取世界坐标系下的旋转矩阵
    (*it) -> T_f_w_ = SE3(rot,pos).inverse();//获取每一帧的变换矩阵
    
    //遍历每个关键帧上的特征点feature
    for(auto ftr = (*it) -> fts_.begin();ftr != (*it) -> fts_.end();++ftr){
      if((*ftr) -> point == NULL) continue;	//判断
      
      if((*ftr) -> point -> last_publish_ts_ = -1000) continue;
      
      (*ftr) -> point -> last_publish_ts_ = -1000;
      
      //然后特征点对应的地图点计算pos
      (*ftr) -> point -> pos_ = s * R * (*ftr) -> point -> pos_ + t;
    }
  }
}

void svo::Map::emptyTrash()
{
  //删除point指针
  std::for_each(trash_points_.begin(),trash_points_.end(),[&](Point* pt){
    delete pt;
    pt = NULL;
  });
  //清空
  trash_points_.clear();
  point_candidates_.emptyTrash();
}

svo::MapPointCandidates::MapPointCandidates() {}

svo::MapPointCandidates::~MapPointCandidates() {}

//创建候选点，来自收敛的种子点
//直到下一个关键帧进来，这些点用来重投影和位姿优化
void svo::MapPointCandidates::newCandidatePoint(svo::Point* point, double depth_sigma2)
{
  //设置为候选点类型
  point -> type_ = Point::TYPE_CANDIDATE;
  
  boost::unique_lock<boost::mutex> lock(mut_);
  //把点和最新的观测添进candidates_
  candidates_.push_back(PointCandidate(point,point -> obs_.front()));
}

//把候选特征点加入到KF中
//PointCandidateList里存储的是PointCandidate,pair<Point*,Feature*>类型
void svo::MapPointCandidates::addCandidatePointToFrame(FramePtr frame)
{
  boost::unique_lock<boost::mutex> lock(mut_);
  PointCandidateList::iterator it = candidates_.begin();
  while(it != candidates_.end()){
    if(it -> first -> obs_.front() -> frame == frame.get()){//从最近被观测到的关键点的帧开始遍历
      it -> first -> type_ = Point::TYPE_UNKNOWN;//把这个点设置为位置
      it -> first -> n_failed_reproj_ = 0;//重投影失败次数为0
      it -> second -> frame -> addFeature(it -> second);//把特征加入到frame里
      it = candidates_.erase(it);//然后从候选点里把它删除
    }else{
      ++it;//继续
    }
  }
}

//候选点中删除Point
bool svo::MapPointCandidates::deleteCandidatePoint(svo::Point* point)
{
  boost::unique_lock<boost::mutex> lock(mut_);
  
  //仍然遍历上述的candidates_
  for(auto it = candidates_.begin(),ite = candidates_.end();it != ite;++it){
    if(it -> first == point){	//删除点的步骤
      deleteCandidate(*it);
      candidates_.erase(it);
      return true;
    }
  }
  return false;
}

//删除frame上feature对应的候选点
void svo::MapPointCandidates::removeFrameCandidates(FramePtr frame)
{
  boost::unique_lock<boost::mutex> lock(mut_);
  auto it = candidates_.begin();	//遍历上面的candidates_
  while(it != candidates_.end()){
    if(it -> second -> frame == frame.get()){	//读取candidates_的second的frame里的feature
      deleteCandidate(*it);	//删除指针地址
      it = candidates_.earse(it);	//从candidates_中清除对象
    }
    else{
      ++it;//继续
    }
  }
}

//reset候选点的队列
void svo::MapPointCandidates::reset()
{
  boost::unique_lock<boost::mutex> lock(mut_);
  std::for_each(candidates_.begin(),candidates_.end(),[&](PointCandidate& c){
    delete c.first;
    delete c.second;
  });
  candidates_.clear();
}

//删除候选点，trash是为了防止别的帧也有观测点
void svo::MapPointCandidates::deleteCandidate(PointCandidate& c)
{
  delete c.second;c.second = NULL;	//先从point里的obs_中删除
  c.first -> type_ = Point::TYPE_DELETED;	//把点置为删除状态，放入trash
  trash_points_.push_back(c.first);
}

//清空垃圾桶
void svo::MapPointCandidates::emptyTrash()
{
  std::for_each(trash_points_.begin(),trash_points_.end(),[&](Point*& p){
    delete p; p = NULL;});
  trash_points_.clear();
}

//检查数据一致性
namespace map_debug{

  // 检查地图
void mapValidation(Map* map, int id)
{
  // 检查地图中的keyframe
  for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it)
    frameValidation(it->get(), id);
}

// 检查frame函数
// 之前各种删除，可能会有这种情况。。。point与frame本应该是双向的
// 之前说为什么feature留着，还真是留着了，他是中介，中介不能黄
void frameValidation(Frame* frame, int id)
{
  for(auto it = frame->fts_.begin(); it!=frame->fts_.end(); ++it)
  {
    if((*it)->point==NULL)
      continue;
    // 该frame没有对应的点
    if((*it)->point->type_ == Point::TYPE_DELETED)
      printf("ERROR DataValidation %i: Referenced point was deleted.\n", id);
    // frame有对应的点，点没对应的frame
    if(!(*it)->point->findFrameRef(frame))
      printf("ERROR DataValidation %i: Frame has reference but point does not have a reference back.\n", id);
    // 验证点
    pointValidation((*it)->point, id);
  }
  // 验证keyframe的keypoint是否正确
  for(auto it=frame->key_pts_.begin(); it!=frame->key_pts_.end(); ++it)
    if(*it != NULL)
      if((*it)->point == NULL)
        printf("ERROR DataValidation %i: KeyPoints not correct!\n", id);
}

//对上面点一致性的判断
void pointValidation(Point* point,int id){
    for(auto it = point -> obs_.begin(); it != point -> obs_.end();++it){
      bool found = false;
      for(auto it_ftr=(*it)->frame->fts_.begin();it_ftr!=(*it)->frame->fts_.end();++it_ftr){
	if((*it_ftr)->point == point){
	  found == true;break;
	}
      if(!found)
	printf("ERROR DataValidation %i: Point %i has inconsistent reference in frame %i, is candidate = %i\n", 
	       id, point->id_, (*it)->frame->id_, (int) point->type_);
      }
    }
}

//地图统计
void mapStatistics(Map* map){
  
  //平均每个关键帧有多少特征点
  size_t n_pt_obs(0);
  for(auto it=map->keyframes_.begin();it!=map-> keyframes_.end();++it)
    n_pt_obs += (*it)-> nObs();	//累加所有关键帧上的特征点
  printf("\n\nMap Statistics: Frame avg. point obs = %f\n", 
	   (float) n_pt_obs/map->size());	//计算均值
    
  size_t n_frame_obs(0);	//平均每个点有多少个观测
  size_t n_pts(0);
  
  std::set<Point*> points;
  for(auto it = map->keyframes_.begin();it!=map->keyframes_.end();++it){//遍历每一个地图中的关键帧
    for(auto ftr=(*it)->fts_.begin();ftr!=(*it)->fts_.end();++ftr){//遍历每一个关键帧中的feature与point关联的ftr
      if((*ftr)->point == NULL) continue;
      
      //用set来过滤掉指向相同对象的指针
      if(points.insert((*ftr)->point).second){
	++n_pts;//统计map中有多少个point
	n_frame_obs += (*ftr)-> point -> nRefs();//统计point有多少个特征点
      }
    }
  }
  //每一个point平均对应多少特征点
  printf("Map Statistics: Point avg. frame obs = %f\n\n", (float) n_frame_obs/n_pts);
}

}
}