Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
/*
|
|
|
|
|
* Key.h
|
|
|
|
|
*
|
|
|
|
|
* Created on: Jan 12, 2010
|
|
|
|
|
* @author: Frank Dellaert
|
|
|
|
|
* @author: Richard Roberts
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
#pragma once
|
|
|
|
|
|
2010-01-19 03:11:22 +08:00
|
|
|
#include <list>
|
2010-01-19 00:18:02 +08:00
|
|
|
#include <iostream>
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
#include <boost/format.hpp>
|
2010-08-27 23:31:20 +08:00
|
|
|
//#include <boost/serialization/serialization.hpp>
|
2010-08-22 13:54:06 +08:00
|
|
|
#include <boost/serialization/nvp.hpp>
|
2010-01-19 03:36:02 +08:00
|
|
|
#ifdef GTSAM_MAGIC_KEY
|
2010-01-18 03:34:57 +08:00
|
|
|
#include <boost/lexical_cast.hpp>
|
2010-01-19 03:36:02 +08:00
|
|
|
#endif
|
2010-01-18 03:34:57 +08:00
|
|
|
|
2010-08-20 01:23:19 +08:00
|
|
|
#include <gtsam/base/Testable.h>
|
2010-01-19 00:18:02 +08:00
|
|
|
|
2010-01-18 03:34:57 +08:00
|
|
|
#define ALPHA '\224'
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
|
|
|
|
|
namespace gtsam {
|
|
|
|
|
|
|
|
|
|
/**
|
2010-01-18 03:34:57 +08:00
|
|
|
* TypedSymbol key class is templated on
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
* 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>
|
2010-01-19 00:18:02 +08:00
|
|
|
class TypedSymbol : Testable<TypedSymbol<T,C> > {
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
|
2010-01-19 00:18:02 +08:00
|
|
|
protected:
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
size_t j_;
|
|
|
|
|
|
|
|
|
|
public:
|
|
|
|
|
|
2010-02-03 21:47:13 +08:00
|
|
|
// typedefs
|
|
|
|
|
typedef T Value_t;
|
|
|
|
|
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
// Constructors:
|
|
|
|
|
|
2010-01-19 03:36:02 +08:00
|
|
|
TypedSymbol():j_(0) {}
|
2010-01-18 03:34:57 +08:00
|
|
|
TypedSymbol(size_t j):j_(j) {}
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
|
|
|
|
|
// Get stuff:
|
|
|
|
|
|
|
|
|
|
size_t index() const { return j_;}
|
2010-02-09 11:59:45 +08:00
|
|
|
static char chr() { return C; }
|
2010-01-14 23:31:42 +08:00
|
|
|
const char* c_str() const { return (std::string)(*this).c_str();}
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
operator std::string() const { return (boost::format("%c%d") % C % j_).str(); }
|
|
|
|
|
std::string latex() const { return (boost::format("%c_{%d}") % C % j_).str(); }
|
|
|
|
|
|
|
|
|
|
// logic:
|
|
|
|
|
|
2010-01-18 03:34:57 +08:00
|
|
|
bool operator< (const TypedSymbol& compare) const { return j_<compare.j_;}
|
|
|
|
|
bool operator== (const TypedSymbol& compare) const { return j_==compare.j_;}
|
2010-04-01 01:58:42 +08:00
|
|
|
bool operator!= (const TypedSymbol& compare) const { return j_!=compare.j_;}
|
2010-01-18 03:34:57 +08:00
|
|
|
int compare(const TypedSymbol& compare) const {return j_-compare.j_;}
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
|
2010-01-19 00:18:02 +08:00
|
|
|
// Testable Requirements
|
2010-02-09 11:59:45 +08:00
|
|
|
virtual void print(const std::string& s="") const {
|
2010-01-22 10:27:26 +08:00
|
|
|
std::cout << s << ": " << (std::string)(*this) << std::endl;
|
|
|
|
|
}
|
2010-02-01 01:00:27 +08:00
|
|
|
bool equals(const TypedSymbol& expected, double tol=0.0) const { return (*this)==expected; }
|
2010-01-19 00:18:02 +08:00
|
|
|
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
private:
|
|
|
|
|
|
|
|
|
|
/** Serialization function */
|
|
|
|
|
friend class boost::serialization::access;
|
|
|
|
|
template<class Archive>
|
|
|
|
|
void serialize(Archive & ar, const unsigned int version) {
|
|
|
|
|
ar & BOOST_SERIALIZATION_NVP(j_);
|
|
|
|
|
}
|
2010-01-18 03:34:57 +08:00
|
|
|
};
|
|
|
|
|
|
2010-02-10 23:27:21 +08:00
|
|
|
/** forward declaration to avoid circular dependencies */
|
|
|
|
|
template<class T, char C, typename L>
|
|
|
|
|
class TypedLabeledSymbol;
|
2010-01-18 03:34:57 +08:00
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* Character and index key used in VectorConfig, 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.
|
|
|
|
|
*/
|
2010-01-22 10:27:26 +08:00
|
|
|
class Symbol : Testable<Symbol> {
|
2010-02-09 11:59:45 +08:00
|
|
|
protected:
|
2010-01-18 03:34:57 +08:00
|
|
|
unsigned char c_;
|
|
|
|
|
size_t j_;
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
|
2010-01-18 03:34:57 +08:00
|
|
|
public:
|
|
|
|
|
/** Default constructor */
|
2010-01-19 03:36:02 +08:00
|
|
|
Symbol() : c_(0), j_(0) {}
|
2010-01-18 03:34:57 +08:00
|
|
|
|
|
|
|
|
/** 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()) {}
|
|
|
|
|
|
2010-02-09 11:59:45 +08:00
|
|
|
/** Casting constructor from TypedLabeledSymbol */
|
|
|
|
|
template<class T, char C, typename L>
|
2010-02-10 22:39:58 +08:00
|
|
|
Symbol(const TypedLabeledSymbol<T,C,L>& symbol): c_(C), j_(symbol.encode()) {}
|
2010-02-09 11:59:45 +08:00
|
|
|
|
2010-01-18 03:34:57 +08:00
|
|
|
/** "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
|
|
|
|
|
|
2010-01-22 10:27:26 +08:00
|
|
|
// Testable Requirements
|
2010-02-09 11:59:45 +08:00
|
|
|
void print(const std::string& s="") const {
|
2010-01-22 10:27:26 +08:00
|
|
|
std::cout << s << ": " << (std::string)(*this) << std::endl;
|
|
|
|
|
}
|
2010-02-01 01:00:27 +08:00
|
|
|
bool equals(const Symbol& expected, double tol=0.0) const { return (*this)==expected; }
|
2010-01-22 10:27:26 +08:00
|
|
|
|
2010-01-18 03:34:57 +08:00
|
|
|
/** 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_);
|
|
|
|
|
}
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
};
|
|
|
|
|
|
2010-01-19 03:11:22 +08:00
|
|
|
// Conversion utilities
|
|
|
|
|
|
2010-01-18 13:51:19 +08:00
|
|
|
template<class Key> Symbol key2symbol(Key key) {
|
|
|
|
|
return Symbol(key);
|
|
|
|
|
}
|
2010-01-19 03:11:22 +08:00
|
|
|
|
|
|
|
|
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;
|
|
|
|
|
}
|
|
|
|
|
|
2010-02-09 11:59:45 +08:00
|
|
|
/**
|
2010-02-10 23:27:21 +08:00
|
|
|
* 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
|
2010-02-09 11:59:45 +08:00
|
|
|
*
|
2010-02-10 23:27:21 +08:00
|
|
|
* The labels will be compared first when comparing Keys, followed by the
|
|
|
|
|
* index
|
2010-02-09 11:59:45 +08:00
|
|
|
*/
|
2010-02-10 23:27:21 +08:00
|
|
|
template <class T, char C, typename L>
|
|
|
|
|
class TypedLabeledSymbol : public TypedSymbol<T, C>, Testable<TypedLabeledSymbol<T,C,L> > {
|
|
|
|
|
|
|
|
|
|
protected:
|
|
|
|
|
// Label
|
|
|
|
|
L label_;
|
|
|
|
|
|
|
|
|
|
public:
|
|
|
|
|
|
2010-07-27 01:23:18 +08:00
|
|
|
typedef TypedSymbol<T, C> Base;
|
|
|
|
|
|
2010-02-10 23:27:21 +08:00
|
|
|
// Constructors:
|
|
|
|
|
|
|
|
|
|
TypedLabeledSymbol() {}
|
2010-07-27 01:23:18 +08:00
|
|
|
TypedLabeledSymbol(size_t j, L label) : Base(j), label_(label) {}
|
2010-02-09 11:59:45 +08:00
|
|
|
|
2010-02-10 23:27:21 +08:00
|
|
|
/** 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
|
|
|
|
|
}
|
|
|
|
|
|
2010-07-27 01:23:18 +08:00
|
|
|
/** Constructor to upgrade an existing typed label with a label */
|
|
|
|
|
TypedLabeledSymbol(const Base& key, L label) : Base(key.index()), label_(label) {}
|
|
|
|
|
|
2010-02-10 23:27:21 +08:00
|
|
|
// Get stuff:
|
|
|
|
|
|
|
|
|
|
L label() const { return label_;}
|
|
|
|
|
const char* c_str() const { return (std::string)(*this).c_str();}
|
2010-02-11 05:03:49 +08:00
|
|
|
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();
|
|
|
|
|
}
|
2010-02-10 23:27:21 +08:00
|
|
|
|
|
|
|
|
// 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) {
|
2010-08-27 23:31:20 +08:00
|
|
|
typedef TypedSymbol<T,C> Base;
|
|
|
|
|
ar & boost::serialization::make_nvp("TypedLabeledSymbol",
|
|
|
|
|
boost::serialization::base_object<Base>(*this));
|
2010-02-10 23:27:21 +08:00
|
|
|
ar & BOOST_SERIALIZATION_NVP(label_);
|
|
|
|
|
}
|
|
|
|
|
};
|
Large gtsam refactoring
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-14 06:25:03 +08:00
|
|
|
} // namespace gtsam
|
|
|
|
|
|
