12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
gtsam/inference/Key.h

276 lines
7.9 KiB
C
Raw Normal View History

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
Moved tree stuff to graph, removed ordering-inl.h
2010-01-19 03:11:22 +08:00
#include <list>
Added tests for Keys, Added TypedLabeledSymbol class to allow for runtime symbols on keys
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>
#include <boost/serialization/serialization.hpp>
Removed unnecessary headers, now only included when GTSAM_MAGIC_KEY is defined
2010-01-19 03:36:02 +08:00
#ifdef GTSAM_MAGIC_KEY
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
2010-01-18 03:34:57 +08:00
#include <boost/lexical_cast.hpp>
Removed unnecessary headers, now only included when GTSAM_MAGIC_KEY is defined
2010-01-19 03:36:02 +08:00
#endif
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
2010-01-18 03:34:57 +08:00
Added tests for Keys, Added TypedLabeledSymbol class to allow for runtime symbols on keys
2010-01-19 00:18:02 +08:00
#include "Testable.h"
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
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 {
/**
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
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>
Added tests for Keys, Added TypedLabeledSymbol class to allow for runtime symbols on keys
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
Added tests for Keys, Added TypedLabeledSymbol class to allow for runtime symbols on keys
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:
Partially implemented version of a recursive TupleConfig that is still very much under testing.
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:
Removed unnecessary headers, now only included when GTSAM_MAGIC_KEY is defined
2010-01-19 03:36:02 +08:00
TypedSymbol():j_(0) {}
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
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_;}
static chr() function for TypedSymbol, and another version of Symbol (don't use)
2010-02-09 11:59:45 +08:00
static char chr() { return C; }
c_str
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:
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
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_;}
add != to TypedSymbol
2010-04-01 01:58:42 +08:00
bool operator!= (const TypedSymbol& compare) const { return j_!=compare.j_;}
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
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
Added tests for Keys, Added TypedLabeledSymbol class to allow for runtime symbols on keys
2010-01-19 00:18:02 +08:00
// Testable Requirements
static chr() function for TypedSymbol, and another version of Symbol (don't use)
2010-02-09 11:59:45 +08:00
virtual void print(const std::string& s="") const {
Fixed insert to use IndexTable, a new class
2010-01-22 10:27:26 +08:00
std::cout << s << ": " << (std::string)(*this) << std::endl;
}
Added default tolerance value to equals in Key classes so that equals() works properly
2010-02-01 01:00:27 +08:00
bool equals(const TypedSymbol& expected, double tol=0.0) const { return (*this)==expected; }
Added tests for Keys, Added TypedLabeledSymbol class to allow for runtime symbols on keys
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_);
}
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
2010-01-18 03:34:57 +08:00
};
Added a constructor for TypedLabeledSymbol from Symbol that decodes the index to split out a label
2010-02-10 23:27:21 +08:00
/** forward declaration to avoid circular dependencies */
template<class T, char C, typename L>
class TypedLabeledSymbol;
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
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.
*/
Fixed insert to use IndexTable, a new class
2010-01-22 10:27:26 +08:00
class Symbol : Testable<Symbol> {
static chr() function for TypedSymbol, and another version of Symbol (don't use)
2010-02-09 11:59:45 +08:00
protected:
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
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
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
2010-01-18 03:34:57 +08:00
public:
/** Default constructor */
Removed unnecessary headers, now only included when GTSAM_MAGIC_KEY is defined
2010-01-19 03:36:02 +08:00
Symbol() : c_(0), j_(0) {}
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
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()) {}
static chr() function for TypedSymbol, and another version of Symbol (don't use)
2010-02-09 11:59:45 +08:00
/** Casting constructor from TypedLabeledSymbol */
template<class T, char C, typename L>
TypedLabeledSymbols now convert properly to Symbols, so they can be used to add a runtime label to a TypedKey to express "Pose 1 of robot A"
2010-02-10 22:39:58 +08:00
Symbol(const TypedLabeledSymbol<T,C,L>& symbol): c_(C), j_(symbol.encode()) {}
static chr() function for TypedSymbol, and another version of Symbol (don't use)
2010-02-09 11:59:45 +08:00
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
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
Fixed insert to use IndexTable, a new class
2010-01-22 10:27:26 +08:00
// Testable Requirements
static chr() function for TypedSymbol, and another version of Symbol (don't use)
2010-02-09 11:59:45 +08:00
void print(const std::string& s="") const {
Fixed insert to use IndexTable, a new class
2010-01-22 10:27:26 +08:00
std::cout << s << ": " << (std::string)(*this) << std::endl;
}
Added default tolerance value to equals in Key classes so that equals() works properly
2010-02-01 01:00:27 +08:00
bool equals(const Symbol& expected, double tol=0.0) const { return (*this)==expected; }
Fixed insert to use IndexTable, a new class
2010-01-22 10:27:26 +08:00
Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>
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
};
Moved tree stuff to graph, removed ordering-inl.h
2010-01-19 03:11:22 +08:00
// Conversion utilities
add linear system solver as a template class parameter in nonlinear optimizer.
2010-01-18 13:51:19 +08:00
template<class Key> Symbol key2symbol(Key key) {
return Symbol(key);
}
Moved tree stuff to graph, removed ordering-inl.h
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;
}
static chr() function for TypedSymbol, and another version of Symbol (don't use)
2010-02-09 11:59:45 +08:00
/**
Added a constructor for TypedLabeledSymbol from Symbol that decodes the index to split out a label
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
static chr() function for TypedSymbol, and another version of Symbol (don't use)
2010-02-09 11:59:45 +08:00
*
Added a constructor for TypedLabeledSymbol from Symbol that decodes the index to split out a label
2010-02-10 23:27:21 +08:00
* The labels will be compared first when comparing Keys, followed by the
* index
static chr() function for TypedSymbol, and another version of Symbol (don't use)
2010-02-09 11:59:45 +08:00
*/
Added a constructor for TypedLabeledSymbol from Symbol that decodes the index to split out a label
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:
// Constructors:
TypedLabeledSymbol() {}
TypedLabeledSymbol(size_t j, L label):TypedSymbol<T,C>(j), label_(label) {}
static chr() function for TypedSymbol, and another version of Symbol (don't use)
2010-02-09 11:59:45 +08:00
Added a constructor for TypedLabeledSymbol from Symbol that decodes the index to split out a label
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
}
// Get stuff:
L label() const { return label_;}
const char* c_str() const { return (std::string)(*this).c_str();}
Fixed bug in print function of TypedLabeledSymbol
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();
}
Added a constructor for TypedLabeledSymbol from Symbol that decodes the index to split out a label
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) {
ar & BOOST_SERIALIZATION_NVP(this->j_);
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