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

Merge pull request #1392 from kartikarcot/verdant/typedef-optional-references

release/4.3a0
Frank Dellaert 2023-01-20 22:48:44 -08:00 committed by GitHub
parent 22e9e80f43 dc25a78f76
commit c7e18e64e5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
138 changed files with 1093 additions and 772 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -6,6 +6,8 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH)
set(CMAKE_MACOSX_RPATH 0)
endif()
set(CMAKE_CXX_STANDARD 17)
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 3)
@ -87,7 +89,7 @@ add_subdirectory(timing)
# Build gtsam_unstable
if (GTSAM_BUILD_UNSTABLE)
add_subdirectory(gtsam_unstable)
add_subdirectory(gtsam_unstable)
endif()
# This is the new wrapper

7
doc/Code/LocalizationFactor.cpp
View File

@ -2,11 +2,14 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
double mx_, my_; ///< X and Y measurements
public:
// Provide access to the Matrix& version of evaluateError:
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,
boost::optional<Matrix&> H = boost::none) 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,

5
examples/CameraResectioning.cpp
View File

@ -46,10 +46,9 @@ public:
}
/// evaluate the error
Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const override {
Vector evaluateError(const Pose3& pose, OptionalMatrixType H) const override {
PinholeCamera<Cal3_S2> camera(pose, *K_);
return camera.project(P_, H, boost::none, boost::none) - p_;
return camera.project(P_, H, OptionalNone, OptionalNone) - p_;
}
};

6
examples/LocalizationExample.cpp
View File

@ -71,6 +71,10 @@ class UnaryFactor: public NoiseModelFactorN<Pose2> {
double mx_, my_;
public:
// Provide access to Matrix& version of evaluateError:
using NoiseModelFactor1<Pose2>::evaluateError;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<UnaryFactor> shared_ptr;
@ -84,7 +88,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, boost::optional<Matrix&> H = boost::none) 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/base/OptionalJacobian.h
View File

