一 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); }
三 实验效果
改前:
改后:
四 代码参考:
commit 4122702ced85b20bd458d0e74624b9610c19f8cc
added binary version of vocabulary to speedup ORB_SLAM2 start
五 工程实例: