gtsam笔记

Gtsam源码阅读笔记

Key其实是一个无符号整数类型， Types.h中将其定义为 uint64_t

Symbol 中重载括号运算符，返回一个 Key(), Key()利用移位，得到一个Key，即还是一个整数

Values-inl.h中

// insert a templated value   template<typename ValueType>   void Values::insert(Key j, const ValueType& val) {     insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));   }

过程为，将 val 包装成一个GenericValue对象

GenericValue是模板类， 模板参数为ValueType， 构造函数入参为val

GenericValue的构造函数中将 val赋值给value_，value_类型为ValueType

GenericValue 继承自Value.

 

// define Value::cast here since now GenericValue has been declared template<typename ValueType> const ValueType& Value::cast() const {   return dynamic_cast<const GenericValue<ValueType>&>(*this).value(); }

这个也很重要! 用于类型恢复

 

void Values::print(const string& str, const KeyFormatter& keyFormatter) const {     cout << str << "Values with " << size() << " values:" << endl;     for(const_iterator key_value = begin(); key_value != end(); ++key_value) {       cout << "Value " << keyFormatter(key_value->key) << ": ";       key_value->value.print("");       cout << "\n";     }   }

对value进行遍历

 

string _defaultKeyFormatter(Key key) {   const Symbol asSymbol(key);   if (asSymbol.chr() > 0)     return (string) asSymbol;   else     return boost::lexical_cast<string>(key); }

 

得到一个Key后，获取对应的Symbol对象！

 

/** Retrieve key character */   unsigned char chr() const {     return c_;   }     /** Retrieve key index */   std::uint64_t index() const {     return j_;   }

利用Symbol成员函数，获取对应数据

 

最终形成一个例子

// 假设已经得到优化结果  Values result = DoglegOptimizer(graph, initialEstimate).optimize();   //result.print("Final results:\n");     for(Values::iterator key_value = result.begin(); key_value != result.end(); ++key_value)   { Key key = key_value->key; Symbol asSymbol(key); cout << asSymbol.chr() << std::endl; cout << asSymbol.index() << std::endl;   if(asSymbol.chr() == 'l') { Point3 pt3 = key_value->value.cast<Point3>(); pt3.print("pt : "); } else if(asSymbol.chr() == 'x') { Pose3 p3 = key_value->value.cast<Pose3>(); p3.print("pose3 : ");   Matrix4 mm = p3.matrix(); }   }