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

converted all NoiseModelFactorX to inherit from NoiseModelFactorN

release/4.3a0
Gerry Chen 2021-12-03 03:00:47 -05:00
parent bee4eeefdd
commit ed07edbfe4
No known key found for this signature in database
GPG Key ID: E9845092D3A57286
1 changed files with 137 additions and 365 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

502
gtsam/nonlinear/NonlinearFactor.h
View File

@ -316,16 +316,16 @@ public:
* more general than just vectors, e.g., Rot3 or Pose3, * more general than just vectors, e.g., Rot3 or Pose3,
* which are objects in non-linear manifolds (Lie groups). * which are objects in non-linear manifolds (Lie groups).
*/ */
template<class... VALUES> template <class... VALUES>
class NoiseModelFactorN: public NoiseModelFactor { class NoiseModelFactorN : public NoiseModelFactor {
public: public:
/** The type of the N'th template param can be obtained with VALUE<N> */ /** The type of the N'th template param can be obtained with VALUE<N> */
template <int N> template <int N>
using VALUE = typename std::tuple_element<N, std::tuple<VALUES...>>::type; using VALUE = typename std::tuple_element<N, std::tuple<VALUES...>>::type;
protected: protected:
typedef NoiseModelFactor Base; using Base = NoiseModelFactor;
typedef NoiseModelFactorN<VALUES...> This; using This = NoiseModelFactorN<VALUES...>;
/* "Dummy templated" alias is used to expand fixed-type parameter packs with /* "Dummy templated" alias is used to expand fixed-type parameter packs with
* same length as VALUES. This ignores the template parameter. */ * same length as VALUES. This ignores the template parameter. */
@ -366,9 +366,11 @@ class NoiseModelFactorN: public NoiseModelFactor {
~NoiseModelFactorN() override {} ~NoiseModelFactorN() override {}
// /** Method to retrieve keys */ /** Method to retrieve keys */
// template <int N> template <int N>
// inline Key key() const { return keys_[N]; } inline Key key() const {
return keys_[N];
}
/** Calls the n-key specific version of evaluateError, which is pure virtual /** Calls the n-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
@ -384,9 +386,8 @@ class NoiseModelFactorN: public NoiseModelFactor {
* compute both the function evaluation and its derivative(s) in the requested * compute both the function evaluation and its derivative(s) in the requested
* variables. * variables.
*/ */
virtual Vector evaluateError( virtual Vector evaluateError(const VALUES&... x,
const VALUES& ... x, optional_matrix_type<VALUES>... H) const = 0;
optional_matrix_type<VALUES> ... H) const = 0;
/** No-jacobians requested function overload (since parameter packs can't have /** No-jacobians requested function overload (since parameter packs can't have
* default args). This specializes the version below to avoid recursive calls * default args). This specializes the version below to avoid recursive calls
@ -408,7 +409,6 @@ class NoiseModelFactorN: public NoiseModelFactor {
} }
private: private:
/** Pack expansion with index_sequence template pattern */ /** Pack expansion with index_sequence template pattern */
template <std::size_t... Inds> template <std::size_t... Inds>
Vector unwhitenedError( Vector unwhitenedError(
@ -428,250 +428,110 @@ class NoiseModelFactorN: public NoiseModelFactor {
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar& boost::serialization::make_nvp(
boost::serialization::base_object<Base>(*this)); "NoiseModelFactor", boost::serialization::base_object<Base>(*this));
} }
}; // \class NoiseModelFactorN }; // \class NoiseModelFactorN
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 1 /** A convenient base class for creating your own NoiseModelFactor with 1
* variable. To derive from this class, implement evaluateError(). */ * variable. To derive from this class, implement evaluateError(). */
template<class VALUE> template <class VALUE>
class NoiseModelFactor1: public NoiseModelFactor { class NoiseModelFactor1 : public NoiseModelFactorN<VALUE> {
public:
// aliases for value types pulled from keys
using X = VALUE;
public: protected:
using Base = NoiseModelFactor; // grandparent, for backwards compatibility
// typedefs for value types pulled from keys using This = NoiseModelFactor1<VALUE>;
typedef VALUE X;
protected:
typedef NoiseModelFactor Base;
typedef NoiseModelFactor1<VALUE> This;
public:
/// @name Constructors
/// @{
/** Default constructor for I/O only */
NoiseModelFactor1() {}
public:
// inherit NoiseModelFactorN's constructors
using NoiseModelFactorN<VALUE>::NoiseModelFactorN;
~NoiseModelFactor1() override {} ~NoiseModelFactor1() override {}
inline Key key() const { return keys_[0]; } inline Key key() const { return this->keys_[0]; }
/** private:
* Constructor
* @param noiseModel shared pointer to noise model
* @param key1 by which to look up X value in Values
*/
NoiseModelFactor1(const SharedNoiseModel &noiseModel, Key key1)
: Base(noiseModel, cref_list_of<1>(key1)) {}
/// @}
/// @name NoiseModelFactor methods
/// @{
/**
* Calls the 1-key specific version of evaluateError below, which is pure
* virtual so must be implemented in the derived class.
*/
Vector unwhitenedError(
const Values &x,
boost::optional<std::vector<Matrix> &> H = boost::none) const override {
if (this->active(x)) {
const X &x1 = x.at<X>(keys_[0]);
if (H) {
return evaluateError(x1, (*H)[0]);
} else {
return evaluateError(x1);
}
} else {
return Vector::Zero(this->dim());
}
}
/// @}
/// @name Virtual methods
/// @{
/**
* Override this method to finish implementing a unary factor.
* If the optional Matrix reference argument is specified, it should compute
* both the function evaluation and its derivative in X.
*/
virtual Vector
evaluateError(const X &x,
boost::optional<Matrix &> H = boost::none) const = 0;
/// @}
private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar& boost::serialization::make_nvp(
boost::serialization::base_object<Base>(*this)); "NoiseModelFactor", boost::serialization::base_object<Base>(*this));
} }
};// \class NoiseModelFactor1 }; // \class NoiseModelFactor1
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 2 /** A convenient base class for creating your own NoiseModelFactor with 2
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class VALUE1, class VALUE2> template <class VALUE1, class VALUE2>
class NoiseModelFactor2: public NoiseModelFactor { class NoiseModelFactor2 : public NoiseModelFactorN<VALUE1, VALUE2> {
public:
// aliases for value types pulled from keys
using X1 = VALUE1;
using X2 = VALUE2;
public: protected:
using Base = NoiseModelFactor;
// typedefs for value types pulled from keys using This = NoiseModelFactor2<VALUE1, VALUE2>;
typedef VALUE1 X1;
typedef VALUE2 X2;
protected:
typedef NoiseModelFactor Base;
typedef NoiseModelFactor2<VALUE1, VALUE2> This;
public:
/**
* Default Constructor for I/O
*/
NoiseModelFactor2() {}
/**
* Constructor
* @param noiseModel shared pointer to noise model
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) :
Base(noiseModel, cref_list_of<2>(j1)(j2)) {}
public:
// inherit NoiseModelFactorN's constructors
using NoiseModelFactorN<VALUE1, VALUE2>::NoiseModelFactorN;
~NoiseModelFactor2() override {} ~NoiseModelFactor2() override {}
/** methods to retrieve both keys */ /** methods to retrieve both keys */
inline Key key1() const { return keys_[0]; } inline Key key1() const { return this->keys_[0]; }
inline Key key2() const { return keys_[1]; } inline Key key2() const { return this->keys_[1]; }
/** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if(this->active(x)) {
const X1& x1 = x.at<X1>(keys_[0]);
const X2& x2 = x.at<X2>(keys_[1]);
if(H) {
return evaluateError(x1, x2, (*H)[0], (*H)[1]);
} else {
return evaluateError(x1, x2);
}
} else {
return Vector::Zero(this->dim());
}
}
/**
* Override this method to finish implementing a binary factor.
* If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2).
*/
virtual Vector
evaluateError(const X1&, const X2&, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const = 0;
private:
private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar& boost::serialization::make_nvp(
boost::serialization::base_object<Base>(*this)); "NoiseModelFactor", boost::serialization::base_object<Base>(*this));
} }
}; // \class NoiseModelFactor2 }; // \class NoiseModelFactor2
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 3 /** A convenient base class for creating your own NoiseModelFactor with 3
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class VALUE1, class VALUE2, class VALUE3> template <class VALUE1, class VALUE2, class VALUE3>
class NoiseModelFactor3: public NoiseModelFactor { class NoiseModelFactor3 : public NoiseModelFactorN<VALUE1, VALUE2, VALUE3> {
public:
// aliases for value types pulled from keys
using X1 = VALUE1;
using X2 = VALUE2;
using X3 = VALUE3;
public: protected:
using Base = NoiseModelFactor;
// typedefs for value types pulled from keys using This = NoiseModelFactor3<VALUE1, VALUE2, VALUE3>;
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef VALUE3 X3;
protected:
typedef NoiseModelFactor Base;
typedef NoiseModelFactor3<VALUE1, VALUE2, VALUE3> This;
public:
/**
* Default Constructor for I/O
*/
NoiseModelFactor3() {}
/**
* Constructor
* @param noiseModel shared pointer to noise model
* @param j1 key of the first variable
* @param j2 key of the second variable
* @param j3 key of the third variable
*/
NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) :
Base(noiseModel, cref_list_of<3>(j1)(j2)(j3)) {}
public:
// inherit NoiseModelFactorN's constructors
using NoiseModelFactorN<VALUE1, VALUE2, VALUE3>::NoiseModelFactorN;
~NoiseModelFactor3() override {} ~NoiseModelFactor3() override {}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline Key key1() const { return keys_[0]; } inline Key key1() const { return this->keys_[0]; }
inline Key key2() const { return keys_[1]; } inline Key key2() const { return this->keys_[1]; }
inline Key key3() const { return keys_[2]; } inline Key key3() const { return this->keys_[2]; }
/** Calls the 3-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if(this->active(x)) {
if(H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
else
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]));
} else {
return Vector::Zero(this->dim());
}
}
/**
* Override this method to finish implementing a trinary factor.
* If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3).
*/
virtual Vector
evaluateError(const X1&, const X2&, const X3&,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const = 0;
private:
private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar& boost::serialization::make_nvp(
boost::serialization::base_object<Base>(*this)); "NoiseModelFactor", boost::serialization::base_object<Base>(*this));
} }
}; // \class NoiseModelFactor3 }; // \class NoiseModelFactor3
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 4 /** A convenient base class for creating your own NoiseModelFactor with 4
@ -687,7 +547,7 @@ class NoiseModelFactor4
using X4 = VALUE4; using X4 = VALUE4;
protected: protected:
using Base = NoiseModelFactor; // grandparent, for backwards compatibility using Base = NoiseModelFactor;
using This = NoiseModelFactor4<VALUE1, VALUE2, VALUE3, VALUE4>; using This = NoiseModelFactor4<VALUE1, VALUE2, VALUE3, VALUE4>;
public: public:
@ -714,175 +574,87 @@ class NoiseModelFactor4
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 5 /** A convenient base class for creating your own NoiseModelFactor with 5
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5> template <class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
class NoiseModelFactor5: public NoiseModelFactor { class NoiseModelFactor5
: public NoiseModelFactorN<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5> {
public:
// aliases for value types pulled from keys
using X1 = VALUE1;
using X2 = VALUE2;
using X3 = VALUE3;
using X4 = VALUE4;
using X5 = VALUE5;
public: protected:
using Base = NoiseModelFactor;
// typedefs for value types pulled from keys using This = NoiseModelFactor5<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5>;
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef VALUE3 X3;
typedef VALUE4 X4;
typedef VALUE5 X5;
protected:
typedef NoiseModelFactor Base;
typedef NoiseModelFactor5<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5> This;
public:
/**
* Default Constructor for I/O
*/
NoiseModelFactor5() {}
/**
* Constructor
* @param noiseModel shared pointer to noise model
* @param j1 key of the first variable
* @param j2 key of the second variable
* @param j3 key of the third variable
* @param j4 key of the fourth variable
* @param j5 key of the fifth variable
*/
NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) :
Base(noiseModel, cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {}
public:
// inherit NoiseModelFactorN's constructors
using NoiseModelFactorN<VALUE1, VALUE2, VALUE3, VALUE4,
VALUE5>::NoiseModelFactorN;
~NoiseModelFactor5() override {} ~NoiseModelFactor5() override {}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline Key key1() const { return keys_[0]; } inline Key key1() const { return this->keys_[0]; }
inline Key key2() const { return keys_[1]; } inline Key key2() const { return this->keys_[1]; }
inline Key key3() const { return keys_[2]; } inline Key key3() const { return this->keys_[2]; }
inline Key key4() const { return keys_[3]; } inline Key key4() const { return this->keys_[3]; }
inline Key key5() const { return keys_[4]; } inline Key key5() const { return this->keys_[4]; }
/** Calls the 5-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if(this->active(x)) {
if(H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
else
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]));
} else {
return Vector::Zero(this->dim());
}
}
/**
* Override this method to finish implementing a 5-way factor.
* If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3).
*/
virtual Vector
evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const = 0;
private:
private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar& boost::serialization::make_nvp(
boost::serialization::base_object<Base>(*this)); "NoiseModelFactor", boost::serialization::base_object<Base>(*this));
} }
}; // \class NoiseModelFactor5 }; // \class NoiseModelFactor5
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 6 /** A convenient base class for creating your own NoiseModelFactor with 6
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6> template <class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5,
class NoiseModelFactor6: public NoiseModelFactor { class VALUE6>
class NoiseModelFactor6
: public NoiseModelFactorN<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6> {
public:
// aliases for value types pulled from keys
using X1 = VALUE1;
using X2 = VALUE2;
using X3 = VALUE3;
using X4 = VALUE4;
using X5 = VALUE5;
using X6 = VALUE6;
public: protected:
using Base = NoiseModelFactor;
// typedefs for value types pulled from keys using This =
typedef VALUE1 X1; NoiseModelFactor6<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6>;
typedef VALUE2 X2;
typedef VALUE3 X3;
typedef VALUE4 X4;
typedef VALUE5 X5;
typedef VALUE6 X6;
protected:
typedef NoiseModelFactor Base;
typedef NoiseModelFactor6<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6> This;
public:
/**
* Default Constructor for I/O
*/
NoiseModelFactor6() {}
/**
* Constructor
* @param noiseModel shared pointer to noise model
* @param j1 key of the first variable
* @param j2 key of the second variable
* @param j3 key of the third variable
* @param j4 key of the fourth variable
* @param j5 key of the fifth variable
* @param j6 key of the fifth variable
*/
NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
Base(noiseModel, cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {}
public:
// inherit NoiseModelFactorN's constructors
using NoiseModelFactorN<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5,
VALUE6>::NoiseModelFactorN;
~NoiseModelFactor6() override {} ~NoiseModelFactor6() override {}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline Key key1() const { return keys_[0]; } inline Key key1() const { return this->keys_[0]; }
inline Key key2() const { return keys_[1]; } inline Key key2() const { return this->keys_[1]; }
inline Key key3() const { return keys_[2]; } inline Key key3() const { return this->keys_[2]; }
inline Key key4() const { return keys_[3]; } inline Key key4() const { return this->keys_[3]; }
inline Key key5() const { return keys_[4]; } inline Key key5() const { return this->keys_[4]; }
inline Key key6() const { return keys_[5]; } inline Key key6() const { return this->keys_[5]; }
/** Calls the 6-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if(this->active(x)) {
if(H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
else
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]));
} else {
return Vector::Zero(this->dim());
}
}
/**
* Override this method to finish implementing a 6-way factor.
* If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3).
*/
virtual Vector
evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none,
boost::optional<Matrix&> H6 = boost::none) const = 0;
private:
private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar& boost::serialization::make_nvp(
boost::serialization::base_object<Base>(*this)); "NoiseModelFactor", boost::serialization::base_object<Base>(*this));
} }
}; // \class NoiseModelFactor6 }; // \class NoiseModelFactor6
} // \namespace gtsam } // \namespace gtsam