All unit tests pass with TypedSymbol removed
parent
2f7f535f34
commit
3d40f5e6fc
|
@ -16,7 +16,7 @@
|
||||||
* @date Aug 23, 2011
|
* @date Aug 23, 2011
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
#include <gtsam/geometry/Pose3.h>
|
#include <gtsam/geometry/Pose3.h>
|
||||||
#include <gtsam/geometry/Cal3_S2.h>
|
#include <gtsam/geometry/Cal3_S2.h>
|
||||||
#include <gtsam/geometry/SimpleCamera.h>
|
#include <gtsam/geometry/SimpleCamera.h>
|
||||||
|
|
|
@ -19,7 +19,7 @@
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
||||||
// for all nonlinear keys
|
// for all nonlinear keys
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
|
|
||||||
// for points and poses
|
// for points and poses
|
||||||
#include <gtsam/geometry/Point2.h>
|
#include <gtsam/geometry/Point2.h>
|
||||||
|
|
|
@ -22,7 +22,7 @@
|
||||||
#include <gtsam/slam/PriorFactor.h>
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
#include <gtsam/geometry/Rot2.h>
|
#include <gtsam/geometry/Rot2.h>
|
||||||
#include <gtsam/linear/NoiseModel.h>
|
#include <gtsam/linear/NoiseModel.h>
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
#include <gtsam/nonlinear/NonlinearOptimization.h>
|
#include <gtsam/nonlinear/NonlinearOptimization.h>
|
||||||
|
|
||||||
|
|
|
@ -23,7 +23,7 @@
|
||||||
#include <gtsam/slam/PriorFactor.h>
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
#include <gtsam/slam/BetweenFactor.h>
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
#include <gtsam/nonlinear/Ordering.h>
|
#include <gtsam/nonlinear/Ordering.h>
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||||
#include <gtsam/linear/GaussianBayesNet.h>
|
#include <gtsam/linear/GaussianBayesNet.h>
|
||||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||||
|
|
|
@ -20,7 +20,7 @@
|
||||||
#include <boost/foreach.hpp>
|
#include <boost/foreach.hpp>
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
|
|
|
@ -19,7 +19,7 @@
|
||||||
#include <boost/shared_ptr.hpp>
|
#include <boost/shared_ptr.hpp>
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
|
|
|
@ -25,7 +25,7 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||||
|
|
|
@ -21,7 +21,7 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/inference/BayesNet.h>
|
#include <gtsam/inference/BayesNet.h>
|
||||||
|
|
|
@ -24,7 +24,7 @@ using namespace boost::assign;
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
#include <gtsam/base/TestableAssertions.h>
|
#include <gtsam/base/TestableAssertions.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/nonlinear/Ordering.h>
|
#include <gtsam/nonlinear/Ordering.h>
|
||||||
|
|
|
@ -22,7 +22,7 @@
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <map>
|
#include <map>
|
||||||
|
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
|
@ -15,9 +15,6 @@
|
||||||
* @author Alireza Fathi
|
* @author Alireza Fathi
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
|
|
||||||
/*STL/C++*/
|
/*STL/C++*/
|
||||||
|
|
|
@ -1,371 +0,0 @@
|
||||||
/* ----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
||||||
* Atlanta, Georgia 30332-0415
|
|
||||||
* All Rights Reserved
|
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
||||||
|
|
||||||
* See LICENSE for the license information
|
|
||||||
|
|
||||||
* -------------------------------------------------------------------------- */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file Key.h
|
|
||||||
* @date Jan 12, 2010
|
|
||||||
* @author: Frank Dellaert
|
|
||||||
* @author: Richard Roberts
|
|
||||||
*/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <list>
|
|
||||||
#include <iostream>
|
|
||||||
#include <boost/mpl/char.hpp>
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
#include <boost/serialization/nvp.hpp>
|
|
||||||
#ifdef GTSAM_MAGIC_KEY
|
|
||||||
#include <boost/lexical_cast.hpp>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#define ALPHA '\224'
|
|
||||||
|
|
||||||
namespace gtsam {
|
|
||||||
|
|
||||||
// Forward declarations
|
|
||||||
class Symbol;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* TypedSymbol key class is templated on
|
|
||||||
* 1) the type T it is supposed to retrieve, for extra type checking
|
|
||||||
* 2) the character constant used for its string representation
|
|
||||||
*/
|
|
||||||
template<class T, char C>
|
|
||||||
class TypedSymbol {
|
|
||||||
|
|
||||||
protected:
|
|
||||||
size_t j_;
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
// typedefs
|
|
||||||
typedef T Value;
|
|
||||||
typedef boost::mpl::char_<C> Chr; // to reconstruct the type: use Chr::value
|
|
||||||
|
|
||||||
// Constructors:
|
|
||||||
|
|
||||||
TypedSymbol() :
|
|
||||||
j_(0) {
|
|
||||||
}
|
|
||||||
TypedSymbol(size_t j) :
|
|
||||||
j_(j) {
|
|
||||||
}
|
|
||||||
|
|
||||||
virtual ~TypedSymbol() {}
|
|
||||||
|
|
||||||
// Get stuff:
|
|
||||||
///TODO: comment
|
|
||||||
size_t index() const {
|
|
||||||
return j_;
|
|
||||||
}
|
|
||||||
static char chr() {
|
|
||||||
return C;
|
|
||||||
}
|
|
||||||
const char* c_str() const {
|
|
||||||
return ((std::string) (*this)).c_str();
|
|
||||||
}
|
|
||||||
operator std::string() const {
|
|
||||||
return (boost::format("%c%d") % C % j_).str();
|
|
||||||
}
|
|
||||||
std::string latex() const {
|
|
||||||
return (boost::format("%c_{%d}") % C % j_).str();
|
|
||||||
}
|
|
||||||
Symbol symbol() const;
|
|
||||||
|
|
||||||
// logic:
|
|
||||||
|
|
||||||
bool operator<(const TypedSymbol& compare) const {
|
|
||||||
return j_ < compare.j_;
|
|
||||||
}
|
|
||||||
bool operator==(const TypedSymbol& compare) const {
|
|
||||||
return j_ == compare.j_;
|
|
||||||
}
|
|
||||||
bool operator!=(const TypedSymbol& compare) const {
|
|
||||||
return j_ != compare.j_;
|
|
||||||
}
|
|
||||||
int compare(const TypedSymbol& compare) const {
|
|
||||||
return j_ - compare.j_;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Testable Requirements
|
|
||||||
virtual void print(const std::string& s = "") const {
|
|
||||||
std::cout << s << ": " << (std::string) (*this) << std::endl;
|
|
||||||
}
|
|
||||||
bool equals(const TypedSymbol& expected, double tol = 0.0) const {
|
|
||||||
return (*this) == expected;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template<class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(j_);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
/** forward declaration to avoid circular dependencies */
|
|
||||||
template<class T, char C, typename L>
|
|
||||||
class TypedLabeledSymbol;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Character and index key used in VectorValues, GaussianFactorGraph,
|
|
||||||
* GaussianFactor, etc. These keys are generated at runtime from TypedSymbol
|
|
||||||
* keys when linearizing a nonlinear factor graph. This key is not type
|
|
||||||
* safe, so cannot be used with any Nonlinear* classes.
|
|
||||||
*/
|
|
||||||
class Symbol {
|
|
||||||
protected:
|
|
||||||
unsigned char c_;
|
|
||||||
size_t j_;
|
|
||||||
|
|
||||||
public:
|
|
||||||
/** Default constructor */
|
|
||||||
Symbol() :
|
|
||||||
c_(0), j_(0) {
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Copy constructor */
|
|
||||||
Symbol(const Symbol& key) :
|
|
||||||
c_(key.c_), j_(key.j_) {
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Constructor */
|
|
||||||
Symbol(unsigned char c, size_t j) :
|
|
||||||
c_(c), j_(j) {
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Casting constructor from TypedSymbol */
|
|
||||||
template<class T, char C>
|
|
||||||
Symbol(const TypedSymbol<T, C>& symbol) :
|
|
||||||
c_(C), j_(symbol.index()) {
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Casting constructor from TypedLabeledSymbol */
|
|
||||||
template<class T, char C, typename L>
|
|
||||||
Symbol(const TypedLabeledSymbol<T, C, L>& symbol) :
|
|
||||||
c_(C), j_(symbol.encode()) {
|
|
||||||
}
|
|
||||||
|
|
||||||
/** "Magic" key casting constructor from string */
|
|
||||||
#ifdef GTSAM_MAGIC_KEY
|
|
||||||
Symbol(const std::string& str) {
|
|
||||||
if(str.length() < 1)
|
|
||||||
throw std::invalid_argument("Cannot parse string key '" + str + "'");
|
|
||||||
else {
|
|
||||||
const char *c_str = str.c_str();
|
|
||||||
c_ = c_str[0];
|
|
||||||
if(str.length() > 1)
|
|
||||||
j_ = boost::lexical_cast<size_t>(c_str+1);
|
|
||||||
else
|
|
||||||
j_ = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
Symbol(const char *c_str) {
|
|
||||||
std::string str(c_str);
|
|
||||||
if(str.length() < 1)
|
|
||||||
throw std::invalid_argument("Cannot parse string key '" + str + "'");
|
|
||||||
else {
|
|
||||||
c_ = c_str[0];
|
|
||||||
if(str.length() > 1)
|
|
||||||
j_ = boost::lexical_cast<size_t>(c_str+1);
|
|
||||||
else
|
|
||||||
j_ = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// Testable Requirements
|
|
||||||
void print(const std::string& s = "") const {
|
|
||||||
std::cout << s << ": " << (std::string) (*this) << std::endl;
|
|
||||||
}
|
|
||||||
bool equals(const Symbol& expected, double tol = 0.0) const {
|
|
||||||
return (*this) == expected;
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Retrieve key character */
|
|
||||||
unsigned char chr() const {
|
|
||||||
return c_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Retrieve key index */
|
|
||||||
size_t index() const {
|
|
||||||
return j_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Create a string from the key */
|
|
||||||
operator std::string() const {
|
|
||||||
return str(boost::format("%c%d") % c_ % j_);
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Comparison for use in maps */
|
|
||||||
bool operator<(const Symbol& comp) const {
|
|
||||||
return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_);
|
|
||||||
}
|
|
||||||
bool operator==(const Symbol& comp) const {
|
|
||||||
return comp.c_ == c_ && comp.j_ == j_;
|
|
||||||
}
|
|
||||||
bool operator!=(const Symbol& comp) const {
|
|
||||||
return comp.c_ != c_ || comp.j_ != j_;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template<class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(c_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(j_);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
// Conversion utilities
|
|
||||||
|
|
||||||
template<class KEY> Symbol key2symbol(KEY key) {
|
|
||||||
return Symbol(key);
|
|
||||||
}
|
|
||||||
|
|
||||||
template<class KEY> std::list<Symbol> keys2symbols(std::list<KEY> keys) {
|
|
||||||
std::list<Symbol> symbols;
|
|
||||||
std::transform(keys.begin(), keys.end(), std::back_inserter(symbols),
|
|
||||||
key2symbol<KEY> );
|
|
||||||
return symbols;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* TypedLabeledSymbol is a variation of the TypedSymbol that allows
|
|
||||||
* for a runtime label to be placed on the label, so as to express
|
|
||||||
* "Pose 5 for robot 3"
|
|
||||||
* Labels should be kept to base datatypes (int, char, etc) to
|
|
||||||
* minimize cost of comparisons
|
|
||||||
*
|
|
||||||
* The labels will be compared first when comparing Keys, followed by the
|
|
||||||
* index
|
|
||||||
*/
|
|
||||||
template<class T, char C, typename L>
|
|
||||||
class TypedLabeledSymbol: public TypedSymbol<T, C> {
|
|
||||||
|
|
||||||
protected:
|
|
||||||
// Label
|
|
||||||
L label_;
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
typedef TypedSymbol<T, C> Base;
|
|
||||||
|
|
||||||
// Constructors:
|
|
||||||
|
|
||||||
TypedLabeledSymbol() {
|
|
||||||
}
|
|
||||||
TypedLabeledSymbol(size_t j, L label) :
|
|
||||||
Base(j), label_(label) {
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Constructor that decodes encoded labels */
|
|
||||||
TypedLabeledSymbol(const Symbol& sym) :
|
|
||||||
TypedSymbol<T, C> (0) {
|
|
||||||
size_t shift = (sizeof(size_t) - sizeof(short)) * 8;
|
|
||||||
this->j_ = (sym.index() << shift) >> shift; // truncate upper bits
|
|
||||||
label_ = (L) (sym.index() >> shift); // remove lower bits
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Constructor to upgrade an existing typed label with a label */
|
|
||||||
TypedLabeledSymbol(const Base& key, L label) :
|
|
||||||
Base(key.index()), label_(label) {
|
|
||||||
}
|
|
||||||
|
|
||||||
// Get stuff:
|
|
||||||
|
|
||||||
L label() const {
|
|
||||||
return label_;
|
|
||||||
}
|
|
||||||
const char* c_str() const {
|
|
||||||
return ((std::string)(*this)).c_str();
|
|
||||||
}
|
|
||||||
operator std::string() const {
|
|
||||||
std::string label_s = (boost::format("%1%") % label_).str();
|
|
||||||
return (boost::format("%c%s_%d") % C % label_s % this->j_).str();
|
|
||||||
}
|
|
||||||
std::string latex() const {
|
|
||||||
std::string label_s = (boost::format("%1%") % label_).str();
|
|
||||||
return (boost::format("%c%s_{%d}") % C % label_s % this->j_).str();
|
|
||||||
}
|
|
||||||
Symbol symbol() const {
|
|
||||||
return Symbol(*this);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Needed for conversion to LabeledSymbol
|
|
||||||
size_t convertLabel() const {
|
|
||||||
return label_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Encoding two numbers into a single size_t for conversion to Symbol
|
|
||||||
* Stores the label in the upper bytes of the index
|
|
||||||
*/
|
|
||||||
size_t encode() const {
|
|
||||||
short label = (short) label_; //bound size of label to 2 bytes
|
|
||||||
size_t shift = (sizeof(size_t) - sizeof(short)) * 8;
|
|
||||||
size_t modifier = ((size_t) label) << shift;
|
|
||||||
return this->j_ + modifier;
|
|
||||||
}
|
|
||||||
|
|
||||||
// logic:
|
|
||||||
|
|
||||||
bool operator<(const TypedLabeledSymbol& compare) const {
|
|
||||||
if (label_ == compare.label_) // sort by label first
|
|
||||||
return this->j_ < compare.j_;
|
|
||||||
else
|
|
||||||
return label_ < compare.label_;
|
|
||||||
}
|
|
||||||
bool operator==(const TypedLabeledSymbol& compare) const {
|
|
||||||
return this->j_ == compare.j_ && label_ == compare.label_;
|
|
||||||
}
|
|
||||||
int compare(const TypedLabeledSymbol& compare) const {
|
|
||||||
if (label_ == compare.label_) // sort by label first
|
|
||||||
return this->j_ - compare.j_;
|
|
||||||
else
|
|
||||||
return label_ - compare.label_;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Testable Requirements
|
|
||||||
void print(const std::string& s = "") const {
|
|
||||||
std::cout << s << ": " << (std::string) (*this) << std::endl;
|
|
||||||
}
|
|
||||||
bool equals(const TypedLabeledSymbol& expected, double tol = 0.0) const {
|
|
||||||
return (*this) == expected;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template<class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
||||||
typedef TypedSymbol<T, C> Base;
|
|
||||||
ar & boost::serialization::make_nvp("TypedLabeledSymbol",
|
|
||||||
boost::serialization::base_object<Base>(*this));
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(label_);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
template<class T, char C>
|
|
||||||
Symbol TypedSymbol<T,C>::symbol() const {
|
|
||||||
return Symbol(*this);
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace gtsam
|
|
||||||
|
|
|
@ -21,7 +21,7 @@ sources += Values.cpp
|
||||||
check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering
|
check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering
|
||||||
|
|
||||||
# Nonlinear nonlinear
|
# Nonlinear nonlinear
|
||||||
headers += Key.h
|
headers += Symbol.h
|
||||||
headers += NonlinearFactorGraph.h
|
headers += NonlinearFactorGraph.h
|
||||||
headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h
|
headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h
|
||||||
headers += NonlinearFactor.h
|
headers += NonlinearFactor.h
|
||||||
|
|
|
@ -19,7 +19,7 @@
|
||||||
|
|
||||||
#include <map>
|
#include <map>
|
||||||
#include <set>
|
#include <set>
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
#include <gtsam/inference/inference.h>
|
#include <gtsam/inference/inference.h>
|
||||||
|
|
||||||
#include <boost/foreach.hpp>
|
#include <boost/foreach.hpp>
|
||||||
|
|
|
@ -0,0 +1,136 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Symbol.h
|
||||||
|
* @date Jan 12, 2010
|
||||||
|
* @author: Frank Dellaert
|
||||||
|
* @author: Richard Roberts
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <list>
|
||||||
|
#include <iostream>
|
||||||
|
#include <boost/mpl/char.hpp>
|
||||||
|
#include <boost/format.hpp>
|
||||||
|
#include <boost/serialization/nvp.hpp>
|
||||||
|
#ifdef GTSAM_MAGIC_KEY
|
||||||
|
#include <boost/lexical_cast.hpp>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define ALPHA '\224'
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Character and index key used in VectorValues, GaussianFactorGraph,
|
||||||
|
* GaussianFactor, etc. These keys are generated at runtime from TypedSymbol
|
||||||
|
* keys when linearizing a nonlinear factor graph. This key is not type
|
||||||
|
* safe, so cannot be used with any Nonlinear* classes.
|
||||||
|
*/
|
||||||
|
class Symbol {
|
||||||
|
protected:
|
||||||
|
unsigned char c_;
|
||||||
|
size_t j_;
|
||||||
|
|
||||||
|
public:
|
||||||
|
/** Default constructor */
|
||||||
|
Symbol() :
|
||||||
|
c_(0), j_(0) {
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Copy constructor */
|
||||||
|
Symbol(const Symbol& key) :
|
||||||
|
c_(key.c_), j_(key.j_) {
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Constructor */
|
||||||
|
Symbol(unsigned char c, size_t j) :
|
||||||
|
c_(c), j_(j) {
|
||||||
|
}
|
||||||
|
|
||||||
|
/** "Magic" key casting constructor from string */
|
||||||
|
#ifdef GTSAM_MAGIC_KEY
|
||||||
|
Symbol(const std::string& str) {
|
||||||
|
if(str.length() < 1)
|
||||||
|
throw std::invalid_argument("Cannot parse string key '" + str + "'");
|
||||||
|
else {
|
||||||
|
const char *c_str = str.c_str();
|
||||||
|
c_ = c_str[0];
|
||||||
|
if(str.length() > 1)
|
||||||
|
j_ = boost::lexical_cast<size_t>(c_str+1);
|
||||||
|
else
|
||||||
|
j_ = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Symbol(const char *c_str) {
|
||||||
|
std::string str(c_str);
|
||||||
|
if(str.length() < 1)
|
||||||
|
throw std::invalid_argument("Cannot parse string key '" + str + "'");
|
||||||
|
else {
|
||||||
|
c_ = c_str[0];
|
||||||
|
if(str.length() > 1)
|
||||||
|
j_ = boost::lexical_cast<size_t>(c_str+1);
|
||||||
|
else
|
||||||
|
j_ = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Testable Requirements
|
||||||
|
void print(const std::string& s = "") const {
|
||||||
|
std::cout << s << ": " << (std::string) (*this) << std::endl;
|
||||||
|
}
|
||||||
|
bool equals(const Symbol& expected, double tol = 0.0) const {
|
||||||
|
return (*this) == expected;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Retrieve key character */
|
||||||
|
unsigned char chr() const {
|
||||||
|
return c_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Retrieve key index */
|
||||||
|
size_t index() const {
|
||||||
|
return j_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Create a string from the key */
|
||||||
|
operator std::string() const {
|
||||||
|
return str(boost::format("%c%d") % c_ % j_);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Comparison for use in maps */
|
||||||
|
bool operator<(const Symbol& comp) const {
|
||||||
|
return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_);
|
||||||
|
}
|
||||||
|
bool operator==(const Symbol& comp) const {
|
||||||
|
return comp.c_ == c_ && comp.j_ == j_;
|
||||||
|
}
|
||||||
|
bool operator!=(const Symbol& comp) const {
|
||||||
|
return comp.c_ != c_ || comp.j_ != j_;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
/** Serialization function */
|
||||||
|
friend class boost::serialization::access;
|
||||||
|
template<class ARCHIVE>
|
||||||
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(c_);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(j_);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace gtsam
|
||||||
|
|
|
@ -37,7 +37,7 @@
|
||||||
#include <gtsam/base/Value.h>
|
#include <gtsam/base/Value.h>
|
||||||
#include <gtsam/base/FastMap.h>
|
#include <gtsam/base/FastMap.h>
|
||||||
#include <gtsam/linear/VectorValues.h>
|
#include <gtsam/linear/VectorValues.h>
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
#include <gtsam/nonlinear/Ordering.h>
|
#include <gtsam/nonlinear/Ordering.h>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
|
@ -19,106 +19,11 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
||||||
class Pose3;
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
TEST ( TypedSymbol, basic_operations ) {
|
|
||||||
typedef TypedSymbol<Pose3, 'x'> Key;
|
|
||||||
|
|
||||||
Key key1(0),
|
|
||||||
key2(0),
|
|
||||||
key3(1),
|
|
||||||
key4(2);
|
|
||||||
|
|
||||||
CHECK(key1.index()==0);
|
|
||||||
CHECK(key1 == key2);
|
|
||||||
CHECK(assert_equal(key1, key2));
|
|
||||||
CHECK(!(key1 == key3));
|
|
||||||
CHECK(key1 < key3);
|
|
||||||
CHECK(key3 < key4);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
TEST ( TypedLabledSymbol, basic_operations ) {
|
|
||||||
typedef TypedSymbol<Pose3, 'x'> SimpleKey;
|
|
||||||
typedef TypedLabeledSymbol<Pose3, 'x', int> RobotKey;
|
|
||||||
|
|
||||||
SimpleKey key7(1);
|
|
||||||
RobotKey key1(0, 1),
|
|
||||||
key2(0, 1),
|
|
||||||
key3(1, 1),
|
|
||||||
key4(2, 1),
|
|
||||||
key5(0, 2),
|
|
||||||
key6(1, 2),
|
|
||||||
key8(1, 3),
|
|
||||||
key9(key7, 3);
|
|
||||||
|
|
||||||
|
|
||||||
CHECK(key1.label()==1);
|
|
||||||
CHECK(key1.index()==0);
|
|
||||||
CHECK(key1 == key2);
|
|
||||||
CHECK(assert_equal(key1, key2));
|
|
||||||
CHECK(!(key1 == key3));
|
|
||||||
CHECK(key1 < key3);
|
|
||||||
CHECK(key3 < key4);
|
|
||||||
CHECK(!(key1 == key5));
|
|
||||||
CHECK(key1 < key5);
|
|
||||||
CHECK(key5 < key6);
|
|
||||||
CHECK(assert_equal(key9, key8));
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
TEST ( TypedLabledSymbol, encoding ) {
|
|
||||||
typedef TypedLabeledSymbol<Pose3, 'x', char> RobotKey;
|
|
||||||
|
|
||||||
RobotKey key1(37, 'A');
|
|
||||||
|
|
||||||
// Note: calculations done in test due to possible differences between machines
|
|
||||||
// take the upper two bytes for the label
|
|
||||||
short label = key1.label();
|
|
||||||
|
|
||||||
// find the shift necessary
|
|
||||||
size_t shift = (sizeof(size_t)-sizeof(short)) * 8;
|
|
||||||
size_t modifier = label;
|
|
||||||
modifier = modifier << shift;
|
|
||||||
size_t index = key1.index() + modifier;
|
|
||||||
|
|
||||||
// check index encoding
|
|
||||||
Symbol act1(key1), exp('x', index);
|
|
||||||
CHECK(assert_equal(exp, act1));
|
|
||||||
|
|
||||||
// check casting
|
|
||||||
Symbol act2 = (Symbol) key1;
|
|
||||||
CHECK(assert_equal(exp, act2));
|
|
||||||
|
|
||||||
// decode
|
|
||||||
CHECK(assert_equal(key1, RobotKey(act1)));
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
TEST ( TypedLabledSymbol, template_reconstruction ) {
|
|
||||||
typedef TypedSymbol<Pose3, 'x'> Key;
|
|
||||||
typedef TypedLabeledSymbol<Key::Value, Key::Chr::value, char> NewKey;
|
|
||||||
NewKey k(1, 'A');
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
TEST ( Key, keys2symbols )
|
|
||||||
{
|
|
||||||
typedef TypedSymbol<int, 'x'> Key;
|
|
||||||
list<Symbol> expected;
|
|
||||||
expected += Key(1), Key(2), Key(3);
|
|
||||||
|
|
||||||
list<TypedSymbol<int, 'x'> > typeds;
|
|
||||||
typeds += 1, 2, 3;
|
|
||||||
CHECK(expected == keys2symbols(typeds));
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -20,6 +20,8 @@
|
||||||
#include <boost/assign/std/list.hpp> // for operator +=
|
#include <boost/assign/std/list.hpp> // for operator +=
|
||||||
using namespace boost::assign;
|
using namespace boost::assign;
|
||||||
|
|
||||||
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/LieVector.h>
|
#include <gtsam/base/LieVector.h>
|
||||||
#include <gtsam/geometry/Pose2.h>
|
#include <gtsam/geometry/Pose2.h>
|
||||||
|
@ -29,9 +31,7 @@ using namespace gtsam;
|
||||||
using namespace std;
|
using namespace std;
|
||||||
static double inf = std::numeric_limits<double>::infinity();
|
static double inf = std::numeric_limits<double>::infinity();
|
||||||
|
|
||||||
typedef TypedSymbol<LieVector, 'v'> VecKey;
|
Symbol key1("v1"), key2("v2"), key3("v3"), key4("v4");
|
||||||
|
|
||||||
VecKey key1(1), key2(2), key3(3), key4(4);
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( Values, equals1 )
|
TEST( Values, equals1 )
|
||||||
|
@ -114,11 +114,11 @@ TEST( Values, update_element )
|
||||||
|
|
||||||
cfg.insert(key1, v1);
|
cfg.insert(key1, v1);
|
||||||
CHECK(cfg.size() == 1);
|
CHECK(cfg.size() == 1);
|
||||||
CHECK(assert_equal(v1, cfg.at(key1)));
|
CHECK(assert_equal(v1, cfg.at<LieVector>(key1)));
|
||||||
|
|
||||||
cfg.update(key1, v2);
|
cfg.update(key1, v2);
|
||||||
CHECK(cfg.size() == 1);
|
CHECK(cfg.size() == 1);
|
||||||
CHECK(assert_equal(v2, cfg.at(key1)));
|
CHECK(assert_equal(v2, cfg.at<LieVector>(key1)));
|
||||||
}
|
}
|
||||||
|
|
||||||
///* ************************************************************************* */
|
///* ************************************************************************* */
|
||||||
|
@ -200,10 +200,9 @@ TEST(Values, expmap_d)
|
||||||
CHECK(equal(config0, config0));
|
CHECK(equal(config0, config0));
|
||||||
CHECK(config0.equals(config0));
|
CHECK(config0.equals(config0));
|
||||||
|
|
||||||
typedef TypedSymbol<Pose2, 'p'> PoseKey;
|
|
||||||
Values poseconfig;
|
Values poseconfig;
|
||||||
poseconfig.insert(PoseKey(1), Pose2(1,2,3));
|
poseconfig.insert("p1", Pose2(1,2,3));
|
||||||
poseconfig.insert(PoseKey(2), Pose2(0.3, 0.4, 0.5));
|
poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
|
||||||
|
|
||||||
CHECK(equal(config0, config0));
|
CHECK(equal(config0, config0));
|
||||||
CHECK(config0.equals(config0));
|
CHECK(config0.equals(config0));
|
||||||
|
@ -212,16 +211,15 @@ TEST(Values, expmap_d)
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST(Values, extract_keys)
|
TEST(Values, extract_keys)
|
||||||
{
|
{
|
||||||
typedef TypedSymbol<Pose2, 'x'> PoseKey;
|
|
||||||
Values config;
|
Values config;
|
||||||
|
|
||||||
config.insert(PoseKey(1), Pose2());
|
config.insert("x1", Pose2());
|
||||||
config.insert(PoseKey(2), Pose2());
|
config.insert("x2", Pose2());
|
||||||
config.insert(PoseKey(4), Pose2());
|
config.insert("x4", Pose2());
|
||||||
config.insert(PoseKey(5), Pose2());
|
config.insert("x5", Pose2());
|
||||||
|
|
||||||
FastList<Symbol> expected, actual;
|
FastList<Symbol> expected, actual;
|
||||||
expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
|
expected += "x1", "x2", "x4", "x5";
|
||||||
actual = config.keys();
|
actual = config.keys();
|
||||||
|
|
||||||
CHECK(actual.size() == expected.size());
|
CHECK(actual.size() == expected.size());
|
||||||
|
@ -238,7 +236,7 @@ TEST(Values, exists_)
|
||||||
config0.insert(key1, LieVector(Vector_(1, 1.)));
|
config0.insert(key1, LieVector(Vector_(1, 1.)));
|
||||||
config0.insert(key2, LieVector(Vector_(1, 2.)));
|
config0.insert(key2, LieVector(Vector_(1, 2.)));
|
||||||
|
|
||||||
boost::optional<const LieVector&> v = config0.exists(key1);
|
boost::optional<const LieVector&> v = config0.exists<LieVector>(key1);
|
||||||
CHECK(assert_equal(Vector_(1, 1.),*v));
|
CHECK(assert_equal(Vector_(1, 1.),*v));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -20,7 +20,7 @@
|
||||||
#include <gtsam/geometry/Pose2.h>
|
#include <gtsam/geometry/Pose2.h>
|
||||||
#include <gtsam/slam/PriorFactor.h>
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
#include <gtsam/slam/BetweenFactor.h>
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
#include <gtsam/nonlinear/Values.h>
|
#include <gtsam/nonlinear/Values.h>
|
||||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
|
|
|
@ -20,7 +20,7 @@
|
||||||
#include <gtsam/geometry/Pose3.h>
|
#include <gtsam/geometry/Pose3.h>
|
||||||
#include <gtsam/slam/PriorFactor.h>
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
#include <gtsam/slam/BetweenFactor.h>
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||||
|
|
|
@ -23,7 +23,7 @@
|
||||||
#include <gtsam/geometry/Point3.h>
|
#include <gtsam/geometry/Point3.h>
|
||||||
#include <gtsam/linear/VectorValues.h>
|
#include <gtsam/linear/VectorValues.h>
|
||||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
|
|
||||||
// \namespace
|
// \namespace
|
||||||
|
|
||||||
|
|
|
@ -24,7 +24,7 @@
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
|
|
|
@ -12,7 +12,7 @@
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
|
|
|
@ -12,7 +12,7 @@
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
|
|
|
@ -14,7 +14,7 @@
|
||||||
* @author Frank Dellaert, Viorela Ila
|
* @author Frank Dellaert, Viorela Ila
|
||||||
**/
|
**/
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/slam/pose2SLAM.h>
|
#include <gtsam/slam/pose2SLAM.h>
|
||||||
|
|
|
@ -28,7 +28,7 @@ using namespace boost::assign;
|
||||||
// TODO: DANGEROUS, create shared pointers
|
// TODO: DANGEROUS, create shared pointers
|
||||||
#define GTSAM_MAGIC_GAUSSIAN 6
|
#define GTSAM_MAGIC_GAUSSIAN 6
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/slam/pose3SLAM.h>
|
#include <gtsam/slam/pose3SLAM.h>
|
||||||
|
|
|
@ -18,7 +18,7 @@
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/slam/visualSLAM.h>
|
#include <gtsam/slam/visualSLAM.h>
|
||||||
|
|
|
@ -22,7 +22,7 @@
|
||||||
#include <boost/shared_ptr.hpp>
|
#include <boost/shared_ptr.hpp>
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
|
|
|
@ -23,7 +23,7 @@
|
||||||
#include <gtsam/slam/ProjectionFactor.h>
|
#include <gtsam/slam/ProjectionFactor.h>
|
||||||
#include <gtsam/slam/StereoFactor.h>
|
#include <gtsam/slam/StereoFactor.h>
|
||||||
#include <gtsam/slam/RangeFactor.h>
|
#include <gtsam/slam/RangeFactor.h>
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||||
|
|
|
@ -25,7 +25,7 @@
|
||||||
#include <boost/assign/std/list.hpp> // for operator +=
|
#include <boost/assign/std/list.hpp> // for operator +=
|
||||||
using namespace boost::assign;
|
using namespace boost::assign;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
|
|
|
@ -26,7 +26,7 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
|
|
|
@ -28,7 +28,7 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
|
|
|
@ -21,7 +21,7 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/geometry/Rot2.h>
|
#include <gtsam/geometry/Rot2.h>
|
||||||
|
|
|
@ -25,7 +25,7 @@
|
||||||
#include <boost/assign/std/vector.hpp>
|
#include <boost/assign/std/vector.hpp>
|
||||||
using namespace boost::assign;
|
using namespace boost::assign;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/debug.h>
|
#include <gtsam/base/debug.h>
|
||||||
|
|
|
@ -23,6 +23,8 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
// TODO: DANGEROUS, create shared pointers
|
// TODO: DANGEROUS, create shared pointers
|
||||||
#define GTSAM_MAGIC_GAUSSIAN 3
|
#define GTSAM_MAGIC_GAUSSIAN 3
|
||||||
|
|
||||||
|
@ -37,22 +39,21 @@ using namespace gtsam;
|
||||||
// -> x3 -> x4
|
// -> x3 -> x4
|
||||||
// -> x5
|
// -> x5
|
||||||
TEST ( Ordering, predecessorMap2Keys ) {
|
TEST ( Ordering, predecessorMap2Keys ) {
|
||||||
typedef TypedSymbol<Pose2,'x'> PoseKey;
|
PredecessorMap<Symbol> p_map;
|
||||||
PredecessorMap<PoseKey> p_map;
|
p_map.insert("x1","x1");
|
||||||
p_map.insert(1,1);
|
p_map.insert("x2","x1");
|
||||||
p_map.insert(2,1);
|
p_map.insert("x3","x1");
|
||||||
p_map.insert(3,1);
|
p_map.insert("x4","x3");
|
||||||
p_map.insert(4,3);
|
p_map.insert("x5","x1");
|
||||||
p_map.insert(5,1);
|
|
||||||
|
|
||||||
list<PoseKey> expected;
|
list<Symbol> expected;
|
||||||
expected += 4,5,3,2,1;//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1);
|
expected += "x4","x5","x3","x2","x1";//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1);
|
||||||
|
|
||||||
list<PoseKey> actual = predecessorMap2Keys<PoseKey>(p_map);
|
list<Symbol> actual = predecessorMap2Keys<Symbol>(p_map);
|
||||||
LONGS_EQUAL(expected.size(), actual.size());
|
LONGS_EQUAL(expected.size(), actual.size());
|
||||||
|
|
||||||
list<PoseKey>::const_iterator it1 = expected.begin();
|
list<Symbol>::const_iterator it1 = expected.begin();
|
||||||
list<PoseKey>::const_iterator it2 = actual.begin();
|
list<Symbol>::const_iterator it2 = actual.begin();
|
||||||
for(; it1!=expected.end(); it1++, it2++)
|
for(; it1!=expected.end(); it1++, it2++)
|
||||||
CHECK(*it1 == *it2)
|
CHECK(*it1 == *it2)
|
||||||
}
|
}
|
||||||
|
|
|
@ -17,7 +17,7 @@
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||||
|
|
|
@ -25,7 +25,7 @@
|
||||||
// TODO: DANGEROUS, create shared pointers
|
// TODO: DANGEROUS, create shared pointers
|
||||||
#define GTSAM_MAGIC_GAUSSIAN 2
|
#define GTSAM_MAGIC_GAUSSIAN 2
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
|
|
|
@ -27,7 +27,7 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
|
|
|
@ -26,7 +26,7 @@ using namespace boost::assign;
|
||||||
#include <boost/shared_ptr.hpp>
|
#include <boost/shared_ptr.hpp>
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
|
|
|
@ -145,7 +145,7 @@ bool equalsDereferencedXML(const T& input = T()) {
|
||||||
// Actual Tests
|
// Actual Tests
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/geometry/Cal3DS2.h>
|
#include <gtsam/geometry/Cal3DS2.h>
|
||||||
|
@ -255,16 +255,12 @@ typedef PinholeCamera<Cal3_S2> PinholeCal3S2;
|
||||||
typedef PinholeCamera<Cal3DS2> PinholeCal3DS2;
|
typedef PinholeCamera<Cal3DS2> PinholeCal3DS2;
|
||||||
typedef PinholeCamera<Cal3Bundler> PinholeCal3Bundler;
|
typedef PinholeCamera<Cal3Bundler> PinholeCal3Bundler;
|
||||||
|
|
||||||
typedef TypedSymbol<Cal3_S2,'a'> PinholeCal3S2Key;
|
|
||||||
typedef TypedSymbol<Cal3DS2,'s'> PinholeCal3DS2Key;
|
|
||||||
typedef TypedSymbol<Cal3Bundler,'d'> PinholeCal3BundlerKey;
|
|
||||||
|
|
||||||
TEST (Serialization, TemplatedValues) {
|
TEST (Serialization, TemplatedValues) {
|
||||||
Values values;
|
Values values;
|
||||||
values.insert(PinholeCal3S2Key(0), PinholeCal3S2(pose3, cal1));
|
values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1));
|
||||||
values.insert(PinholeCal3DS2Key(5), PinholeCal3DS2(pose3, cal2));
|
values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2));
|
||||||
values.insert(PinholeCal3BundlerKey(47), PinholeCal3Bundler(pose3, cal3));
|
values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3));
|
||||||
values.insert(PinholeCal3S2Key(5), PinholeCal3S2(pose3, cal1));
|
values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1));
|
||||||
EXPECT(equalsObj(values));
|
EXPECT(equalsObj(values));
|
||||||
EXPECT(equalsXML(values));
|
EXPECT(equalsXML(values));
|
||||||
}
|
}
|
||||||
|
|
|
@ -21,7 +21,7 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
|
|
|
@ -20,7 +20,7 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/slam/smallExample.h>
|
||||||
|
|
|
@ -20,7 +20,7 @@
|
||||||
|
|
||||||
#include <gtsam/base/TestableAssertions.h>
|
#include <gtsam/base/TestableAssertions.h>
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/geometry/Pose2.h>
|
#include <gtsam/geometry/Pose2.h>
|
||||||
|
@ -29,7 +29,7 @@
|
||||||
#include <gtsam/geometry/Point3.h>
|
#include <gtsam/geometry/Point3.h>
|
||||||
|
|
||||||
#include <gtsam/base/Vector.h>
|
#include <gtsam/base/Vector.h>
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
#include <gtsam/nonlinear/TupleValues.h>
|
#include <gtsam/nonlinear/TupleValues.h>
|
||||||
|
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
|
@ -15,7 +15,7 @@
|
||||||
* @author Frank Dellaert
|
* @author Frank Dellaert
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
|
|
Loading…
Reference in New Issue