removed some new lines
parent
7efc411aa1
commit
6233619095
|
@ -2,6 +2,8 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
|
||||||
double mx_, my_; ///< X and Y measurements
|
double mx_, my_; ///< X and Y measurements
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using gtsam::NoiseModelFactor1<Pose2>::evaluateError;
|
using gtsam::NoiseModelFactor1<Pose2>::evaluateError;
|
||||||
|
|
||||||
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
|
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
|
||||||
|
|
|
@ -53,7 +53,6 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
||||||
~MagPoseFactor() override {}
|
~MagPoseFactor() override {}
|
||||||
|
|
||||||
/// Default constructor - only use for serialization.
|
/// Default constructor - only use for serialization.
|
||||||
|
|
|
@ -36,13 +36,13 @@ namespace gtsam {
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
||||||
/** These typedefs and aliases will help with making the evaluateError interface
|
/** These typedefs and aliases will help with making the evaluateError interface
|
||||||
* independent of boost
|
* independent of boost
|
||||||
* TODO(kartikarcot): Change this to OptionalMatrixNone
|
* TODO(kartikarcot): Change this to OptionalMatrixNone
|
||||||
* This typedef is used to indicate that the Jacobian is not required
|
* This typedef is used to indicate that the Jacobian is not required
|
||||||
* and the default value used for optional matrix pointer arguments in evaluateError.
|
* and the default value used for optional matrix pointer arguments in evaluateError.
|
||||||
* Had to use the static_cast of a nullptr, because the compiler is not able to
|
* Had to use the static_cast of a nullptr, because the compiler is not able to
|
||||||
* deduce the type of the nullptr when expanding the evaluateError templates.
|
* deduce the type of the nullptr when expanding the evaluateError templates.
|
||||||
*/
|
*/
|
||||||
#define OptionalNone static_cast<Matrix*>(nullptr)
|
#define OptionalNone static_cast<Matrix*>(nullptr)
|
||||||
|
|
||||||
/** This typedef will be used everywhere boost::optional<Matrix&> reference was used
|
/** This typedef will be used everywhere boost::optional<Matrix&> reference was used
|
||||||
|
@ -53,7 +53,8 @@ using OptionalMatrixType = Matrix*;
|
||||||
|
|
||||||
/** The OptionalMatrixVecType is a pointer to a vector of matrices. It will
|
/** The OptionalMatrixVecType is a pointer to a vector of matrices. It will
|
||||||
* be used in situations where a vector of matrices is optional, like in
|
* be used in situations where a vector of matrices is optional, like in
|
||||||
* unwhitenedError. */
|
* unwhitenedError.
|
||||||
|
*/
|
||||||
using OptionalMatrixVecType = std::vector<Matrix>*;
|
using OptionalMatrixVecType = std::vector<Matrix>*;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -254,10 +255,11 @@ public:
|
||||||
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const = 0;
|
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const = 0;
|
||||||
|
|
||||||
/** support taking in the actual vector instead of the pointer as well
|
/** support taking in the actual vector instead of the pointer as well
|
||||||
* to get access to this version of the function from derived classes
|
* to get access to this version of the function from derived classes
|
||||||
* one will need to use the "using" keyword and specify that like this:
|
* one will need to use the "using" keyword and specify that like this:
|
||||||
* public:
|
* public:
|
||||||
* using NoiseModelFactor::unwhitenedError; */
|
* using NoiseModelFactor::unwhitenedError;
|
||||||
|
*/
|
||||||
Vector unwhitenedError(const Values& x, std::vector<Matrix>& H) const {
|
Vector unwhitenedError(const Values& x, std::vector<Matrix>& H) const {
|
||||||
return unwhitenedError(x, &H);
|
return unwhitenedError(x, &H);
|
||||||
}
|
}
|
||||||
|
@ -613,12 +615,13 @@ protected:
|
||||||
virtual Vector evaluateError(const ValueTypes&... x,
|
virtual Vector evaluateError(const ValueTypes&... x,
|
||||||
OptionalMatrixTypeT<ValueTypes>... H) const = 0;
|
OptionalMatrixTypeT<ValueTypes>... H) const = 0;
|
||||||
|
|
||||||
// If someone uses the evaluateError function by supplying all the optional
|
/** If all the optional arguments are matrices then redirect the call to
|
||||||
// arguments then redirect the call to the one which takes pointers
|
* the one which takes pointers.
|
||||||
// to get access to this version of the function from derived classes
|
* To get access to this version of the function from derived classes
|
||||||
// one will need to use the "using" keyword and specify that like this:
|
* one will need to use the "using" keyword and specify that like this:
|
||||||
// public:
|
* public:
|
||||||
// using NoiseModelFactorN<list the value types here>::evaluateError;
|
* using NoiseModelFactorN<list the value types here>::evaluateError;
|
||||||
|
*/
|
||||||
|
|
||||||
Vector evaluateError(const ValueTypes&... x, MatrixTypeT<ValueTypes>&... H) const {
|
Vector evaluateError(const ValueTypes&... x, MatrixTypeT<ValueTypes>&... H) const {
|
||||||
return evaluateError(x..., (&H)...);
|
return evaluateError(x..., (&H)...);
|
||||||
|
|
|
@ -52,7 +52,7 @@ namespace gtsam {
|
||||||
public:
|
public:
|
||||||
|
|
||||||
/// shorthand for base class type
|
/// shorthand for base class type
|
||||||
typedef NoiseModelFactor2<POSE, LANDMARK> Base;
|
typedef NoiseModelFactorN<POSE, LANDMARK> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
|
@ -114,7 +114,6 @@ public:
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
||||||
// shorthand for a smart pointer to a factor
|
// shorthand for a smart pointer to a factor
|
||||||
typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel> shared_ptr;
|
typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel> shared_ptr;
|
||||||
|
|
||||||
|
|
|
@ -112,7 +112,6 @@ public:
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
||||||
// shorthand for a smart pointer to a factor
|
// shorthand for a smart pointer to a factor
|
||||||
typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel_NoBias> shared_ptr;
|
typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel_NoBias> shared_ptr;
|
||||||
|
|
||||||
|
|
|
@ -57,7 +57,6 @@ public:
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
||||||
// shorthand for a smart pointer to a factor
|
// shorthand for a smart pointer to a factor
|
||||||
typedef typename boost::shared_ptr<GaussMarkov1stOrderFactor> shared_ptr;
|
typedef typename boost::shared_ptr<GaussMarkov1stOrderFactor> shared_ptr;
|
||||||
|
|
||||||
|
|
|
@ -99,7 +99,6 @@ public:
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
||||||
// shorthand for a smart pointer to a factor
|
// shorthand for a smart pointer to a factor
|
||||||
typedef typename boost::shared_ptr<InertialNavFactor_GlobalVelocity> shared_ptr;
|
typedef typename boost::shared_ptr<InertialNavFactor_GlobalVelocity> shared_ptr;
|
||||||
|
|
||||||
|
|
|
@ -39,7 +39,6 @@ public:
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
||||||
/// shorthand for this class
|
/// shorthand for this class
|
||||||
typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
|
typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
|
||||||
|
|
||||||
|
|
|
@ -39,7 +39,6 @@ public:
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
||||||
/// shorthand for this class
|
/// shorthand for this class
|
||||||
typedef InvDepthFactorVariant1 This;
|
typedef InvDepthFactorVariant1 This;
|
||||||
|
|
||||||
|
|
|
@ -41,7 +41,6 @@ public:
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
||||||
/// shorthand for this class
|
/// shorthand for this class
|
||||||
typedef InvDepthFactorVariant2 This;
|
typedef InvDepthFactorVariant2 This;
|
||||||
|
|
||||||
|
|
|
@ -39,7 +39,6 @@ public:
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
||||||
/// shorthand for this class
|
/// shorthand for this class
|
||||||
typedef InvDepthFactorVariant3a This;
|
typedef InvDepthFactorVariant3a This;
|
||||||
|
|
||||||
|
|
|
@ -186,6 +186,9 @@ namespace gtsam {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
/** A function overload to accept a vector<matrix> instead of a pointer to
|
||||||
|
* the said type.
|
||||||
|
*/
|
||||||
gtsam::Vector whitenedError(const gtsam::Values& x, std::vector<Matrix>& H) const {
|
gtsam::Vector whitenedError(const gtsam::Values& x, std::vector<Matrix>& H) const {
|
||||||
return whitenedError(x, &H);
|
return whitenedError(x, &H);
|
||||||
}
|
}
|
||||||
|
|
|
@ -179,6 +179,9 @@ namespace gtsam {
|
||||||
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
/** A function overload to accept a vector<matrix> instead of a pointer to
|
||||||
|
* the said type.
|
||||||
|
*/
|
||||||
Vector whitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const {
|
Vector whitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const {
|
||||||
|
|
||||||
bool debug = true;
|
bool debug = true;
|
||||||
|
|
Loading…
Reference in New Issue