【SLAM】添加并使用ORBvoc.bin加速词典读取ORB-SLAM2

最新推荐文章于 2023-11-16 16:13:16 发布

Venistar

最新推荐文章于 2023-11-16 16:13:16 发布

阅读量2.8k

点赞数 5

分类专栏： slam ORB-SLAM2 文章标签： slam

原文链接：https://github.com/raulmur/ORB_SLAM2/pull/21/files

版权

slam 同时被 2 个专栏收录

5 篇文章 6 订阅

订阅专栏

ORB-SLAM2

2 篇文章 1 订阅

订阅专栏

添加并使用ORBvoc.bin加速词典读取ORB-SLAM2

1 文件修改
2 运行数据集

ORB-SLAM的作者对词典读取进行了改进，详见：https://github.com/raulmur/ORB_SLAM2/pull/21/files

1 文件修改

（1）CMakeLists.txt

在98行加入

Examples/Monocular/mono_kitti.cc)
target_link_libraries(mono_kitti ${PROJECT_NAME})

# Build tools
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/tools)
add_executable(bin_vocabulary
tools/bin_vocabulary.cc)
target_link_libraries(bin_vocabulary ${PROJECT_NAME})

在这里插入图片描述

（2）Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h

在249行加入

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

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

在1466行加入

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();
}

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

在这里插入图片描述

（3）build.sh

在32行加入

cd ..

echo "Converting vocabulary to binary"
./tools/bin_vocabulary

在这里插入图片描述

（4）src/PnPsolver.cc

将199行的

           vAvailableIndices[idx] = vAvailableIndices.back();

改为

           vAvailableIndices[randi] = vAvailableIndices.back();

在这里插入图片描述

（5）src/System.cc

在28行添加

#include <time.h>

bool has_suffix(const std::string &str, const std::string &suffix) {
  std::size_t index = str.find(suffix, str.size() - suffix.size());
  return (index != std::string::npos);
}

在新的69行添加

 clock_t tStart = clock();

将新的71行中

bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);

改为

bool bVocLoad = false; // chose loading method based on file extension
    if (has_suffix(strVocFile, ".txt"))
	  bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
	else
	  bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile);

将新的79行中的

cerr << "Falied to open at: " << strVocFile << endl;

改为

      cerr << "Failed to open at: " << strVocFile << endl;

将新的82行中的


    cout << "Vocabulary loaded!" << endl << endl;

改为

    printf("Vocabulary loaded in %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);

在这里插入图片描述

（6）tools/bin_vocabulary.cc

根目录新增文件夹tools并新建文件bin_vocabulary.cc

#include <time.h>

#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 运行数据集

重新编译ORB-SLAM2后，运行数据集进行测试

加载时间从8秒多减少到0.12秒

Loading fom text: 8.57s
Saving as binary: 0.12s

TUM数据集运行

./Examples/Monocular/mono_tum Vocabulary/ORBvoc.bin Examples/Monocular/TUMX.yaml PATH_TO_SEQUENCE_FOLDER

KITTI数据集运行

./Examples/Monocular/mono_kitti Vocabulary/ORBvoc.bin Examples/Monocular/KITTIX.yaml PATH_TO_DATASET_FOLDER/dataset/sequences/SEQUENCE_NUMBER

EuRoC数据集运行

对于MH序列，命令：

./Examples/Monocular/mono_euroc Vocabulary/ORBvoc.bin Examples/Monocular/EuRoC.yaml PATH_TO_SEQUENCE_FOLDER/mav0/cam0/data Examples/Monocular/EuRoC_TimeStamps/SEQUENCE.txt

对于V1和V2序列，命令：

./Examples/Monocular/mono_euroc Vocabulary/ORBvoc.bin Examples/Monocular/EuRoC.yaml PATH_TO_SEQUENCE/cam0/data Examples/Monocular/EuRoC_TimeStamps/SEQUENCE.txt