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

everything compiles but tests fail in no boost mode

release/4.3a0
kartik arcot 2023-01-06 16:29:15 -08:00
parent a070cf3130
commit ce02873140
45 changed files with 315 additions and 215 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
doc/Code/LocalizationFactor.cpp
View File

@ -2,11 +2,11 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
double mx_, my_; ///< X and Y measurements
public:
using gtsam::NoiseModelFactor1<Pose2>::evaluateError;
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
Vector evaluateError(const Pose2& q,
OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const Pose2& q, OptionalMatrixType H) const override {
const Rot2& R = q.rotation();
if (H) (*H) = (gtsam::Matrix(2, 3) <<
R.c(), -R.s(), 0.0,

3
examples/LocalizationExample.cpp
View File

@ -71,6 +71,7 @@ class UnaryFactor: public NoiseModelFactorN<Pose2> {
double mx_, my_;
public:
using NoiseModelFactor1<Pose2>::evaluateError;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<UnaryFactor> shared_ptr;
@ -84,7 +85,7 @@ class UnaryFactor: public NoiseModelFactorN<Pose2> {
// The first is the 'evaluateError' function. This function implements the desired measurement
// function, returning a vector of errors when evaluated at the provided variable value. It
// must also calculate the Jacobians for this measurement function, if requested.
Vector evaluateError(const Pose2& q, OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const Pose2& q, OptionalMatrixType H) const override {
// The measurement function for a GPS-like measurement h(q) which predicts the measurement (m) is h(q) = q, q = [qx qy qtheta]
// The error is then simply calculated as E(q) = h(q) - m:
// error_x = q.x - mx

6
gtsam/navigation/AHRSFactor.h
View File

@ -140,6 +140,7 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
public:
using Base::evaluateError;
/** Shorthand for a smart pointer to a factor */
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
typedef typename boost::shared_ptr<AHRSFactor> shared_ptr;
@ -179,9 +180,8 @@ public:
/// vector of errors
Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
const Vector3& bias, OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone, OptionalMatrixType H3 =
OptionalNone) const override;
const Vector3& bias, OptionalMatrixType H1,
OptionalMatrixType H2, OptionalMatrixType H3) const override;
/// predicted states from IMU
/// TODO(frank): relationship with PIM predict ??

6
gtsam/navigation/AttitudeFactor.h
View File

@ -122,8 +122,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/** vector of errors */
Vector evaluateError(const Rot3& nRb, //
OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override {
return attitudeError(nRb, H);
}
@ -198,8 +197,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/** vector of errors */
Vector evaluateError(const Pose3& nTb, //
OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override {
Vector e = attitudeError(nTb.rotation(), H);
if (H) {
Matrix H23 = *H;

10
gtsam/navigation/CombinedImuFactor.h
View File

@ -268,6 +268,7 @@ private:
PreintegratedCombinedMeasurements _PIM_;
public:
using Base::evaluateError;
/** Shorthand for a smart pointer to a factor */
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
@ -324,12 +325,9 @@ public:
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone,
OptionalMatrixType H4 = OptionalNone,
OptionalMatrixType H5 = OptionalNone,
OptionalMatrixType H6 = OptionalNone) const override;
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3, OptionalMatrixType H4,
OptionalMatrixType H5, OptionalMatrixType H6) const override;
private:
/** Serialization function */

8
gtsam/navigation/GPSFactor.h
View File

@ -42,6 +42,7 @@ private:
public:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GPSFactor> shared_ptr;
@ -78,8 +79,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const Pose3& p,
OptionalMatrixType H = OptionalNone) const override;
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
inline const Point3 & measurementIn() const {
return nT_;
@ -120,6 +120,7 @@ private:
Point3 nT_; ///< Position measurement in cartesian coordinates
public:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GPSFactor2> shared_ptr;
@ -151,8 +152,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const NavState& p,
OptionalMatrixType H = OptionalNone) const override;
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
inline const Point3 & measurementIn() const {
return nT_;

11
gtsam/navigation/ImuFactor.h
View File

@ -229,10 +229,8 @@ public:
/// vector of errors
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, OptionalMatrixType H1 =
OptionalNone, OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone, OptionalMatrixType H4 =
OptionalNone, OptionalMatrixType H5 = OptionalNone) const override;
const imuBias::ConstantBias& bias_i, OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5) const override;
#ifdef GTSAM_TANGENT_PREINTEGRATION
/// Merge two pre-integrated measurement classes
@ -308,9 +306,8 @@ public:
/// vector of errors
Vector evaluateError(const NavState& state_i, const NavState& state_j,
const imuBias::ConstantBias& bias_i, //
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const override;
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3) const override;
private:

18
gtsam/navigation/MagFactor.h
View File

@ -37,6 +37,7 @@ class MagFactor: public NoiseModelFactorN<Rot2> {
const Point3 bias_; ///< bias
public:
using NoiseModelFactor1<Rot2>::evaluateError;
/**
* Constructor of factor that estimates nav to body rotation bRn
@ -74,8 +75,7 @@ public:
/**
* @brief vector of errors
*/
Vector evaluateError(const Rot2& nRb,
OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const Rot2& nRb, OptionalMatrixType H) const override {
// measured bM = nRb<52> * nM + b
Point3 hx = unrotate(nRb, nM_, H) + bias_;
return (hx - measured_);
@ -94,6 +94,7 @@ class MagFactor1: public NoiseModelFactorN<Rot3> {
const Point3 bias_; ///< bias
public:
using NoiseModelFactor1<Rot3>::evaluateError;
/** Constructor */
MagFactor1(Key key, const Point3& measured, double scale,
@ -112,8 +113,7 @@ public:
/**
* @brief vector of errors
*/
Vector evaluateError(const Rot3& nRb,
OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override {
// measured bM = nRb<52> * nM + b
Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
return (hx - measured_);
@ -131,6 +131,7 @@ class MagFactor2: public NoiseModelFactorN<Point3, Point3> {
const Rot3 bRn_; ///< The assumed known rotation from nav to body
public:
using NoiseModelFactor2<Point3, Point3>::evaluateError;
/** Constructor */
MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
@ -151,8 +152,7 @@ public:
* @param bias (unknown) 3D bias
*/
Vector evaluateError(const Point3& nM, const Point3& bias,
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 =
OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
// measured bM = nRb<52> * nM + b, where b is unknown bias
Point3 hx = bRn_.rotate(nM, OptionalNone, H1) + bias;
if (H2)
@ -172,6 +172,7 @@ class MagFactor3: public NoiseModelFactorN<double, Unit3, Point3> {
const Rot3 bRn_; ///< The assumed known rotation from nav to body
public:
using NoiseModelFactor3<double, Unit3, Point3>::evaluateError;
/** Constructor */
MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
@ -192,9 +193,8 @@ public:
* @param bias (unknown) 3D bias
*/
Vector evaluateError(const double& scale, const Unit3& direction,
const Point3& bias, OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone, OptionalMatrixType H3 =
OptionalNone) const override {
const Point3& bias, OptionalMatrixType H1,
OptionalMatrixType H2, OptionalMatrixType H3) const override {
// measured bM = nRb<52> * nM + b, where b is unknown bias
Unit3 rotated = bRn_.rotate(direction, OptionalNone, H2);
Point3 hx = scale * rotated.point3() + bias;

4
gtsam/navigation/MagPoseFactor.h
View File

@ -49,6 +49,8 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
GTSAM_CONCEPT_POSE_TYPE(POSE)
public:
using Base::evaluateError;
~MagPoseFactor() override {}
/// Default constructor - only use for serialization.
@ -108,7 +110,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
* Return the factor's error h(x) - z, and the optional Jacobian. Note that
* the measurement error is expressed in the body frame.
*/
Vector evaluateError(const POSE& nPb, OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const POSE& nPb, OptionalMatrixType H) const override {
// Predict the measured magnetic field h(x) in the *body* frame.
// If body_P_sensor was given, bias_ will have been rotated into the body frame.
Matrix H_rot = Matrix::Zero(MeasDim, RotDim);

19
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -21,6 +21,8 @@
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <functional>
#include "gtsam/base/Matrix.h"
#include <CppUnitLite/TestHarness.h>
using namespace std::placeholders;
@ -47,11 +49,11 @@ TEST( Rot3AttitudeFactor, Constructor ) {
Rot3 nRb;
EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5));
std::function<Vector(const Rot3&)> err_fn = [&factor](const Rot3& r){
return factor.evaluateError(r, OptionalNone);
};
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector, Rot3>(
std::bind(&Rot3AttitudeFactor::evaluateError, &factor,
std::placeholders::_1, boost::none),
nRb);
Matrix expectedH = numericalDerivative11<Vector, Rot3>(err_fn, nRb);
// Use the factor to calculate the derivative
Matrix actualH;
@ -98,10 +100,13 @@ TEST( Pose3AttitudeFactor, Constructor ) {
Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0));
EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(T),1e-5));
Matrix actualH1;
std::function<Vector(const Pose3&)> err_fn = [&factor](const Pose3& p){
return factor.evaluateError(p, OptionalNone);
};
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Pose3>(
std::bind(&Pose3AttitudeFactor::evaluateError, &factor, std::placeholders::_1,
boost::none), T);
Matrix expectedH = numericalDerivative11<Vector,Pose3>(err_fn, T);
// Use the factor to calculate the derivative
Matrix actualH;

18
gtsam/navigation/tests/testBarometricFactor.cpp
View File

@ -58,15 +58,11 @@ TEST(BarometricFactor, Constructor) {
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative21<Vector, Pose3, double>(
std::bind(&BarometricFactor::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, boost::none,
boost::none),
T, baroBias);
[&factor](const Pose3& p, const double& d) {return factor.evaluateError(p, d);},
T, baroBias);
Matrix expectedH2 = numericalDerivative22<Vector, Pose3, double>(
std::bind(&BarometricFactor::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, boost::none,
boost::none),
[&factor](const Pose3& p, const double& d) {return factor.evaluateError(p, d);},
T, baroBias);
// Use the factor to calculate the derivative
@ -99,15 +95,11 @@ TEST(BarometricFactor, nonZero) {
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative21<Vector, Pose3, double>(
std::bind(&BarometricFactor::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, boost::none,
boost::none),
[&factor](const Pose3& p, const double& d) {return factor.evaluateError(p, d);},
T, baroBias);
Matrix expectedH2 = numericalDerivative22<Vector, Pose3, double>(
std::bind(&BarometricFactor::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, boost::none,
boost::none),
[&factor](const Pose3& p, const double& d) {return factor.evaluateError(p, d);},
T, baroBias);
// Use the factor to calculate the derivative and the error

10
gtsam/navigation/tests/testMagPoseFactor.cpp
View File

@ -78,8 +78,7 @@ TEST(MagPoseFactor, JacobianPose2) {
MagPoseFactor<Pose2> f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
(std::bind(&MagPoseFactor<Pose2>::evaluateError, &f,
std::placeholders::_1, boost::none),
([&f] (const Pose2& p) {return f.evaluateError(p);},
n_P2_b),
H2, 1e-7));
}
@ -92,8 +91,7 @@ TEST(MagPoseFactor, JacobianPose3) {
MagPoseFactor<Pose3> f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
(std::bind(&MagPoseFactor<Pose3>::evaluateError, &f,
std::placeholders::_1, boost::none),
([&f] (const Pose3& p) {return f.evaluateError(p);},
n_P3_b),
H3, 1e-7));
}
@ -109,7 +107,7 @@ TEST(MagPoseFactor, body_P_sensor2) {
MagPoseFactor<Pose2> f = MagPoseFactor<Pose2>(Symbol('X', 0), sM, scale, dir2, bias2, model2, body_P2_sensor);
CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
(std::bind(&MagPoseFactor<Pose2>::evaluateError, &f, std::placeholders::_1, boost::none), n_P2_b), H2, 1e-7));
([&f] (const Pose2& p) {return f.evaluateError(p);},n_P2_b), H2, 1e-7));
}
// *****************************************************************************
@ -123,7 +121,7 @@ TEST(MagPoseFactor, body_P_sensor3) {
MagPoseFactor<Pose3> f = MagPoseFactor<Pose3>(Symbol('X', 0), sM, scale, dir3, bias3, model3, body_P3_sensor);
CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
(std::bind(&MagPoseFactor<Pose3>::evaluateError, &f, std::placeholders::_1, boost::none), n_P3_b), H3, 1e-7));
([&f] (const Pose3& p) {return f.evaluateError(p);}, n_P3_b), H3, 1e-7));
}
// *****************************************************************************

114
gtsam/nonlinear/ExpressionFactor.h
View File

@ -28,6 +28,97 @@
namespace gtsam {
template <typename... ValueTypes>
class EvaluateErrorInterface {
public:
enum { N = sizeof...(ValueTypes) };
private:
template <int I>
using IndexIsValid = typename std::enable_if<(I >= 1) && (I <= N),
void>::type; // 1-indexed!
public:
/**
* Override `evaluateError` to finish implementing an n-way factor.
*
* Both the `x` and `H` arguments are written here as parameter packs, but
* when overriding this method, you probably want to explicitly write them
* out. For example, for a 2-way factor with variable types Pose3 and Point3,
* you should implement:
* ```
* Vector evaluateError(
* const Pose3& x1, const Point3& x2,
* boost::optional<Matrix&> H1 = boost::none,
* boost::optional<Matrix&> H2 = boost::none) const override { ... }
* ```
*
* If any of the optional Matrix reference arguments are specified, it should
* compute both the function evaluation and its derivative(s) in the requested
* variables.
*
* @param x The values of the variables to evaluate the error for. Passed in
* as separate arguments.
* @param[out] H The Jacobian with respect to each variable (optional).
*/
virtual Vector evaluateError(const ValueTypes&... x, OptionalMatrixTypeT<ValueTypes>... H) const = 0;
#ifdef NO_BOOST_CPP17
// if someone uses the evaluateError function by supplying all the optional
// arguments then redirect the call to the one which takes pointers
Vector evaluateError(const ValueTypes&... x, MatrixTypeT<ValueTypes>&... H) const {
return evaluateError(x..., (&H)...);
}
#endif
/// @}
/// @name Convenience method overloads
/// @{
/** No-Jacobians requested function overload.
* This specializes the version below to avoid recursive calls since this is
* commonly used.
*
* e.g. `const Vector error = factor.evaluateError(pose, point);`
*/
inline Vector evaluateError(const ValueTypes&... x) const {
return evaluateError(x..., OptionalMatrixTypeT<ValueTypes>()...);
}
/** Some (but not all) optional Jacobians are omitted (function overload)
*
* e.g. `const Vector error = factor.evaluateError(pose, point, Hpose);`
*/
template <typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>
inline Vector evaluateError(const ValueTypes&... x, OptionalJacArgs&&... H) const {
#ifdef NO_BOOST_CPP17
// A check to ensure all arguments passed are all either matrices or are all pointers to matrices
constexpr bool are_all_mat = (... && (std::is_same<Matrix, std::decay_t<OptionalJacArgs>>::value));
constexpr bool are_all_ptrs = (... && (std::is_same<OptionalMatrixType, std::decay_t<OptionalJacArgs>>::value ||
std::is_same<OptionalNoneType, std::decay_t<OptionalJacArgs>>::value));
static_assert((are_all_mat || are_all_ptrs),
"Arguments that are passed to the evaluateError function can only be of following the types: Matrix, "
"or Matrix*");
// if they pass all matrices then we want to pass their pointers instead
if constexpr (are_all_mat) {
return evaluateError(x..., (&H)...);
} else {
return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., static_cast<OptionalMatrixType>(OptionalNone));
}
#else
// A check to ensure all arguments passed are all either matrices or are optionals of matrix references
constexpr bool are_all_mat = (... && (std::is_same<Matrix&, OptionalJacArgs>::value ||
std::is_same<OptionalMatrixType, std::decay_t<OptionalJacArgs>>::value ||
std::is_same<OptionalNoneType, std::decay_t<OptionalJacArgs>>::value));
static_assert(
are_all_mat,
"Arguments that are passed to the evaluateError function can only be of following the types: Matrix&, "
"boost::optional<Matrix&>, or boost::none_t");
return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., OptionalNone);
#endif
}
};
/**
* Factor that supports arbitrary expressions via AD.
*
@ -40,8 +131,8 @@ namespace gtsam {
* \tparam T Type for measurements.
*
*/
template<typename T>
class ExpressionFactor: public NoiseModelFactor {
template <typename T>
class ExpressionFactor : public NoiseModelFactor {
BOOST_CONCEPT_ASSERT((IsTestable<T>));
protected:
@ -55,6 +146,7 @@ protected:
public:
using NoiseModelFactor::unwhitenedError;
typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
/**
@ -243,6 +335,7 @@ class ExpressionFactorN : public ExpressionFactor<T> {
public:
static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args);
using ArrayNKeys = std::array<Key, NARY_EXPRESSION_SIZE>;
using ExpressionFactor<T>::unwhitenedError;
/// Destructor
~ExpressionFactorN() override = default;
@ -305,15 +398,14 @@ struct traits<ExpressionFactorN<T, Args...>>
* @deprecated Prefer the more general ExpressionFactorN<>.
*/
template <typename T, typename A1, typename A2>
class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN<T, A1, A2>, public EvaluateErrorInterface<A1, A2> {
public:
/// Destructor
~ExpressionFactor2() override {}
/// Backwards compatible evaluateError, to make existing tests compile
Vector evaluateError(const A1 &a1, const A2 &a2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const {
virtual Vector evaluateError(const A1& a1, const A2& a2, OptionalMatrixType H1,
OptionalMatrixType H2) const override {
Values values;
values.insert(this->keys_[0], a1);
values.insert(this->keys_[1], a2);
@ -327,12 +419,9 @@ public:
/// Recreate expression from given keys_ and measured_, used in load
/// Needed to deserialize a derived factor
virtual Expression<T> expression(Key key1, Key key2) const {
throw std::runtime_error(
"ExpressionFactor2::expression not provided: cannot deserialize.");
throw std::runtime_error("ExpressionFactor2::expression not provided: cannot deserialize.");
}
Expression<T>
expression(const typename ExpressionFactorN<T, A1, A2>::ArrayNKeys &keys)
const override {
Expression<T> expression(const typename ExpressionFactorN<T, A1, A2>::ArrayNKeys& keys) const override {
return expression(keys[0], keys[1]);
}
@ -341,8 +430,7 @@ protected:
ExpressionFactor2() {}
/// Constructor takes care of keys, but still need to call initialize
ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel,
const T &measurement)
ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel& noiseModel, const T& measurement)
: ExpressionFactorN<T, A1, A2>({key1, key2}, noiseModel, measurement) {}
};
// ExpressionFactor2

2
gtsam/nonlinear/FunctorizedFactor.h
View File

@ -65,6 +65,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
std::function<R(T, OptionalMatrixType)> func_; ///< functor instance
public:
using Base::evaluateError;
/** default constructor - only use for serialization */
FunctorizedFactor() {}
@ -165,6 +166,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
FunctionType func_; ///< functor instance
public:
using Base::evaluateError;
/** default constructor - only use for serialization */
FunctorizedFactor2() {}

11
gtsam/nonlinear/NonlinearEquality.h
View File

@ -139,8 +139,7 @@ public:
}
/// Error function
Vector evaluateError(const T& xj,
OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const T& xj, OptionalMatrixType H) const override {
const size_t nj = traits<T>::GetDimension(feasible_);
if (allow_error_) {
if (H)
@ -209,6 +208,7 @@ class NonlinearEquality1: public NoiseModelFactorN<VALUE> {
public:
typedef VALUE X;
using NoiseModelFactor1<VALUE>::evaluateError;
protected:
typedef NoiseModelFactorN<VALUE> Base;
@ -249,8 +249,7 @@ public:
}
/// g(x) with optional derivative
Vector evaluateError(const X& x1,
OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const X& x1, OptionalMatrixType H) const override {
if (H)
(*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
// manifold equivalent of h(x)-z -> log(z,h(x))
@ -305,6 +304,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
public:
typedef boost::shared_ptr<NonlinearEquality2<T>> shared_ptr;
using Base::evaluateError;
/**
* Constructor
@ -325,8 +325,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
/// g(x) with optional derivative2
Vector evaluateError(
const T& x1, const T& x2, OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
const T& x1, const T& x2, OptionalMatrixType H1, OptionalMatrixType H2) const override {
static const size_t p = traits<T>::dimension;
if (H1) *H1 = -Matrix::Identity(p, p);
if (H2) *H2 = Matrix::Identity(p, p);

2
gtsam/nonlinear/NonlinearFactor.h
View File

@ -433,6 +433,8 @@ class NoiseModelFactorN
/// N is the number of variables (N-way factor)
enum { N = sizeof...(ValueTypes) };
using NoiseModelFactor::unwhitenedError;
protected:
using Base = NoiseModelFactor;
using This = NoiseModelFactorN<ValueTypes...>;

3
gtsam/nonlinear/PriorFactor.h
View File

@ -31,6 +31,7 @@ namespace gtsam {
public:
typedef VALUE T;
using NoiseModelFactor1<VALUE>::evaluateError;
private:
@ -91,7 +92,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const T& x, OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const T& x, OptionalMatrixType H) const override {
if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
// manifold equivalent of z-x -> Local(x,z)
return -traits<T>::Local(x, prior_);

5
gtsam/nonlinear/tests/testFunctorizedFactor.cpp
View File

@ -160,7 +160,7 @@ TEST(FunctorizedFactor, Functional) {
Matrix X = Matrix::Identity(3, 3);
Matrix measurement = multiplier * Matrix::Identity(3, 3);
std::function<Matrix(Matrix, boost::optional<Matrix &>)> functional =
std::function<Matrix(Matrix, OptionalMatrixType)> functional =
MultiplyFunctor(multiplier);
auto factor =
MakeFunctorizedFactor<Matrix>(key, measurement, model, functional);
@ -233,8 +233,7 @@ TEST(FunctorizedFactor, Functional2) {
Vector3 x(1, 2, 3);
Vector measurement = A * x;
std::function<Matrix(Matrix, Matrix, boost::optional<Matrix &>,
boost::optional<Matrix &>)>
std::function<Matrix(Matrix, Matrix, OptionalMatrixType, OptionalMatrixType)>
functional = ProjectionFunctor();
auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement,
model2, functional);

10
gtsam/sam/BearingFactor.h
View File

@ -34,9 +34,10 @@ struct Bearing;
*/
template <typename A1, typename A2,
typename T = typename Bearing<A1, A2>::result_type>
struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
struct BearingFactor : public ExpressionFactorN<T, A1, A2>, public EvaluateErrorInterface<A1, A2>{
typedef ExpressionFactorN<T, A1, A2> Base;
using EvaluateErrorInterface<A1,A2>::evaluateError;
/// default constructor
BearingFactor() {}
@ -61,13 +62,12 @@ struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
Base::print(s, kf);
}
Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const
virtual Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1, OptionalMatrixType H2) const override
{
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = unwhitenedError(
const Vector error = this->unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs);
if (H1) *H1 = Hs[0];

8
gtsam/sam/BearingRangeFactor.h
View File

@ -33,7 +33,7 @@ template <typename A1, typename A2,
typename B = typename Bearing<A1, A2>::result_type,
typename R = typename Range<A1, A2>::result_type>
class BearingRangeFactor
: public ExpressionFactorN<BearingRange<A1, A2>, A1, A2> {
: public ExpressionFactorN<BearingRange<A1, A2>, A1, A2>, public EvaluateErrorInterface<A1, A2>{
private:
typedef BearingRange<A1, A2> T;
typedef ExpressionFactorN<T, A1, A2> Base;
@ -41,6 +41,7 @@ class BearingRangeFactor
public:
typedef boost::shared_ptr<This> shared_ptr;
using EvaluateErrorInterface<A1, A2>::evaluateError;
/// Default constructor
BearingRangeFactor() {}
@ -73,9 +74,8 @@ class BearingRangeFactor
Expression<A2>(keys[1]));
}
Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const
virtual Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1, OptionalMatrixType H2) const override
{
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();

25
gtsam/sam/RangeFactor.h
View File

@ -32,12 +32,13 @@ struct Range;
* @ingroup sam
*/
template <typename A1, typename A2 = A1, typename T = double>
class RangeFactor : public ExpressionFactorN<T, A1, A2> {
class RangeFactor : public ExpressionFactorN<T, A1, A2> , public EvaluateErrorInterface<A1, A2>{
private:
typedef RangeFactor<A1, A2> This;
typedef ExpressionFactorN<T, A1, A2> Base;
public:
using EvaluateErrorInterface<A1,A2>::evaluateError;
/// default constructor
RangeFactor() {}
@ -58,16 +59,12 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> {
Expression<A2> a2_(keys[1]);
return Expression<T>(Range<A1, A2>(), a1_, a2_);
}
Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const
{
virtual Vector evaluateError(const A1& a1, const A2& a2, OptionalMatrixType H1,
OptionalMatrixType H2) const override {
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = Base::unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs);
const auto& keys = Factor::keys();
const Vector error = Base::unwhitenedError({{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, Hs);
if (H1) *H1 = Hs[0];
if (H2) *H2 = Hs[1];
return error;
@ -100,7 +97,7 @@ struct traits<RangeFactor<A1, A2, T> >
*/
template <typename A1, typename A2 = A1,
typename T = typename Range<A1, A2>::result_type>
class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> , public EvaluateErrorInterface<A1, A2>{
private:
typedef RangeFactorWithTransform<A1, A2> This;
typedef ExpressionFactorN<T, A1, A2> Base;
@ -108,6 +105,7 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
A1 body_T_sensor_; ///< The pose of the sensor in the body frame
public:
using EvaluateErrorInterface<A1, A2>::evaluateError;
//// Default constructor
RangeFactorWithTransform() {}
@ -136,9 +134,8 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
}
Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const
virtual Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1, OptionalMatrixType H2) const override
{
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();

5
gtsam/slam/BetweenFactor.h
View File

@ -55,6 +55,7 @@ namespace gtsam {
VALUE measured_; /** The measurement */
public:
using Base::evaluateError;
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<BetweenFactor> shared_ptr;
@ -105,8 +106,8 @@ namespace gtsam {
/// @{
/// evaluate error, returns vector of errors size of tangent space
Vector evaluateError(const T& p1, const T& p2, OptionalMatrixType H1 =
OptionalNone, OptionalMatrixType H2 = OptionalNone) const override {
Vector evaluateError(const T& p1, const T& p2,
OptionalMatrixType H1, OptionalMatrixType H2) const override {
T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR

10
gtsam/slam/BoundingConstraint.h
View File

@ -35,6 +35,8 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
typedef NoiseModelFactorN<VALUE> Base;
typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
using Base::evaluateError;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
@ -64,8 +66,7 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
}
Vector evaluateError(const X& x, OptionalMatrixType H =
OptionalNone) const override {
Vector evaluateError(const X& x, OptionalMatrixType H) const override {
Matrix D;
double error = value(x, D) - threshold_;
if (H) {
@ -105,6 +106,8 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
typedef NoiseModelFactorN<VALUE1, VALUE2> Base;
typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
using Base::evaluateError;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
@ -134,8 +137,7 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
}
Vector evaluateError(const X1& x1, const X2& x2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
Matrix D1, D2;
double error = value(x1, x2, D1, D2) - threshold_;
if (H1) {

4
gtsam/slam/EssentialMatrixConstraint.h
View File

@ -37,6 +37,7 @@ private:
EssentialMatrix measuredE_; /** The measurement is an essential matrix */
public:
using Base::evaluateError;
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
@ -79,8 +80,7 @@ public:
/** vector of errors */
Vector evaluateError(const Pose3& p1, const Pose3& p2,
OptionalMatrixType Hp1 = OptionalNone, //
OptionalMatrixType Hp2 = OptionalNone) const override;
OptionalMatrixType Hp1, OptionalMatrixType Hp2) const override;
/** return the measured */
const EssentialMatrix& measured() const {

8
gtsam/slam/EssentialMatrixFactor.h
View File

@ -295,7 +295,15 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
// Version with derivatives
Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
#ifdef NO_BOOST_CPP17
// Had to do this since the only overloaded function EssentialMatrixFactor2
// uses the type OptionalMatrixType. Which would be a pointer when we are
// not using boost. There is no way to redirect that call to the top (NoiseModelFactorN)
// dereference it and bring it back to the Base (EssentialMatrixFactor2)
Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd);
#else
Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd);
#endif
*DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5)
return e;
}

7
gtsam/slam/FrobeniusFactor.h
View File

@ -79,6 +79,7 @@ class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
using NoiseModelFactor2<Rot, Rot>::evaluateError;
/// Constructor
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
@ -86,8 +87,7 @@ class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
/// Error is just Frobenius norm between rotation matrices.
Vector evaluateError(const Rot& R1, const Rot& R2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
Vector error = R2.vec(H2) - R1.vec(H1);
if (H1) *H1 = -*H1;
return error;
@ -151,8 +151,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
/// Error is Frobenius norm between R1*R12 and R2.
Vector evaluateError(const Rot& R1, const Rot& R2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
const Rot R2hat = R1.compose(R12_);
Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);

8
gtsam/slam/OrientedPlane3Factor.h
View File

@ -21,6 +21,7 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, Oriente
typedef NoiseModelFactorN<Pose3, OrientedPlane3> Base;
public:
using NoiseModelFactor2<Pose3, OrientedPlane3>::evaluateError;
/// Constructor
OrientedPlane3Factor() {
}
@ -44,8 +45,7 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, Oriente
/// evaluateError
Vector evaluateError(
const Pose3& pose, const OrientedPlane3& plane,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override;
OptionalMatrixType H1, OptionalMatrixType H2) const override;
};
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
@ -55,6 +55,7 @@ class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<Orien
typedef NoiseModelFactorN<OrientedPlane3> Base;
public:
using Base::evaluateError;
typedef OrientedPlane3DirectionPrior This;
/// Constructor
OrientedPlane3DirectionPrior() {
@ -72,8 +73,7 @@ class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<Orien
/// equals
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
Vector evaluateError(const OrientedPlane3& plane,
OptionalMatrixType H = OptionalNone) const override;
Vector evaluateError(const OrientedPlane3& plane, OptionalMatrixType H) const override;
};
} // gtsam

4
gtsam/slam/PoseRotationPrior.h
View File

@ -25,6 +25,8 @@ public:
typedef typename POSE::Translation Translation;
typedef typename POSE::Rotation Rotation;
using Base::evaluateError;
GTSAM_CONCEPT_POSE_TYPE(Pose)
GTSAM_CONCEPT_GROUP_TYPE(Pose)
GTSAM_CONCEPT_LIE_TYPE(Rotation)
@ -75,7 +77,7 @@ public:
}
/** h(x)-z */
Vector evaluateError(const Pose& pose, OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const Pose& pose, OptionalMatrixType H) const override {
const Rotation& newR = pose.rotation();
if (H) {
*H = Matrix::Zero(rDim, xDim);

4
gtsam/slam/PoseTranslationPrior.h
View File

@ -25,6 +25,8 @@ public:
typedef POSE Pose;
typedef typename POSE::Translation Translation;
typedef typename POSE::Rotation Rotation;
using Base::evaluateError;
GTSAM_CONCEPT_POSE_TYPE(Pose)
GTSAM_CONCEPT_GROUP_TYPE(Pose)
@ -59,7 +61,7 @@ public:
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** h(x)-z */
Vector evaluateError(const Pose& pose, OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const Pose& pose, OptionalMatrixType H) const override {
const Translation& newTrans = pose.translation();
const Rotation& R = pose.rotation();
const int tDim = traits<Translation>::GetDimension(newTrans);

4
gtsam/slam/ProjectionFactor.h
View File

@ -60,6 +60,8 @@ namespace gtsam {
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
using Base::evaluateError;
/// Default constructor
GenericProjectionFactor() :
measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
@ -133,7 +135,7 @@ namespace gtsam {
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Point3& point,
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
try {
if(body_P_sensor_) {
if(H1) {

4
gtsam/slam/ReferenceFrameFactor.h
View File

@ -29,8 +29,8 @@ namespace gtsam {
template<class T, class P>
P transform_point(
const T& trans, const P& global,
boost::optional<Matrix&> Dtrans,
boost::optional<Matrix&> Dglobal) {
OptionalMatrixType Dtrans,
OptionalMatrixType Dglobal) {
return trans.transformFrom(global, Dtrans, Dglobal);
}

7
gtsam/slam/RotateFactor.h
View File

@ -28,6 +28,7 @@ class RotateFactor: public NoiseModelFactorN<Rot3> {
typedef RotateFactor This;
public:
using Base::evaluateError;
/// Constructor
RotateFactor(Key key, const Rot3& P, const Rot3& Z,
@ -50,8 +51,7 @@ public:
}
/// vector of errors returns 2D vector
Vector evaluateError(const Rot3& R,
OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const Rot3& R, OptionalMatrixType H) const override {
// predict p_ as q = R*z_, derivative H will be filled if not none
Point3 q = R.rotate(z_,H);
// error is just difference, and note derivative of that wrpt q is I3
@ -72,6 +72,7 @@ class RotateDirectionsFactor: public NoiseModelFactorN<Rot3> {
typedef RotateDirectionsFactor This;
public:
using Base::evaluateError;
/// Constructor
RotateDirectionsFactor(Key key, const Unit3& i_p, const Unit3& c_z,
@ -102,7 +103,7 @@ public:
}
/// vector of errors returns 2D vector
Vector evaluateError(const Rot3& iRc, OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const Rot3& iRc, OptionalMatrixType H) const override {
Unit3 i_q = iRc * c_z_;
Vector error = i_p_.error(i_q, H);
if (H) {

5
gtsam/slam/StereoFactor.h
View File

@ -48,6 +48,9 @@ public:
typedef boost::shared_ptr<GenericStereoFactor> shared_ptr; ///< typedef for shared pointer to this object
typedef POSE CamPose; ///< typedef for Pose Lie Value type
using Base::evaluateError;
/**
* Default constructor
*/
@ -120,7 +123,7 @@ public:
/** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point,
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
try {
if(body_P_sensor_) {
if(H1) {

4
gtsam/slam/TriangulationFactor.h
View File

@ -61,6 +61,7 @@ public:
/// shorthand for a smart pointer to a factor
using shared_ptr = boost::shared_ptr<This>;
using NoiseModelFactor1<Point3>::evaluateError;
/// Default constructor
TriangulationFactor() :
@ -119,8 +120,7 @@ public:
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Point3& point, OptionalMatrixType H2 =
OptionalNone) const override {
Vector evaluateError(const Point3& point, OptionalMatrixType H2) const override {
try {
return traits<Measurement>::Local(measured_, camera_.project2(point, OptionalNone, H2));
} catch (CheiralityException& e) {

9
gtsam/slam/tests/testBetweenFactor.cpp
View File

@ -36,16 +36,13 @@ TEST(BetweenFactor, Rot3) {
EXPECT(assert_equal(expected,actual/*, 1e-100*/)); // Uncomment to make unit test fail
Matrix numericalH1 = numericalDerivative21<Vector3, Rot3, Rot3>(
std::function<Vector(const Rot3&, const Rot3&)>(std::bind(
&BetweenFactor<Rot3>::evaluateError, factor, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none)),
[&factor](const Rot3& r1, const Rot3& r2) {return factor.evaluateError(r1, r2);},
R1, R2, 1e-5);
EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
Matrix numericalH2 = numericalDerivative22<Vector3,Rot3,Rot3>(
std::function<Vector(const Rot3&, const Rot3&)>(std::bind(
&BetweenFactor<Rot3>::evaluateError, factor, std::placeholders::_1, std::placeholders::_2, boost::none,
boost::none)), R1, R2, 1e-5);
[&factor](const Rot3& r1, const Rot3& r2) {return factor.evaluateError(r1, r2);},
R1, R2, 1e-5);
EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
}

9
gtsam/slam/tests/testEssentialMatrixConstraint.cpp
View File

@ -53,12 +53,11 @@ TEST( EssentialMatrixConstraint, test ) {
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector5, Pose3>(
std::bind(&EssentialMatrixConstraint::evaluateError, &factor,
std::placeholders::_1, pose2, boost::none, boost::none),
pose1);
[&factor, &pose2](const Pose3& p1) {return factor.evaluateError(p1, pose2);},
pose1);
Matrix expectedH2 = numericalDerivative11<Vector5, Pose3>(
std::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1,
std::placeholders::_1, boost::none, boost::none),
[&factor, &pose1](const Pose3& p2) {return factor.evaluateError(pose1, p2);},
pose2);
// Use the factor to calculate the derivative

15
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -140,9 +140,9 @@ TEST( OrientedPlane3Factor, Derivatives ) {
OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey);
// Calculate numerical derivatives
std::function<Vector(const Pose3 &, const OrientedPlane3 &)> f = std::bind(
&OrientedPlane3Factor::evaluateError, factor, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
std::function<Vector(const Pose3&, const OrientedPlane3&)> f = [&factor](const Pose3& p, const OrientedPlane3& o) {
return factor.evaluateError(p, o);
};
Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
@ -180,16 +180,13 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, OrientedPlane3>(
std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
boost::none), T1);
[&factor](const OrientedPlane3& o) {return factor.evaluateError(o);}, T1);
Matrix expectedH2 = numericalDerivative11<Vector, OrientedPlane3>(
std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
boost::none), T2);
[&factor](const OrientedPlane3& o) {return factor.evaluateError(o);}, T2);
Matrix expectedH3 = numericalDerivative11<Vector, OrientedPlane3>(
std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
boost::none), T3);
[&factor](const OrientedPlane3& o) { return factor.evaluateError(o); }, T3);
// Use the factor to calculate the derivative
Matrix actualH1, actualH2, actualH3;

14
gtsam/slam/tests/testRotateFactor.cpp
View File

@ -69,14 +69,14 @@ TEST (RotateFactor, test) {
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Vector3,Rot3>(
std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), iRc);
expected = numericalDerivative11<Vector3, Rot3>(
[&f](const Rot3& r) { return f.evaluateError(r); }, iRc);
f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
{
expected = numericalDerivative11<Vector3,Rot3>(
std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), R);
expected = numericalDerivative11<Vector3, Rot3>(
[&f](const Rot3& r) { return f.evaluateError(r); }, R);
f.evaluateError(R, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
@ -141,15 +141,13 @@ TEST (RotateDirectionsFactor, test) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Vector,Rot3>(
std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1,
boost::none), iRc);
[&f](const Rot3& r) {return f.evaluateError(r);}, iRc);
f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
{
expected = numericalDerivative11<Vector,Rot3>(
std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1,
boost::none), R);
[&f](const Rot3& r) {return f.evaluateError(r);}, R);
f.evaluateError(R, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}

6
gtsam/slam/tests/testTriangulationFactor.cpp
View File

@ -59,7 +59,7 @@ TEST( triangulation, TriangulationFactor ) {
factor.evaluateError(landmark, HActual);
Matrix HExpected = numericalDerivative11<Vector,Point3>(
std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark);
[&factor](const Point3& l) { return factor.evaluateError(l);}, landmark);
// Verify the Jacobians are correct
CHECK(assert_equal(HExpected, HActual, 1e-3));
@ -82,8 +82,8 @@ TEST( triangulation, TriangulationFactorStereo ) {
Matrix HActual;
factor.evaluateError(landmark, HActual);
Matrix HExpected = numericalDerivative11<Vector,Point3>(
std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark);
Matrix HExpected = numericalDerivative11<Vector, Point3>(
[&factor](const Point3& l) { return factor.evaluateError(l);}, landmark);
// Verify the Jacobians are correct
CHECK(assert_equal(HExpected, HActual, 1e-3));

26
tests/simulated2D.h
View File

@ -21,6 +21,7 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/geometry/Point2.h>
#include "gtsam/base/OptionalJacobian.h"
// \namespace
@ -89,7 +90,7 @@ namespace simulated2D {
}
/// Prior on a single pose, optionally returns derivative
inline Point2 prior(const Point2& x, boost::optional<Matrix&> H = boost::none) {
inline Point2 prior(const Point2& x, OptionalJacobian<2,2> H = OptionalNone) {
if (H) *H = I_2x2;
return x;
}
@ -100,8 +101,9 @@ namespace simulated2D {
}
/// odometry between two poses, optionally returns derivative
inline Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) {
inline Point2 odo(const Point2& x1, const Point2& x2,
OptionalJacobian<2,2> H1 = OptionalNone,
OptionalJacobian<2,2> H2 = OptionalNone) {
if (H1) *H1 = -I_2x2;
if (H2) *H2 = I_2x2;
return x2 - x1;
@ -113,8 +115,8 @@ namespace simulated2D {
}
/// measurement between landmark and pose, optionally returns derivative
inline Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) {
inline Point2 mea(const Point2& x, const Point2& l, OptionalJacobian<2,2> H1 =
OptionalNone, OptionalMatrixType H2 = OptionalNone) {
if (H1) *H1 = -I_2x2;
if (H2) *H2 = I_2x2;
return l - x;
@ -130,6 +132,8 @@ namespace simulated2D {
typedef GenericPrior<VALUE> This;
typedef boost::shared_ptr<GenericPrior<VALUE> > shared_ptr;
typedef VALUE Pose; ///< shortcut to Pose type
using Base::evaluateError;
Pose measured_; ///< prior mean
@ -139,7 +143,7 @@ namespace simulated2D {
}
/// Return error and optional derivative
Vector evaluateError(const Pose& x, OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const Pose& x, OptionalMatrixType H) const override {
return (prior(x, H) - measured_);
}
@ -175,6 +179,8 @@ namespace simulated2D {
typedef boost::shared_ptr<GenericOdometry<VALUE> > shared_ptr;
typedef VALUE Pose; ///< shortcut to Pose type
using Base::evaluateError;
Pose measured_; ///< odometry measurement
/// Create odometry
@ -184,8 +190,7 @@ namespace simulated2D {
/// Evaluate error and optionally return derivatives
Vector evaluateError(const Pose& x1, const Pose& x2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
return (odo(x1, x2, H1, H2) - measured_);
}
@ -222,6 +227,8 @@ namespace simulated2D {
typedef POSE Pose; ///< shortcut to Pose type
typedef LANDMARK Landmark; ///< shortcut to Landmark type
using Base::evaluateError;
Landmark measured_; ///< Measurement
/// Create measurement factor
@ -231,8 +238,7 @@ namespace simulated2D {
/// Evaluate error and optionally return derivatives
Vector evaluateError(const Pose& x1, const Landmark& x2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
return (mea(x1, x2, H1, H2) - measured_);
}

11
tests/simulated2DOriented.h
View File

@ -22,6 +22,7 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include "gtsam/base/OptionalJacobian.h"
// \namespace
namespace simulated2DOriented {
@ -62,7 +63,7 @@ namespace simulated2DOriented {
}
/// Prior on a single pose, optional derivative version
Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none) {
Pose2 prior(const Pose2& x, OptionalJacobian<3,3> H = OptionalNone) {
if (H) *H = I_3x3;
return x;
}
@ -73,8 +74,8 @@ namespace simulated2DOriented {
}
/// odometry between two poses, optional derivative version
Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) {
Pose2 odo(const Pose2& x1, const Pose2& x2, OptionalJacobian<3,3> H1 =
OptionalNone, OptionalJacobian<3,3> H2 = OptionalNone) {
return x1.between(x2, H1, H2);
}
@ -105,6 +106,7 @@ namespace simulated2DOriented {
Pose2 measured_; ///< Between measurement for odometry factor
typedef GenericOdometry<VALUE> This;
using NoiseModelFactor2<VALUE, VALUE>::evaluateError;
/**
* Creates an odometry factor between two poses
@ -118,8 +120,7 @@ namespace simulated2DOriented {
/// Evaluate error and optionally derivative
Vector evaluateError(const VALUE& x1, const VALUE& x2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
return measured_.localCoordinates(odo(x1, x2, H1, H2));
}

24
tests/simulated3D.h
View File

@ -23,6 +23,7 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include "gtsam/base/OptionalJacobian.h"
// \namespace
@ -38,7 +39,7 @@ namespace simulated3D {
/**
* Prior on a single pose
*/
Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none) {
Point3 prior(const Point3& x, OptionalJacobian<3,3> H = OptionalNone) {
if (H) *H = I_3x3;
return x;
}
@ -47,8 +48,8 @@ Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none) {
* odometry between two poses
*/
Point3 odo(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) {
OptionalJacobian<3,3> H1 = OptionalNone,
OptionalJacobian<3,3> H2 = OptionalNone) {
if (H1) *H1 = -1 * I_3x3;
if (H2) *H2 = I_3x3;
return x2 - x1;
@ -58,8 +59,8 @@ Point3 odo(const Point3& x1, const Point3& x2,
* measurement between landmark and pose
*/
Point3 mea(const Point3& x, const Point3& l,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) {
OptionalJacobian<3,3> H1 = OptionalNone,
OptionalJacobian<3,3> H2 = OptionalNone) {
if (H1) *H1 = -1 * I_3x3;
if (H2) *H2 = I_3x3;
return l - x;
@ -68,7 +69,8 @@ Point3 mea(const Point3& x, const Point3& l,
/**
* A prior factor on a single linear robot pose
*/
struct PointPrior3D: public NoiseModelFactorN<Point3> {
struct PointPrior3D: public NoiseModelFactor1<Point3> {
using NoiseModelFactor1<Point3>::evaluateError;
Point3 measured_; ///< The prior pose value for the variable attached to this factor
@ -89,8 +91,7 @@ struct PointPrior3D: public NoiseModelFactorN<Point3> {
* @param H is an optional Jacobian matrix (Dimension: 3x3)
* @return Vector error between prior value and x (Dimension: 3)
*/
Vector evaluateError(const Point3& x, OptionalMatrixType H =
OptionalNone) const override {
Vector evaluateError(const Point3& x, OptionalMatrixType H) const override {
return prior(x, H) - measured_;
}
};
@ -98,7 +99,12 @@ struct PointPrior3D: public NoiseModelFactorN<Point3> {
/**
* Models a linear 3D measurement between 3D points
*/
<<<<<<< HEAD
struct Simulated3DMeasurement: public NoiseModelFactorN<Point3, Point3> {
=======
struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> {
using NoiseModelFactor2<Point3, Point3>::evaluateError;
>>>>>>> 7b3c40e92 (everything compiles but tests fail in no boost mode)
Point3 measured_; ///< Linear displacement between a pose and landmark
@ -121,7 +127,7 @@ struct Simulated3DMeasurement: public NoiseModelFactorN<Point3, Point3> {
* @return vector error between measurement and prediction (Dimension: 3)
*/
Vector evaluateError(const Point3& x1, const Point3& x2,
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
return mea(x1, x2, H1, H2) - measured_;
}
};

3
tests/smallExample.h
View File

@ -329,13 +329,14 @@ inline Matrix H(const Point2& v) {
struct UnaryFactor: public gtsam::NoiseModelFactorN<Point2> {
using gtsam::NoiseModelFactor1<Point2>::evaluateError;
Point2 z_;
UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) :
gtsam::NoiseModelFactorN<Point2>(model, key), z_(z) {
}
Vector evaluateError(const Point2& x, OptionalMatrixType A = OptionalNone) const override {
Vector evaluateError(const Point2& x, OptionalMatrixType A) const override {
if (A) *A = H(x);
return (h(x) - z_);
}

8
tests/testExtendedKalmanFilter.cpp
View File

@ -101,6 +101,8 @@ protected:
Matrix Q_invsqrt_;
public:
using Base::evaluateError;
NonlinearMotionModel(){}
NonlinearMotionModel(const Symbol& TestKey1, const Symbol& TestKey2) :
@ -213,8 +215,7 @@ public:
/** vector of errors */
Vector evaluateError(const Point2& p1, const Point2& p2,
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 =
OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
// error = p2 - f(p1)
// H1 = d error / d p1 = -d f/ d p1 = -F
@ -245,6 +246,7 @@ protected:
Matrix R_invsqrt_; /** The inv sqrt of the measurement error covariance */
public:
using Base::evaluateError;
NonlinearMeasurementModel(){}
NonlinearMeasurementModel(const Symbol& TestKey, Vector z) :
@ -339,7 +341,7 @@ public:
}
/** vector of errors */
Vector evaluateError(const Point2& p, OptionalMatrixType H1 = OptionalNone) const override {
Vector evaluateError(const Point2& p, OptionalMatrixType H1) const override {
// error = z - h(p)
// H = d error / d p = -d h/ d p = -H
Vector z_hat = h(p);

34
tests/testNonlinearFactor.cpp
View File

@ -337,10 +337,11 @@ class TestFactor1 : public NoiseModelFactor1<double> {
public:
typedef NoiseModelFactor1<double> Base;
using Base::evaluateError;
TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {}
using Base::NoiseModelFactor1; // inherit constructors
Vector evaluateError(const double& x1, OptionalMatrixType H1 = OptionalNone) const override {
Vector evaluateError(const double& x1, OptionalMatrixType H1) const override {
if (H1) *H1 = (Matrix(1, 1) << 1.0).finished();
return (Vector(1) << x1).finished();
}
@ -388,15 +389,14 @@ class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
public:
typedef NoiseModelFactor4<double, double, double, double> Base;
using Base::evaluateError;
TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {}
using Base::NoiseModelFactor4; // inherit constructors
Vector
evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone,
OptionalMatrixType H4 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3, OptionalMatrixType H4) const override {
if(H1) {
*H1 = (Matrix(1, 1) << 1.0).finished();
*H2 = (Matrix(1, 1) << 2.0).finished();
@ -479,15 +479,13 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
class TestFactor5 : public NoiseModelFactor5<double, double, double, double, double> {
public:
typedef NoiseModelFactor5<double, double, double, double, double> Base;
using Base::evaluateError;
TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5)) {}
Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone,
OptionalMatrixType H4 = OptionalNone,
OptionalMatrixType H5 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3,
OptionalMatrixType H4, OptionalMatrixType H5) const override {
if(H1) {
*H1 = (Matrix(1, 1) << 1.0).finished();
*H2 = (Matrix(1, 1) << 2.0).finished();
@ -529,16 +527,13 @@ TEST(NonlinearFactor, NoiseModelFactor5) {
class TestFactor6 : public NoiseModelFactor6<double, double, double, double, double, double> {
public:
typedef NoiseModelFactor6<double, double, double, double, double, double> Base;
using Base::evaluateError;
TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5), X(6)) {}
Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone,
OptionalMatrixType H4 = OptionalNone,
OptionalMatrixType H5 = OptionalNone,
OptionalMatrixType H6 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4,
OptionalMatrixType H5, OptionalMatrixType H6) const override {
if(H1) {
*H1 = (Matrix(1, 1) << 1.0).finished();
*H2 = (Matrix(1, 1) << 2.0).finished();
@ -587,16 +582,15 @@ TEST(NonlinearFactor, NoiseModelFactor6) {
class TestFactorN : public NoiseModelFactorN<double, double, double, double> {
public:
typedef NoiseModelFactorN<double, double, double, double> Base;
using Base::evaluateError;
using Type1 = ValueType<1>; // Test that we can use the ValueType<> template
TestFactorN() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {}
Vector
evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone,
OptionalMatrixType H4 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3, OptionalMatrixType H4) const override {
if (H1) *H1 = (Matrix(1, 1) << 1.0).finished();
if (H2) *H2 = (Matrix(1, 1) << 2.0).finished();
if (H3) *H3 = (Matrix(1, 1) << 3.0).finished();