vSLAM 使用二进制词典优化加载速度

一 DBoW2

        ORB-SLAM2和ORB-SLAM3使用的是DBoW2,DBoW2.h中只封装了brief特征(VINS原版使用)和ORB特征,对应两种字典和数据库。

        TemplatedVocabulary.h只封装了brief和ORB特征的字典的save和load。ORB特征(描述子cv::Mat是CV_8U类型,描述子size为1*32)用txt格式存储,brief特征用bin格式存储,没有封装SuperPoint特征(描述子cv::Mat是CV_32F类型,描述子size为1*256)需要自己改写。

#define CV_8U   0
#define CV_8S   1
#define CV_16U  2
#define CV_16S  3
#define CV_32S  4
#define CV_32F  5
#define CV_64F  6
#define CV_USRTYPE1 7

/opt/ros/kinetic/include/opencv-3.3.1-dev/opencv2/core/hal/interface.h

1)ORB特征

        DBOW2中TemplatedVocabulary.h已经提供txt格式:

/**
 * Loads the vocabulary from a text file
 * @param filename
 */
bool loadFromTextFile(const std::string &filename);

/**
 * Saves the vocabulary into a text file
 * @param filename
 */
void saveToTextFile(const std::string &filename) const;

        GitHub - poine/ORB_SLAM2对其改写为binary格式:

template<class TDescriptor, class F>
bool TemplatedVocabulary<TDescriptor,F>::loadFromBinaryFile(const std::string &filename) {
  fstream f;
  f.open(filename.c_str(), ios_base::in|ios::binary);
  unsigned int nb_nodes, size_node;
  f.read((char*)&nb_nodes, sizeof(nb_nodes));
  f.read((char*)&size_node, sizeof(size_node));
  f.read((char*)&m_k, sizeof(m_k));
  f.read((char*)&m_L, sizeof(m_L));
  f.read((char*)&m_scoring, sizeof(m_scoring));
  f.read((char*)&m_weighting, sizeof(m_weighting));
  createScoringObject();
  
  m_words.clear();
  m_words.reserve(pow((double)m_k, (double)m_L + 1));
  m_nodes.clear();
  m_nodes.resize(nb_nodes+1);
  m_nodes[0].id = 0;
  char buf[size_node]; int nid = 1;
  while (!f.eof()) {
   f.read(buf, size_node);
   m_nodes[nid].id = nid;
   // FIXME
   const int* ptr=(int*)buf;
   m_nodes[nid].parent = *ptr;
   //m_nodes[nid].parent = *(const int*)buf;
   m_nodes[m_nodes[nid].parent].children.push_back(nid);
   m_nodes[nid].descriptor = cv::Mat(1, F::L, CV_8U);
   memcpy(m_nodes[nid].descriptor.data, buf+4, F::L);
   m_nodes[nid].weight = *(float*)(buf+4+F::L);
   if (buf[8+F::L]) { // is leaf
     int wid = m_words.size();
     m_words.resize(wid+1);
     m_nodes[nid].word_id = wid;
     m_words[wid] = &m_nodes[nid];
   }
   else
     m_nodes[nid].children.reserve(m_k);
   nid+=1;
  }
  f.close();
  return true;
}


// --------------------------------------------------------------------------

template<class TDescriptor, class F>
void TemplatedVocabulary<TDescriptor,F>::saveToBinaryFile(const std::string &filename) const {
  fstream f;
  f.open(filename.c_str(), ios_base::out|ios::binary);
  unsigned int nb_nodes = m_nodes.size();
  float _weight;
  unsigned int size_node = sizeof(m_nodes[0].parent) + F::L*sizeof(char) + sizeof(_weight) + sizeof(bool);
  f.write((char*)&nb_nodes, sizeof(nb_nodes));
  f.write((char*)&size_node, sizeof(size_node));
  f.write((char*)&m_k, sizeof(m_k));
  f.write((char*)&m_L, sizeof(m_L));
  f.write((char*)&m_scoring, sizeof(m_scoring));
  f.write((char*)&m_weighting, sizeof(m_weighting));
  for(size_t i=1; i<nb_nodes;i++) {
   const Node& node = m_nodes[i];
   f.write((char*)&node.parent, sizeof(node.parent));
   f.write((char*)node.descriptor.data, F::L);
   _weight = node.weight; f.write((char*)&_weight, sizeof(_weight));
   bool is_leaf = node.isLeaf(); f.write((char*)&is_leaf, sizeof(is_leaf)); // i put this one at the end for alignement....
  }
  f.close();
}

        具体用法在tools/bin_vocabulary.cc中:

#include "ORBVocabulary.h"
using namespace std;

bool load_as_text(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) {
  clock_t tStart = clock();
  bool res = voc->loadFromTextFile(infile);
  printf("Loading fom text: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
  return res;
}

void load_as_xml(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) {
  clock_t tStart = clock();
  voc->load(infile);
  printf("Loading fom xml: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
}

void load_as_binary(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) {
  clock_t tStart = clock();
  voc->loadFromBinaryFile(infile);
  printf("Loading fom binary: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
}

void save_as_xml(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) {
  clock_t tStart = clock();
  voc->save(outfile);
  printf("Saving as xml: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
}

void save_as_text(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) {
  clock_t tStart = clock();
  voc->saveToTextFile(outfile);
  printf("Saving as text: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
}

void save_as_binary(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) {
  clock_t tStart = clock();
  voc->saveToBinaryFile(outfile);
  printf("Saving as binary: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
}


int main(int argc, char **argv) {
  cout << "BoW load/save benchmark" << endl;
  ORB_SLAM2::ORBVocabulary* voc = new ORB_SLAM2::ORBVocabulary();

  load_as_text(voc, "Vocabulary/ORBvoc.txt");
  save_as_binary(voc, "Vocabulary/ORBvoc.bin");

  return 0;
}

2)SuperPoint特征

       txt格式可以参考ORB改写:

static int descriptor_length_in_bytes = 256;  // ORB: 32;

void fromString(cv::Mat &a, const std::string &s) {
  a.create(1, descriptor_length_in_bytes, CV_32F);
  float *p = a.ptr<float>();

  std::stringstream ss(s);
  for(int i = 0; i < a.cols; ++i, ++p) {
    float n;
    ss >> n;

    if(!ss.fail())
      *p = n;
  }
}

std::string toString(const cv::Mat &a) {
  std::stringstream ss;

  const float *p = a.ptr<float>();
  for(int i = 0; i < a.cols; ++i, ++p)
    ss << *p << " ";

  return ss.str();
}

bool Vocabulary::loadFromTextFile(const std::string &filename) {
  std::ifstream f;
  f.open(filename.c_str());

  if(f.eof())
    return false;

  m_words.clear();
  m_nodes.clear();

  std::string s;
  getline(f,s);
  std::stringstream ss;
  ss << s;
  ss >> m_k;
  ss >> m_L;
  int n1, n2;
  ss >> n1;
  ss >> n2;

  if(m_k<0 || m_k>20 || m_L<1 || m_L>10 || n1<0 || n1>5 || n2<0 || n2>3) {
    std::cerr << "Vocabulary loading failure: This is not a correct text file!" << std::endl;
    return false;
  }

  m_scoring = (ScoringType)n1;
  m_weighting = (WeightingType)n2;
  createScoringObject();

  // nodes
  int expected_nodes =
      (int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1));
  m_nodes.reserve(expected_nodes);

  m_words.reserve(pow((double)m_k, (double)m_L + 1));

  m_nodes.resize(1);
  m_nodes[0].id = 0;
  while(!f.eof()) {
    std::string snode;
    getline(f,snode);
    std::stringstream ssnode;
    ssnode << snode;

    int nid = m_nodes.size();
    m_nodes.resize(m_nodes.size()+1);
    m_nodes[nid].id = nid;

    int pid ;
    ssnode >> pid;
    m_nodes[nid].parent = pid;
    m_nodes[pid].children.push_back(nid);

    int nIsLeaf;
    ssnode >> nIsLeaf;

    std::stringstream ssd;
    for(int iD=0;iD<descriptor_length_in_bytes;iD++) {
      std::string sElement;
      ssnode >> sElement;
      ssd << sElement << " ";
    }
    fromString(m_nodes[nid].descriptor, ssd.str());

    ssnode >> m_nodes[nid].weight;

    if(nIsLeaf>0) {
      int wid = m_words.size();
      m_words.resize(wid+1);

      m_nodes[nid].word_id = wid;
      m_words[wid] = &m_nodes[nid];
    } else {
      m_nodes[nid].children.reserve(m_k);
    }
  }

  std::cout << "nodes size: " << m_nodes.size()
            << ", world size: " << m_words.size()
            << ", descriptor type: " << m_nodes.back().descriptor.type()
            << ", descriptor size: " << m_nodes.back().descriptor.size << std::endl;
  std::cout << "descriptor: "<< cv::format(m_nodes.back().descriptor, cv::Formatter::FMT_C) << std::endl;
  return true;
}
 

void Vocabulary::saveToTextFile(const std::string &filename) const {
  std::fstream f;
  f.open(filename.c_str(),std::ios_base::out);
  f << m_k << " " << m_L << " " << " " << m_scoring << " " << m_weighting << std::endl;

  for(size_t i=1; i<m_nodes.size();i++) {
    const Node& node = m_nodes[i];

    f << node.parent << " ";
    if(node.isLeaf())
      f << 1 << " ";
    else
      f << 0 << " ";

    f << toString(node.descriptor) << " " << (double)node.weight << std::endl;
  }

  f.close();
}

        二进制加速参考GitHub - poine/ORB_SLAM2改写,目前还有点小问题。

bool Vocabulary::loadFromBinaryFile(const std::string &filename) {
  std::fstream f;
  f.open(filename.c_str(), std::ios_base::in | std::ios::binary);
  unsigned int nb_nodes, size_node;
  f.read((char*)&nb_nodes, sizeof(nb_nodes));
  f.read((char*)&size_node, sizeof(size_node));
  f.read((char*)&m_k, sizeof(m_k));
  f.read((char*)&m_L, sizeof(m_L));
  f.read((char*)&m_scoring, sizeof(m_scoring));
  f.read((char*)&m_weighting, sizeof(m_weighting));
  createScoringObject();

  m_words.clear();
  m_words.reserve(pow((double)m_k, (double)m_L + 1));
  m_nodes.clear();
  m_nodes.resize(nb_nodes+1);
  m_nodes[0].id = 0;
  char buf[size_node]; int nid = 1;
  while (!f.eof()) {
    f.read(buf, size_node);
    std::cout << "buf: " << buf << std::endl;
    m_nodes[nid].id = nid;
    // FIXME
    const float* ptr=(float*)buf;
    m_nodes[nid].parent = *ptr;
    //m_nodes[nid].parent = *(const int*)buf;
    m_nodes[m_nodes[nid].parent].children.push_back(nid);
    memcpy(m_nodes[nid].descriptor.data, buf+4, sizeof(float)*descriptor_length_in_bytes);
//    memcpy(m_nodes[nid].descriptor.data, buf+4, descriptor_length_in_bytes);
    m_nodes[nid].weight = *(float*)(buf+4+sizeof(float)*descriptor_length_in_bytes);
    if (buf[8+sizeof(float)*descriptor_length_in_bytes]) { // is leaf
      int wid = m_words.size();
      m_words.resize(wid+1);
      m_nodes[nid].word_id = wid;
      m_words[wid] = &m_nodes[nid];
    }
    else
      m_nodes[nid].children.reserve(m_k);
    nid+=1;
  }
  f.close();

  std::cout << "nodes size: " << m_nodes.size()
            << ", world size: " << m_words.size()
            << ", descriptor type: " << m_nodes.back().descriptor.type()
            << ", descriptor size: " << m_nodes.back().descriptor.size << std::endl;
  std::cout << "descriptor: "<< cv::format(m_nodes.back().descriptor, cv::Formatter::FMT_C) << std::endl;
  return true;
}

void Vocabulary::saveToBinaryFile(const std::string &filename) const {
  std::fstream f;
  f.open(filename.c_str(), std::ios_base::out | std::ios::binary);
  unsigned int nb_nodes = m_nodes.size();
  float _weight;
  unsigned int size_node = sizeof(m_nodes[0].parent) + descriptor_length_in_bytes*sizeof(float) + sizeof(_weight) + sizeof(bool);
  f.write((char*)&nb_nodes, sizeof(nb_nodes));
  f.write((char*)&size_node, sizeof(size_node));
  f.write((char*)&m_k, sizeof(m_k));
  f.write((char*)&m_L, sizeof(m_L));
  f.write((char*)&m_scoring, sizeof(m_scoring));
  f.write((char*)&m_weighting, sizeof(m_weighting));
  for(size_t i=1; i<nb_nodes;i++) {
    const Node& node = m_nodes[i];
    f.write((char*)&node.parent, sizeof(node.parent));
    f.write((char*)&node.descriptor.data, descriptor_length_in_bytes*sizeof(float));
    _weight = node.weight; f.write((char*)&_weight, sizeof(_weight));
    bool is_leaf = node.isLeaf(); f.write((char*)&is_leaf, sizeof(is_leaf)); // i put this one at the end for alignement....
  }
  f.close();
}

二 DBoW3

        不用定义不同特征的字典模板类,调用dbow3/Vocabulary.cpp中区分load和save函数,通过后缀和是否为二进制类型的文件进行加载和保存字典。

1)txt格式,实际会调用

/** 
 * Saves the vocabulary to a file storage structure
 * @param fn node in file storage
 */
virtual void save(cv::FileStorage &fs, 
  const std::string &name = "vocabulary") const;
/**
 * Loads the vocabulary from a file storage node
 * @param fn first node
 * @param subname name of the child node of fn where the tree is stored.
 *   If not given, the fn node is used instead
 */  
virtual void load(const cv::FileStorage &fs, 
  const std::string &name = "vocabulary");

        其中的不同特征类型对应的不同描述子格式用dbow3/DescManip.cpp中的

std::string DescManip::toString(const cv::Mat &a)
{
    stringstream ss;
    //introduce a magic value to distinguish from DBOw2
    ss<<"dbw3 ";
    //save size and type


    ss <<a.type()<<" "<<a.cols<<" ";

    if (a.type()==CV_8U){
        const unsigned char *p = a.ptr<unsigned char>();
        for(int i = 0; i < a.cols; ++i, ++p)
            ss << (int)*p << " ";
    }else{

        const float *p = a.ptr<float>();
        for(int i = 0; i < a.cols; ++i, ++p)
            ss <<  *p << " ";

    }

    return ss.str();
}

// --------------------------------------------------------------------------
  
void DescManip::fromString(cv::Mat &a, const std::string &s)
{

    //check if the dbow3 is present
    string ss_aux;ss_aux.reserve(10);
    for(size_t i=0;i<10 && i<s.size();i++)
        ss_aux.push_back(s[i]);
    if(ss_aux.find("dbw3")==std::string::npos){//is dbow2
        //READ UNTIL END
        stringstream ss(s);
        int val;
        vector<uchar> data;data.reserve(100);
        while( ss>>val) data.push_back(val);
        //copy to a
        a.create(1,data.size(),CV_8UC1);
        memcpy(a.ptr<char>(0),&data[0],data.size());
    }
    else {
        char szSign[10];
        int type,cols;
        stringstream ss(s);
        ss >> szSign >> type >> cols;
        a.create(1,  cols, type);
        if(type==CV_8UC1){
            unsigned char *p = a.ptr<unsigned char>();
            int n;
            for(int i = 0; i <  a.cols; ++i, ++p)
                if ( ss >> n) *p = (unsigned char)n;
        }
        else{
            float *p = a.ptr<float>();
            for(int i = 0; i <  a.cols; ++i, ++p)
                if ( !(ss >> *p))cerr<<"Error reading. Unexpected EOF. DescManip::fromString"<<endl;
        }

    }

}

2)二进制格式,实际会调用

void toStream(  std::ostream &str, bool compressed=true) const throw(/*std::exception*/);
void fromStream(  std::istream &str )   throw(/*std::exception*/);其

        其中的不同特征类型对应的不同描述子格式用dbow3/DescManip.cpp中的

void DescManip::toStream(const cv::Mat &m,std::ostream &str){
    assert(m.rows==1 || m.isContinuous());
    int type=m.type();
    int cols=m.cols;
    int rows=m.rows;
    str.write((char*)&cols,sizeof(cols));
    str.write((char*)&rows,sizeof(rows));
    str.write((char*)&type,sizeof(type));
    str.write((char*)m.ptr<char>(0),m.elemSize()*m.cols);
}

void DescManip::fromStream(cv::Mat &m,std::istream &str){
    int type,cols,rows;
    str.read((char*)&cols,sizeof(cols));
    str.read((char*)&rows,sizeof(rows));
    str.read((char*)&type,sizeof(type));
    m.create(rows,cols,type);
    str.read((char*)m.ptr<char>(0),m.elemSize()*m.cols);
}

三 实验效果

改前:

改后:

四 代码参考:

GitHub - poine/ORB_SLAM2: Real-Time SLAM for Monocular, Stereo and RGB-D Cameras, with Loop Detection and Relocalization Capabilities

commit 4122702ced85b20bd458d0e74624b9610c19f8cc

added binary version of vocabulary to speedup ORB_SLAM2 start

五 工程实例:

https://zhuanlan.zhihu.com/p/20589372

https://zhuanlan.zhihu.com/p/20596486

  • 3
    点赞
  • 3
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值