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

all of gtsam compiles and tests pass with ptrs instead of optional matrix refererences

release/4.3a0
kartik arcot 2023-01-09 14:52:56 -08:00
parent 2dc0dd5979
commit b7073e3224
48 changed files with 253 additions and 300 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -10,7 +10,7 @@ option(GTSAM_NO_BOOST_CPP17 "Require and use boost" ON)
add_definitions(-Wno-deprecated-declarations) add_definitions(-Wno-deprecated-declarations)
add_definitions(-ftemplate-backtrace-limit=0) add_definitions(-ftemplate-backtrace-limit=0)
set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD 17)
if (GTSAM_NO_BOOST_CPP17) if (GTSAM_NO_BOOST_CPP17)
add_definitions(-DNO_BOOST_CPP17) add_definitions(-DNO_BOOST_CPP17)
endif() endif()
@ -99,7 +99,7 @@ add_subdirectory(timing)
# Build gtsam_unstable # Build gtsam_unstable
if (GTSAM_BUILD_UNSTABLE) if (GTSAM_BUILD_UNSTABLE)
# add_subdirectory(gtsam_unstable) add_subdirectory(gtsam_unstable)
endif() endif()
# This is the new wrapper # This is the new wrapper

5
gtsam/slam/ProjectionFactor.h
View File