@ -94,8 +94,10 @@ public:
/// Constructor that will resize a dynamic matrix (unless already correct)
OptionalJacobian(Eigen::MatrixXd* dynamic) :
map_(nullptr) {
dynamic->resize(Rows, Cols); // no malloc if correct size
usurp(dynamic->data());
if (dynamic) {
dynamic->resize(Rows, Cols); // no malloc if correct size
usurp(dynamic->data());
}
}
/**

2
gtsam/base/numericalDerivative.h
View File

@ -35,7 +35,7 @@ namespace gtsam {
*
* Usage of the boost bind version to rearrange arguments:
* for a function with one relevant param and an optional derivative:
* Foo bar(const Obj& a, boost::optional<Matrix&> H1)
* Foo bar(const Obj& a, OptionalMatrixType H1)
* Use boost.bind to restructure:
* std::bind(bar, std::placeholders::_1, boost::none)
* This syntax will fix the optional argument to boost::none, while using the first argument provided

29
gtsam/geometry/CameraSet.h
View File

@ -106,8 +106,8 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
*/
template <class POINT>
ZVector project2(const POINT& point, //
boost::optional<FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const {
FBlocks* Fs = nullptr, //
Matrix* E = nullptr) const {
static const int N = FixedDimension<POINT>::value;
// Allocate result
@ -131,14 +131,35 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
return z;
}
/** An overload o the project2 function to accept
* full matrices and vectors and pass it to the pointer
* version of the function
*/
template <class POINT, class... OptArgs>
ZVector project2(const POINT& point, OptArgs&... args) const {
// pass it to the pointer version of the function
return project2(point, (&args)...);
}
/// Calculate vector [project2(point)-z] of re-projection errors
template <class POINT>
Vector reprojectionError(const POINT& point, const ZVector& measured,
boost::optional<FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const {
FBlocks* Fs = nullptr, //
Matrix* E = nullptr) const {
return ErrorVector(project2(point, Fs, E), measured);
}
/** An overload o the reprojectionError function to accept
* full matrices and vectors and pass it to the pointer
* version of the function
*/
template <class POINT, class... OptArgs>
Vector reprojectionError(const POINT& point, const ZVector& measured,
OptArgs&... args) const {
// pass it to the pointer version of the function
return reprojectionError(point, measured, (&args)...);
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* G = F' * F - F' * E * P * E' * F

3
gtsam/inference/VariableIndex-inl.h
View File

@ -24,8 +24,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class FG>
void VariableIndex::augment(const FG& factors,
boost::optional<const FactorIndices&> newFactorIndices) {
void VariableIndex::augment(const FG& factors, const FactorIndices* newFactorIndices) {
gttic(VariableIndex_augment);
// Augment index for each factor

11
gtsam/inference/VariableIndex.h
View File

@ -126,7 +126,16 @@ class GTSAM_EXPORT VariableIndex {
* solving problems incrementally.
*/
template<class FG>
void augment(const FG& factors, boost::optional<const FactorIndices&> newFactorIndices = boost::none);
void augment(const FG& factors, const FactorIndices* newFactorIndices = nullptr);
/**
* An overload of augment() that takes a single factor. and l-value
* reference to FactorIndeces.
*/
template<class FG>
void augment(const FG& factor, const FactorIndices& newFactorIndices) {
augment(factor, &newFactorIndices);
}
/**
* Augment the variable index after an existing factor now affects to more

3
gtsam/linear/IterativeSolver.cpp
View File

@ -84,8 +84,7 @@ string IterativeOptimizationParameters::verbosityTranslator(
/*****************************************************************************/
VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
boost::optional<const KeyInfo&> keyInfo,
boost::optional<const std::map<Key, Vector>&> lambda) {
const KeyInfo* keyInfo, const std::map<Key, Vector>* lambda) {
return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg),
lambda ? *lambda : std::map<Key, Vector>());
}

4
gtsam/linear/IterativeSolver.h
View File

@ -93,8 +93,8 @@ public:
/* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg,
boost::optional<const KeyInfo&> = boost::none,
boost::optional<const std::map<Key, Vector>&> lambda = boost::none);
const KeyInfo* = nullptr,
const std::map<Key, Vector>* lambda = nullptr);
/* interface to the nonlinear optimizer, without initial estimate */
GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo,

7
gtsam/linear/linearAlgorithms-inst.h
View File

@ -21,9 +21,10 @@
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <boost/optional.hpp>
#include <boost/shared_ptr.hpp>
#include <optional>
namespace gtsam
{
namespace internal
@ -32,7 +33,7 @@ namespace gtsam
{
/* ************************************************************************* */
struct OptimizeData {
boost::optional<OptimizeData&> parentData;
OptimizeData* parentData = nullptr;
FastMap<Key, VectorValues::const_iterator> cliqueResults;
//VectorValues ancestorResults;
//VectorValues results;
@ -55,7 +56,7 @@ namespace gtsam
OptimizeData& parentData)
{
OptimizeData myData;
myData.parentData = parentData;
myData.parentData = &parentData;
// Take any ancestor results we'll need
for(Key parent: clique->conditional_->parents())
myData.cliqueResults.emplace(parent, myData.parentData->cliqueResults.at(parent));

4
gtsam/navigation/AHRSFactor.cpp
View File

@ -119,8 +119,8 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
//------------------------------------------------------------------------------
Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
const Vector3& bias, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3) const {
const Vector3& bias, OptionalMatrixType H1,
OptionalMatrixType H2, OptionalMatrixType H3) const {
// Do bias correction, if (H3) will contain 3*3 derivative used below
const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3);

8
gtsam/navigation/AHRSFactor.h
View File

@ -140,6 +140,9 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
public:
// Provide access to the Matrix& version of evaluateError:
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 +182,8 @@ public:
/// vector of errors
Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const override;
const Vector3& bias, OptionalMatrixType H1,
OptionalMatrixType H2, OptionalMatrixType H3) const override;
/// predicted states from IMU
/// TODO(frank): relationship with PIM predict ??

12
gtsam/navigation/AttitudeFactor.h
View File

@ -82,6 +82,9 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public At
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<Rot3AttitudeFactor> shared_ptr;
@ -121,8 +124,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/** vector of errors */
Vector evaluateError(const Rot3& nRb, //
boost::optional<Matrix&> H = boost::none) const override {
Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override {
return attitudeError(nRb, H);
}
@ -157,6 +159,9 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<Pose3AttitudeFactor> shared_ptr;
@ -196,8 +201,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/** vector of errors */
Vector evaluateError(const Pose3& nTb, //
boost::optional<Matrix&> H = boost::none) const override {
Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override {
Vector e = attitudeError(nTb.rotation(), H);
if (H) {
Matrix H23 = *H;

4
gtsam/navigation/BarometricFactor.cpp
View File

@ -43,8 +43,8 @@ bool BarometricFactor::equals(const NonlinearFactor& expected,
//***************************************************************************
Vector BarometricFactor::evaluateError(const Pose3& p, const double& bias,
boost::optional<Matrix&> H,
boost::optional<Matrix&> H2) const {
OptionalMatrixType H,
OptionalMatrixType H2) const {
Matrix tH;
Vector ret = (Vector(1) << (p.translation(tH).z() + bias - nT_)).finished();
if (H) (*H) = tH.block<1, 6>(2, 0);

10
gtsam/navigation/BarometricFactor.h
View File

@ -38,6 +38,10 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
double nT_; ///< Height Measurement based on a standard atmosphere
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<BarometricFactor> shared_ptr;
@ -76,10 +80,8 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(
const Pose3& p, const double& b,
boost::optional<Matrix&> H = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override;
Vector evaluateError(const Pose3& p, const double& b,
OptionalMatrixType H, OptionalMatrixType H2) const override;
inline const double& measurementIn() const { return nT_; }

6
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -222,9 +222,9 @@ bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const {
Vector CombinedImuFactor::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,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3, OptionalMatrixType H4,
OptionalMatrixType H5, OptionalMatrixType H6) const {
// error wrt bias evolution model (random walk)
Matrix6 Hbias_i, Hbias_j;

10
gtsam/navigation/CombinedImuFactor.h
View File

@ -269,6 +269,9 @@ private:
public:
// Provide access to Matrix& version of evaluateError:
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<CombinedImuFactor> shared_ptr;
@ -324,10 +327,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,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none, boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3, OptionalMatrixType H4,
OptionalMatrixType H5, OptionalMatrixType H6) const override;
private:
/** Serialization function */

8
gtsam/navigation/ConstantVelocityFactor.h
View File

@ -30,7 +30,10 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
double dt_;
public:
using Base = NoiseModelFactorN<NavState, NavState>;
using Base = NoiseModelFactor2<NavState, NavState>;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
public:
ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
@ -48,8 +51,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
* @return * Vector
*/
gtsam::Vector evaluateError(const NavState &x1, const NavState &x2,
boost::optional<gtsam::Matrix &> H1 = boost::none,
boost::optional<gtsam::Matrix &> H2 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
// only used to use update() below
static const Vector3 b_accel{0.0, 0.0, 0.0};
static const Vector3 b_omega{0.0, 0.0, 0.0};

4
gtsam/navigation/GPSFactor.cpp
View File

@ -38,7 +38,7 @@ bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
//***************************************************************************
Vector GPSFactor::evaluateError(const Pose3& p,
boost::optional<Matrix&> H) const {
OptionalMatrixType H) const {
return p.translation(H) -nT_;
}
@ -80,7 +80,7 @@ bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const {
//***************************************************************************
Vector GPSFactor2::evaluateError(const NavState& p,
boost::optional<Matrix&> H) const {
OptionalMatrixType H) const {
return p.position(H) -nT_;
}

12
gtsam/navigation/GPSFactor.h
View File

@ -42,6 +42,9 @@ private:
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GPSFactor> shared_ptr;
@ -78,8 +81,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const Pose3& p,
boost::optional<Matrix&> H = boost::none) const override;
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
inline const Point3 & measurementIn() const {
return nT_;
@ -121,6 +123,9 @@ private:
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GPSFactor2> shared_ptr;
@ -151,8 +156,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const NavState& p,
boost::optional<Matrix&> H = boost::none) const override;
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
inline const Point3 & measurementIn() const {
return nT_;

10
gtsam/navigation/ImuFactor.cpp
View File

@ -151,9 +151,9 @@ bool ImuFactor::equals(const NonlinearFactor& other, double tol) const {
//------------------------------------------------------------------------------
Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3,
boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const {
const imuBias::ConstantBias& bias_i, OptionalMatrixType H1,
OptionalMatrixType H2, OptionalMatrixType H3,
OptionalMatrixType H4, OptionalMatrixType H5) const {
return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
H1, H2, H3, H4, H5);
}
@ -247,8 +247,8 @@ bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const {
Vector ImuFactor2::evaluateError(const NavState& state_i,
const NavState& state_j,
const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3) const {
return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3);
}

17
gtsam/navigation/ImuFactor.h
View File

@ -180,6 +180,9 @@ private:
public:
// Provide access to the Matrix& version of evaluateError:
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<ImuFactor> shared_ptr;
@ -228,10 +231,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, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
boost::none, boost::optional<Matrix&> H5 = boost::none) const 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
@ -270,6 +271,9 @@ private:
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/** Default constructor - only use for serialization */
ImuFactor2() {}
@ -307,9 +311,8 @@ public:
/// vector of errors
Vector evaluateError(const NavState& state_i, const NavState& state_j,
const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override;
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3) const override;
private:

37
gtsam/navigation/MagFactor.h
View File

@ -38,6 +38,9 @@ class MagFactor: public NoiseModelFactorN<Rot2> {
public:
// Provide access to Matrix& version of evaluateError:
using NoiseModelFactor1<Rot2>::evaluateError;
/**
* Constructor of factor that estimates nav to body rotation bRn
* @param key of the unknown rotation bRn in the factor graph
@ -61,7 +64,7 @@ public:
}
static Point3 unrotate(const Rot2& R, const Point3& p,
boost::optional<Matrix&> HR = boost::none) {
OptionalMatrixType HR = OptionalNone) {
Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none);
if (HR) {
// assign to temporary first to avoid error in Win-Debug mode
@ -74,8 +77,7 @@ public:
/**
* @brief vector of errors
*/
Vector evaluateError(const Rot2& nRb,
boost::optional<Matrix&> H = boost::none) 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_);
@ -95,6 +97,10 @@ class MagFactor1: public NoiseModelFactorN<Rot3> {
public:
// Provide access to Matrix& version of evaluateError:
using NoiseModelFactor1<Rot3>::evaluateError;
/** Constructor */
MagFactor1(Key key, const Point3& measured, double scale,
const Unit3& direction, const Point3& bias,
@ -112,10 +118,9 @@ public:
/**
* @brief vector of errors
*/
Vector evaluateError(const Rot3& nRb,
boost::optional<Matrix&> H = boost::none) 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_;
Point3 hx = nRb.unrotate(nM_, H, OptionalNone) + bias_;
return (hx - measured_);
}
};
@ -132,6 +137,10 @@ class MagFactor2: public NoiseModelFactorN<Point3, Point3> {
public:
// Provide access to Matrix& version of evaluateError:
using NoiseModelFactor2<Point3, Point3>::evaluateError;
/** Constructor */
MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
const SharedNoiseModel& model) :
@ -151,10 +160,9 @@ public:
* @param bias (unknown) 3D bias
*/
Vector evaluateError(const Point3& nM, const Point3& bias,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
// measured bM = nRb<52> * nM + b, where b is unknown bias
Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
Point3 hx = bRn_.rotate(nM, OptionalNone, H1) + bias;
if (H2)
*H2 = I_3x3;
return (hx - measured_);
@ -173,6 +181,10 @@ class MagFactor3: public NoiseModelFactorN<double, Unit3, Point3> {
public:
// Provide access to Matrix& version of evaluateError:
using NoiseModelFactor3<double, Unit3, Point3>::evaluateError;
/** Constructor */
MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
const Rot3& nRb, const SharedNoiseModel& model) :
@ -192,11 +204,10 @@ public:
* @param bias (unknown) 3D bias
*/
Vector evaluateError(const double& scale, const Unit3& direction,
const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) 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, boost::none, H2);
Unit3 rotated = bRn_.rotate(direction, OptionalNone, H2);
Point3 hx = scale * rotated.point3() + bias;
if (H1)
*H1 = rotated.point3();

8
gtsam/navigation/MagPoseFactor.h
View File

@ -49,6 +49,10 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
GTSAM_CONCEPT_POSE_TYPE(POSE)
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
~MagPoseFactor() override {}
/// Default constructor - only use for serialization.
@ -108,11 +112,11 @@ 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, boost::optional<Matrix&> H = boost::none) 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);
const Point hx = nPb.rotation().unrotate(nM_, H_rot, boost::none) + bias_;
const Point hx = nPb.rotation().unrotate(nM_, H_rot, OptionalNone) + bias_;
if (H) {
// Fill in the relevant part of the Jacobian (just rotation columns).

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));
auto 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;
auto 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

6
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -20,8 +20,6 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <GeographicLib/Config.h>
@ -71,7 +69,7 @@ TEST( GPSFactor, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector, Pose3>(
std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T);
[&factor](const Pose3& T) { return factor.evaluateError(T); }, T);
// Use the factor to calculate the derivative
Matrix actualH;
@ -100,7 +98,7 @@ TEST( GPSFactor2, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector, NavState>(
std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T);
[&factor](const NavState& T) { return factor.evaluateError(T); }, T);
// Use the factor to calculate the derivative
Matrix actualH;

29
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -56,15 +56,13 @@ Unit3 dir(nM);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
} // namespace
using boost::none;
// *************************************************************************
TEST( MagFactor, unrotate ) {
Matrix H;
Point3 expected(22735.5, 314.502, 44202.5);
EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,&H),1e-1));
EXPECT(assert_equal(numericalDerivative11<Point3, Rot2> //
(std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6));
(std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, OptionalNone), theta), H, 1e-6));
}
// *************************************************************************
@ -75,37 +73,32 @@ TEST( MagFactor, Factors ) {
// MagFactor
MagFactor f(1, measured, s, dir, bias, model);
EXPECT(assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector, Rot2> //
(std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector, Rot2>
([&f] (const Rot2& theta) { return f.evaluateError(theta); }, theta), H1, 1e-7));
// MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT(assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
EXPECT(assert_equal(numericalDerivative11<Vector, Rot3> //
(std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector, Rot3>
([&f1] (const Rot3& nRb) { return f1.evaluateError(nRb); }, nRb), H1, 1e-7));
// MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT(assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),//
H1, 1e-7));
([&f2] (const Point3& scaled) { return f2.evaluateError(scaled,bias); }, scaled), H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),//
H2, 1e-7));
([&f2] (const Point3& bias) { return f2.evaluateError(scaled,bias); }, bias), H2, 1e-7));
// MagFactor3
MagFactor3 f3(1, 2, 3, measured, nRb, model);
EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
(std::bind(&MagFactor3::evaluateError, &f3, std::placeholders::_1, dir, bias, none, none, none), s),//
H1, 1e-7));
([&f3] (double s) { return f3.evaluateError(s,dir,bias); }, s), H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
(std::bind(&MagFactor3::evaluateError, &f3, s, std::placeholders::_1, bias, none, none, none), dir),//
H2, 1e-7));
([&f3] (const Unit3& dir) { return f3.evaluateError(s,dir,bias); }, dir), H2, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
(std::bind(&MagFactor3::evaluateError, &f3, s, dir, std::placeholders::_1, none, none, none), bias),//
H3, 1e-7));
([&f3] (const Point3& bias) { return f3.evaluateError(s,dir,bias); }, bias), H3, 1e-7));
}
// *************************************************************************

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));
}
// *****************************************************************************

