converted all NoiseModelFactorX to inherit from NoiseModelFactorN
Gerry Chen
parent
bee4eeefdd
commit
ed07edbfe4
|
|
@ -316,16 +316,16 @@ public:
|
|||
* more general than just vectors, e.g., Rot3 or Pose3,
|
||||
* which are objects in non-linear manifolds (Lie groups).
|
||||
*/
|
||||
template<class... VALUES>
|
||||
class NoiseModelFactorN: public NoiseModelFactor {
|
||||
template <class... VALUES>
|
||||
class NoiseModelFactorN : public NoiseModelFactor {
|
||||
public:
|
||||
/** The type of the N'th template param can be obtained with VALUE<N> */
|
||||
template <int N>
|
||||
using VALUE = typename std::tuple_element<N, std::tuple<VALUES...>>::type;
|
||||
|
||||
protected:
|
||||
typedef NoiseModelFactor Base;
|
||||
typedef NoiseModelFactorN<VALUES...> This;
|
||||
using Base = NoiseModelFactor;
|
||||
using This = NoiseModelFactorN<VALUES...>;
|
||||
|
||||
/* "Dummy templated" alias is used to expand fixed-type parameter packs with
|
||||
* same length as VALUES. This ignores the template parameter. */
|
||||
|
|
@ -366,9 +366,11 @@ class NoiseModelFactorN: public NoiseModelFactor {
|
|||
|
||||
~NoiseModelFactorN() override {}
|
||||
|
||||
// /** Method to retrieve keys */
|
||||
// template <int N>
|
||||
// inline Key key() const { return keys_[N]; }
|
||||
/** Method to retrieve keys */
|
||||
template <int N>
|
||||
inline Key key() const {
|
||||
return keys_[N];
|
||||
}
|
||||
|
||||
/** Calls the n-key specific version of evaluateError, which is pure virtual
|
||||
* 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
|
||||
* variables.
|
||||
*/
|
||||
virtual Vector evaluateError(
|
||||
const VALUES& ... x,
|
||||
optional_matrix_type<VALUES> ... H) const = 0;
|
||||
virtual Vector evaluateError(const VALUES&... x,
|
||||
optional_matrix_type<VALUES>... H) const = 0;
|
||||
|
||||
/** No-jacobians requested function overload (since parameter packs can't have
|
||||
* default args). This specializes the version below to avoid recursive calls
|
||||
|
|
@ -408,7 +409,6 @@ class NoiseModelFactorN: public NoiseModelFactor {
|
|||
}
|
||||
|
||||
private:
|
||||
|
||||
/** Pack expansion with index_sequence template pattern */
|
||||
template <std::size_t... Inds>
|
||||
Vector unwhitenedError(
|
||||
|
|
@ -428,250 +428,110 @@ class NoiseModelFactorN: public NoiseModelFactor {
|
|||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp(
|
||||
"NoiseModelFactor", boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
}; // \class NoiseModelFactorN
|
||||
|
||||
|
||||
}; // \class NoiseModelFactorN
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** A convenient base class for creating your own NoiseModelFactor with 1
|
||||
* variable. To derive from this class, implement evaluateError(). */
|
||||
template<class VALUE>
|
||||
class NoiseModelFactor1: public NoiseModelFactor {
|
||||
template <class VALUE>
|
||||
class NoiseModelFactor1 : public NoiseModelFactorN<VALUE> {
|
||||
public:
|
||||
// aliases for value types pulled from keys
|
||||
using X = VALUE;
|
||||
|
||||
public:
|
||||
|
||||
// typedefs for value types pulled from keys
|
||||
typedef VALUE X;
|
||||
|
||||
protected:
|
||||
|
||||
typedef NoiseModelFactor Base;
|
||||
typedef NoiseModelFactor1<VALUE> This;
|
||||
|
||||
public:
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/** Default constructor for I/O only */
|
||||
NoiseModelFactor1() {}
|
||||
protected:
|
||||
using Base = NoiseModelFactor; // grandparent, for backwards compatibility
|
||||
using This = NoiseModelFactor1<VALUE>;
|
||||
|
||||
public:
|
||||
// inherit NoiseModelFactorN's constructors
|
||||
using NoiseModelFactorN<VALUE>::NoiseModelFactorN;
|
||||
~NoiseModelFactor1() override {}
|
||||
|
||||
inline Key key() const { return keys_[0]; }
|
||||
inline Key key() const { return this->keys_[0]; }
|
||||
|
||||
/**
|
||||
* 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:
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp(
|
||||
"NoiseModelFactor", boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
};// \class NoiseModelFactor1
|
||||
|
||||
}; // \class NoiseModelFactor1
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** A convenient base class for creating your own NoiseModelFactor with 2
|
||||
* variables. To derive from this class, implement evaluateError(). */
|
||||
template<class VALUE1, class VALUE2>
|
||||
class NoiseModelFactor2: public NoiseModelFactor {
|
||||
template <class VALUE1, class VALUE2>
|
||||
class NoiseModelFactor2 : public NoiseModelFactorN<VALUE1, VALUE2> {
|
||||
public:
|
||||
// aliases for value types pulled from keys
|
||||
using X1 = VALUE1;
|
||||
using X2 = VALUE2;
|
||||
|
||||
public:
|
||||
|
||||
// typedefs for value types pulled from keys
|
||||
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)) {}
|
||||
protected:
|
||||
using Base = NoiseModelFactor;
|
||||
using This = NoiseModelFactor2<VALUE1, VALUE2>;
|
||||
|
||||
public:
|
||||
// inherit NoiseModelFactorN's constructors
|
||||
using NoiseModelFactorN<VALUE1, VALUE2>::NoiseModelFactorN;
|
||||
~NoiseModelFactor2() override {}
|
||||
|
||||
/** methods to retrieve both keys */
|
||||
inline Key key1() const { return keys_[0]; }
|
||||
inline Key key2() const { return 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:
|
||||
inline Key key1() const { return this->keys_[0]; }
|
||||
inline Key key2() const { return this->keys_[1]; }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp(
|
||||
"NoiseModelFactor", boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
}; // \class NoiseModelFactor2
|
||||
}; // \class NoiseModelFactor2
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** A convenient base class for creating your own NoiseModelFactor with 3
|
||||
* variables. To derive from this class, implement evaluateError(). */
|
||||
template<class VALUE1, class VALUE2, class VALUE3>
|
||||
class NoiseModelFactor3: public NoiseModelFactor {
|
||||
template <class VALUE1, class VALUE2, class VALUE3>
|
||||
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:
|
||||
|
||||
// typedefs for value types pulled from keys
|
||||
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)) {}
|
||||
protected:
|
||||
using Base = NoiseModelFactor;
|
||||
using This = NoiseModelFactor3<VALUE1, VALUE2, VALUE3>;
|
||||
|
||||
public:
|
||||
// inherit NoiseModelFactorN's constructors
|
||||
using NoiseModelFactorN<VALUE1, VALUE2, VALUE3>::NoiseModelFactorN;
|
||||
~NoiseModelFactor3() override {}
|
||||
|
||||
/** methods to retrieve keys */
|
||||
inline Key key1() const { return keys_[0]; }
|
||||
inline Key key2() const { return keys_[1]; }
|
||||
inline Key key3() const { return 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:
|
||||
inline Key key1() const { return this->keys_[0]; }
|
||||
inline Key key2() const { return this->keys_[1]; }
|
||||
inline Key key3() const { return this->keys_[2]; }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp(
|
||||
"NoiseModelFactor", boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
}; // \class NoiseModelFactor3
|
||||
}; // \class NoiseModelFactor3
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** A convenient base class for creating your own NoiseModelFactor with 4
|
||||
|
|
@ -687,7 +547,7 @@ class NoiseModelFactor4
|
|||
using X4 = VALUE4;
|
||||
|
||||
protected:
|
||||
using Base = NoiseModelFactor; // grandparent, for backwards compatibility
|
||||
using Base = NoiseModelFactor;
|
||||
using This = NoiseModelFactor4<VALUE1, VALUE2, VALUE3, VALUE4>;
|
||||
|
||||
public:
|
||||
|
|
@ -714,175 +574,87 @@ class NoiseModelFactor4
|
|||
/* ************************************************************************* */
|
||||
/** A convenient base class for creating your own NoiseModelFactor with 5
|
||||
* variables. To derive from this class, implement evaluateError(). */
|
||||
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
|
||||
class NoiseModelFactor5: public NoiseModelFactor {
|
||||
template <class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
|
||||
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:
|
||||
|
||||
// typedefs for value types pulled from keys
|
||||
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)) {}
|
||||
protected:
|
||||
using Base = NoiseModelFactor;
|
||||
using This = NoiseModelFactor5<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5>;
|
||||
|
||||
public:
|
||||
// inherit NoiseModelFactorN's constructors
|
||||
using NoiseModelFactorN<VALUE1, VALUE2, VALUE3, VALUE4,
|
||||
VALUE5>::NoiseModelFactorN;
|
||||
~NoiseModelFactor5() override {}
|
||||
|
||||
/** methods to retrieve keys */
|
||||
inline Key key1() const { return keys_[0]; }
|
||||
inline Key key2() const { return keys_[1]; }
|
||||
inline Key key3() const { return keys_[2]; }
|
||||
inline Key key4() const { return keys_[3]; }
|
||||
inline Key key5() const { return 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:
|
||||
inline Key key1() const { return this->keys_[0]; }
|
||||
inline Key key2() const { return this->keys_[1]; }
|
||||
inline Key key3() const { return this->keys_[2]; }
|
||||
inline Key key4() const { return this->keys_[3]; }
|
||||
inline Key key5() const { return this->keys_[4]; }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp(
|
||||
"NoiseModelFactor", boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
}; // \class NoiseModelFactor5
|
||||
}; // \class NoiseModelFactor5
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** A convenient base class for creating your own NoiseModelFactor with 6
|
||||
* variables. To derive from this class, implement evaluateError(). */
|
||||
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6>
|
||||
class NoiseModelFactor6: public NoiseModelFactor {
|
||||
template <class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5,
|
||||
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:
|
||||
|
||||
// typedefs for value types pulled from keys
|
||||
typedef VALUE1 X1;
|
||||
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)) {}
|
||||
protected:
|
||||
using Base = NoiseModelFactor;
|
||||
using This =
|
||||
NoiseModelFactor6<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6>;
|
||||
|
||||
public:
|
||||
// inherit NoiseModelFactorN's constructors
|
||||
using NoiseModelFactorN<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5,
|
||||
VALUE6>::NoiseModelFactorN;
|
||||
~NoiseModelFactor6() override {}
|
||||
|
||||
/** methods to retrieve keys */
|
||||
inline Key key1() const { return keys_[0]; }
|
||||
inline Key key2() const { return keys_[1]; }
|
||||
inline Key key3() const { return keys_[2]; }
|
||||
inline Key key4() const { return keys_[3]; }
|
||||
inline Key key5() const { return keys_[4]; }
|
||||
inline Key key6() const { return 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:
|
||||
inline Key key1() const { return this->keys_[0]; }
|
||||
inline Key key2() const { return this->keys_[1]; }
|
||||
inline Key key3() const { return this->keys_[2]; }
|
||||
inline Key key4() const { return this->keys_[3]; }
|
||||
inline Key key5() const { return this->keys_[4]; }
|
||||
inline Key key6() const { return this->keys_[5]; }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& boost::serialization::make_nvp(
|
||||
"NoiseModelFactor", boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
}; // \class NoiseModelFactor6
|
||||
}; // \class NoiseModelFactor6
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
Loading…
Reference in New Issue