@ -52,7 +52,8 @@ namespace gtsam {
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactorN<POSE, LANDMARK> Base; typedef NoiseModelFactor2<POSE, LANDMARK> Base;
using Base::evaluateError;
/// shorthand for this class /// shorthand for this class
typedef GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> This; typedef GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
@ -60,8 +61,6 @@ namespace gtsam {
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
using Base::evaluateError;
/// Default constructor /// Default constructor
GenericProjectionFactor() : GenericProjectionFactor() :
measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) { measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {

7
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -34,6 +34,7 @@ protected:
double dt_; /// time between measurements double dt_; /// time between measurements
public: public:
using Base::evaluateError;
/** Standard constructor */ /** Standard constructor */
FullIMUFactor(const Vector3& accel, const Vector3& gyro, FullIMUFactor(const Vector3& accel, const Vector3& gyro,
@ -84,8 +85,7 @@ public:
* z - h(x1,x2) * z - h(x1,x2)
*/ */
Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2) const override {
boost::optional<Matrix&> H2 = boost::none) const override {
Vector9 z; Vector9 z;
z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang
z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
@ -99,8 +99,7 @@ public:
/** dummy version that fails for non-dynamic poses */ /** dummy version that fails for non-dynamic poses */
virtual Vector evaluateError(const Pose3& x1, const Pose3& x2, virtual Vector evaluateError(const Pose3& x1, const Pose3& x2,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2) const {
boost::optional<Matrix&> H2 = boost::none) const {
assert(false); assert(false);
return Vector6::Zero(); return Vector6::Zero();
} }

7
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -32,6 +32,7 @@ protected:
double dt_; /// time between measurements double dt_; /// time between measurements
public: public:
using Base::evaluateError;
/** Standard constructor */ /** Standard constructor */
IMUFactor(const Vector3& accel, const Vector3& gyro, IMUFactor(const Vector3& accel, const Vector3& gyro,
@ -77,8 +78,7 @@ public:
* z - h(x1,x2) * z - h(x1,x2)
*/ */
Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2) const override {
boost::optional<Matrix&> H2 = boost::none) const override {
const Vector6 meas = z(); const Vector6 meas = z();
if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>( if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_, meas), x1, x2, 1e-5); std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_, meas), x1, x2, 1e-5);
@ -89,8 +89,7 @@ public:
/** dummy version that fails for non-dynamic poses */ /** dummy version that fails for non-dynamic poses */
virtual Vector evaluateError(const Pose3& x1, const Pose3& x2, virtual Vector evaluateError(const Pose3& x1, const Pose3& x2,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2) const {
boost::optional<Matrix&> H2 = boost::none) const {
assert(false); // no corresponding factor here assert(false); // no corresponding factor here
return Vector6::Zero(); return Vector6::Zero();
} }

26
gtsam_unstable/dynamics/Pendulum.h
View File

@ -32,6 +32,7 @@ protected:
double h_; // time step double h_; // time step
public: public:
using Base::evaluateError;
typedef boost::shared_ptr<PendulumFactor1> shared_ptr; typedef boost::shared_ptr<PendulumFactor1> shared_ptr;
@ -46,9 +47,8 @@ public:
/** q_k + h*v - q_k1 = 0, with optional derivatives */ /** q_k + h*v - q_k1 = 0, with optional derivatives */
Vector evaluateError(const double& qk1, const double& qk, const double& v, Vector evaluateError(const double& qk1, const double& qk, const double& v,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2,
boost::optional<Matrix&> H2 = boost::none, OptionalMatrixType H3) const override {
boost::optional<Matrix&> H3 = boost::none) const override {
const size_t p = 1; const size_t p = 1;
if (H1) *H1 = -Matrix::Identity(p,p); if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Identity(p,p); if (H2) *H2 = Matrix::Identity(p,p);
@ -81,6 +81,8 @@ protected:
public: public:
using Base::evaluateError;
typedef boost::shared_ptr<PendulumFactor2 > shared_ptr; typedef boost::shared_ptr<PendulumFactor2 > shared_ptr;
///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step ///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step
@ -94,9 +96,8 @@ public:
/** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */ /** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */
Vector evaluateError(const double & vk1, const double & vk, const double & q, Vector evaluateError(const double & vk1, const double & vk, const double & q,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2,
boost::optional<Matrix&> H2 = boost::none, OptionalMatrixType H3) const override {
boost::optional<Matrix&> H3 = boost::none) const override {
const size_t p = 1; const size_t p = 1;
if (H1) *H1 = -Matrix::Identity(p,p); if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Identity(p,p); if (H2) *H2 = Matrix::Identity(p,p);
@ -130,6 +131,8 @@ protected:
public: public:
using Base::evaluateError;
typedef boost::shared_ptr<PendulumFactorPk > shared_ptr; typedef boost::shared_ptr<PendulumFactorPk > shared_ptr;
///Constructor ///Constructor
@ -145,9 +148,8 @@ public:
/** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */ /** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */
Vector evaluateError(const double & pk, const double & qk, const double & qk1, Vector evaluateError(const double & pk, const double & qk, const double & qk1,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2,
boost::optional<Matrix&> H2 = boost::none, OptionalMatrixType H3) const override {
boost::optional<Matrix&> H3 = boost::none) const override {
const size_t p = 1; const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1; double qmid = (1-alpha_)*qk + alpha_*qk1;
@ -185,6 +187,7 @@ protected:
double alpha_; //! in [0,1], define the mid-point between [q_k,q_{k+1}] for approximation. The sympletic rule above can be obtained as a special case when alpha = 0. double alpha_; //! in [0,1], define the mid-point between [q_k,q_{k+1}] for approximation. The sympletic rule above can be obtained as a special case when alpha = 0.
public: public:
using Base::evaluateError;
typedef boost::shared_ptr<PendulumFactorPk1 > shared_ptr; typedef boost::shared_ptr<PendulumFactorPk1 > shared_ptr;
@ -201,9 +204,8 @@ public:
/** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */ /** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */
Vector evaluateError(const double & pk1, const double & qk, const double & qk1, Vector evaluateError(const double & pk1, const double & qk, const double & qk1,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2,
boost::optional<Matrix&> H2 = boost::none, OptionalMatrixType H3) const override {
boost::optional<Matrix&> H3 = boost::none) const override {
const size_t p = 1; const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1; double qmid = (1-alpha_)*qk + alpha_*qk1;

18
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -29,6 +29,7 @@ class Reconstruction : public NoiseModelFactorN<Pose3, Pose3, Vector6> {
double h_; // time step double h_; // time step
typedef NoiseModelFactorN<Pose3, Pose3, Vector6> Base; typedef NoiseModelFactorN<Pose3, Pose3, Vector6> Base;
public: public:
using Base::evaluateError;
Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) : Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) :
Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey, Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey,
xiKey), h_(h) { xiKey), h_(h) {
@ -42,9 +43,8 @@ public:
/** \f$ log((g_k\exp(h\xi_k))^{-1}g_{k+1}) = 0 \f$, with optional derivatives */ /** \f$ log((g_k\exp(h\xi_k))^{-1}g_{k+1}) = 0 \f$, with optional derivatives */
Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik, Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2,
boost::optional<Matrix&> H2 = boost::none, OptionalMatrixType H3) const override {
boost::optional<Matrix&> H3 = boost::none) const override {
Matrix6 D_exphxi_xi; Matrix6 D_exphxi_xi;
Pose3 exphxi = Pose3::Expmap(h_ * xik, H3 ? &D_exphxi_xi : 0); Pose3 exphxi = Pose3::Expmap(h_ * xik, H3 ? &D_exphxi_xi : 0);
@ -89,6 +89,8 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactorN<Vector6, Vector
typedef NoiseModelFactorN<Vector6, Vector6, Pose3> Base; typedef NoiseModelFactorN<Vector6, Vector6, Pose3> Base;
public: public:
using Base::evaluateError;
DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,
double h, const Matrix& Inertia, const Vector& Fu, double m, double h, const Matrix& Inertia, const Vector& Fu, double m,
double mu = 1000.0) : double mu = 1000.0) :
@ -108,9 +110,8 @@ public:
* pk_1 = CT_TLN(-h*xi_k_1)*Inertia*xi_k_1 * pk_1 = CT_TLN(-h*xi_k_1)*Inertia*xi_k_1
* */ * */
Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk, Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2,
boost::optional<Matrix&> H2 = boost::none, OptionalMatrixType H3) const override {
boost::optional<Matrix&> H3 = boost::none) const override {
Vector muk = Inertia_*xik; Vector muk = Inertia_*xik;
Vector muk_1 = Inertia_*xik_1; Vector muk_1 = Inertia_*xik_1;
@ -161,9 +162,8 @@ public:
} }
Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk, Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2,
boost::optional<Matrix&> H2 = boost::none, OptionalMatrixType H3) const {
boost::optional<Matrix&> H3 = boost::none) const {
if (H1) { if (H1) {
(*H1) = numericalDerivative31( (*H1) = numericalDerivative31(
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>( std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(

6
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -34,7 +34,8 @@ typedef enum {
*/ */
class VelocityConstraint : public gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> { class VelocityConstraint : public gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> {
public: public:
typedef gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> Base; typedef gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> Base;
using Base::evaluateError;
protected: protected:
@ -83,8 +84,7 @@ public:
* Calculates the error for trapezoidal model given * Calculates the error for trapezoidal model given
*/ */
gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<gtsam::Matrix&> H1=boost::none, OptionalMatrixType H1, OptionalMatrixType H2) const override {
boost::optional<gtsam::Matrix&> H2=boost::none) const override {
if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>( if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>(
std::bind(VelocityConstraint::evaluateError_, std::placeholders::_1, std::bind(VelocityConstraint::evaluateError_, std::placeholders::_1,
std::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); std::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5);

6
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

@ -23,6 +23,7 @@ protected:
public: public:
using Base::evaluateError;
typedef boost::shared_ptr<VelocityConstraint3 > shared_ptr; typedef boost::shared_ptr<VelocityConstraint3 > shared_ptr;
///TODO: comment ///TODO: comment
@ -37,9 +38,8 @@ public:
/** x1 + v*dt - x2 = 0, with optional derivatives */ /** x1 + v*dt - x2 = 0, with optional derivatives */
Vector evaluateError(const double& x1, const double& x2, const double& v, Vector evaluateError(const double& x1, const double& x2, const double& v,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2,
boost::optional<Matrix&> H2 = boost::none, OptionalMatrixType H3) const override {
boost::optional<Matrix&> H3 = boost::none) const override {
const size_t p = 1; const size_t p = 1;
if (H1) *H1 = Matrix::Identity(p,p); if (H1) *H1 = Matrix::Identity(p,p);
if (H2) *H2 = -Matrix::Identity(p,p); if (H2) *H2 = -Matrix::Identity(p,p);

56
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -7,6 +7,8 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/dynamics/SimpleHelicopter.h> #include <gtsam_unstable/dynamics/SimpleHelicopter.h>
#include "gtsam/base/Vector.h"
#include "gtsam/geometry/Pose3.h"
/* ************************************************************************* */ /* ************************************************************************* */
using namespace std::placeholders; using namespace std::placeholders;
@ -56,29 +58,15 @@ TEST( Reconstruction, evaluateError) {
EXPECT( EXPECT(
assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol));
Matrix numericalH1 = numericalDerivative31( std::function<Vector(const Pose3&, const Pose3&, const Vector6&)> f = [&constraint](const Pose3& a1, const Pose3& a2,
std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>( const Vector6& a3) {
std::bind(&Reconstruction::evaluateError, constraint, return constraint.evaluateError(a1, a2, a3);
std::placeholders::_1, std::placeholders::_2, };
std::placeholders::_3, boost::none, boost::none, Matrix numericalH1 = numericalDerivative31(f, g2, g1, V1_g1, 1e-5);
boost::none)),
g2, g1, V1_g1, 1e-5);
Matrix numericalH2 = numericalDerivative32( Matrix numericalH2 = numericalDerivative32(f, g2, g1, V1_g1, 1e-5);
std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
std::bind(&Reconstruction::evaluateError, constraint,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
g2, g1, V1_g1, 1e-5);
Matrix numericalH3 = numericalDerivative33( Matrix numericalH3 = numericalDerivative33(f, g2, g1, V1_g1, 1e-5);
std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
std::bind(&Reconstruction::evaluateError, constraint,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
g2, g1, V1_g1, 1e-5);
EXPECT(assert_equal(numericalH1,H1,1e-5)); EXPECT(assert_equal(numericalH1,H1,1e-5));
EXPECT(assert_equal(numericalH2,H2,1e-5)); EXPECT(assert_equal(numericalH2,H2,1e-5));
@ -118,26 +106,16 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
Matrix H1, H2, H3; Matrix H1, H2, H3;
EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0));
Matrix numericalH1 = numericalDerivative31( std::function<Vector(const Vector6&, const Vector6&, const Pose3&)> f =
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>( [&constraint](const Vector6& a1, const Vector6& a2, const Pose3& a3) {
std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) return constraint.evaluateError(a1, a2, a3);
), };
expectedv2, V1_g1, g2, 1e-5
);
Matrix numericalH2 = numericalDerivative32( Matrix numericalH1 = numericalDerivative31(f, expectedv2, V1_g1, g2, 1e-5);
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none)
),
expectedv2, V1_g1, g2, 1e-5
);
Matrix numericalH3 = numericalDerivative33( Matrix numericalH2 = numericalDerivative32(f, expectedv2, V1_g1, g2, 1e-5);
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) Matrix numericalH3 = numericalDerivative33(f, expectedv2, V1_g1, g2, 1e-5);
),
expectedv2, V1_g1, g2, 1e-5
);
EXPECT(assert_equal(numericalH1,H1,1e-5)); EXPECT(assert_equal(numericalH1,H1,1e-5));
EXPECT(assert_equal(numericalH2,H2,1e-5)); EXPECT(assert_equal(numericalH2,H2,1e-5));

7
gtsam_unstable/geometry/InvDepthCamera3.h
View File

@ -19,6 +19,7 @@
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam { namespace gtsam {
@ -83,9 +84,9 @@ public:
*/ */
inline gtsam::Point2 project(const Vector5& pw, inline gtsam::Point2 project(const Vector5& pw,
double rho, double rho,
boost::optional<gtsam::Matrix&> H1 = boost::none, OptionalJacobian<2,6> H1 = {},
boost::optional<gtsam::Matrix&> H2 = boost::none, OptionalJacobian<2,5> H2 = {},
boost::optional<gtsam::Matrix&> H3 = boost::none) const { OptionalJacobian<2,1> H3 = {}) const {
gtsam::Point3 ray_base(pw.segment(0,3)); gtsam::Point3 ray_base(pw.segment(0,3));
double theta = pw(3), phi = pw(4); double theta = pw(3), phi = pw(4);

6
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
View File

@ -94,7 +94,7 @@ TEST( InvDepthFactor, Dproject_pose)
Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual; Matrix actual;
inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none); inv_camera.project(landmark, inv_depth, actual, {}, {});
EXPECT(assert_equal(expected,actual,1e-6)); EXPECT(assert_equal(expected,actual,1e-6));
} }
@ -106,7 +106,7 @@ TEST( InvDepthFactor, Dproject_landmark)
Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual; Matrix actual;
inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none); inv_camera.project(landmark, inv_depth, {}, actual, {});
EXPECT(assert_equal(expected,actual,1e-7)); EXPECT(assert_equal(expected,actual,1e-7));
} }
@ -118,7 +118,7 @@ TEST( InvDepthFactor, Dproject_inv_depth)
Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual; Matrix actual;
inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual); inv_camera.project(landmark, inv_depth, {}, {}, actual);
EXPECT(assert_equal(expected,actual,1e-7)); EXPECT(assert_equal(expected,actual,1e-7));
} }

8
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -145,7 +145,7 @@ public:
/* ************************************************************************* */ /* ************************************************************************* */
Vector whitenedError(const Values& x, Vector whitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const { OptionalMatrixVecType H = OptionalMatrixVecNone) const {
bool debug = true; bool debug = true;
@ -228,6 +228,12 @@ public:
return err_wh_eq; return err_wh_eq;
} }
#ifdef NO_BOOST_CPP17
Vector whitenedError(const Values& x, std::vector<Matrix>& H) const {
return whitenedError(x, &H);
}
#endif
/* ************************************************************************* */ /* ************************************************************************* */
Vector calcIndicatorProb(const Values& x) const { Vector calcIndicatorProb(const Values& x) const {

4
gtsam_unstable/slam/BiasedGPSFactor.h
View File

@ -37,6 +37,7 @@ namespace gtsam {
Point3 measured_; /** The measurement */ Point3 measured_; /** The measurement */
public: public:
using Base::evaluateError;
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef boost::shared_ptr<BiasedGPSFactor> shared_ptr; typedef boost::shared_ptr<BiasedGPSFactor> shared_ptr;
@ -73,8 +74,7 @@ namespace gtsam {
/** vector of errors */ /** vector of errors */
Vector evaluateError(const Pose3& pose, const Point3& bias, Vector evaluateError(const Pose3& pose, const Point3& bias,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = OptionalMatrixType H1, OptionalMatrixType H2) const override {
boost::none) const override {
if (H1 || H2){ if (H1 || H2){
H1->resize(3,6); // jacobian wrt pose H1->resize(3,6); // jacobian wrt pose

9
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -111,6 +111,8 @@ private:
public: public:
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;
@ -299,11 +301,8 @@ public:
} }
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4,
boost::optional<Matrix&> H2 = boost::none, OptionalMatrixType H5) const override {
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const override {
// TODO: Write analytical derivative calculations // TODO: Write analytical derivative calculations
// Jacobian w.r.t. Pose1 // Jacobian w.r.t. Pose1

7
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -109,6 +109,7 @@ private:
public: public:
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;
@ -270,10 +271,8 @@ public:
} }
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2, Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3,
boost::optional<Matrix&> H2 = boost::none, OptionalMatrixType H4) const {
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none) const {
// TODO: Write analytical derivative calculations // TODO: Write analytical derivative calculations
// Jacobian w.r.t. Pose1 // Jacobian w.r.t. Pose1

4
gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
View File

@ -53,6 +53,7 @@ private:
Vector tau_; Vector tau_;
public: public:
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;
@ -88,8 +89,7 @@ public:
/** vector of errors */ /** vector of errors */
Vector evaluateError(const VALUE& p1, const VALUE& p2, Vector evaluateError(const VALUE& p1, const VALUE& p2,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2) const override {
boost::optional<Matrix&> H2 = boost::none) const override {
Vector v1( traits<VALUE>::Logmap(p1) ); Vector v1( traits<VALUE>::Logmap(p1) );
Vector v2( traits<VALUE>::Logmap(p2) ); Vector v2( traits<VALUE>::Logmap(p2) );

8
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -95,6 +95,7 @@ private:
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
public: public:
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;
@ -226,11 +227,8 @@ public:
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4,
boost::optional<Matrix&> H2 = boost::none, OptionalMatrixType H5) const override {
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const override {
// TODO: Write analytical derivative calculations // TODO: Write analytical derivative calculations
// Jacobian w.r.t. Pose1 // Jacobian w.r.t. Pose1

7
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -34,7 +34,8 @@ protected:
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactorN<POSE, LANDMARK, INVDEPTH> Base; typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
using Base::evaluateError;
/// shorthand for this class /// shorthand for this class
typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This; typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
@ -83,9 +84,7 @@ public:
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth, Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
boost::optional<Matrix&> H1=boost::none, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const override {
try { try {
InvDepthCamera3<Cal3_S2> camera(pose, K_); InvDepthCamera3<Cal3_S2> camera(pose, K_);
return camera.project(point, invDepth, H1, H2, H3) - measured_; return camera.project(point, invDepth, H1, H2, H3) - measured_;

6
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -34,7 +34,8 @@ protected:
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactorN<Pose3, Vector6> Base; typedef NoiseModelFactor2<Pose3, Vector6> Base;
using Base::evaluateError;
/// shorthand for this class /// shorthand for this class
typedef InvDepthFactorVariant1 This; typedef InvDepthFactorVariant1 This;
@ -103,8 +104,7 @@ public:
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Vector6& landmark, Vector evaluateError(const Pose3& pose, const Vector6& landmark,
boost::optional<Matrix&> H1=boost::none, OptionalMatrixType H1, OptionalMatrixType H2) const override {
boost::optional<Matrix&> H2=boost::none) const override {
if (H1) { if (H1) {
(*H1) = numericalDerivative11<Vector, Pose3>( (*H1) = numericalDerivative11<Vector, Pose3>(

6
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -36,7 +36,8 @@ protected:
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactorN<Pose3, Vector3> Base; typedef NoiseModelFactor2<Pose3, Vector3> Base;
using Base::evaluateError;
/// shorthand for this class /// shorthand for this class
typedef InvDepthFactorVariant2 This; typedef InvDepthFactorVariant2 This;
@ -106,8 +107,7 @@ public:
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Vector3& landmark, Vector evaluateError(const Pose3& pose, const Vector3& landmark,
boost::optional<Matrix&> H1=boost::none, OptionalMatrixType H1, OptionalMatrixType H2) const override {
boost::optional<Matrix&> H2=boost::none) const override {
if (H1) { if (H1) {
(*H1) = numericalDerivative11<Vector, Pose3>( (*H1) = numericalDerivative11<Vector, Pose3>(

10
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -34,7 +34,8 @@ protected:
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactorN<Pose3, Vector3> Base; typedef NoiseModelFactor2<Pose3, Vector3> Base;
using Base::evaluateError;
/// shorthand for this class /// shorthand for this class
typedef InvDepthFactorVariant3a This; typedef InvDepthFactorVariant3a This;
@ -106,8 +107,7 @@ public:
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Vector3& landmark, Vector evaluateError(const Pose3& pose, const Vector3& landmark,
boost::optional<Matrix&> H1=boost::none, OptionalMatrixType H1, OptionalMatrixType H2) const override {
boost::optional<Matrix&> H2=boost::none) const override {
if(H1) { if(H1) {
(*H1) = numericalDerivative11<Vector, Pose3>( (*H1) = numericalDerivative11<Vector, Pose3>(
@ -232,9 +232,7 @@ public:
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark, Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark,
boost::optional<Matrix&> H1=boost::none, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const override {
if(H1) if(H1)
(*H1) = numericalDerivative11<Vector, Pose3>( (*H1) = numericalDerivative11<Vector, Pose3>(

4
gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp
View File

@ -24,8 +24,8 @@ void LocalOrientedPlane3Factor::print(const string& s,
//*************************************************************************** //***************************************************************************
Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi, Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi,
const Pose3& wTwa, const OrientedPlane3& a_plane, const Pose3& wTwa, const OrientedPlane3& a_plane,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, OptionalMatrixType H1, OptionalMatrixType H2,
boost::optional<Matrix&> H3) const { OptionalMatrixType H3) const {
Matrix66 aTai_H_wTwa, aTai_H_wTwi; Matrix66 aTai_H_wTwa, aTai_H_wTwi;
Matrix36 predicted_H_aTai; Matrix36 predicted_H_aTai;

7
gtsam_unstable/slam/LocalOrientedPlane3Factor.h
View File

@ -40,6 +40,7 @@ class GTSAM_UNSTABLE_EXPORT LocalOrientedPlane3Factor
OrientedPlane3 measured_p_; OrientedPlane3 measured_p_;
typedef NoiseModelFactorN<Pose3, Pose3, OrientedPlane3> Base; typedef NoiseModelFactorN<Pose3, Pose3, OrientedPlane3> Base;
public: public:
using Base::evaluateError;
/// Constructor /// Constructor
LocalOrientedPlane3Factor() {} LocalOrientedPlane3Factor() {}
@ -84,10 +85,8 @@ public:
* world frame. * world frame.
*/ */
Vector evaluateError(const Pose3& wTwi, const Pose3& wTwa, Vector evaluateError(const Pose3& wTwi, const Pose3& wTwa,
const OrientedPlane3& a_plane, const OrientedPlane3& a_plane, OptionalMatrixType H1,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H2, OptionalMatrixType H3) const override;
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override;
}; };
} // namespace gtsam } // namespace gtsam

3
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -23,6 +23,7 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <boost/optional.hpp> #include <boost/optional.hpp>
#include "gtsam/geometry/Cal3_S2.h"
namespace gtsam { namespace gtsam {
@ -164,7 +165,7 @@ namespace gtsam {
Vector evaluateError(const Pose3& pose, const Point3& point, Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const { OptionalJacobian<2, 6> H1 = {}, OptionalJacobian<2,3> H2 = {}) const {
try { try {
if(body_P_sensor_) { if(body_P_sensor_) {
if(H1) { if(H1) {

3
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -61,6 +61,7 @@ namespace gtsam {
: Base(model, key) {} : Base(model, key) {}
public: public:
using Base::evaluateError;
~PartialPriorFactor() override {} ~PartialPriorFactor() override {}
@ -106,7 +107,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** Returns a vector of errors for the measured tangent parameters. */ /** Returns a vector of errors for the measured tangent parameters. */
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const override { Vector evaluateError(const T& p, OptionalMatrixType H) const override {
Eigen::Matrix<double, T::dimension, T::dimension> H_local; Eigen::Matrix<double, T::dimension, T::dimension> H_local;
// If the Rot3 Cayley map is used, Rot3::LocalCoordinates will throw a runtime error // If the Rot3 Cayley map is used, Rot3::LocalCoordinates will throw a runtime error

4
gtsam_unstable/slam/PoseBetweenFactor.h
View File

@ -43,6 +43,7 @@ namespace gtsam {
GTSAM_CONCEPT_TESTABLE_TYPE(POSE) GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
GTSAM_CONCEPT_POSE_TYPE(POSE) GTSAM_CONCEPT_POSE_TYPE(POSE)
public: public:
using Base::evaluateError;
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<PoseBetweenFactor> shared_ptr; typedef typename boost::shared_ptr<PoseBetweenFactor> shared_ptr;
@ -89,8 +90,7 @@ namespace gtsam {
/** vector of errors */ /** vector of errors */
Vector evaluateError(const POSE& p1, const POSE& p2, Vector evaluateError(const POSE& p1, const POSE& p2,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2) const override {
boost::optional<Matrix&> H2 = boost::none) const override {
if(body_P_sensor_) { if(body_P_sensor_) {
POSE hx; POSE hx;
if(H1 || H2) { if(H1 || H2) {

3
gtsam_unstable/slam/PosePriorFactor.h
View File

@ -40,6 +40,7 @@ namespace gtsam {
GTSAM_CONCEPT_TESTABLE_TYPE(POSE) GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
GTSAM_CONCEPT_POSE_TYPE(POSE) GTSAM_CONCEPT_POSE_TYPE(POSE)
public: public:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<PosePriorFactor<POSE> > shared_ptr; typedef typename boost::shared_ptr<PosePriorFactor<POSE> > shared_ptr;
@ -83,7 +84,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
Vector evaluateError(const POSE& p, boost::optional<Matrix&> H = boost::none) const override { Vector evaluateError(const POSE& p, OptionalMatrixType H) const override {
if(body_P_sensor_) { if(body_P_sensor_) {
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return prior_.localCoordinates(p.compose(*body_P_sensor_, H)); return prior_.localCoordinates(p.compose(*body_P_sensor_, H));

6
gtsam_unstable/slam/PoseToPointFactor.h
View File

@ -29,6 +29,8 @@ class PoseToPointFactor : public NoiseModelFactorN<POSE, POINT> {
POINT measured_; /** the point measurement in local coordinates */ POINT measured_; /** the point measurement in local coordinates */
public: public:
using Base::evaluateError;
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef boost::shared_ptr<PoseToPointFactor> shared_ptr; typedef boost::shared_ptr<PoseToPointFactor> shared_ptr;
@ -79,8 +81,8 @@ class PoseToPointFactor : public NoiseModelFactorN<POSE, POINT> {
*/ */
Vector evaluateError( Vector evaluateError(
const POSE& w_T_b, const POINT& w_P, const POSE& w_T_b, const POINT& w_P,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1,
boost::optional<Matrix&> H2 = boost::none) const override { OptionalMatrixType H2) const override {
return w_T_b.transformTo(w_P, H1, H2) - measured_; return w_T_b.transformTo(w_P, H1, H2) - measured_;
} }

7
gtsam_unstable/slam/ProjectionFactorPPP.h
View File

@ -45,7 +45,8 @@ namespace gtsam {
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactorN<POSE, POSE, LANDMARK> Base; typedef NoiseModelFactor3<POSE, POSE, LANDMARK> Base;
using Base::evaluateError;
/// shorthand for this class /// shorthand for this class
typedef ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> This; typedef ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> This;
@ -122,9 +123,7 @@ namespace gtsam {
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point, Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override {
try { try {
if(H1 || H2 || H3) { if(H1 || H2 || H3) {
Matrix H0, H02; Matrix H0, H02;

9
gtsam_unstable/slam/ProjectionFactorPPPC.h
View File

@ -44,7 +44,8 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactorN<POSE, POSE, LANDMARK, CALIBRATION> Base; typedef NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> Base;
using Base::evaluateError;
/// shorthand for this class /// shorthand for this class
typedef ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> This; typedef ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> This;
@ -108,10 +109,8 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point, const CALIBRATION& K, Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point, const CALIBRATION& K,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3,
boost::optional<Matrix&> H2 = boost::none, OptionalMatrixType H4) const override {
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none) const override {
try { try {
if(H1 || H2 || H3 || H4) { if(H1 || H2 || H3 || H4) {
Matrix H0, H02; Matrix H0, H02;

4
gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp
View File

@ -21,8 +21,8 @@ namespace gtsam {
Vector ProjectionFactorRollingShutter::evaluateError( Vector ProjectionFactorRollingShutter::evaluateError(
const Pose3& pose_a, const Pose3& pose_b, const Point3& point, const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, OptionalMatrixType H1, OptionalMatrixType H2,
boost::optional<Matrix&> H3) const { OptionalMatrixType H3) const {
try { try {
Pose3 pose = interpolate<Pose3>(pose_a, pose_b, alpha_, H1, H2); Pose3 pose = interpolate<Pose3>(pose_a, pose_b, alpha_, H1, H2);
gtsam::Matrix Hprj; gtsam::Matrix Hprj;

7
gtsam_unstable/slam/ProjectionFactorRollingShutter.h
View File

@ -60,7 +60,8 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactorN<Pose3, Pose3, Point3> Base; typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;
using Base::evaluateError;
/// shorthand for this class /// shorthand for this class
typedef ProjectionFactorRollingShutter This; typedef ProjectionFactorRollingShutter This;
@ -173,9 +174,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError( Vector evaluateError(
const Pose3& pose_a, const Pose3& pose_b, const Point3& point, const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
boost::optional<Matrix&> H1 = boost::none, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override;
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override;
/** return the measurement */ /** return the measurement */
const Point2& measured() const { return measured_; } const Point2& measured() const { return measured_; }

2
gtsam_unstable/slam/RelativeElevationFactor.cpp
View File

@ -18,7 +18,7 @@ RelativeElevationFactor::RelativeElevationFactor(Key poseKey, Key pointKey, doub
/* ************************************************************************* */ /* ************************************************************************* */
Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& point, Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { OptionalMatrixType H1, OptionalMatrixType H2) const {
double hx = pose.z() - point.z(); double hx = pose.z() - point.z();
if (H1) { if (H1) {
*H1 = Matrix::Zero(1,6); *H1 = Matrix::Zero(1,6);

3
gtsam_unstable/slam/RelativeElevationFactor.h
View File

@ -34,6 +34,7 @@ private:
typedef NoiseModelFactorN<Pose3, Point3> Base; typedef NoiseModelFactorN<Pose3, Point3> Base;
public: public:
using Base::evaluateError;
RelativeElevationFactor() : measured_(0.0) {} /* Default constructor */ RelativeElevationFactor() : measured_(0.0) {} /* Default constructor */
@ -49,7 +50,7 @@ public:
/** h(x)-z */ /** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point, Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override; OptionalMatrixType H1, OptionalMatrixType H2) const override;
/** return the measured */ /** return the measured */
inline double measured() const { return measured_; } inline double measured() const { return measured_; }

4
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -42,6 +42,7 @@ class SmartRangeFactor: public NoiseModelFactor {
double variance_; ///< variance on noise double variance_; ///< variance on noise
public: public:
using NoiseModelFactor::unwhitenedError;
/** Default constructor: don't use directly */ /** Default constructor: don't use directly */
SmartRangeFactor() { SmartRangeFactor() {
} }
@ -143,8 +144,7 @@ class SmartRangeFactor: public NoiseModelFactor {
/** /**
* Error function *without* the NoiseModel, \f$ z-h(x) \f$. * Error function *without* the NoiseModel, \f$ z-h(x) \f$.
*/ */
Vector unwhitenedError(const Values& x, Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
boost::optional<std::vector<Matrix>&> H = boost::none) const override {
size_t n = size(); size_t n = size();
if (n < 3) { if (n < 3) {
if (H) { if (H) {

21
gtsam_unstable/slam/TSAMFactors.h
View File

@ -37,6 +37,7 @@ private:
Point2 measured_; ///< the measurement Point2 measured_; ///< the measurement
public: public:
using Base::evaluateError;
/// Constructor /// Constructor
DeltaFactor(Key i, Key j, const Point2& measured, DeltaFactor(Key i, Key j, const Point2& measured,
@ -46,8 +47,7 @@ public:
/// Evaluate measurement error h(x)-z /// Evaluate measurement error h(x)-z
Vector evaluateError(const Pose2& pose, const Point2& point, Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = OptionalMatrixType H1, OptionalMatrixType H2) const override {
boost::none) const override {
return pose.transformTo(point, H1, H2) - measured_; return pose.transformTo(point, H1, H2) - measured_;
} }
}; };
@ -66,7 +66,7 @@ private:
Point2 measured_; ///< the measurement Point2 measured_; ///< the measurement
public: public:
using Base::evaluateError;
/// Constructor /// Constructor
DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2& measured, DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2& measured,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
@ -76,10 +76,8 @@ public:
/// Evaluate measurement error h(x)-z /// Evaluate measurement error h(x)-z
Vector evaluateError(const Pose2& base1, const Pose2& pose, Vector evaluateError(const Pose2& base1, const Pose2& pose,
const Pose2& base2, const Point2& point, // const Pose2& base2, const Point2& point, //
boost::optional<Matrix&> H1 = boost::none, // OptionalMatrixType H1, OptionalMatrixType H2, //
boost::optional<Matrix&> H2 = boost::none, // OptionalMatrixType H3, OptionalMatrixType H4) const override {
boost::optional<Matrix&> H3 = boost::none, //
boost::optional<Matrix&> H4 = boost::none) const override {
if (H1 || H2 || H3 || H4) { if (H1 || H2 || H3 || H4) {
// TODO use fixed-size matrices // TODO use fixed-size matrices
Matrix D_pose_g_base1, D_pose_g_pose; Matrix D_pose_g_base1, D_pose_g_pose;
@ -121,6 +119,7 @@ private:
Pose2 measured_; ///< the measurement Pose2 measured_; ///< the measurement
public: public:
using Base::evaluateError;
/// Constructor /// Constructor
OdometryFactorBase(Key b1, Key i, Key b2, Key j, const Pose2& measured, OdometryFactorBase(Key b1, Key i, Key b2, Key j, const Pose2& measured,
@ -130,11 +129,9 @@ public:
/// Evaluate measurement error h(x)-z /// Evaluate measurement error h(x)-z
Vector evaluateError(const Pose2& base1, const Pose2& pose1, Vector evaluateError(const Pose2& base1, const Pose2& pose1,
const Pose2& base2, const Pose2& pose2, // const Pose2& base2, const Pose2& pose2,
boost::optional<Matrix&> H1 = boost::none, // OptionalMatrixType H1, OptionalMatrixType H2,
boost::optional<Matrix&> H2 = boost::none, // OptionalMatrixType H3, OptionalMatrixType H4) const override {
boost::optional<Matrix&> H3 = boost::none, //
boost::optional<Matrix&> H4 = boost::none) const override {
if (H1 || H2 || H3 || H4) { if (H1 || H2 || H3 || H4) {
// TODO use fixed-size matrices // TODO use fixed-size matrices
Matrix D_pose1_g_base1, D_pose1_g_pose1; Matrix D_pose1_g_base1, D_pose1_g_pose1;

10
gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
View File

@ -156,8 +156,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
gtsam::Vector whitenedError(const gtsam::Values& x, gtsam::Vector whitenedError(const gtsam::Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const {
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
T orgA_T_currA = valA_.at<T>(keyA_); T orgA_T_currA = valA_.at<T>(keyA_);
T orgB_T_currB = valB_.at<T>(keyB_); T orgB_T_currB = valB_.at<T>(keyB_);
@ -186,6 +185,13 @@ namespace gtsam {
return error; return error;
} }
/* ************************************************************************* */
#ifdef NO_BOOST_CPP17
gtsam::Vector whitenedError(const gtsam::Values& x, std::vector<Matrix>& H) const {
return whitenedError(x, &H);
}
#endif
/* ************************************************************************* */ /* ************************************************************************* */
gtsam::Vector unwhitenedError(const gtsam::Values& x) const { gtsam::Vector unwhitenedError(const gtsam::Values& x) const {

10
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -179,8 +179,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Vector whitenedError(const Values& x, Vector whitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const {
boost::optional<std::vector<Matrix>&> H = boost::none) const {
bool debug = true; bool debug = true;
@ -245,6 +244,13 @@ namespace gtsam {
return err_wh_eq; return err_wh_eq;
} }
/* ************************************************************************* */
#ifdef NO_BOOST_CPP17
Vector whitenedError(const Values& x, std::vector<Matrix>& H) const {
return whitenedError(x, &H);
}
#endif
/* ************************************************************************* */ /* ************************************************************************* */
Vector calcIndicatorProb(const Values& x) const { Vector calcIndicatorProb(const Values& x) const {

15
gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp
View File

@ -66,18 +66,13 @@ TEST(BiasedGPSFactor, jacobian) {
Matrix actualH1, actualH2; Matrix actualH1, actualH2;
factor.evaluateError(pose,bias, actualH1, actualH2); factor.evaluateError(pose,bias, actualH1, actualH2);
Matrix numericalH1 = numericalDerivative21( std::function<Vector(const Pose3&, const Point3&)> f = [&factor](const Pose3& pose, const Point3& bias) {
std::function<Vector(const Pose3&, const Point3&)>(std::bind( return factor.evaluateError(pose, bias);
&BiasedGPSFactor::evaluateError, factor, std::placeholders::_1, };
std::placeholders::_2, boost::none, boost::none)), Matrix numericalH1 = numericalDerivative21(f, pose, bias, 1e-5);
pose, bias, 1e-5);
EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
Matrix numericalH2 = numericalDerivative22( Matrix numericalH2 = numericalDerivative22(f, pose, bias, 1e-5);
std::function<Vector(const Pose3&, const Point3&)>(std::bind(
&BiasedGPSFactor::evaluateError, factor, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none)),
pose, bias, 1e-5);
EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
} }

11
gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
View File

@ -23,6 +23,9 @@
#include <gtsam/nonlinear/ISAM2.h> #include <gtsam/nonlinear/ISAM2.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "gtsam/base/Vector.h"
#include "gtsam/geometry/OrientedPlane3.h"
#include "gtsam/geometry/Pose3.h"
using namespace std::placeholders; using namespace std::placeholders;
using namespace gtsam; using namespace gtsam;
@ -141,10 +144,10 @@ TEST(LocalOrientedPlane3Factor, Derivatives) {
LocalOrientedPlane3Factor factor(p, noise, poseKey, anchorPoseKey, planeKey); LocalOrientedPlane3Factor factor(p, noise, poseKey, anchorPoseKey, planeKey);
// Calculate numerical derivatives // Calculate numerical derivatives
auto f = auto f = [&factor] (const Pose3& p1, const Pose3& p2, const OrientedPlane3& a_plane) {
std::bind(&LocalOrientedPlane3Factor::evaluateError, factor, return factor.evaluateError(p1, p2, a_plane);
std::placeholders::_1, std::placeholders::_2, };
std::placeholders::_3, boost::none, boost::none, boost::none);
Matrix numericalH1 = numericalDerivative31<Vector3, Pose3, Pose3, Matrix numericalH1 = numericalDerivative31<Vector3, Pose3, Pose3,
OrientedPlane3>(f, poseLin, anchorPoseLin, pLin); OrientedPlane3>(f, poseLin, anchorPoseLin, pLin);
Matrix numericalH2 = numericalDerivative32<Vector3, Pose3, Pose3, Matrix numericalH2 = numericalDerivative32<Vector3, Pose3, Pose3,

20
gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
View File

@ -82,9 +82,7 @@ TEST(PartialPriorFactor, JacobianPartialTranslation2) {
// Calculate numerical derivatives. // Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose2>( Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
std::bind(&TestPartialPriorFactor2::evaluateError, &factor, [&factor](const Pose2& p) { return factor.evaluateError(p); }, pose);
std::placeholders::_1, boost::none),
pose);
// Use the factor to calculate the derivative. // Use the factor to calculate the derivative.
Matrix actualH1; Matrix actualH1;
@ -107,9 +105,7 @@ TEST(PartialPriorFactor, JacobianFullTranslation2) {
// Calculate numerical derivatives. // Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose2>( Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
std::bind(&TestPartialPriorFactor2::evaluateError, &factor, [&factor](const Pose2& p) { return factor.evaluateError(p); }, pose);
std::placeholders::_1, boost::none),
pose);
// Use the factor to calculate the derivative. // Use the factor to calculate the derivative.
Matrix actualH1; Matrix actualH1;
@ -131,7 +127,7 @@ TEST(PartialPriorFactor, JacobianTheta) {
// Calculate numerical derivatives. // Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose2>( Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
std::bind(&TestPartialPriorFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), pose); [&factor](const Pose2& p) { return factor.evaluateError(p); }, pose);
// Use the factor to calculate the derivative. // Use the factor to calculate the derivative.
Matrix actualH1; Matrix actualH1;
@ -182,7 +178,7 @@ TEST(PartialPriorFactor, JacobianAtIdentity3) {
// Calculate numerical derivatives. // Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); [&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
// Use the factor to calculate the derivative. // Use the factor to calculate the derivative.
Matrix actualH1; Matrix actualH1;
@ -204,7 +200,7 @@ TEST(PartialPriorFactor, JacobianPartialTranslation3) {
// Calculate numerical derivatives. // Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); [&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
// Use the factor to calculate the derivative. // Use the factor to calculate the derivative.
Matrix actualH1; Matrix actualH1;
@ -228,7 +224,7 @@ TEST(PartialPriorFactor, JacobianFullTranslation3) {
// Calculate numerical derivatives. // Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); [&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
// Use the factor to calculate the derivative. // Use the factor to calculate the derivative.
Matrix actualH1; Matrix actualH1;
@ -252,7 +248,7 @@ TEST(PartialPriorFactor, JacobianTxTz3) {
// Calculate numerical derivatives. // Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); [&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
// Use the factor to calculate the derivative. // Use the factor to calculate the derivative.
Matrix actualH1; Matrix actualH1;
@ -275,7 +271,7 @@ TEST(PartialPriorFactor, JacobianFullRotation3) {
// Calculate numerical derivatives. // Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); [&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
// Use the factor to calculate the derivative. // Use the factor to calculate the derivative.
Matrix actualH1; Matrix actualH1;

16
gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
View File

@ -199,13 +199,9 @@ TEST( PoseBetweenFactor, Jacobian ) {
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPoseBetweenFactor::evaluateError, &factor, [&factor, &pose2](const Pose3& p) { return factor.evaluateError(p, pose2); }, pose1);
std::placeholders::_1, pose2, boost::none, boost::none),
pose1);
Matrix expectedH2 = numericalDerivative11<Vector, Pose3>( Matrix expectedH2 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, [&factor, &pose1](const Pose3& p) { return factor.evaluateError(pose1, p); }, pose2);
std::placeholders::_1, boost::none, boost::none),
pose2);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH1; Matrix actualH1;
@ -234,13 +230,9 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) {
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPoseBetweenFactor::evaluateError, &factor, [&factor, &pose2](const Pose3& p) { return factor.evaluateError(p, pose2); }, pose1);
std::placeholders::_1, pose2, boost::none, boost::none),
pose1);
Matrix expectedH2 = numericalDerivative11<Vector, Pose3>( Matrix expectedH2 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, [&factor, &pose1](const Pose3& p) { return factor.evaluateError(pose1, p); }, pose2);
std::placeholders::_1, boost::none, boost::none),
pose2);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH1; Matrix actualH1;

8
gtsam_unstable/slam/tests/testPosePriorFactor.cpp
View File

@ -188,9 +188,7 @@ TEST( PosePriorFactor, Jacobian ) {
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPosePriorFactor::evaluateError, &factor, [&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
std::placeholders::_1, boost::none),
pose);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH1; Matrix actualH1;
@ -215,9 +213,7 @@ TEST( PosePriorFactor, JacobianWithTransform ) {
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPosePriorFactor::evaluateError, &factor, [&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
std::placeholders::_1, boost::none),
pose);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH1; Matrix actualH1;

13
gtsam_unstable/slam/tests/testPoseToPointFactor.cpp
View File

@ -67,9 +67,9 @@ TEST(PoseToPointFactor, jacobian_2D) {
PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, l_meas, noise); PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, l_meas, noise);
// Calculate numerical derivatives // Calculate numerical derivatives
auto f = std::bind(&PoseToPointFactor<Pose2,Point2>::evaluateError, factor, auto f = [&factor] (const Pose2& pose, const Point2& pt) {
std::placeholders::_1, std::placeholders::_2, boost::none, return factor.evaluateError(pose, pt);
boost::none); };
Matrix numerical_H1 = numericalDerivative21<Vector, Pose2, Point2>(f, p, l); Matrix numerical_H1 = numericalDerivative21<Vector, Pose2, Point2>(f, p, l);
Matrix numerical_H2 = numericalDerivative22<Vector, Pose2, Point2>(f, p, l); Matrix numerical_H2 = numericalDerivative22<Vector, Pose2, Point2>(f, p, l);
@ -137,9 +137,10 @@ TEST(PoseToPointFactor, jacobian_3D) {
PoseToPointFactor<Pose3,Point3> factor(pose_key, point_key, l_meas, noise); PoseToPointFactor<Pose3,Point3> factor(pose_key, point_key, l_meas, noise);
// Calculate numerical derivatives // Calculate numerical derivatives
auto f = std::bind(&PoseToPointFactor<Pose3,Point3>::evaluateError, factor, auto f = [&factor] (const Pose3& pose, const Point3& pt) {
std::placeholders::_1, std::placeholders::_2, boost::none, return factor.evaluateError(pose, pt);
boost::none); };
Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l); Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l);
Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l); Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);

16
gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp
View File

@ -177,11 +177,9 @@ TEST( ProjectionFactorPPP, Jacobian ) {
// Verify H2 with numerical derivative // Verify H2 with numerical derivative
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>( Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>( [&factor](const Pose3& pose, const Pose3& transform, const Point3& point) {
std::bind(&TestProjectionFactor::evaluateError, &factor, return factor.evaluateError(pose, transform, point);
std::placeholders::_1, std::placeholders::_2, },
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose, Pose3(), point); pose, Pose3(), point);
CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
@ -215,11 +213,9 @@ TEST( ProjectionFactorPPP, JacobianWithTransform ) {
// Verify H2 with numerical derivative // Verify H2 with numerical derivative
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>( Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>( [&factor](const Pose3& pose, const Pose3& transform, const Point3& point) {
std::bind(&TestProjectionFactor::evaluateError, &factor, return factor.evaluateError(pose, transform, point);
std::placeholders::_1, std::placeholders::_2, },
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose, body_P_sensor, point); pose, body_P_sensor, point);
CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5));

20
gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp
View File

@ -136,15 +136,11 @@ TEST( ProjectionFactorPPPC, Jacobian ) {
// Verify H2 and H4 with numerical derivatives // Verify H2 and H4 with numerical derivatives
Matrix H2Expected = numericalDerivative11<Vector, Pose3>( Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
std::bind(&TestProjectionFactor::evaluateError, &factor, pose, [&factor, &point, &pose](const Pose3& pose_arg) { return factor.evaluateError(pose, pose_arg, point, *K1); },
std::placeholders::_1, point, *K1, boost::none, boost::none,
boost::none, boost::none),
Pose3()); Pose3());
Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>( Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
std::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), [&factor, &point, &pose](const Cal3_S2& K_arg) { return factor.evaluateError(pose, Pose3(), point, K_arg); },
point, std::placeholders::_1, boost::none, boost::none,
boost::none, boost::none),
*K1); *K1);
CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
@ -176,12 +172,16 @@ TEST( ProjectionFactorPPPC, JacobianWithTransform ) {
// Verify H2 and H4 with numerical derivatives // Verify H2 and H4 with numerical derivatives
Matrix H2Expected = numericalDerivative11<Vector, Pose3>( Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
std::bind(&TestProjectionFactor::evaluateError, &factor, pose, std::placeholders::_1, point, [&factor, &pose, &point](const Pose3& body_P_sensor) {
*K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor); return factor.evaluateError(pose, body_P_sensor, point, *K1);
},
body_P_sensor);
Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>( Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
std::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point, [&factor, &pose, &body_P_sensor, &point](const Cal3_S2& K) {
std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), *K1); return factor.evaluateError(pose, body_P_sensor, point, K);
},
*K1);
CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
CHECK(assert_equal(H4Expected, H4Actual, 1e-5)); CHECK(assert_equal(H4Expected, H4Actual, 1e-5));

54
gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp
View File

@ -230,30 +230,15 @@ TEST(ProjectionFactorRollingShutter, Jacobian) {
Matrix H1Actual, H2Actual, H3Actual; Matrix H1Actual, H2Actual, H3Actual;
factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual); factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual);
auto f = [&factor](const Pose3& p1, const Pose3& p2, const Point3& p3) {
return factor.evaluateError(p1, p2, p3);
};
// Expected Jacobians via numerical derivatives // Expected Jacobians via numerical derivatives
Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>( Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>(f, pose1, pose2, point);
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>( Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(f, pose1, pose2, point);
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>( Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(f, pose1, pose2, point);
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
@ -280,30 +265,15 @@ TEST(ProjectionFactorRollingShutter, JacobianWithTransform) {
Matrix H1Actual, H2Actual, H3Actual; Matrix H1Actual, H2Actual, H3Actual;
factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual); factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual);
auto f = [&factor](const Pose3& p1, const Pose3& p2, const Point3& p3) {
return factor.evaluateError(p1, p2, p3);
};
// Expected Jacobians via numerical derivatives // Expected Jacobians via numerical derivatives
Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>( Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>(f, pose1, pose2, point);
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>( Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(f, pose1, pose2, point);
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>( Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(f, pose1, pose2, point);
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose1, pose2, point);
CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5));

56
gtsam_unstable/slam/tests/testTSAMFactors.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "gtsam/geometry/Point2.h"
using namespace std::placeholders; using namespace std::placeholders;
using namespace std; using namespace std;
@ -46,12 +47,11 @@ TEST( DeltaFactor, all ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector2, Pose2>( H1Expected = numericalDerivative11<Vector2, Pose2>(
std::bind(&DeltaFactor::evaluateError, &factor, std::placeholders::_1, point, boost::none, [&factor, &point](const Pose2& pose) { return factor.evaluateError(pose, point); }, pose);
boost::none), pose);
H2Expected = numericalDerivative11<Vector2, Point2>( H2Expected = numericalDerivative11<Vector2, Point2>(
std::bind(&DeltaFactor::evaluateError, &factor, pose, std::placeholders::_1, boost::none, [&factor, &pose](const Point2& point) { return factor.evaluateError(pose, point); }, point);
boost::none), point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
@ -81,17 +81,25 @@ TEST( DeltaFactorBase, all ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected, H3Expected, H4Expected; Matrix H1Expected, H2Expected, H3Expected, H4Expected;
H1Expected = numericalDerivative11<Vector2, Pose2>( H1Expected = numericalDerivative11<Vector2, Pose2>(
std::bind(&DeltaFactorBase::evaluateError, &factor, std::placeholders::_1, pose, base2, [&factor, &pose, &base2, &point](const Pose2& pose_arg) {
point, boost::none, boost::none, boost::none, boost::none), base1); return factor.evaluateError(pose_arg, pose, base2, point);
},
base1);
H2Expected = numericalDerivative11<Vector2, Pose2>( H2Expected = numericalDerivative11<Vector2, Pose2>(
std::bind(&DeltaFactorBase::evaluateError, &factor, base1, std::placeholders::_1, base2, [&factor, &point, &base1, &base2](const Pose2& pose_arg) {
point, boost::none, boost::none, boost::none, boost::none), pose); return factor.evaluateError(base1, pose_arg, base2, point);
},
pose);
H3Expected = numericalDerivative11<Vector2, Pose2>( H3Expected = numericalDerivative11<Vector2, Pose2>(
std::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, std::placeholders::_1, [&factor, &pose, &base1, &point](const Pose2& pose_arg) {
point, boost::none, boost::none, boost::none, boost::none), base2); return factor.evaluateError(base1, pose, pose_arg, point);
},
base2);
H4Expected = numericalDerivative11<Vector2, Point2>( H4Expected = numericalDerivative11<Vector2, Point2>(
std::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, [&factor, &pose, &base1, &base2](const Point2& point_arg) {
std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), point); return factor.evaluateError(base1, pose, base2, point_arg);
},
point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
@ -121,18 +129,26 @@ TEST( OdometryFactorBase, all ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected, H3Expected, H4Expected; Matrix H1Expected, H2Expected, H3Expected, H4Expected;
// using lambdas to replace bind
H1Expected = numericalDerivative11<Vector3, Pose2>( H1Expected = numericalDerivative11<Vector3, Pose2>(
std::bind(&OdometryFactorBase::evaluateError, &factor, std::placeholders::_1, pose1, base2, [&factor, &pose1, &pose2, &base2](const Pose2& pose_arg) {
pose2, boost::none, boost::none, boost::none, boost::none), base1); return factor.evaluateError(pose_arg, pose1, base2, pose2);
},
base1);
H2Expected = numericalDerivative11<Vector3, Pose2>( H2Expected = numericalDerivative11<Vector3, Pose2>(
std::bind(&OdometryFactorBase::evaluateError, &factor, base1, std::placeholders::_1, base2, [&factor, &pose2, &base1, &base2](const Pose2& pose_arg) {
pose2, boost::none, boost::none, boost::none, boost::none), pose1); return factor.evaluateError(base1, pose_arg, base2, pose2);
},
pose1);
H3Expected = numericalDerivative11<Vector3, Pose2>( H3Expected = numericalDerivative11<Vector3, Pose2>(
std::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, std::placeholders::_1, [&factor, &pose1, &base1, &pose2](const Pose2& pose_arg) {
pose2, boost::none, boost::none, boost::none, boost::none), base2); return factor.evaluateError(base1, pose1, pose_arg, pose2);
},
base2);
H4Expected = numericalDerivative11<Vector3, Pose2>( H4Expected = numericalDerivative11<Vector3, Pose2>(
std::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, [&factor, &pose1, &base1, &base2](const Pose2& pose_arg) {
base2, std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), return factor.evaluateError(base1, pose1, base2, pose_arg);
},
pose2); pose2);
// Verify the Jacobians are correct // Verify the Jacobians are correct