4
gtsam/nonlinear/CustomFactor.cpp
View File

@ -22,7 +22,7 @@ namespace gtsam {
/*
* Calculates the unwhitened error by invoking the callback functor (i.e. from Python).
*/
Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const {
Vector CustomFactor::unwhitenedError(const Values& x, OptionalMatrixVecType H) const {
if(this->active(x)) {
if(H) {
@ -43,7 +43,7 @@ Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vecto
* return error
* ```
*/
return this->error_function_(*this, x, H.get_ptr());
return this->error_function_(*this, x, H);
} else {
/*
* In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python.

2
gtsam/nonlinear/CustomFactor.h
View File

@ -75,7 +75,7 @@ public:
* Calls the errorFunction closure, which is a std::function object
* One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array
*/
Vector unwhitenedError(const Values &x, boost::optional<std::vector<Matrix> &> H = boost::none) const override;
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H = nullptr) const override;
/** print */
void print(const std::string &s,

3
gtsam/nonlinear/Expression-inl.h
View File

@ -138,8 +138,7 @@ void Expression<T>::print(const std::string& s) const {
template<typename T>
T Expression<T>::value(const Values& values,
boost::optional<std::vector<Matrix>&> H) const {
std::vector<Matrix>* H) const {
if (H) {
// Call private version that returns derivatives in H
KeyVector keys;

11
gtsam/nonlinear/Expression.h
View File

@ -155,8 +155,15 @@ public:
* Notes: this is not terribly efficient, and H should have correct size.
* The order of the Jacobians is same as keys in either keys() or dims()
*/
T value(const Values& values, boost::optional<std::vector<Matrix>&> H =
boost::none) const;
T value(const Values& values, std::vector<Matrix>* H = nullptr) const;
/**
* An overload of the value function to accept reference to vector of matrices instead of
* a pointer to vector of matrices.
*/
T value(const Values& values, std::vector<Matrix>& H) const {
return value(values, &H);
}
/**
* @return a "deep" copy of this Expression

27
gtsam/nonlinear/ExpressionFactor.h
View File

@ -40,8 +40,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 +55,9 @@ protected:
public:
// Provide access to the Matrix& version of unwhitenedError:
using NoiseModelFactor::unwhitenedError;
typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
/**
@ -97,7 +100,7 @@ protected:
* both the function evaluation and its derivative(s) in H.
*/
Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const override {
OptionalMatrixVecType H = nullptr) const override {
if (H) {
const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
// NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here
@ -243,6 +246,9 @@ class ExpressionFactorN : public ExpressionFactor<T> {
public:
static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args);
using ArrayNKeys = std::array<Key, NARY_EXPRESSION_SIZE>;
// Provide access to the Matrix& version of unwhitenedError:
using ExpressionFactor<T>::unwhitenedError;
/// Destructor
~ExpressionFactorN() override = default;
@ -311,9 +317,8 @@ public:
~ExpressionFactor2() override {}
/// Backwards compatible evaluateError, to make existing tests compile
Vector evaluateError(const A1 &a1, const A2 &a2,
boost::optional<Matrix &> H1 = boost::none,
boost::optional<Matrix &> H2 = boost::none) const {
Vector evaluateError(const A1& a1, const A2& a2, OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const {
Values values;
values.insert(this->keys_[0], a1);
values.insert(this->keys_[1], a2);
@ -324,15 +329,13 @@ public:
return error;
}
/// 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,7 +344,7 @@ protected:
ExpressionFactor2() {}
/// Constructor takes care of keys, but still need to call initialize
ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel,
ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel& noiseModel,
const T &measurement)
: ExpressionFactorN<T, A1, A2>({key1, key2}, noiseModel, measurement) {}
};

21
gtsam/nonlinear/FunctorizedFactor.h
View File

@ -62,9 +62,13 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
R measured_; ///< value that is compared with functor return value
SharedNoiseModel noiseModel_; ///< noise model
std::function<R(T, boost::optional<Matrix &>)> func_; ///< functor instance
std::function<R(T, OptionalMatrixType)> func_; ///< functor instance
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/** default constructor - only use for serialization */
FunctorizedFactor() {}
@ -76,7 +80,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
* @param func: The instance of the functor object
*/
FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model,
const std::function<R(T, boost::optional<Matrix &>)> func)
const std::function<R(T, OptionalMatrixType)> func)
: Base(model, key), measured_(z), noiseModel_(model), func_(func) {}
~FunctorizedFactor() override {}
@ -87,8 +91,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
}
Vector evaluateError(const T &params, boost::optional<Matrix &> H =
boost::none) const override {
Vector evaluateError(const T &params, OptionalMatrixType H) const override {
R x = func_(params, H);
Vector error = traits<R>::Local(measured_, x);
return error;
@ -162,11 +165,14 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
R measured_; ///< value that is compared with functor return value
SharedNoiseModel noiseModel_; ///< noise model
using FunctionType = std::function<R(T1, T2, boost::optional<Matrix &>,
boost::optional<Matrix &>)>;
using FunctionType = std::function<R(T1, T2, OptionalMatrixType, OptionalMatrixType)>;
FunctionType func_; ///< functor instance
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/** default constructor - only use for serialization */
FunctorizedFactor2() {}
@ -194,8 +200,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
Vector evaluateError(
const T1 &params1, const T2 &params2,
boost::optional<Matrix &> H1 = boost::none,
boost::optional<Matrix &> H2 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
R x = func_(params1, params2, H1, H2);
Vector error = traits<R>::Local(measured_, x);
return error;

4
gtsam/nonlinear/ISAM2.cpp
View File

@ -478,8 +478,8 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
/* ************************************************************************* */
void ISAM2::marginalizeLeaves(
const FastList<Key>& leafKeysList,
boost::optional<FactorIndices&> marginalFactorsIndices,
boost::optional<FactorIndices&> deletedFactorsIndices) {
FactorIndices* marginalFactorsIndices,
FactorIndices* deletedFactorsIndices) {
// Convert to ordered set
KeySet leafKeys(leafKeysList.begin(), leafKeysList.end());

16
gtsam/nonlinear/ISAM2.h
View File

@ -198,8 +198,20 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
*/
void marginalizeLeaves(
const FastList<Key>& leafKeys,
boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);
FactorIndices* marginalFactorsIndices = nullptr,
FactorIndices* deletedFactorsIndices = nullptr);
/** An overload of marginalizeLeaves that takes references
* to vectors instead of pointers to vectors and passes
* it to the pointer version of the function.
*/
template <class... OptArgs>
void marginalizeLeaves(const FastList<Key>& leafKeys,
OptArgs&&... optArgs) {
// dereference the optional arguments and pass
// it to the pointer version
marginalizeLeaves(leafKeys, (&optArgs)...);
}
/// Access the current linearization point
const Values& getLinearizationPoint() const { return theta_; }

19
gtsam/nonlinear/NonlinearEquality.h
View File

@ -47,6 +47,9 @@ class NonlinearEquality: public NoiseModelFactorN<VALUE> {
public:
typedef VALUE T;
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor1<VALUE>::evaluateError;
private:
// feasible value
@ -138,8 +141,7 @@ public:
}
/// Error function
Vector evaluateError(const T& xj,
boost::optional<Matrix&> H = boost::none) 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 +211,9 @@ class NonlinearEquality1: public NoiseModelFactorN<VALUE> {
public:
typedef VALUE X;
// Provide access to Matrix& version of evaluateError:
using NoiseModelFactor1<VALUE>::evaluateError;
protected:
typedef NoiseModelFactorN<VALUE> Base;
typedef NonlinearEquality1<VALUE> This;
@ -248,8 +253,7 @@ public:
}
/// g(x) with optional derivative
Vector evaluateError(const X& x1,
boost::optional<Matrix&> H = boost::none) 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 +309,10 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
public:
typedef boost::shared_ptr<NonlinearEquality2<T>> shared_ptr;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/**
* Constructor
* @param key1 the key for the first unknown variable to be constrained
@ -324,8 +332,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
/// g(x) with optional derivative2
Vector evaluateError(
const T& x1, const T& x2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) 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);

133
gtsam/nonlinear/NonlinearFactor.h
View File

@ -29,11 +29,35 @@
#include <gtsam/base/utilities.h> // boost::index_sequence
#include <boost/serialization/base_object.hpp>
#include <cstddef>
#include <type_traits>
namespace gtsam {
/* ************************************************************************* */
/** These typedefs and aliases will help with making the evaluateError interface
* independent of boost
* TODO(kartikarcot): Change this to OptionalMatrixNone
* This typedef is used to indicate that the Jacobian is not required
* and the default value used for optional matrix pointer arguments in evaluateError.
* Had to use the static_cast of a nullptr, because the compiler is not able to
* deduce the type of the nullptr when expanding the evaluateError templates.
*/
#define OptionalNone static_cast<Matrix*>(nullptr)
/** This typedef will be used everywhere boost::optional<Matrix&> reference was used
* previously. This is used to indicate that the Jacobian is optional. In the future
* we will change this to OptionalJacobian
*/
using OptionalMatrixType = Matrix*;
/** The OptionalMatrixVecType is a pointer to a vector of matrices. It will
* be used in situations where a vector of matrices is optional, like in
* unwhitenedError.
*/
using OptionalMatrixVecType = std::vector<Matrix>*;
/**
* Nonlinear factor base class
*
@ -206,7 +230,6 @@ protected:
NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {}
public:
/** Print */
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
@ -230,8 +253,17 @@ public:
* If the optional arguments is specified, it should compute
* both the function evaluation and its derivative(s) in H.
*/
virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const = 0;
/** support taking in the actual vector instead of the pointer as well
* to get access to this version of the function from derived classes
* one will need to use the "using" keyword and specify that like this:
* public:
* using NoiseModelFactor::unwhitenedError;
*/
Vector unwhitenedError(const Values& x, std::vector<Matrix>& H) const {
return unwhitenedError(x, &H);
}
/**
* Vector of errors, whitened
@ -359,8 +391,8 @@ struct NoiseModelFactorAliases<T1, T2, T3, T4, T5, T6, TExtra...> {
*
* Vector evaluateError(
* const Pose3& T, const Point3& p,
* boost::optional<Matrix&> H_T = boost::none,
* boost::optional<Matrix&> H_p = boost::none) const override {
* OptionalMatrixType H_T = OptionalNone,
* OptionalMatrixType H_p = OptionalNone) const override {
* Matrix36 t_H_T; // partial derivative of translation w.r.t. pose T
*
* // Only compute t_H_T if needed:
@ -402,7 +434,9 @@ class NoiseModelFactorN
/// N is the number of variables (N-way factor)
enum { N = sizeof...(ValueTypes) };
protected:
using NoiseModelFactor::unwhitenedError;
protected:
using Base = NoiseModelFactor;
using This = NoiseModelFactorN<ValueTypes...>;
@ -423,20 +457,49 @@ class NoiseModelFactorN
template <typename Container>
using IsContainerOfKeys = IsConvertible<ContainerElementType<Container>, Key>;
/** A helper alias to check if a list of args
* are all references to a matrix or not. It will be used
* to choose the right overload of evaluateError.
*/
template <typename Ret, typename ...Args>
using AreAllMatrixRefs = std::enable_if_t<(... &&
std::is_convertible<Args, Matrix&>::value), Ret>;
template<typename Arg>
using IsMatrixPointer = std::is_same<typename std::decay_t<Arg>, Matrix*>;
template<typename Arg>
using IsNullpointer = std::is_same<typename std::decay_t<Arg>, std::nullptr_t>;
/** A helper alias to check if a list of args
* are all pointers to a matrix or not. It will be used
* to choose the right overload of evaluateError.
*/
template <typename Ret, typename ...Args>
using AreAllMatrixPtrs = std::enable_if_t<(... &&
(IsMatrixPointer<Args>::value || IsNullpointer<Args>::value)), Ret>;
/// @}
/* Like std::void_t, except produces `boost::optional<Matrix&>` instead of
/* Like std::void_t, except produces `OptionalMatrixType` instead of
* `void`. Used to expand fixed-type parameter-packs with same length as
* ValueTypes. */
template <typename T>
using OptionalMatrix = boost::optional<Matrix&>;
template <typename T = void>
using OptionalMatrixTypeT = Matrix*;
/* Like std::void_t, except produces `Key` instead of `void`. Used to expand
* fixed-type parameter-packs with same length as ValueTypes. */
template <typename T>
using KeyType = Key;
public:
/* Like std::void_t, except produces `Matrix` instead of
* `void`. Used to expand fixed-type parameter-packs with same length as
* ValueTypes. This helps in creating an evaluateError overload that accepts
* Matrices instead of pointers to matrices */
template <typename T = void>
using MatrixTypeT = Matrix;
public:
/**
* The type of the I'th template param can be obtained as ValueType<I>.
* I is 1-indexed for backwards compatibility/consistency! So for example,
@ -541,7 +604,7 @@ class NoiseModelFactorN
*/
Vector unwhitenedError(
const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const override {
OptionalMatrixVecType H = nullptr) const override {
return unwhitenedError(boost::mp11::index_sequence_for<ValueTypes...>{}, x,
H);
}
@ -560,8 +623,8 @@ class NoiseModelFactorN
* ```
* Vector evaluateError(
* const Pose3& x1, const Point3& x2,
* boost::optional<Matrix&> H1 = boost::none,
* boost::optional<Matrix&> H2 = boost::none) const override { ... }
* OptionalMatrixType H1 = OptionalNone,
* OptionalMatrixType H2 = OptionalNone) const override { ... }
* ```
*
* If any of the optional Matrix reference arguments are specified, it should
@ -573,10 +636,20 @@ class NoiseModelFactorN
* @param[out] H The Jacobian with respect to each variable (optional).
*/
virtual Vector evaluateError(const ValueTypes&... x,
OptionalMatrix<ValueTypes>... H) const = 0;
OptionalMatrixTypeT<ValueTypes>... H) const = 0;
/** If all the optional arguments are matrices then redirect the call to
* the one which takes pointers.
* To get access to this version of the function from derived classes
* one will need to use the "using" keyword and specify that like this:
* public:
* using NoiseModelFactorN<list the value types here>::evaluateError;
*/
Vector evaluateError(const ValueTypes&... x, MatrixTypeT<ValueTypes>&... H) const {
return evaluateError(x..., (&H)...);
}
/// @}
/// @name Convenience method overloads
/// @{
@ -587,19 +660,31 @@ class NoiseModelFactorN
* e.g. `const Vector error = factor.evaluateError(pose, point);`
*/
inline Vector evaluateError(const ValueTypes&... x) const {
return evaluateError(x..., OptionalMatrix<ValueTypes>()...);
return evaluateError(x..., OptionalMatrixTypeT<ValueTypes>()...);
}
/** Some (but not all) optional Jacobians are omitted (function overload)
*
* and the jacobians are l-value references to matrices.
* 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 {
return evaluateError(x..., std::forward<OptionalJacArgs>(H)...,
boost::none);
template <typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>
inline AreAllMatrixRefs<Vector, OptionalJacArgs...> evaluateError(const ValueTypes&... x,
OptionalJacArgs&&... H) const {
return evaluateError(x..., (&H)...);
}
/** Some (but not all) optional Jacobians are omitted (function overload)
* and the jacobians are pointers to matrices.
* e.g. `const Vector error = factor.evaluateError(pose, point, &Hpose);`
*/
template <typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>
inline AreAllMatrixPtrs<Vector, OptionalJacArgs...> evaluateError(const ValueTypes&... x,
OptionalJacArgs&&... H) const {
// If they are pointer version, ensure to cast them all to be Matrix* types
// This will ensure any arguments inferred as std::nonetype_t are cast to (Matrix*) nullptr
// This guides the compiler to the correct overload which is the one that takes pointers
return evaluateError(x...,
std::forward<OptionalJacArgs>(H)..., static_cast<OptionalMatrixType>(OptionalNone));
}
/// @}
@ -615,7 +700,7 @@ class NoiseModelFactorN
inline Vector unwhitenedError(
boost::mp11::index_sequence<Indices...>, //
const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
OptionalMatrixVecType H = nullptr) const {
if (this->active(x)) {
if (H) {
return evaluateError(x.at<ValueTypes>(keys_[Indices])...,

12
gtsam/nonlinear/PriorFactor.h
View File

@ -32,6 +32,10 @@ namespace gtsam {
public:
typedef VALUE T;
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor1<VALUE>::evaluateError;
private:
typedef NoiseModelFactorN<VALUE> Base;
@ -91,7 +95,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) 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_);
@ -111,10 +115,10 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(prior_);
}
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
/// traits

6
gtsam/nonlinear/Values-inl.h
View File

@ -373,7 +373,7 @@ namespace gtsam {
/* ************************************************************************* */
template<typename ValueType>
boost::optional<const ValueType&> Values::exists(Key j) const {
const ValueType * Values::exists(Key j) const {
// Find the item
KeyValueMap::const_iterator item = values_.find(j);
@ -381,14 +381,14 @@ namespace gtsam {
// dynamic cast the type and throw exception if incorrect
auto ptr = dynamic_cast<const GenericValue<ValueType>*>(item->second);
if (ptr) {
return ptr->value();
return &ptr->value();
} else {
// NOTE(abe): clang warns about potential side effects if done in typeid
const Value* value = item->second;
throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
}
} else {
return boost::none;
return nullptr;
}
}

6
gtsam/nonlinear/Values.h
View File

@ -184,12 +184,12 @@ namespace gtsam {
* exists. */
bool exists(Key j) const;
/** Check if a value with key \c j exists, returns the value with type
* \c Value if the key does exist, or boost::none if it does not exist.
/** Check if a value with key \c j exists, returns a pointer to the const version of the value
* \c Value if the key does exist, or nullptr if it does not exist.
* Throws DynamicValuesIncorrectType if the value type associated with the
* requested key does not match the stored value type. */
template<typename ValueType>
boost::optional<const ValueType&> exists(Key j) const;
const ValueType * exists(Key j) const;
/** Find an element by key, returning an iterator, or end() if the key was
* not found. */

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);

2
gtsam/nonlinear/tests/testValues.cpp
View File

@ -308,7 +308,7 @@ TEST(Values, exists_)
config0.insert(key1, 1.0);
config0.insert(key2, 2.0);
boost::optional<const double&> v = config0.exists<double>(key1);
const double* v = config0.exists<double>(key1);
DOUBLES_EQUAL(1.0,*v,1e-9);
}

4
gtsam/sam/BearingFactor.h
View File

@ -62,9 +62,7 @@ struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
}
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
{
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const {
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = unwhitenedError(

4
gtsam/sam/BearingRangeFactor.h
View File

@ -74,9 +74,7 @@ class BearingRangeFactor
}
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
{
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const {
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = this->unwhitenedError(

25
gtsam/sam/RangeFactor.h
View File

@ -58,16 +58,15 @@ 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,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
{
Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const {
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const auto& keys = Factor::keys();
const Vector error = Base::unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs);
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs);
if (H1) *H1 = Hs[0];
if (H2) *H2 = Hs[1];
return error;
@ -137,9 +136,7 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
}
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
{
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const {
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = Base::unwhitenedError(
@ -150,6 +147,12 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
return error;
}
// An evaluateError overload to accept matrices (Matrix&) and pass it to the
// OptionalMatrixType evaluateError overload
Vector evaluateError(const A1& a1, const A2& a2, Matrix& H1, Matrix& H2) const {
return evaluateError(a1, a2, &H1, &H2);
}
/** print contents */
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {

2
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -214,7 +214,7 @@ TEST( RangeFactor, Jacobian2D ) {
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
factor.evaluateError(pose, point, &H1Actual, &H2Actual);
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;

8
gtsam/sfm/ShonanFactor.cpp
View File

@ -75,8 +75,8 @@ bool ShonanFactor<d>::equals(const NonlinearFactor &expected,
//******************************************************************************
template <size_t d>
void ShonanFactor<d>::fillJacobians(const Matrix &M1, const Matrix &M2,
boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const {
OptionalMatrixType H1,
OptionalMatrixType H2) const {
gttic(ShonanFactor_Jacobians);
const size_t dim = p_ * d; // Stiefel manifold dimension
@ -106,8 +106,8 @@ void ShonanFactor<d>::fillJacobians(const Matrix &M1, const Matrix &M2,
//******************************************************************************
template <size_t d>
Vector ShonanFactor<d>::evaluateError(const SOn &Q1, const SOn &Q2,
boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const {
OptionalMatrixType H1,
OptionalMatrixType H2) const {
gttic(ShonanFactor_evaluateError);
const Matrix &M1 = Q1.matrix();

13
gtsam/sfm/ShonanFactor.h
View File

@ -42,6 +42,10 @@ class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN<SOn, SOn> {
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
public:
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor2<SOn, SOn>::evaluateError;
/// @name Constructor
/// @{
@ -71,17 +75,14 @@ public:
/// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]
/// projects down from SO(p) to the Stiefel manifold of px3 matrices.
Vector
evaluateError(const SOn &Q1, const SOn &Q2,
boost::optional<Matrix &> H1 = boost::none,
boost::optional<Matrix &> H2 = boost::none) const override;
Vector evaluateError(const SOn& Q1, const SOn& Q2, OptionalMatrixType H1, OptionalMatrixType H2) const override;
/// @}
private:
/// Calculate Jacobians if asked, Only implemented for d=2 and 3 in .cpp
void fillJacobians(const Matrix &M1, const Matrix &M2,
boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const;
OptionalMatrixType H1,
OptionalMatrixType H2) const;
};
// Explicit instantiation for d=2 and d=3 in .cpp file:

8
gtsam/sfm/TranslationFactor.h
View File

@ -45,6 +45,10 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
Point3 measured_w_aZb_;
public:
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor2<Point3, Point3>::evaluateError;
/// default constructor
TranslationFactor() {}
@ -65,8 +69,8 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
*/
Vector evaluateError(
const Point3& Ta, const Point3& Tb,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1,
OptionalMatrixType H2) const override {
const Point3 dir = Tb - Ta;
Matrix33 H_predicted_dir;
const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);

9
gtsam/slam/BetweenFactor.h
View File

@ -56,6 +56,9 @@ namespace gtsam {
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<BetweenFactor> shared_ptr;
@ -105,13 +108,13 @@ namespace gtsam {
/// @{
/// evaluate error, returns vector of errors size of tangent space
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) 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
typename traits<T>::ChartJacobian::Jacobian Hlocal;
Vector rval = traits<T>::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0);
Vector rval = traits<T>::Local(measured_, hx, OptionalNone, (H1 || H2) ? &Hlocal : 0);
if (H1) *H1 = Hlocal * (*H1);
if (H2) *H2 = Hlocal * (*H2);
return rval;

24
gtsam/slam/BoundingConstraint.h
View File

@ -35,6 +35,9 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
typedef NoiseModelFactorN<VALUE> Base;
typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
@ -54,8 +57,8 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
* Must have optional argument for derivative with 1xN matrix, where
* N = X::dim()
*/
virtual double value(const X& x, boost::optional<Matrix&> H =
boost::none) const = 0;
virtual double value(const X& x, OptionalMatrixType H =
OptionalNone) const = 0;
/** active when constraint *NOT* met */
bool active(const Values& c) const override {
@ -64,10 +67,9 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
}
Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const override {
Vector evaluateError(const X& x, OptionalMatrixType H) const override {
Matrix D;
double error = value(x, D) - threshold_;
double error = value(x, &D) - threshold_;
if (H) {
if (isGreaterThan_) *H = D;
else *H = -1.0 * D;
@ -105,6 +107,9 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
typedef NoiseModelFactorN<VALUE1, VALUE2> Base;
typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
@ -123,8 +128,8 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
* Must have optional argument for derivatives)
*/
virtual double value(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const = 0;
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const = 0;
/** active when constraint *NOT* met */
bool active(const Values& c) const override {
@ -134,10 +139,9 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
}
Vector evaluateError(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
Matrix D1, D2;
double error = value(x1, x2, D1, D2) - threshold_;
double error = value(x1, x2, &D1, &D2) - threshold_;
if (H1) {
if (isGreaterThan_) *H1 = D1;
else *H1 = -1.0 * D1;

12
gtsam/slam/EssentialMatrixConstraint.cpp
View File

@ -43,16 +43,20 @@ bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected,
/* ************************************************************************* */
Vector EssentialMatrixConstraint::evaluateError(const Pose3& p1,
const Pose3& p2, boost::optional<Matrix&> Hp1,
boost::optional<Matrix&> Hp2) const {
const Pose3& p2, OptionalMatrixType Hp1,
OptionalMatrixType Hp2) const {
// compute relative Pose3 between p1 and p2
Pose3 _1P2_ = p1.between(p2, Hp1, Hp2);
// convert to EssentialMatrix
Matrix D_hx_1P2;
EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_,
(Hp1 || Hp2) ? boost::optional<Matrix&>(D_hx_1P2) : boost::none);
EssentialMatrix hx;
if (Hp1 || Hp2) {
hx = EssentialMatrix::FromPose3(_1P2_, D_hx_1P2);
} else {
hx = EssentialMatrix::FromPose3(_1P2_, OptionalNone);
}
// Calculate derivatives if needed
if (Hp1) {

6
gtsam/slam/EssentialMatrixConstraint.h
View File

@ -38,6 +38,9 @@ private:
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
@ -79,8 +82,7 @@ public:
/** vector of errors */
Vector evaluateError(const Pose3& p1, const Pose3& p2,
boost::optional<Matrix&> Hp1 = boost::none, //
boost::optional<Matrix&> Hp2 = boost::none) const override;
OptionalMatrixType Hp1, OptionalMatrixType Hp2) const override;
/** return the measured */
const EssentialMatrix& measured() const {

38
gtsam/slam/EssentialMatrixFactor.h
View File

@ -38,6 +38,10 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
typedef EssentialMatrixFactor This;
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/**
* Constructor
* @param key Essential Matrix variable key
@ -90,8 +94,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
/// vector of errors returns 1D vector
Vector evaluateError(
const EssentialMatrix& E,
boost::optional<Matrix&> H = boost::none) const override {
const EssentialMatrix& E, OptionalMatrixType H) const override {
Vector error(1);
error << E.error(vA_, vB_, H);
return error;
@ -115,6 +118,10 @@ class EssentialMatrixFactor2
typedef EssentialMatrixFactor2 This;
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/**
* Constructor
* @param key1 Essential Matrix variable key
@ -173,8 +180,7 @@ class EssentialMatrixFactor2
*/
Vector evaluateError(
const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> Dd = boost::none) const override {
OptionalMatrixType DE, OptionalMatrixType Dd) const override {
// We have point x,y in image 1
// Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
// We then convert to second camera by P2 = 1R2'*(P1-1T2)
@ -235,6 +241,10 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
Rot3 cRb_; ///< Rotation from body to camera frame
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/**
* Constructor
* @param key1 Essential Matrix variable key
@ -284,18 +294,19 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
*/
Vector evaluateError(
const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> Dd = boost::none) const override {
OptionalMatrixType DE, OptionalMatrixType Dd) const override {
if (!DE) {
// Convert E from body to camera frame
EssentialMatrix cameraE = cRb_ * E;
// Evaluate error
return Base::evaluateError(cameraE, d, boost::none, Dd);
return Base::evaluateError(cameraE, d, OptionalNone, Dd);
} else {
// Version with derivatives
Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd);
// Using the pointer version of evaluateError since the Base class (EssentialMatrixFactor2)
// does not have the matrix reference version of evaluateError
Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd);
*DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5)
return e;
}
@ -332,6 +343,10 @@ class EssentialMatrixFactor4
typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/**
* Constructor
* @param keyE Essential Matrix (from camera B to A) variable key
@ -372,13 +387,12 @@ class EssentialMatrixFactor4
*/
Vector evaluateError(
const EssentialMatrix& E, const CALIBRATION& K,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
// converting from pixel coordinates to normalized coordinates cA and cB
JacobianCalibration cA_H_K; // dcA/dK
JacobianCalibration cB_H_K; // dcB/dK
Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none);
Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none);
Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, OptionalNone);
Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, OptionalNone);
// convert to homogeneous coordinates
Vector3 vA = EssentialMatrix::Homogeneous(cA);

21
gtsam/slam/FrobeniusFactor.h
View File

@ -54,6 +54,10 @@ class FrobeniusPrior : public NoiseModelFactorN<Rot> {
Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate
public:
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor1<Rot>::evaluateError;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Constructor
@ -64,8 +68,7 @@ class FrobeniusPrior : public NoiseModelFactorN<Rot> {
}
/// Error is just Frobenius norm between Rot element and vectorized matrix M.
Vector evaluateError(const Rot& R,
boost::optional<Matrix&> H = boost::none) const override {
Vector evaluateError(const Rot& R, OptionalMatrixType H) const override {
return R.vec(H) - vecM_; // Jacobian is computed only when needed.
}
};
@ -79,6 +82,10 @@ class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor2<Rot, Rot>::evaluateError;
/// Constructor
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
@ -86,8 +93,7 @@ class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
/// Error is just Frobenius norm between rotation matrices.
Vector evaluateError(const Rot& R1, const Rot& R2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
Vector error = R2.vec(H2) - R1.vec(H1);
if (H1) *H1 = -*H1;
return error;
@ -108,6 +114,10 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor2<Rot, Rot>::evaluateError;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// @name Constructor
@ -150,8 +160,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
/// Error is Frobenius norm between R1*R12 and R2.
Vector evaluateError(const Rot& R1, const Rot& R2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) 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);

10
gtsam/slam/GeneralSFMFactor.h
View File

@ -76,6 +76,9 @@ public:
typedef GeneralSFMFactor<CAMERA, LANDMARK> This;///< typedef for this object
typedef NoiseModelFactorN<CAMERA, LANDMARK> Base;///< typedef for the base class
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
@ -123,7 +126,7 @@ public:
/** h(x)-z */
Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
try {
return camera.project2(point,H1,H2) - measured_;
}
@ -258,10 +261,7 @@ public:
/** h(x)-z */
Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const override
{
OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
try {
Camera camera(pose3,calib);
return camera.project(point, H1, H2, H3) - measured_;

6
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -23,8 +23,8 @@ void OrientedPlane3Factor::print(const string& s,
//***************************************************************************
Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
const OrientedPlane3& plane, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
const OrientedPlane3& plane, OptionalMatrixType H1,
OptionalMatrixType H2) const {
Matrix36 predicted_H_pose;
Matrix33 predicted_H_plane, error_H_predicted;
@ -64,7 +64,7 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
//***************************************************************************
Vector OrientedPlane3DirectionPrior::evaluateError(
const OrientedPlane3& plane, boost::optional<Matrix&> H) const {
const OrientedPlane3& plane, OptionalMatrixType H) const {
Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal();
Matrix2 H_p;

14
gtsam/slam/OrientedPlane3Factor.h
View File

@ -21,6 +21,10 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, Oriente
typedef NoiseModelFactorN<Pose3, OrientedPlane3> Base;
public:
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor2<Pose3, OrientedPlane3>::evaluateError;
/// Constructor
OrientedPlane3Factor() {
}
@ -44,8 +48,7 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, Oriente
/// evaluateError
Vector evaluateError(
const Pose3& pose, const OrientedPlane3& plane,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override;
OptionalMatrixType H1, OptionalMatrixType H2) const override;
};
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
@ -55,6 +58,10 @@ class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<Orien
typedef NoiseModelFactorN<OrientedPlane3> Base;
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
typedef OrientedPlane3DirectionPrior This;
/// Constructor
OrientedPlane3DirectionPrior() {
@ -72,8 +79,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,
boost::optional<Matrix&> H = boost::none) const override;
Vector evaluateError(const OrientedPlane3& plane, OptionalMatrixType H) const override;
};
} // gtsam

5
gtsam/slam/PoseRotationPrior.h
View File

@ -25,6 +25,9 @@ public:
typedef typename POSE::Translation Translation;
typedef typename POSE::Rotation Rotation;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
GTSAM_CONCEPT_POSE_TYPE(Pose)
GTSAM_CONCEPT_GROUP_TYPE(Pose)
GTSAM_CONCEPT_LIE_TYPE(Rotation)
@ -75,7 +78,7 @@ public:
}
/** h(x)-z */
Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
Vector evaluateError(const Pose& pose, OptionalMatrixType H) const override {
const Rotation& newR = pose.rotation();
if (H) {
*H = Matrix::Zero(rDim, xDim);

5
gtsam/slam/PoseTranslationPrior.h
View File

@ -25,6 +25,9 @@ public:
typedef POSE Pose;
typedef typename POSE::Translation Translation;
typedef typename POSE::Rotation Rotation;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
GTSAM_CONCEPT_POSE_TYPE(Pose)
GTSAM_CONCEPT_GROUP_TYPE(Pose)
@ -59,7 +62,7 @@ public:
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** h(x)-z */
Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) 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);

5
gtsam/slam/ProjectionFactor.h
View File

@ -54,6 +54,9 @@ namespace gtsam {
/// shorthand for base class type
typedef NoiseModelFactorN<POSE, LANDMARK> Base;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// shorthand for this class
typedef GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
@ -133,7 +136,7 @@ namespace gtsam {
/// Evaluate error h(x)-z and optionally derivatives
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 {
try {
if(body_P_sensor_) {
if(H1) {

15
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);
}
@ -63,6 +63,9 @@ public:
typedef NoiseModelFactorN<POINT, TRANSFORM, POINT> Base;
typedef ReferenceFrameFactor<POINT, TRANSFORM> This;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
typedef POINT Point;
typedef TRANSFORM Transform;
@ -95,12 +98,12 @@ public:
/** Combined cost and derivative function using boost::optional */
Vector evaluateError(const Point& global, const Transform& trans, const Point& local,
boost::optional<Matrix&> Dforeign = boost::none,
boost::optional<Matrix&> Dtrans = boost::none,
boost::optional<Matrix&> Dlocal = boost::none) const override {
OptionalMatrixType Dforeign, OptionalMatrixType Dtrans,
OptionalMatrixType Dlocal) const override {
Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
if (Dlocal)
if (Dlocal) {
*Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);
}
return traits<Point>::Local(local,newlocal);
}

11
gtsam/slam/RotateFactor.h
View File

@ -29,6 +29,9 @@ class RotateFactor: public NoiseModelFactorN<Rot3> {
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// Constructor
RotateFactor(Key key, const Rot3& P, const Rot3& Z,
const SharedNoiseModel& model) :
@ -50,8 +53,7 @@ public:
}
/// vector of errors returns 2D vector
Vector evaluateError(const Rot3& R,
boost::optional<Matrix&> H = boost::none) 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
@ -73,6 +75,9 @@ class RotateDirectionsFactor: public NoiseModelFactorN<Rot3> {
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// Constructor
RotateDirectionsFactor(Key key, const Unit3& i_p, const Unit3& c_z,
const SharedNoiseModel& model) :
@ -102,7 +107,7 @@ public:
}
/// vector of errors returns 2D vector
Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) 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) {

43
gtsam/slam/SmartFactorBase.h
View File

@ -198,13 +198,16 @@ protected:
}
}
/// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and
/// derivatives. This is the error before the noise model is applied.
/** Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and
* derivatives. This is the error before the noise model is applied.
* The templated version described above must finally get resolved to this
* function.
*/
template <class POINT>
Vector unwhitenedError(
const Cameras& cameras, const POINT& point,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const {
typename Cameras::FBlocks* Fs = nullptr, //
Matrix* E = nullptr) const {
// Reproject, with optional derivatives.
Vector error = cameras.reprojectionError(point, measured_, Fs, E);
@ -233,6 +236,19 @@ protected:
return error;
}
/**
* An overload of unwhitenedError. This allows
* end users to provide optional arguments that are l-value references
* to the matrices and vectors that will be used to store the results instead
* of pointers.
*/
template<class POINT, class ...OptArgs>
Vector unwhitenedError(
const Cameras& cameras, const POINT& point,
OptArgs&&... optArgs) const {
return unwhitenedError(cameras, point, (&optArgs)...);
}
/**
* This corrects the Jacobians for the case in which some 2D measurement is
* missing (nan). In practice, this does not do anything in the monocular
@ -240,8 +256,21 @@ protected:
*/
virtual void correctForMissingMeasurements(
const Cameras& cameras, Vector& ue,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
boost::optional<Matrix&> E = boost::none) const {}
typename Cameras::FBlocks* Fs = nullptr,
Matrix* E = nullptr) const {}
/**
* An overload of correctForMissingMeasurements. This allows
* end users to provide optional arguments that are l-value references
* to the matrices and vectors that will be used to store the results instead
* of pointers.
*/
template<class ...OptArgs>
void correctForMissingMeasurements(
const Cameras& cameras, Vector& ue,
OptArgs&&... optArgs) const {
correctForMissingMeasurements(cameras, ue, (&optArgs)...);
}
/**
* Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) -
@ -288,7 +317,7 @@ protected:
// As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar)
// Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z|
// = |A*dx - (z-h(x_bar))|
b = -unwhitenedError(cameras, point, Fs, E);
b = -unwhitenedError(cameras, point, &Fs, &E);
}
/**

5
gtsam/slam/SmartProjectionFactor.h
View File

@ -337,8 +337,9 @@ protected:
*/
bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
cameras.project2(*result_, boost::none, E);
if (nonDegenerate) {
cameras.project2(*result_, nullptr, &E);
}
return nonDegenerate;
}

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
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/**
* Default constructor
*/
@ -120,7 +123,7 @@ public:
/** h(x)-z */
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 {
try {
if(body_P_sensor_) {
if(H1) {

8
gtsam/slam/TriangulationFactor.h
View File

@ -62,6 +62,9 @@ public:
/// shorthand for a smart pointer to a factor
using shared_ptr = boost::shared_ptr<This>;
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor1<Point3>::evaluateError;
/// Default constructor
TriangulationFactor() :
throwCheirality_(false), verboseCheirality_(false) {
@ -119,10 +122,9 @@ public:
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
boost::none) const override {
Vector evaluateError(const Point3& point, OptionalMatrixType H2) const override {
try {
return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
return traits<Measurement>::Local(measured_, camera_.project2(point, OptionalNone, H2));
} catch (CheiralityException& e) {
if (H2)
*H2 = Matrix::Zero(traits<Measurement>::dimension, 3);

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);
auto 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));

9
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -35,6 +35,9 @@ protected:
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/** Standard constructor */
FullIMUFactor(const Vector3& accel, const Vector3& gyro,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
@ -84,8 +87,7 @@ public:
* z - h(x1,x2)
*/
Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
Vector9 z;
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
@ -99,8 +101,7 @@ public:
/** dummy version that fails for non-dynamic poses */
virtual Vector evaluateError(const Pose3& x1, const Pose3& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
OptionalMatrixType H1, OptionalMatrixType H2) const {
assert(false);
return Vector6::Zero();
}

9
gtsam_unstable/dynamics/IMUFactor.h
View File

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

32
gtsam_unstable/dynamics/Pendulum.h
View File

@ -33,6 +33,9 @@ protected:
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
typedef boost::shared_ptr<PendulumFactor1> shared_ptr;
///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step
@ -46,9 +49,8 @@ public:
/** q_k + h*v - q_k1 = 0, with optional derivatives */
Vector evaluateError(const double& qk1, const double& qk, const double& v,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3) const override {
const size_t p = 1;
if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Identity(p,p);
@ -81,6 +83,9 @@ protected:
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
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
@ -94,9 +99,8 @@ public:
/** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */
Vector evaluateError(const double & vk1, const double & vk, const double & q,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3) const override {
const size_t p = 1;
if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Identity(p,p);
@ -130,6 +134,9 @@ protected:
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
typedef boost::shared_ptr<PendulumFactorPk > shared_ptr;
///Constructor
@ -145,9 +152,8 @@ public:
/** 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,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3) const override {
const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1;
@ -186,6 +192,9 @@ protected:
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
typedef boost::shared_ptr<PendulumFactorPk1 > shared_ptr;
///Constructor
@ -201,9 +210,8 @@ public:
/** 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,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3) const override {
const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1;

23
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -29,6 +29,10 @@ class Reconstruction : public NoiseModelFactorN<Pose3, Pose3, Vector6> {
double h_; // time step
typedef NoiseModelFactorN<Pose3, Pose3, Vector6> Base;
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) :
Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey,
xiKey), h_(h) {
@ -42,9 +46,8 @@ public:
/** \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,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3) const override {
Matrix6 D_exphxi_xi;
Pose3 exphxi = Pose3::Expmap(h_ * xik, H3 ? &D_exphxi_xi : 0);
@ -89,6 +92,10 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactorN<Vector6, Vector
typedef NoiseModelFactorN<Vector6, Vector6, Pose3> Base;
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,
double h, const Matrix& Inertia, const Vector& Fu, double m,
double mu = 1000.0) :
@ -108,9 +115,8 @@ public:
* pk_1 = CT_TLN(-h*xi_k_1)*Inertia*xi_k_1
* */
Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3) const override {
Vector muk = Inertia_*xik;
Vector muk_1 = Inertia_*xik_1;
@ -161,9 +167,8 @@ public:
}
Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3) const {
if (H1) {
(*H1) = numericalDerivative31(
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(

8
gtsam_unstable/dynamics/VelocityConstraint.h
View File

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

8
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

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

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

@ -7,6 +7,8 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/dynamics/SimpleHelicopter.h>
#include "gtsam/base/Vector.h"
#include "gtsam/geometry/Pose3.h"
/* ************************************************************************* */
using namespace std::placeholders;
@ -56,29 +58,16 @@ TEST( Reconstruction, evaluateError) {
EXPECT(
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&)>(
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);
std::function<Vector(const Pose3&, const Pose3&, const Vector6&)> f =
[&constraint](const Pose3& a1, const Pose3& a2, const Vector6& a3) {
return constraint.evaluateError(a1, a2, a3);
};
Matrix numericalH2 = numericalDerivative32(
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 numericalH1 = numericalDerivative31(f, g2, g1, V1_g1, 1e-5);
Matrix numericalH3 = numericalDerivative33(
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 numericalH2 = numericalDerivative32(f, g2, g1, V1_g1, 1e-5);
Matrix numericalH3 = numericalDerivative33(f, g2, g1, V1_g1, 1e-5);
EXPECT(assert_equal(numericalH1,H1,1e-5));
EXPECT(assert_equal(numericalH2,H2,1e-5));
@ -118,26 +107,16 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
Matrix H1, H2, H3;
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&)>(
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
);
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)> f =
[&constraint](const Vector6& a1, const Vector6& a2, const Pose3& a3) {
return constraint.evaluateError(a1, a2, a3);
};
Matrix numericalH2 = numericalDerivative32(
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 numericalH1 = numericalDerivative31(f, expectedv2, V1_g1, g2, 1e-5);
Matrix numericalH3 = numericalDerivative33(
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 numericalH2 = numericalDerivative32(f, expectedv2, V1_g1, g2, 1e-5);
Matrix numericalH3 = numericalDerivative33(f, expectedv2, V1_g1, g2, 1e-5);
EXPECT(assert_equal(numericalH1,H1,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/Pose3.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
@ -83,9 +84,9 @@ public:
*/
inline gtsam::Point2 project(const Vector5& pw,
double rho,
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none) const {
OptionalJacobian<2,6> H1 = {},
OptionalJacobian<2,5> H2 = {},
OptionalJacobian<2,1> H3 = {}) const {
gtsam::Point3 ray_base(pw.segment(0,3));
double theta = pw(3), phi = pw(4);

43
gtsam_unstable/geometry/Pose3Upright.cpp
View File

@ -5,7 +5,10 @@
* @author Alex Cunningham
*/
#include <boost/mpl/assert.hpp>
#include <iostream>
#include "gtsam/base/OptionalJacobian.h"
#include "gtsam/base/Vector.h"
#include <gtsam_unstable/geometry/Pose3Upright.h>
@ -78,27 +81,34 @@ Pose3 Pose3Upright::pose() const {
}
/* ************************************************************************* */
Pose3Upright Pose3Upright::inverse(boost::optional<Matrix&> H1) const {
Pose3Upright result(T_.inverse(H1), -z_);
if (H1) {
Matrix H1_ = -I_4x4;
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
*H1 = H1_;
Pose3Upright Pose3Upright::inverse(OptionalJacobian<4, 4> H1) const {
if (!H1) {
return Pose3Upright(T_.inverse(), -z_);
}
OptionalJacobian<3, 3>::Jacobian H3x3;
// TODO(kartikarcot): Could not use reference to a view into H1 and reuse memory
// Eigen::Ref<Eigen::Matrix<double, 3, 3>> H3x3 = H1->topLeftCorner(3,3);
Pose3Upright result(T_.inverse(H3x3), -z_);
Matrix H1_ = -I_4x4;
H1_.topLeftCorner(2, 2) = H3x3.topLeftCorner(2, 2);
H1_.topRightCorner(2, 1) = H3x3.topRightCorner(2, 1);
*H1 = H1_;
return result;
}
/* ************************************************************************* */
Pose3Upright Pose3Upright::compose(const Pose3Upright& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<4,4> H1, OptionalJacobian<4,4> H2) const {
if (!H1 && !H2)
return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_);
Pose3Upright result(T_.compose(p2.T_, H1), z_ + p2.z_);
// TODO(kartikarcot): Could not use reference to a view into H1 and reuse memory
OptionalJacobian<3, 3>::Jacobian H3x3;
Pose3Upright result(T_.compose(p2.T_, H3x3), z_ + p2.z_);
if (H1) {
Matrix H1_ = I_4x4;
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
H1_.topLeftCorner(2,2) = H3x3.topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H3x3.topRightCorner(2, 1);
*H1 = H1_;
}
if (H2) *H2 = I_4x4;
@ -107,14 +117,17 @@ Pose3Upright Pose3Upright::compose(const Pose3Upright& p2,
/* ************************************************************************* */
Pose3Upright Pose3Upright::between(const Pose3Upright& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<4,4> H1, OptionalJacobian<4,4> H2) const {
if (!H1 && !H2)
return Pose3Upright(T_.between(p2.T_), p2.z_ - z_);
Pose3Upright result(T_.between(p2.T_, H1, H2), p2.z_ - z_);
// TODO(kartikarcot): Could not use reference to a view into H1 and H2 to reuse memory
OptionalJacobian<3, 3>::Jacobian H3x3_1, H3x3_2;
Pose3Upright result(T_.between(p2.T_, H3x3_1, H3x3_2), p2.z_ - z_);
if (H1) {
Matrix H1_ = -I_4x4;
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
H1_.topLeftCorner(2,2) = H3x3_1.topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H3x3_1.topRightCorner(2, 1);
*H1 = H1_;
}
if (H2) *H2 = I_4x4;

10
gtsam_unstable/geometry/Pose3Upright.h
View File

@ -98,12 +98,12 @@ public:
static Pose3Upright Identity() { return Pose3Upright(); }
/// inverse transformation with derivatives
Pose3Upright inverse(boost::optional<Matrix&> H1=boost::none) const;
Pose3Upright inverse(OptionalJacobian<4,4> H1=boost::none) const;
///compose this transformation onto another (first *this and then p2)
Pose3Upright compose(const Pose3Upright& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<4,4> H1=boost::none,
OptionalJacobian<4,4> H2=boost::none) const;
/// compose syntactic sugar
inline Pose3Upright operator*(const Pose3Upright& T) const { return compose(T); }
@ -113,8 +113,8 @@ public:
* as well as optionally the derivatives
*/
Pose3Upright between(const Pose3Upright& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<4,4> H1=boost::none,
OptionalJacobian<4,4> H2=boost::none) const;
/// @}
/// @name Lie Group

2
gtsam_unstable/geometry/SimWall2D.cpp
View File

@ -25,7 +25,7 @@ bool SimWall2D::equals(const SimWall2D& other, double tol) const {
}
/* ************************************************************************* */
bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) const {
bool SimWall2D::intersects(const SimWall2D& B, Point2* pt) const {
const bool debug = false;
const SimWall2D& A = *this;

10
gtsam_unstable/geometry/SimWall2D.h
View File

@ -51,7 +51,15 @@ namespace gtsam {
* returns true if they intersect, with the intersection
* point in the optional second argument
*/
bool intersects(const SimWall2D& wall, boost::optional<Point2&> pt=boost::none) const;
bool intersects(const SimWall2D& wall, Point2* pt = nullptr) const;
/**
* An overload of intersects that takes an l-value reference to a Point2
* instead of a pointer.
*/
bool intersects(const SimWall2D& wall, Point2& pt) const {
return intersects(wall, &pt);
}
/**
* norm is a 2D point representing the norm of the wall

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);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
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));
}
@ -106,7 +106,7 @@ TEST( InvDepthFactor, Dproject_landmark)
Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
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));
}
@ -118,7 +118,7 @@ TEST( InvDepthFactor, Dproject_inv_depth)
Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
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));
}

8
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -145,7 +145,7 @@ public:
/* ************************************************************************* */
Vector whitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
OptionalMatrixVecType H = nullptr) const {
bool debug = true;
@ -228,6 +228,12 @@ public:
return err_wh_eq;
}
// A function overload that takes a vector of matrices and passes it to the
// function above which uses a pointer to a vector instead.
Vector whitenedError(const Values& x, std::vector<Matrix>& H) const {
return whitenedError(x, &H);
}
/* ************************************************************************* */
Vector calcIndicatorProb(const Values& x) const {

6
gtsam_unstable/slam/BiasedGPSFactor.h
View File

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

10
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -110,6 +110,9 @@ private:
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel> shared_ptr;
@ -299,11 +302,8 @@ public:
}
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4,
OptionalMatrixType H5) const override {
// TODO: Write analytical derivative calculations
// Jacobian w.r.t. Pose1

9
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -109,6 +109,9 @@ private:
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel_NoBias> shared_ptr;
@ -270,10 +273,8 @@ public:
}
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none) const {
OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3,
OptionalMatrixType H4) const {
// TODO: Write analytical derivative calculations
// Jacobian w.r.t. Pose1

6
gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
View File

@ -54,6 +54,9 @@ private:
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<GaussMarkov1stOrderFactor> shared_ptr;
@ -88,8 +91,7 @@ public:
/** vector of errors */
Vector evaluateError(const VALUE& p1, const VALUE& p2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
Vector v1( traits<VALUE>::Logmap(p1) );
Vector v2( traits<VALUE>::Logmap(p2) );

10
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -96,6 +96,9 @@ private:
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<InertialNavFactor_GlobalVelocity> shared_ptr;
@ -226,11 +229,8 @@ public:
/** implement functions needed to derive from Factor */
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4,
OptionalMatrixType H5) const override {
// TODO: Write analytical derivative calculations
// Jacobian w.r.t. Pose1

9
gtsam_unstable/slam/InvDepthFactor3.h
View File

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

8
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

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

8
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

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

12
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -34,7 +34,10 @@ protected:
public:
/// shorthand for base class type
typedef NoiseModelFactorN<Pose3, Vector3> Base;
typedef NoiseModelFactor2<Pose3, Vector3> Base;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// shorthand for this class
typedef InvDepthFactorVariant3a This;
@ -106,8 +109,7 @@ public:
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Vector3& landmark,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
if(H1) {
(*H1) = numericalDerivative11<Vector, Pose3>(
@ -232,9 +234,7 @@ public:
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const override {
OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
if(H1)
(*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,
const Pose3& wTwa, const OrientedPlane3& a_plane,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3) const {
Matrix66 aTai_H_wTwa, aTai_H_wTwi;
Matrix36 predicted_H_aTai;

Some files were not shown because too many files have changed in this diff Show More