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

OptionalJacobian

release/4.3a0
kartik arcot 2023-01-13 14:11:03 -08:00
parent 5880471136
commit 9329bddd8a
91 changed files with 496 additions and 517 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam/base/Lie.h
View File

@ -54,14 +54,14 @@ struct LieGroup {
} }
Class compose(const Class& g, ChartJacobian H1, Class compose(const Class& g, ChartJacobian H1,
ChartJacobian H2 = boost::none) const { ChartJacobian H2 = {}) const {
if (H1) *H1 = g.inverse().AdjointMap(); if (H1) *H1 = g.inverse().AdjointMap();
if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity(); if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
return derived() * g; return derived() * g;
} }
Class between(const Class& g, ChartJacobian H1, Class between(const Class& g, ChartJacobian H1,
ChartJacobian H2 = boost::none) const { ChartJacobian H2 = {}) const {
Class result = derived().inverse() * g; Class result = derived().inverse() * g;
if (H1) *H1 = - result.inverse().AdjointMap(); if (H1) *H1 = - result.inverse().AdjointMap();
if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity(); if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
@ -87,7 +87,7 @@ struct LieGroup {
/// expmap with optional derivatives /// expmap with optional derivatives
Class expmap(const TangentVector& v, // Class expmap(const TangentVector& v, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const { ChartJacobian H1, ChartJacobian H2 = {}) const {
Jacobian D_g_v; Jacobian D_g_v;
Class g = Class::Expmap(v,H2 ? &D_g_v : 0); Class g = Class::Expmap(v,H2 ? &D_g_v : 0);
Class h = compose(g); // derivatives inlined below Class h = compose(g); // derivatives inlined below
@ -98,7 +98,7 @@ struct LieGroup {
/// logmap with optional derivatives /// logmap with optional derivatives
TangentVector logmap(const Class& g, // TangentVector logmap(const Class& g, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const { ChartJacobian H1, ChartJacobian H2 = {}) const {
Class h = between(g); // derivatives inlined below Class h = between(g); // derivatives inlined below
Jacobian D_v_h; Jacobian D_v_h;
TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0); TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0);
@ -139,7 +139,7 @@ struct LieGroup {
/// retract with optional derivatives /// retract with optional derivatives
Class retract(const TangentVector& v, // Class retract(const TangentVector& v, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const { ChartJacobian H1, ChartJacobian H2 = {}) const {
Jacobian D_g_v; Jacobian D_g_v;
Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0); Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0);
Class h = compose(g); // derivatives inlined below Class h = compose(g); // derivatives inlined below
@ -150,7 +150,7 @@ struct LieGroup {
/// localCoordinates with optional derivatives /// localCoordinates with optional derivatives
TangentVector localCoordinates(const Class& g, // TangentVector localCoordinates(const Class& g, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const { ChartJacobian H1, ChartJacobian H2 = {}) const {
Class h = between(g); // derivatives inlined below Class h = between(g); // derivatives inlined below
Jacobian D_v_h; Jacobian D_v_h;
TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
@ -188,38 +188,38 @@ struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
typedef OptionalJacobian<dimension, dimension> ChartJacobian; typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static TangentVector Local(const Class& origin, const Class& other, static TangentVector Local(const Class& origin, const Class& other,
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
return origin.localCoordinates(other, Horigin, Hother); return origin.localCoordinates(other, Horigin, Hother);
} }
static Class Retract(const Class& origin, const TangentVector& v, static Class Retract(const Class& origin, const TangentVector& v,
ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { ChartJacobian Horigin = {}, ChartJacobian Hv = {}) {
return origin.retract(v, Horigin, Hv); return origin.retract(v, Horigin, Hv);
} }
/// @} /// @}
/// @name Lie Group /// @name Lie Group
/// @{ /// @{
static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { static TangentVector Logmap(const Class& m, ChartJacobian Hm = {}) {
return Class::Logmap(m, Hm); return Class::Logmap(m, Hm);
} }
static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) {
return Class::Expmap(v, Hv); return Class::Expmap(v, Hv);
} }
static Class Compose(const Class& m1, const Class& m2, // static Class Compose(const Class& m1, const Class& m2, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
return m1.compose(m2, H1, H2); return m1.compose(m2, H1, H2);
} }
static Class Between(const Class& m1, const Class& m2, // static Class Between(const Class& m1, const Class& m2, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
return m1.between(m2, H1, H2); return m1.between(m2, H1, H2);
} }
static Class Inverse(const Class& m, // static Class Inverse(const Class& m, //
ChartJacobian H = boost::none) { ChartJacobian H = {}) {
return m.inverse(H); return m.inverse(H);
} }
/// @} /// @}
@ -325,8 +325,8 @@ T expm(const Vector& x, int K=7) {
*/ */
template <typename T> template <typename T>
T interpolate(const T& X, const T& Y, double t, T interpolate(const T& X, const T& Y, double t,
typename MakeOptionalJacobian<T, T>::type Hx = boost::none, typename MakeOptionalJacobian<T, T>::type Hx = {},
typename MakeOptionalJacobian<T, T>::type Hy = boost::none) { typename MakeOptionalJacobian<T, T>::type Hy = {}) {
if (Hx || Hy) { if (Hx || Hy) {
typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x; typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
const T between = const T between =

12
gtsam/base/Matrix.h
View File

@ -459,8 +459,8 @@ struct MultiplyWithInverse {
/// A.inverse() * b, with optional derivatives /// A.inverse() * b, with optional derivatives
VectorN operator()(const MatrixN& A, const VectorN& b, VectorN operator()(const MatrixN& A, const VectorN& b,
OptionalJacobian<N, N* N> H1 = boost::none, OptionalJacobian<N, N* N> H1 = {},
OptionalJacobian<N, N> H2 = boost::none) const { OptionalJacobian<N, N> H2 = {}) const {
const MatrixN invA = A.inverse(); const MatrixN invA = A.inverse();
const VectorN c = invA * b; const VectorN c = invA * b;
// The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA] // The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA]
@ -495,16 +495,16 @@ struct MultiplyWithInverseFunction {
/// f(a).inverse() * b, with optional derivatives /// f(a).inverse() * b, with optional derivatives
VectorN operator()(const T& a, const VectorN& b, VectorN operator()(const T& a, const VectorN& b,
OptionalJacobian<N, M> H1 = boost::none, OptionalJacobian<N, M> H1 = {},
OptionalJacobian<N, N> H2 = boost::none) const { OptionalJacobian<N, N> H2 = {}) const {
MatrixN A; MatrixN A;
phi_(a, b, boost::none, A); // get A = f(a) by calling f once phi_(a, b, {}, A); // get A = f(a) by calling f once
const MatrixN invA = A.inverse(); const MatrixN invA = A.inverse();
const VectorN c = invA * b; const VectorN c = invA * b;
if (H1) { if (H1) {
Eigen::Matrix<double, N, M> H; Eigen::Matrix<double, N, M> H;
phi_(a, c, H, boost::none); // get derivative H of forward mapping phi_(a, c, H, {}); // get derivative H of forward mapping
*H1 = -invA* H; *H1 = -invA* H;
} }
if (H2) *H2 = invA; if (H2) *H2 = invA;

53
gtsam/base/OptionalJacobian.h
View File

@ -18,23 +18,18 @@
*/ */
#pragma once #pragma once
#include <cstddef>
#include <gtsam/config.h> // Configuration from CMake #include <gtsam/config.h> // Configuration from CMake
#include <Eigen/Dense> #include <Eigen/Dense>
#include <stdexcept> #include <stdexcept>
#include <string> #include <string>
#ifndef OPTIONALJACOBIAN_NOBOOST
#include <boost/optional.hpp>
#endif
namespace gtsam { namespace gtsam {
/** /**
* OptionalJacobian is an Eigen::Ref like class that can take be constructed using * OptionalJacobian is an Eigen::Ref like class that can take be constructed using
* either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic * either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic
* matrix will be resized. Finally, there is a constructor that takes * matrix will be resized.
* boost::none, the default constructor acts like boost::none, and
* boost::optional<Eigen::MatrixXd&> is also supported for backwards compatibility.
* Below this class, a dynamic version is also implemented. * Below this class, a dynamic version is also implemented.
*/ */
template<int Rows, int Cols> template<int Rows, int Cols>
@ -66,11 +61,18 @@ private:
public: public:
/// Default constructor acts like boost::none /// Default constructor
OptionalJacobian() : OptionalJacobian() :
map_(nullptr) { map_(nullptr) {
} }
/// Default constructor with nullptr_t
/// To guide the compiler when nullptr
/// is passed to args of the type OptionalJacobian
OptionalJacobian(std::nullptr_t /*unused*/) :
map_(nullptr) {
}
/// Constructor that will usurp data of a fixed-size matrix /// Constructor that will usurp data of a fixed-size matrix
OptionalJacobian(Jacobian& fixed) : OptionalJacobian(Jacobian& fixed) :
map_(nullptr) { map_(nullptr) {
@ -118,24 +120,6 @@ public:
} }
} }
#ifndef OPTIONALJACOBIAN_NOBOOST
/// Constructor with boost::none just makes empty
OptionalJacobian(boost::none_t /*none*/) :
map_(nullptr) {
}
/// Constructor compatible with old-style derivatives
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
map_(nullptr) {
if (optional) {
optional->resize(Rows, Cols);
usurp(optional->data());
}
}
#endif
/// Constructor that will usurp data of a block expression /// Constructor that will usurp data of a block expression
/// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible
// template <typename Derived, bool InnerPanel> // template <typename Derived, bool InnerPanel>
@ -200,7 +184,7 @@ private:
public: public:
/// Default constructor acts like boost::none /// Default constructor acts like
OptionalJacobian() : OptionalJacobian() :
pointer_(nullptr) { pointer_(nullptr) {
} }
@ -211,21 +195,6 @@ public:
/// Construct from refrence to dynamic matrix /// Construct from refrence to dynamic matrix
OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {} OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {}
#ifndef OPTIONALJACOBIAN_NOBOOST
/// Constructor with boost::none just makes empty
OptionalJacobian(boost::none_t /*none*/) :
pointer_(nullptr) {
}
/// Constructor compatible with old-style derivatives
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
pointer_(nullptr) {
if (optional) pointer_ = &(*optional);
}
#endif
/// Return true if allocated, false if default constructor was used /// Return true if allocated, false if default constructor was used
operator bool() const { operator bool() const {
return pointer_!=nullptr; return pointer_!=nullptr;

14
gtsam/base/ProductLieGroup.h
View File

@ -75,14 +75,14 @@ public:
typedef OptionalJacobian<dimension, dimension> ChartJacobian; typedef OptionalJacobian<dimension, dimension> ChartJacobian;
ProductLieGroup retract(const TangentVector& v, // ProductLieGroup retract(const TangentVector& v, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { ChartJacobian H1 = {}, ChartJacobian H2 = {}) const {
if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet"); if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet");
G g = traits<G>::Retract(this->first, v.template head<dimension1>()); G g = traits<G>::Retract(this->first, v.template head<dimension1>());
H h = traits<H>::Retract(this->second, v.template tail<dimension2>()); H h = traits<H>::Retract(this->second, v.template tail<dimension2>());
return ProductLieGroup(g,h); return ProductLieGroup(g,h);
} }
TangentVector localCoordinates(const ProductLieGroup& g, // TangentVector localCoordinates(const ProductLieGroup& g, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { ChartJacobian H1 = {}, ChartJacobian H2 = {}) const {
if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet"); if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet");
typename traits<G>::TangentVector v1 = traits<G>::Local(this->first, g.first); typename traits<G>::TangentVector v1 = traits<G>::Local(this->first, g.first);
typename traits<H>::TangentVector v2 = traits<H>::Local(this->second, g.second); typename traits<H>::TangentVector v2 = traits<H>::Local(this->second, g.second);
@ -101,7 +101,7 @@ protected:
public: public:
ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1, ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1,
ChartJacobian H2 = boost::none) const { ChartJacobian H2 = {}) const {
Jacobian1 D_g_first; Jacobian2 D_h_second; Jacobian1 D_g_first; Jacobian2 D_h_second;
G g = traits<G>::Compose(this->first,other.first, H1 ? &D_g_first : 0); G g = traits<G>::Compose(this->first,other.first, H1 ? &D_g_first : 0);
H h = traits<H>::Compose(this->second,other.second, H1 ? &D_h_second : 0); H h = traits<H>::Compose(this->second,other.second, H1 ? &D_h_second : 0);
@ -114,7 +114,7 @@ public:
return ProductLieGroup(g,h); return ProductLieGroup(g,h);
} }
ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1, ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1,
ChartJacobian H2 = boost::none) const { ChartJacobian H2 = {}) const {
Jacobian1 D_g_first; Jacobian2 D_h_second; Jacobian1 D_g_first; Jacobian2 D_h_second;
G g = traits<G>::Between(this->first,other.first, H1 ? &D_g_first : 0); G g = traits<G>::Between(this->first,other.first, H1 ? &D_g_first : 0);
H h = traits<H>::Between(this->second,other.second, H1 ? &D_h_second : 0); H h = traits<H>::Between(this->second,other.second, H1 ? &D_h_second : 0);
@ -137,7 +137,7 @@ public:
} }
return ProductLieGroup(g,h); return ProductLieGroup(g,h);
} }
static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = {}) {
Jacobian1 D_g_first; Jacobian2 D_h_second; Jacobian1 D_g_first; Jacobian2 D_h_second;
G g = traits<G>::Expmap(v.template head<dimension1>(), Hv ? &D_g_first : 0); G g = traits<G>::Expmap(v.template head<dimension1>(), Hv ? &D_g_first : 0);
H h = traits<H>::Expmap(v.template tail<dimension2>(), Hv ? &D_h_second : 0); H h = traits<H>::Expmap(v.template tail<dimension2>(), Hv ? &D_h_second : 0);
@ -148,7 +148,7 @@ public:
} }
return ProductLieGroup(g,h); return ProductLieGroup(g,h);
} }
static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = {}) {
Jacobian1 D_g_first; Jacobian2 D_h_second; Jacobian1 D_g_first; Jacobian2 D_h_second;
typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first, Hp ? &D_g_first : 0); typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first, Hp ? &D_g_first : 0);
typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second, Hp ? &D_h_second : 0); typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second, Hp ? &D_h_second : 0);
@ -161,7 +161,7 @@ public:
} }
return v; return v;
} }
static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = {}) {
return Logmap(p, Hp); return Logmap(p, Hp);
} }
ProductLieGroup expmap(const TangentVector& v) const { ProductLieGroup expmap(const TangentVector& v) const {

6
gtsam/base/TestableAssertions.h
View File

@ -50,11 +50,11 @@ template<class V>
bool assert_equal(const std::optional<V>& expected, bool assert_equal(const std::optional<V>& expected,
const std::optional<V>& actual, double tol = 1e-9) { const std::optional<V>& actual, double tol = 1e-9) {
if (!expected && actual) { if (!expected && actual) {
std::cout << "expected is boost::none, while actual is not" << std::endl; std::cout << "expected is {}, while actual is not" << std::endl;
return false; return false;
} }
if (expected && !actual) { if (expected && !actual) {
std::cout << "actual is boost::none, while expected is not" << std::endl; std::cout << "actual is {}, while expected is not" << std::endl;
return false; return false;
} }
if (!expected && !actual) if (!expected && !actual)
@ -65,7 +65,7 @@ bool assert_equal(const std::optional<V>& expected,
template<class V> template<class V>
bool assert_equal(const V& expected, const std::optional<V>& actual, double tol = 1e-9) { bool assert_equal(const V& expected, const std::optional<V>& actual, double tol = 1e-9) {
if (!actual) { if (!actual) {
std::cout << "actual is boost::none" << std::endl; std::cout << "actual is {}" << std::endl;
return false; return false;
} }
return assert_equal(expected, *actual, tol); return assert_equal(expected, *actual, tol);

1
gtsam/base/Value.h
View File

@ -21,6 +21,7 @@
#include <gtsam/config.h> // Configuration from CMake #include <gtsam/config.h> // Configuration from CMake
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <boost/make_shared.hpp>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <boost/serialization/assume_abstract.hpp> #include <boost/serialization/assume_abstract.hpp>
#include <memory> #include <memory>

60
gtsam/base/VectorSpace.h
View File

@ -32,7 +32,7 @@ struct VectorSpaceImpl {
static int GetDimension(const Class&) { return N;} static int GetDimension(const Class&) { return N;}
static TangentVector Local(const Class& origin, const Class& other, static TangentVector Local(const Class& origin, const Class& other,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
if (H1) *H1 = - Jacobian::Identity(); if (H1) *H1 = - Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity();
Class v = other-origin; Class v = other-origin;
@ -40,7 +40,7 @@ struct VectorSpaceImpl {
} }
static Class Retract(const Class& origin, const TangentVector& v, static Class Retract(const Class& origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
if (H1) *H1 = Jacobian::Identity(); if (H1) *H1 = Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity();
return origin + v; return origin + v;
@ -51,31 +51,31 @@ struct VectorSpaceImpl {
/// @name Lie Group /// @name Lie Group
/// @{ /// @{
static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { static TangentVector Logmap(const Class& m, ChartJacobian Hm = {}) {
if (Hm) *Hm = Jacobian::Identity(); if (Hm) *Hm = Jacobian::Identity();
return m.vector(); return m.vector();
} }
static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) {
if (Hv) *Hv = Jacobian::Identity(); if (Hv) *Hv = Jacobian::Identity();
return Class(v); return Class(v);
} }
static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = {},
ChartJacobian H2 = boost::none) { ChartJacobian H2 = {}) {
if (H1) *H1 = Jacobian::Identity(); if (H1) *H1 = Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity();
return v1 + v2; return v1 + v2;
} }
static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = {},
ChartJacobian H2 = boost::none) { ChartJacobian H2 = {}) {
if (H1) *H1 = - Jacobian::Identity(); if (H1) *H1 = - Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity();
return v2 - v1; return v2 - v1;
} }
static Class Inverse(const Class& v, ChartJacobian H = boost::none) { static Class Inverse(const Class& v, ChartJacobian H = {}) {
if (H) *H = - Jacobian::Identity(); if (H) *H = - Jacobian::Identity();
return -v; return -v;
} }
@ -106,7 +106,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
} }
static TangentVector Local(const Class& origin, const Class& other, static TangentVector Local(const Class& origin, const Class& other,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
if (H1) *H1 = - Eye(origin); if (H1) *H1 = - Eye(origin);
if (H2) *H2 = Eye(other); if (H2) *H2 = Eye(other);
Class v = other-origin; Class v = other-origin;
@ -114,7 +114,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
} }
static Class Retract(const Class& origin, const TangentVector& v, static Class Retract(const Class& origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
if (H1) *H1 = Eye(origin); if (H1) *H1 = Eye(origin);
if (H2) *H2 = Eye(origin); if (H2) *H2 = Eye(origin);
return origin + v; return origin + v;
@ -125,12 +125,12 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
/// @name Lie Group /// @name Lie Group
/// @{ /// @{
static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { static TangentVector Logmap(const Class& m, ChartJacobian Hm = {}) {
if (Hm) *Hm = Eye(m); if (Hm) *Hm = Eye(m);
return m.vector(); return m.vector();
} }
static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) {
Class result(v); Class result(v);
if (Hv) if (Hv)
*Hv = Eye(v); *Hv = Eye(v);
@ -138,14 +138,14 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
} }
static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1, static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1,
ChartJacobian H2 = boost::none) { ChartJacobian H2 = {}) {
if (H1) *H1 = Eye(v1); if (H1) *H1 = Eye(v1);
if (H2) *H2 = Eye(v2); if (H2) *H2 = Eye(v2);
return v1 + v2; return v1 + v2;
} }
static Class Between(const Class& v1, const Class& v2, ChartJacobian H1, static Class Between(const Class& v1, const Class& v2, ChartJacobian H1,
ChartJacobian H2 = boost::none) { ChartJacobian H2 = {}) {
if (H1) *H1 = - Eye(v1); if (H1) *H1 = - Eye(v1);
if (H2) *H2 = Eye(v2); if (H2) *H2 = Eye(v2);
return v2 - v1; return v2 - v1;
@ -237,7 +237,7 @@ struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
typedef OptionalJacobian<1, 1> ChartJacobian; typedef OptionalJacobian<1, 1> ChartJacobian;
static TangentVector Local(Scalar origin, Scalar other, static TangentVector Local(Scalar origin, Scalar other,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
if (H1) (*H1)[0] = -1.0; if (H1) (*H1)[0] = -1.0;
if (H2) (*H2)[0] = 1.0; if (H2) (*H2)[0] = 1.0;
TangentVector result; TangentVector result;
@ -246,7 +246,7 @@ struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
} }
static Scalar Retract(Scalar origin, const TangentVector& v, static Scalar Retract(Scalar origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
if (H1) (*H1)[0] = 1.0; if (H1) (*H1)[0] = 1.0;
if (H2) (*H2)[0] = 1.0; if (H2) (*H2)[0] = 1.0;
return origin + v[0]; return origin + v[0];
@ -255,12 +255,12 @@ struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
/// @name Lie Group /// @name Lie Group
/// @{ /// @{
static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) { static TangentVector Logmap(Scalar m, ChartJacobian H = {}) {
if (H) (*H)[0] = 1.0; if (H) (*H)[0] = 1.0;
return Local(0, m); return Local(0, m);
} }
static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) { static Scalar Expmap(const TangentVector& v, ChartJacobian H = {}) {
if (H) (*H)[0] = 1.0; if (H) (*H)[0] = 1.0;
return v[0]; return v[0];
} }
@ -312,7 +312,7 @@ struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
typedef OptionalJacobian<dimension, dimension> ChartJacobian; typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static TangentVector Local(const Fixed& origin, const Fixed& other, static TangentVector Local(const Fixed& origin, const Fixed& other,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
if (H1) (*H1) = -Jacobian::Identity(); if (H1) (*H1) = -Jacobian::Identity();
if (H2) (*H2) = Jacobian::Identity(); if (H2) (*H2) = Jacobian::Identity();
TangentVector result; TangentVector result;
@ -321,7 +321,7 @@ struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
} }
static Fixed Retract(const Fixed& origin, const TangentVector& v, static Fixed Retract(const Fixed& origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
if (H1) (*H1) = Jacobian::Identity(); if (H1) (*H1) = Jacobian::Identity();
if (H2) (*H2) = Jacobian::Identity(); if (H2) (*H2) = Jacobian::Identity();
return origin + Eigen::Map<const Fixed>(v.data()); return origin + Eigen::Map<const Fixed>(v.data());
@ -330,14 +330,14 @@ struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
/// @name Lie Group /// @name Lie Group
/// @{ /// @{
static TangentVector Logmap(const Fixed& m, ChartJacobian H = boost::none) { static TangentVector Logmap(const Fixed& m, ChartJacobian H = {}) {
if (H) *H = Jacobian::Identity(); if (H) *H = Jacobian::Identity();
TangentVector result; TangentVector result;
Eigen::Map<Fixed>(result.data()) = m; Eigen::Map<Fixed>(result.data()) = m;
return result; return result;
} }
static Fixed Expmap(const TangentVector& v, ChartJacobian H = boost::none) { static Fixed Expmap(const TangentVector& v, ChartJacobian H = {}) {
Fixed m; Fixed m;
m.setZero(); m.setZero();
if (H) *H = Jacobian::Identity(); if (H) *H = Jacobian::Identity();
@ -393,7 +393,7 @@ struct DynamicTraits {
} }
static TangentVector Local(const Dynamic& m, const Dynamic& other, // static TangentVector Local(const Dynamic& m, const Dynamic& other, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
if (H1) *H1 = -Eye(m); if (H1) *H1 = -Eye(m);
if (H2) *H2 = Eye(m); if (H2) *H2 = Eye(m);
TangentVector v(GetDimension(m)); TangentVector v(GetDimension(m));
@ -402,7 +402,7 @@ struct DynamicTraits {
} }
static Dynamic Retract(const Dynamic& m, const TangentVector& v, // static Dynamic Retract(const Dynamic& m, const TangentVector& v, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
if (H1) *H1 = Eye(m); if (H1) *H1 = Eye(m);
if (H2) *H2 = Eye(m); if (H2) *H2 = Eye(m);
return m + Eigen::Map<const Dynamic>(v.data(), m.rows(), m.cols()); return m + Eigen::Map<const Dynamic>(v.data(), m.rows(), m.cols());
@ -411,32 +411,32 @@ struct DynamicTraits {
/// @name Lie Group /// @name Lie Group
/// @{ /// @{
static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost::none) { static TangentVector Logmap(const Dynamic& m, ChartJacobian H = {}) {
if (H) *H = Eye(m); if (H) *H = Eye(m);
TangentVector result(GetDimension(m)); TangentVector result(GetDimension(m));
Eigen::Map<Dynamic>(result.data(), m.cols(), m.rows()) = m; Eigen::Map<Dynamic>(result.data(), m.cols(), m.rows()) = m;
return result; return result;
} }
static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost::none) { static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = {}) {
static_cast<void>(H); static_cast<void>(H);
throw std::runtime_error("Expmap not defined for dynamic types"); throw std::runtime_error("Expmap not defined for dynamic types");
} }
static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) { static Dynamic Inverse(const Dynamic& m, ChartJacobian H = {}) {
if (H) *H = -Eye(m); if (H) *H = -Eye(m);
return -m; return -m;
} }
static Dynamic Compose(const Dynamic& v1, const Dynamic& v2, static Dynamic Compose(const Dynamic& v1, const Dynamic& v2,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
if (H1) *H1 = Eye(v1); if (H1) *H1 = Eye(v1);
if (H2) *H2 = Eye(v1); if (H2) *H2 = Eye(v1);
return v1 + v2; return v1 + v2;
} }
static Dynamic Between(const Dynamic& v1, const Dynamic& v2, static Dynamic Between(const Dynamic& v1, const Dynamic& v2,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
if (H1) *H1 = -Eye(v1); if (H1) *H1 = -Eye(v1);
if (H2) *H2 = Eye(v1); if (H2) *H2 = Eye(v1);
return v2 - v1; return v2 - v1;

4
gtsam/base/numericalDerivative.h
View File

@ -37,8 +37,8 @@ namespace gtsam {
* for a function with one relevant param and an optional derivative: * for a function with one relevant param and an optional derivative:
* Foo bar(const Obj& a, OptionalMatrixType H1) * Foo bar(const Obj& a, OptionalMatrixType H1)
* Use boost.bind to restructure: * Use boost.bind to restructure:
* std::bind(bar, std::placeholders::_1, boost::none) * std::bind(bar, std::placeholders::_1, {})
* This syntax will fix the optional argument to boost::none, while using the first argument provided * This syntax will fix the optional argument to {}, while using the first argument provided
* *
* For member functions, such as below, with an instantiated copy instanceOfSomeClass * For member functions, such as below, with an instantiated copy instanceOfSomeClass
* Foo SomeClass::bar(const Obj& a) * Foo SomeClass::bar(const Obj& a)

13
gtsam/base/tests/testOptionalJacobian.cpp
View File

@ -32,7 +32,6 @@ using namespace gtsam;
TEST( OptionalJacobian, Constructors ) { TEST( OptionalJacobian, Constructors ) {
Matrix23 fixed; Matrix23 fixed;
Matrix dynamic; Matrix dynamic;
boost::optional<Matrix&> optional(dynamic);
OptionalJacobian<2, 3> H; OptionalJacobian<2, 3> H;
EXPECT(!H); EXPECT(!H);
@ -41,22 +40,18 @@ TEST( OptionalJacobian, Constructors ) {
TEST_CONSTRUCTOR(2, 3, &fixed, true); TEST_CONSTRUCTOR(2, 3, &fixed, true);
TEST_CONSTRUCTOR(2, 3, dynamic, true); TEST_CONSTRUCTOR(2, 3, dynamic, true);
TEST_CONSTRUCTOR(2, 3, &dynamic, true); TEST_CONSTRUCTOR(2, 3, &dynamic, true);
TEST_CONSTRUCTOR(2, 3, boost::none, false);
TEST_CONSTRUCTOR(2, 3, optional, true);
// Test dynamic // Test dynamic
OptionalJacobian<-1, -1> H7; OptionalJacobian<-1, -1> H7;
EXPECT(!H7); EXPECT(!H7);
TEST_CONSTRUCTOR(-1, -1, dynamic, true); TEST_CONSTRUCTOR(-1, -1, dynamic, true);
TEST_CONSTRUCTOR(-1, -1, boost::none, false);
TEST_CONSTRUCTOR(-1, -1, optional, true);
} }
//****************************************************************************** //******************************************************************************
Matrix kTestMatrix = (Matrix23() << 11,12,13,21,22,23).finished(); Matrix kTestMatrix = (Matrix23() << 11,12,13,21,22,23).finished();
void test(OptionalJacobian<2, 3> H = boost::none) { void test(OptionalJacobian<2, 3> H = {}) {
if (H) if (H)
*H = kTestMatrix; *H = kTestMatrix;
} }
@ -116,7 +111,7 @@ TEST( OptionalJacobian, Fixed) {
} }
//****************************************************************************** //******************************************************************************
void test2(OptionalJacobian<-1,-1> H = boost::none) { void test2(OptionalJacobian<-1,-1> H = {}) {
if (H) if (H)
*H = kTestMatrix; // resizes *H = kTestMatrix; // resizes
} }
@ -145,12 +140,12 @@ TEST( OptionalJacobian, Dynamic) {
} }
//****************************************************************************** //******************************************************************************
void test3(double add, OptionalJacobian<2,1> H = boost::none) { void test3(double add, OptionalJacobian<2,1> H = {}) {
if (H) *H << add + 10, add + 20; if (H) *H << add + 10, add + 20;
} }
// This function calls the above function three times, one for each column // This function calls the above function three times, one for each column
void test4(OptionalJacobian<2, 3> H = boost::none) { void test4(OptionalJacobian<2, 3> H = {}) {
if (H) { if (H) {
test3(1, H.cols<1>(0)); test3(1, H.cols<1>(0));
test3(2, H.cols<1>(1)); test3(2, H.cols<1>(1));

28
gtsam/basis/Basis.h
View File

@ -152,14 +152,14 @@ class Basis {
/// Regular 1D evaluation /// Regular 1D evaluation
double apply(const typename DERIVED::Parameters& p, double apply(const typename DERIVED::Parameters& p,
OptionalJacobian<-1, -1> H = boost::none) const { OptionalJacobian<-1, -1> H = {}) const {
if (H) *H = weights_; if (H) *H = weights_;
return (weights_ * p)(0); return (weights_ * p)(0);
} }
/// c++ sugar /// c++ sugar
double operator()(const typename DERIVED::Parameters& p, double operator()(const typename DERIVED::Parameters& p,
OptionalJacobian<-1, -1> H = boost::none) const { OptionalJacobian<-1, -1> H = {}) const {
return apply(p, H); // might call apply in derived return apply(p, H); // might call apply in derived
} }
@ -212,14 +212,14 @@ class Basis {
/// M-dimensional evaluation /// M-dimensional evaluation
VectorM apply(const ParameterMatrix<M>& P, VectorM apply(const ParameterMatrix<M>& P,
OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const { OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
if (H) *H = H_; if (H) *H = H_;
return P.matrix() * this->weights_.transpose(); return P.matrix() * this->weights_.transpose();
} }
/// c++ sugar /// c++ sugar
VectorM operator()(const ParameterMatrix<M>& P, VectorM operator()(const ParameterMatrix<M>& P,
OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const { OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
return apply(P, H); return apply(P, H);
} }
}; };
@ -271,14 +271,14 @@ class Basis {
/// Calculate component of component rowIndex_ of P /// Calculate component of component rowIndex_ of P
double apply(const ParameterMatrix<M>& P, double apply(const ParameterMatrix<M>& P,
OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const { OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
if (H) *H = H_; if (H) *H = H_;
return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
} }
/// c++ sugar /// c++ sugar
double operator()(const ParameterMatrix<M>& P, double operator()(const ParameterMatrix<M>& P,
OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const { OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
return apply(P, H); return apply(P, H);
} }
}; };
@ -315,7 +315,7 @@ class Basis {
/// Manifold evaluation /// Manifold evaluation
T apply(const ParameterMatrix<M>& P, T apply(const ParameterMatrix<M>& P,
OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const { OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
// Interpolate the M-dimensional vector to yield a vector in tangent space // Interpolate the M-dimensional vector to yield a vector in tangent space
Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H); Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
@ -334,7 +334,7 @@ class Basis {
/// c++ sugar /// c++ sugar
T operator()(const ParameterMatrix<M>& P, T operator()(const ParameterMatrix<M>& P,
OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const { OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
return apply(P, H); // might call apply in derived return apply(P, H); // might call apply in derived
} }
}; };
@ -377,13 +377,13 @@ class Basis {
: DerivativeFunctorBase(N, x, a, b) {} : DerivativeFunctorBase(N, x, a, b) {}
double apply(const typename DERIVED::Parameters& p, double apply(const typename DERIVED::Parameters& p,
OptionalJacobian</*1xN*/ -1, -1> H = boost::none) const { OptionalJacobian</*1xN*/ -1, -1> H = {}) const {
if (H) *H = this->weights_; if (H) *H = this->weights_;
return (this->weights_ * p)(0); return (this->weights_ * p)(0);
} }
/// c++ sugar /// c++ sugar
double operator()(const typename DERIVED::Parameters& p, double operator()(const typename DERIVED::Parameters& p,
OptionalJacobian</*1xN*/ -1, -1> H = boost::none) const { OptionalJacobian</*1xN*/ -1, -1> H = {}) const {
return apply(p, H); // might call apply in derived return apply(p, H); // might call apply in derived
} }
}; };
@ -433,14 +433,14 @@ class Basis {
} }
VectorM apply(const ParameterMatrix<M>& P, VectorM apply(const ParameterMatrix<M>& P,
OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const { OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
if (H) *H = H_; if (H) *H = H_;
return P.matrix() * this->weights_.transpose(); return P.matrix() * this->weights_.transpose();
} }
/// c++ sugar /// c++ sugar
VectorM operator()( VectorM operator()(
const ParameterMatrix<M>& P, const ParameterMatrix<M>& P,
OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const { OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
return apply(P, H); return apply(P, H);
} }
}; };
@ -491,13 +491,13 @@ class Basis {
} }
/// Calculate derivative of component rowIndex_ of F /// Calculate derivative of component rowIndex_ of F
double apply(const ParameterMatrix<M>& P, double apply(const ParameterMatrix<M>& P,
OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const { OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
if (H) *H = H_; if (H) *H = H_;
return P.row(rowIndex_) * this->weights_.transpose(); return P.row(rowIndex_) * this->weights_.transpose();
} }
/// c++ sugar /// c++ sugar
double operator()(const ParameterMatrix<M>& P, double operator()(const ParameterMatrix<M>& P,
OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const { OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
return apply(P, H); return apply(P, H);
} }
}; };

9
gtsam/basis/tests/testChebyshev2.cpp
View File

@ -17,6 +17,7 @@
* methods * methods
*/ */
#include <cstddef>
#include <gtsam/basis/Chebyshev2.h> #include <gtsam/basis/Chebyshev2.h>
#include <gtsam/basis/FitBasis.h> #include <gtsam/basis/FitBasis.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
@ -119,7 +120,7 @@ TEST(Chebyshev2, InterpolateVector) {
// Check derivative // Check derivative
std::function<Vector2(ParameterMatrix<2>)> f = boost::bind( std::function<Vector2(ParameterMatrix<2>)> f = boost::bind(
&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none); &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, nullptr);
Matrix numericalH = Matrix numericalH =
numericalDerivative11<Vector2, ParameterMatrix<2>, 2 * N>(f, X); numericalDerivative11<Vector2, ParameterMatrix<2>, 2 * N>(f, X);
EXPECT(assert_equal(numericalH, actualH, 1e-9)); EXPECT(assert_equal(numericalH, actualH, 1e-9));
@ -377,7 +378,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor) {
// Test Jacobian // Test Jacobian
Matrix expectedH = numericalDerivative11<Vector2, ParameterMatrix<M>, M * N>( Matrix expectedH = numericalDerivative11<Vector2, ParameterMatrix<M>, M * N>(
boost::bind(&VecD::operator(), fx, _1, boost::none), X); boost::bind(&VecD::operator(), fx, _1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-7)); EXPECT(assert_equal(expectedH, actualH, 1e-7));
} }
@ -409,7 +410,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) {
VecD vecd(N, points(0), 0, T); VecD vecd(N, points(0), 0, T);
vecd(X, actualH); vecd(X, actualH);
Matrix expectedH = numericalDerivative11<Vector1, ParameterMatrix<M>, M * N>( Matrix expectedH = numericalDerivative11<Vector1, ParameterMatrix<M>, M * N>(
boost::bind(&VecD::operator(), vecd, _1, boost::none), X); boost::bind(&VecD::operator(), vecd, _1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-6)); EXPECT(assert_equal(expectedH, actualH, 1e-6));
} }
@ -426,7 +427,7 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) {
EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);
Matrix expectedH = numericalDerivative11<double, ParameterMatrix<M>, M * N>( Matrix expectedH = numericalDerivative11<double, ParameterMatrix<M>, M * N>(
boost::bind(&CompFunc::operator(), fx, _1, boost::none), X); boost::bind(&CompFunc::operator(), fx, _1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-7)); EXPECT(assert_equal(expectedH, actualH, 1e-7));
} }

12
gtsam/geometry/BearingRange.h
View File

@ -77,8 +77,8 @@ public:
/// Prediction function that stacks measurements /// Prediction function that stacks measurements
static BearingRange Measure( static BearingRange Measure(
const A1& a1, const A2& a2, const A1& a1, const A2& a2,
OptionalJacobian<dimension, traits<A1>::dimension> H1 = boost::none, OptionalJacobian<dimension, traits<A1>::dimension> H1 = {},
OptionalJacobian<dimension, traits<A2>::dimension> H2 = boost::none) { OptionalJacobian<dimension, traits<A2>::dimension> H2 = {}) {
typename MakeJacobian<B, A1>::type HB1; typename MakeJacobian<B, A1>::type HB1;
typename MakeJacobian<B, A2>::type HB2; typename MakeJacobian<B, A2>::type HB2;
typename MakeJacobian<R, A1>::type HR1; typename MakeJacobian<R, A1>::type HR1;
@ -181,8 +181,8 @@ struct HasBearing {
typedef RT result_type; typedef RT result_type;
RT operator()( RT operator()(
const A1& a1, const A2& a2, const A1& a1, const A2& a2,
OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1=boost::none, OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1={},
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2=boost::none) { OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2={}) {
return a1.bearing(a2, H1, H2); return a1.bearing(a2, H1, H2);
} }
}; };
@ -195,8 +195,8 @@ struct HasRange {
typedef RT result_type; typedef RT result_type;
RT operator()( RT operator()(
const A1& a1, const A2& a2, const A1& a1, const A2& a2,
OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1=boost::none, OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1={},
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2=boost::none) { OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2={}) {
return a1.range(a2, H1, H2); return a1.range(a2, H1, H2);
} }
}; };

4
gtsam/geometry/Cal3.h
View File

@ -45,8 +45,8 @@ namespace gtsam {
*/ */
template <typename Cal, size_t Dim> template <typename Cal, size_t Dim>
void calibrateJacobians(const Cal& calibration, const Point2& pn, void calibrateJacobians(const Cal& calibration, const Point2& pn,
OptionalJacobian<2, Dim> Dcal = boost::none, OptionalJacobian<2, Dim> Dcal = {},
OptionalJacobian<2, 2> Dp = boost::none) { OptionalJacobian<2, 2> Dp = {}) {
if (Dcal || Dp) { if (Dcal || Dp) {
Eigen::Matrix<double, 2, Dim> H_uncal_K; Eigen::Matrix<double, 2, Dim> H_uncal_K;
Matrix22 H_uncal_pn, H_uncal_pn_inv; Matrix22 H_uncal_pn, H_uncal_pn_inv;

4
gtsam/geometry/Cal3Bundler.cpp
View File

@ -131,14 +131,14 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
/* ************************************************************************* */ /* ************************************************************************* */
Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const { Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const {
Matrix2 Dp; Matrix2 Dp;
uncalibrate(p, boost::none, Dp); uncalibrate(p, {}, Dp);
return Dp; return Dp;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const { Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const {
Matrix23 Dcal; Matrix23 Dcal;
uncalibrate(p, Dcal, boost::none); uncalibrate(p, Dcal, {});
return Dcal; return Dcal;
} }

8
gtsam/geometry/Cal3Bundler.h
View File

@ -116,8 +116,8 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates * @return point in image coordinates
*/ */
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = {},
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = {}) const;
/** /**
* Convert a pixel coordinate to ideal coordinate xy * Convert a pixel coordinate to ideal coordinate xy
@ -127,8 +127,8 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates * @return point in intrinsic coordinates
*/ */
Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none, Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = {},
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = {}) const;
/// @deprecated might be removed in next release, use uncalibrate /// @deprecated might be removed in next release, use uncalibrate
Matrix2 D2d_intrinsic(const Point2& p) const; Matrix2 D2d_intrinsic(const Point2& p) const;

8
gtsam/geometry/Cal3DS2_Base.h
View File

@ -122,12 +122,12 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates * @return point in (distorted) image coordinates
*/ */
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {},
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = {}) const;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {},
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = {}) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates /// Derivative of uncalibrate wrpt intrinsic coordinates
Matrix2 D2d_intrinsic(const Point2& p) const; Matrix2 D2d_intrinsic(const Point2& p) const;

2
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -134,7 +134,7 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
Matrix2 jac; Matrix2 jac;
// Calculate the current estimate (uv_hat) and the jacobian // Calculate the current estimate (uv_hat) and the jacobian
const Point2 uv_hat = uncalibrate(pi, boost::none, jac); const Point2 uv_hat = uncalibrate(pi, {}, jac);
// Test convergence // Test convergence
if ((uv_hat - uv).norm() < tol_) break; if ((uv_hat - uv).norm() < tol_) break;

8
gtsam/geometry/Cal3Fisheye.h
View File

@ -121,8 +121,8 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi) * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
* @return point in (distorted) image coordinates * @return point in (distorted) image coordinates
*/ */
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {},
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = {}) const;
/** /**
* Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i, * Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
@ -132,8 +132,8 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi) * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
* @return point in intrinsic coordinates * @return point in intrinsic coordinates
*/ */
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {},
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = {}) const;
/// @} /// @}
/// @name Testable /// @name Testable

8
gtsam/geometry/Cal3Unified.h
View File

@ -106,12 +106,12 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
* @return point in image coordinates * @return point in image coordinates
*/ */
Point2 uncalibrate(const Point2& p, Point2 uncalibrate(const Point2& p,
OptionalJacobian<2, 10> Dcal = boost::none, OptionalJacobian<2, 10> Dcal = {},
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = {}) const;
/// Conver a pixel coordinate to ideal coordinate /// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none, Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = {},
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = {}) const;
/// Convert a 3D point to normalized unit plane /// Convert a 3D point to normalized unit plane
Point2 spaceToNPlane(const Point2& p) const; Point2 spaceToNPlane(const Point2& p) const;

12
gtsam/geometry/Cal3_S2.h
View File

@ -66,8 +66,8 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 {
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates * @return point in image coordinates
*/ */
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none, Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = {},
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = {}) const;
/** /**
* Convert image coordinates uv to intrinsic coordinates xy * Convert image coordinates uv to intrinsic coordinates xy
@ -76,8 +76,8 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 {
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates * @return point in intrinsic coordinates
*/ */
Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none, Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = {},
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = {}) const;
/** /**
* Convert homogeneous image coordinates to intrinsic coordinates * Convert homogeneous image coordinates to intrinsic coordinates
@ -102,8 +102,8 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 {
/// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q) /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
inline Cal3_S2 between(const Cal3_S2& q, inline Cal3_S2 between(const Cal3_S2& q,
OptionalJacobian<5, 5> H1 = boost::none, OptionalJacobian<5, 5> H1 = {},
OptionalJacobian<5, 5> H2 = boost::none) const { OptionalJacobian<5, 5> H2 = {}) const {
if (H1) *H1 = -I_5x5; if (H1) *H1 = -I_5x5;
if (H2) *H2 = I_5x5; if (H2) *H2 = I_5x5;
return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_, return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_,

8
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -64,8 +64,8 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates * @return point in image coordinates
*/ */
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none, Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = {},
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = {}) const;
/** /**
* Convert image coordinates uv to intrinsic coordinates xy * Convert image coordinates uv to intrinsic coordinates xy
@ -74,8 +74,8 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates * @return point in intrinsic coordinates
*/ */
Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none, Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = {},
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = {}) const;
/** /**
* Convert homogeneous image coordinates to intrinsic coordinates * Convert homogeneous image coordinates to intrinsic coordinates

2
gtsam/geometry/CalibratedCamera.cpp
View File

@ -117,7 +117,7 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const { OptionalJacobian<2, 3> Dpoint) const {
Matrix3 Rt; // calculated by transformTo if needed Matrix3 Rt; // calculated by transformTo if needed
const Point3 q = pose().transformTo(point, boost::none, Dpoint ? &Rt : 0); const Point3 q = pose().transformTo(point, {}, Dpoint ? &Rt : 0);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (q.z() <= 0) if (q.z() <= 0)
throw CheiralityException(); throw CheiralityException();

36
gtsam/geometry/CalibratedCamera.h
View File

@ -176,7 +176,7 @@ public:
* @param pc point in camera coordinates * @param pc point in camera coordinates
*/ */
static Point2 Project(const Point3& pc, // static Point2 Project(const Point3& pc, //
OptionalJacobian<2, 3> Dpoint = boost::none); OptionalJacobian<2, 3> Dpoint = {});
/** /**
* Project from 3D point at infinity in camera coordinates into image * Project from 3D point at infinity in camera coordinates into image
@ -184,7 +184,7 @@ public:
* @param pc point in camera coordinates * @param pc point in camera coordinates
*/ */
static Point2 Project(const Unit3& pc, // static Point2 Project(const Unit3& pc, //
OptionalJacobian<2, 2> Dpoint = boost::none); OptionalJacobian<2, 2> Dpoint = {});
/// Project a point into the image and check depth /// Project a point into the image and check depth
std::pair<Point2, bool> projectSafe(const Point3& pw) const; std::pair<Point2, bool> projectSafe(const Point3& pw) const;
@ -195,7 +195,7 @@ public:
* @return the intrinsic coordinates of the projected point * @return the intrinsic coordinates of the projected point
*/ */
Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; {}, OptionalJacobian<2, 3> Dpoint = {}) const;
/** Project point at infinity into the image /** Project point at infinity into the image
* Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
@ -203,13 +203,13 @@ public:
* @return the intrinsic coordinates of the projected point * @return the intrinsic coordinates of the projected point
*/ */
Point2 project2(const Unit3& point, Point2 project2(const Unit3& point,
OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 2> Dpoint = boost::none) const; OptionalJacobian<2, 2> Dpoint = {}) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth /// backproject a 2-dimensional point to a 3-dimensional point at given depth
static Point3 BackprojectFromCamera(const Point2& p, const double depth, static Point3 BackprojectFromCamera(const Point2& p, const double depth,
OptionalJacobian<3, 2> Dpoint = boost::none, OptionalJacobian<3, 2> Dpoint = {},
OptionalJacobian<3, 1> Ddepth = boost::none); OptionalJacobian<3, 1> Ddepth = {});
/// @} /// @}
/// @name Advanced interface /// @name Advanced interface
@ -270,7 +270,7 @@ public:
// Create CalibratedCamera, with derivatives // Create CalibratedCamera, with derivatives
static CalibratedCamera Create(const Pose3& pose, static CalibratedCamera Create(const Pose3& pose,
OptionalJacobian<dimension, 6> H1 = boost::none) { OptionalJacobian<dimension, 6> H1 = {}) {
if (H1) if (H1)
*H1 << I_6x6; *H1 << I_6x6;
return CalibratedCamera(pose); return CalibratedCamera(pose);
@ -346,13 +346,13 @@ public:
* Use project2, which is more consistently named across Pinhole cameras * Use project2, which is more consistently named across Pinhole cameras
*/ */
Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera = Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; {}, OptionalJacobian<2, 3> Dpoint = {}) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth /// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& pn, double depth, Point3 backproject(const Point2& pn, double depth,
OptionalJacobian<3, 6> Dresult_dpose = boost::none, OptionalJacobian<3, 6> Dresult_dpose = {},
OptionalJacobian<3, 2> Dresult_dp = boost::none, OptionalJacobian<3, 2> Dresult_dp = {},
OptionalJacobian<3, 1> Dresult_ddepth = boost::none) const { OptionalJacobian<3, 1> Dresult_ddepth = {}) const {
Matrix32 Dpoint_dpn; Matrix32 Dpoint_dpn;
Matrix31 Dpoint_ddepth; Matrix31 Dpoint_ddepth;
@ -379,8 +379,8 @@ public:
* @return range (double) * @return range (double)
*/ */
double range(const Point3& point, double range(const Point3& point,
OptionalJacobian<1, 6> Dcamera = boost::none, OptionalJacobian<1, 6> Dcamera = {},
OptionalJacobian<1, 3> Dpoint = boost::none) const { OptionalJacobian<1, 3> Dpoint = {}) const {
return pose().range(point, Dcamera, Dpoint); return pose().range(point, Dcamera, Dpoint);
} }
@ -389,8 +389,8 @@ public:
* @param pose Other SO(3) pose * @param pose Other SO(3) pose
* @return range (double) * @return range (double)
*/ */
double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none, double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = {},
OptionalJacobian<1, 6> Dpose = boost::none) const { OptionalJacobian<1, 6> Dpose = {}) const {
return this->pose().range(pose, Dcamera, Dpose); return this->pose().range(pose, Dcamera, Dpose);
} }
@ -400,8 +400,8 @@ public:
* @return range (double) * @return range (double)
*/ */
double range(const CalibratedCamera& camera, // double range(const CalibratedCamera& camera, //
OptionalJacobian<1, 6> H1 = boost::none, // OptionalJacobian<1, 6> H1 = {}, //
OptionalJacobian<1, 6> H2 = boost::none) const { OptionalJacobian<1, 6> H2 = {}) const {
return pose().range(camera.pose(), H1, H2); return pose().range(camera.pose(), H1, H2);
} }

14
gtsam/geometry/EssentialMatrix.h
View File

@ -49,12 +49,12 @@ class EssentialMatrix {
/// Named constructor with derivatives /// Named constructor with derivatives
GTSAM_EXPORT static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb, GTSAM_EXPORT static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
OptionalJacobian<5, 3> H1 = boost::none, OptionalJacobian<5, 3> H1 = {},
OptionalJacobian<5, 2> H2 = boost::none); OptionalJacobian<5, 2> H2 = {});
/// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
GTSAM_EXPORT static EssentialMatrix FromPose3(const Pose3& _1P2_, GTSAM_EXPORT static EssentialMatrix FromPose3(const Pose3& _1P2_,
OptionalJacobian<5, 6> H = boost::none); OptionalJacobian<5, 6> H = {});
/// Random, using Rot3::Random and Unit3::Random /// Random, using Rot3::Random and Unit3::Random
template<typename Engine> template<typename Engine>
@ -139,8 +139,8 @@ class EssentialMatrix {
* @return point in pose coordinates * @return point in pose coordinates
*/ */
GTSAM_EXPORT Point3 transformTo(const Point3& p, GTSAM_EXPORT Point3 transformTo(const Point3& p,
OptionalJacobian<3, 5> DE = boost::none, OptionalJacobian<3, 5> DE = {},
OptionalJacobian<3, 3> Dpoint = boost::none) const; OptionalJacobian<3, 3> Dpoint = {}) const;
/** /**
* Given essential matrix E in camera frame B, convert to body frame C * Given essential matrix E in camera frame B, convert to body frame C
@ -148,7 +148,7 @@ class EssentialMatrix {
* @param E essential matrix E in camera frame C * @param E essential matrix E in camera frame C
*/ */
GTSAM_EXPORT EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE = GTSAM_EXPORT EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE =
boost::none, OptionalJacobian<5, 3> HR = boost::none) const; {}, OptionalJacobian<5, 3> HR = {}) const;
/** /**
* Given essential matrix E in camera frame B, convert to body frame C * Given essential matrix E in camera frame B, convert to body frame C
@ -161,7 +161,7 @@ class EssentialMatrix {
/// epipolar error, algebraic /// epipolar error, algebraic
GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB,
OptionalJacobian<1, 5> H = boost::none) const; OptionalJacobian<1, 5> H = {}) const;
/// @} /// @}

14
gtsam/geometry/Line3.h
View File

@ -32,8 +32,8 @@ class Line3;
* @return Transformed line in camera frame * @return Transformed line in camera frame
*/ */
GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL, GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
OptionalJacobian<4, 6> Dpose = boost::none, OptionalJacobian<4, 6> Dpose = {},
OptionalJacobian<4, 4> Dline = boost::none); OptionalJacobian<4, 4> Dline = {});
/** /**
@ -69,8 +69,8 @@ class GTSAM_EXPORT Line3 {
* @return q: resulting line after adding the increment and mapping to the manifold * @return q: resulting line after adding the increment and mapping to the manifold
*/ */
Line3 retract(const Vector4 &v, Line3 retract(const Vector4 &v,
OptionalJacobian<4, 4> Dp = boost::none, OptionalJacobian<4, 4> Dp = {},
OptionalJacobian<4, 4> Dv = boost::none) const; OptionalJacobian<4, 4> Dv = {}) const;
/** /**
* The localCoordinates method is the inverse of retract and finds the difference * The localCoordinates method is the inverse of retract and finds the difference
@ -82,8 +82,8 @@ class GTSAM_EXPORT Line3 {
* @return v: difference in the tangent space * @return v: difference in the tangent space
*/ */
Vector4 localCoordinates(const Line3 &q, Vector4 localCoordinates(const Line3 &q,
OptionalJacobian<4, 4> Dp = boost::none, OptionalJacobian<4, 4> Dp = {},
OptionalJacobian<4, 4> Dq = boost::none) const; OptionalJacobian<4, 4> Dq = {}) const;
/** /**
* Print R, a, b * Print R, a, b
@ -105,7 +105,7 @@ class GTSAM_EXPORT Line3 {
* @return Unit3 - projected line in image plane, in homogenous coordinates. * @return Unit3 - projected line in image plane, in homogenous coordinates.
* We use Unit3 since it is a manifold with the right dimension. * We use Unit3 since it is a manifold with the right dimension.
*/ */
Unit3 project(OptionalJacobian<2, 4> Dline = boost::none) const; Unit3 project(OptionalJacobian<2, 4> Dline = {}) const;
/** /**
* Returns point on the line that is at a certain distance starting from the * Returns point on the line that is at a certain distance starting from the

14
gtsam/geometry/OrientedPlane3.h
View File

@ -87,8 +87,8 @@ public:
* @return the transformed plane * @return the transformed plane
*/ */
OrientedPlane3 transform(const Pose3& xr, OrientedPlane3 transform(const Pose3& xr,
OptionalJacobian<3, 3> Hp = boost::none, OptionalJacobian<3, 3> Hp = {},
OptionalJacobian<3, 6> Hr = boost::none) const; OptionalJacobian<3, 6> Hr = {}) const;
/** Computes the error between the two planes, with derivatives. /** Computes the error between the two planes, with derivatives.
* This uses Unit3::errorVector, as opposed to the other .error() in this * This uses Unit3::errorVector, as opposed to the other .error() in this
@ -98,8 +98,8 @@ public:
* @param other the other plane * @param other the other plane
*/ */
Vector3 errorVector(const OrientedPlane3& other, Vector3 errorVector(const OrientedPlane3& other,
OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = boost::none) const; OptionalJacobian<3, 3> H2 = {}) const;
/// Dimensionality of tangent space = 3 DOF /// Dimensionality of tangent space = 3 DOF
inline static size_t Dim() { inline static size_t Dim() {
@ -113,7 +113,7 @@ public:
/// The retract function /// The retract function
OrientedPlane3 retract(const Vector3& v, OrientedPlane3 retract(const Vector3& v,
OptionalJacobian<3, 3> H = boost::none) const; OptionalJacobian<3, 3> H = {}) const;
/// The local coordinates function /// The local coordinates function
Vector3 localCoordinates(const OrientedPlane3& s) const; Vector3 localCoordinates(const OrientedPlane3& s) const;
@ -125,13 +125,13 @@ public:
} }
/// Return the normal /// Return the normal
inline Unit3 normal(OptionalJacobian<2, 3> H = boost::none) const { inline Unit3 normal(OptionalJacobian<2, 3> H = {}) const {
if (H) *H << I_2x2, Z_2x1; if (H) *H << I_2x2, Z_2x1;
return n_; return n_;
} }
/// Return the perpendicular distance to the origin /// Return the perpendicular distance to the origin
inline double distance(OptionalJacobian<1, 3> H = boost::none) const { inline double distance(OptionalJacobian<1, 3> H = {}) const {
if (H) *H << 0,0,1; if (H) *H << 0,0,1;
return d_; return d_;
} }

22
gtsam/geometry/PinholeCamera.h
View File

@ -109,8 +109,8 @@ public:
// Create PinholeCamera, with derivatives // Create PinholeCamera, with derivatives
static PinholeCamera Create(const Pose3& pose, const Calibration &K, static PinholeCamera Create(const Pose3& pose, const Calibration &K,
OptionalJacobian<dimension, 6> H1 = boost::none, // OptionalJacobian<dimension, 6> H1 = {}, //
OptionalJacobian<dimension, DimK> H2 = boost::none) { OptionalJacobian<dimension, DimK> H2 = {}) {
typedef Eigen::Matrix<double, DimK, 6> MatrixK6; typedef Eigen::Matrix<double, DimK, 6> MatrixK6;
if (H1) if (H1)
*H1 << I_6x6, MatrixK6::Zero(); *H1 << I_6x6, MatrixK6::Zero();
@ -237,19 +237,19 @@ public:
*Dcamera << Dpose, Dcal; *Dcamera << Dpose, Dcal;
return pi; return pi;
} else { } else {
return Base::project(pw, boost::none, Dpoint, boost::none); return Base::project(pw, {}, Dpoint, {});
} }
} }
/// project a 3D point from world coordinates into the image /// project a 3D point from world coordinates into the image
Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera = Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { {}, OptionalJacobian<2, 3> Dpoint = {}) const {
return _project2(pw, Dcamera, Dpoint); return _project2(pw, Dcamera, Dpoint);
} }
/// project a point at infinity from world coordinates into the image /// project a point at infinity from world coordinates into the image
Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera = Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera =
boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const { {}, OptionalJacobian<2, 2> Dpoint = {}) const {
return _project2(pw, Dcamera, Dpoint); return _project2(pw, Dcamera, Dpoint);
} }
@ -259,7 +259,7 @@ public:
* @return range (double) * @return range (double)
*/ */
double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera = double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera =
boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { {}, OptionalJacobian<1, 3> Dpoint = {}) const {
Matrix16 Dpose_; Matrix16 Dpose_;
double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint); double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint);
if (Dcamera) if (Dcamera)
@ -273,7 +273,7 @@ public:
* @return range (double) * @return range (double)
*/ */
double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera = double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera =
boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const { {}, OptionalJacobian<1, 6> Dpose = {}) const {
Matrix16 Dpose_; Matrix16 Dpose_;
double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose); double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose);
if (Dcamera) if (Dcamera)
@ -288,8 +288,8 @@ public:
*/ */
template<class CalibrationB> template<class CalibrationB>
double range(const PinholeCamera<CalibrationB>& camera, double range(const PinholeCamera<CalibrationB>& camera,
OptionalJacobian<1, dimension> Dcamera = boost::none, OptionalJacobian<1, dimension> Dcamera = {},
OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const { OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = {}) const {
Matrix16 Dcamera_, Dother_; Matrix16 Dcamera_, Dother_;
double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0, double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
Dother ? &Dother_ : 0); Dother ? &Dother_ : 0);
@ -309,8 +309,8 @@ public:
* @return range (double) * @return range (double)
*/ */
double range(const CalibratedCamera& camera, double range(const CalibratedCamera& camera,
OptionalJacobian<1, dimension> Dcamera = boost::none, OptionalJacobian<1, dimension> Dcamera = {},
OptionalJacobian<1, 6> Dother = boost::none) const { OptionalJacobian<1, 6> Dother = {}) const {
return range(camera.pose(), Dcamera, Dother); return range(camera.pose(), Dcamera, Dother);
} }

48
gtsam/geometry/PinholePose.h
View File

@ -115,32 +115,32 @@ public:
} }
/// project a 3D point from world coordinates into the image /// project a 3D point from world coordinates into the image
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 3> Dpoint = boost::none, OptionalJacobian<2, 3> Dpoint = {},
OptionalJacobian<2, DimK> Dcal = boost::none) const { OptionalJacobian<2, DimK> Dcal = {}) const {
return _project(pw, Dpose, Dpoint, Dcal); return _project(pw, Dpose, Dpoint, Dcal);
} }
/// project a 3D point from world coordinates into the image /// project a 3D point from world coordinates into the image
Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none, Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 3> Dpoint = boost::none, OptionalJacobian<2, 3> Dpoint = {},
OptionalJacobian<2, DimK> Dcal = boost::none) const { OptionalJacobian<2, DimK> Dcal = {}) const {
return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured); return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured);
} }
/// project a point at infinity from world coordinates into the image /// project a point at infinity from world coordinates into the image
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 2> Dpoint = boost::none, OptionalJacobian<2, 2> Dpoint = {},
OptionalJacobian<2, DimK> Dcal = boost::none) const { OptionalJacobian<2, DimK> Dcal = {}) const {
return _project(pw, Dpose, Dpoint, Dcal); return _project(pw, Dpose, Dpoint, Dcal);
} }
/// backproject a 2-dimensional point to a 3-dimensional point at given depth /// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& p, double depth, Point3 backproject(const Point2& p, double depth,
OptionalJacobian<3, 6> Dresult_dpose = boost::none, OptionalJacobian<3, 6> Dresult_dpose = {},
OptionalJacobian<3, 2> Dresult_dp = boost::none, OptionalJacobian<3, 2> Dresult_dp = {},
OptionalJacobian<3, 1> Dresult_ddepth = boost::none, OptionalJacobian<3, 1> Dresult_ddepth = {},
OptionalJacobian<3, DimK> Dresult_dcal = boost::none) const { OptionalJacobian<3, DimK> Dresult_dcal = {}) const {
typedef Eigen::Matrix<double, 2, DimK> Matrix2K; typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
Matrix2K Dpn_dcal; Matrix2K Dpn_dcal;
Matrix22 Dpn_dp; Matrix22 Dpn_dp;
@ -179,8 +179,8 @@ public:
* @return range (double) * @return range (double)
*/ */
double range(const Point3& point, double range(const Point3& point,
OptionalJacobian<1, 6> Dcamera = boost::none, OptionalJacobian<1, 6> Dcamera = {},
OptionalJacobian<1, 3> Dpoint = boost::none) const { OptionalJacobian<1, 3> Dpoint = {}) const {
return pose().range(point, Dcamera, Dpoint); return pose().range(point, Dcamera, Dpoint);
} }
@ -189,8 +189,8 @@ public:
* @param pose Other SO(3) pose * @param pose Other SO(3) pose
* @return range (double) * @return range (double)
*/ */
double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none, double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = {},
OptionalJacobian<1, 6> Dpose = boost::none) const { OptionalJacobian<1, 6> Dpose = {}) const {
return this->pose().range(pose, Dcamera, Dpose); return this->pose().range(pose, Dcamera, Dpose);
} }
@ -200,7 +200,7 @@ public:
* @return range (double) * @return range (double)
*/ */
double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> Dcamera = double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> Dcamera =
boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { {}, OptionalJacobian<1, 6> Dother = {}) const {
return pose().range(camera.pose(), Dcamera, Dother); return pose().range(camera.pose(), Dcamera, Dother);
} }
@ -211,8 +211,8 @@ public:
*/ */
template<class CalibrationB> template<class CalibrationB>
double range(const PinholeBaseK<CalibrationB>& camera, double range(const PinholeBaseK<CalibrationB>& camera,
OptionalJacobian<1, 6> Dcamera = boost::none, OptionalJacobian<1, 6> Dcamera = {},
OptionalJacobian<1, 6> Dother = boost::none) const { OptionalJacobian<1, 6> Dother = {}) const {
return pose().range(camera.pose(), Dcamera, Dother); return pose().range(camera.pose(), Dcamera, Dother);
} }
@ -376,14 +376,14 @@ public:
* @param Dpose is the Jacobian w.r.t. the whole camera (really only the pose) * @param Dpose is the Jacobian w.r.t. the whole camera (really only the pose)
* @param Dpoint is the Jacobian w.r.t. point3 * @param Dpoint is the Jacobian w.r.t. point3
*/ */
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 3> Dpoint = boost::none) const { OptionalJacobian<2, 3> Dpoint = {}) const {
return Base::project(pw, Dpose, Dpoint); return Base::project(pw, Dpose, Dpoint);
} }
/// project2 version for point at infinity /// project2 version for point at infinity
Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 2> Dpoint = boost::none) const { OptionalJacobian<2, 2> Dpoint = {}) const {
return Base::project(pw, Dpose, Dpoint); return Base::project(pw, Dpose, Dpoint);
} }

14
gtsam/geometry/Point2.h
View File

@ -36,12 +36,12 @@ GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair
using Point2Pairs = std::vector<Point2Pair>; using Point2Pairs = std::vector<Point2Pair>;
/// Distance of the point from the origin, with Jacobian /// Distance of the point from the origin, with Jacobian
GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = {});
/// distance between two points /// distance between two points
GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q, GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
OptionalJacobian<1, 2> H1 = boost::none, OptionalJacobian<1, 2> H1 = {},
OptionalJacobian<1, 2> H2 = boost::none); OptionalJacobian<1, 2> H2 = {});
// For MATLAB wrapper // For MATLAB wrapper
typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector; typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
@ -56,14 +56,14 @@ inline Point2 operator*(double s, const Point2& p) {
* Calculate f and h, respectively the parallel and perpendicular distance of * Calculate f and h, respectively the parallel and perpendicular distance of
* the intersections of two circles along and from the line connecting the centers. * the intersections of two circles along and from the line connecting the centers.
* Both are dimensionless fractions of the distance d between the circle centers. * Both are dimensionless fractions of the distance d between the circle centers.
* If the circles do not intersect or they are identical, returns boost::none. * If the circles do not intersect or they are identical, returns {}.
* If one solution (touching circles, as determined by tol), h will be exactly zero. * If one solution (touching circles, as determined by tol), h will be exactly zero.
* h is a good measure for how accurate the intersection will be, as when circles touch * h is a good measure for how accurate the intersection will be, as when circles touch
* or nearly touch, the intersection is ill-defined with noisy radius measurements. * or nearly touch, the intersection is ill-defined with noisy radius measurements.
* @param R_d : R/d, ratio of radius of first circle to distance between centers * @param R_d : R/d, ratio of radius of first circle to distance between centers
* @param r_d : r/d, ratio of radius of second circle to distance between centers * @param r_d : r/d, ratio of radius of second circle to distance between centers
* @param tol: absolute tolerance below which we consider touching circles * @param tol: absolute tolerance below which we consider touching circles
* @return optional Point2 with f and h, boost::none if no solution. * @return optional Point2 with f and h, {} if no solution.
*/ */
GTSAM_EXPORT std::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9); GTSAM_EXPORT std::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
@ -97,8 +97,8 @@ template <>
struct Range<Point2, Point2> { struct Range<Point2, Point2> {
typedef double result_type; typedef double result_type;
double operator()(const Point2& p, const Point2& q, double operator()(const Point2& p, const Point2& q,
OptionalJacobian<1, 2> H1 = boost::none, OptionalJacobian<1, 2> H1 = {},
OptionalJacobian<1, 2> H2 = boost::none) { OptionalJacobian<1, 2> H2 = {}) {
return distance2(p, q, H1, H2); return distance2(p, q, H1, H2);
} }
}; };

20
gtsam/geometry/Point3.h
View File

@ -44,24 +44,24 @@ using Point3Pairs = std::vector<Point3Pair>;
/// distance between two points /// distance between two points
GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q, GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H1 = {},
OptionalJacobian<1, 3> H2 = boost::none); OptionalJacobian<1, 3> H2 = {});
/// Distance of the point from the origin, with Jacobian /// Distance of the point from the origin, with Jacobian
GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none); GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {});
/// normalize, with optional Jacobian /// normalize, with optional Jacobian
GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none); GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {});
/// cross product @return this x q /// cross product @return this x q
GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q, GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
OptionalJacobian<3, 3> H_p = boost::none, OptionalJacobian<3, 3> H_p = {},
OptionalJacobian<3, 3> H_q = boost::none); OptionalJacobian<3, 3> H_q = {});
/// dot product /// dot product
GTSAM_EXPORT double dot(const Point3& p, const Point3& q, GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H_p = boost::none, OptionalJacobian<1, 3> H_p = {},
OptionalJacobian<1, 3> H_q = boost::none); OptionalJacobian<1, 3> H_q = {});
/// mean /// mean
template <class CONTAINER> template <class CONTAINER>
@ -82,8 +82,8 @@ template <>
struct Range<Point3, Point3> { struct Range<Point3, Point3> {
typedef double result_type; typedef double result_type;
double operator()(const Point3& p, const Point3& q, double operator()(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H1 = {},
OptionalJacobian<1, 3> H2 = boost::none) { OptionalJacobian<1, 3> H2 = {}) {
return distance3(p, q, H1, H2); return distance3(p, q, H1, H2);
} }
}; };

14
gtsam/geometry/Quaternion.h
View File

@ -55,14 +55,14 @@ struct traits<QUATERNION_TYPE> {
/// @name Lie group traits /// @name Lie group traits
/// @{ /// @{
static Q Compose(const Q &g, const Q & h, static Q Compose(const Q &g, const Q & h,
ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { ChartJacobian Hg = {}, ChartJacobian Hh = {}) {
if (Hg) *Hg = h.toRotationMatrix().transpose(); if (Hg) *Hg = h.toRotationMatrix().transpose();
if (Hh) *Hh = I_3x3; if (Hh) *Hh = I_3x3;
return g * h; return g * h;
} }
static Q Between(const Q &g, const Q & h, static Q Between(const Q &g, const Q & h,
ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { ChartJacobian Hg = {}, ChartJacobian Hh = {}) {
Q d = g.inverse() * h; Q d = g.inverse() * h;
if (Hg) *Hg = -d.toRotationMatrix().transpose(); if (Hg) *Hg = -d.toRotationMatrix().transpose();
if (Hh) *Hh = I_3x3; if (Hh) *Hh = I_3x3;
@ -70,14 +70,14 @@ struct traits<QUATERNION_TYPE> {
} }
static Q Inverse(const Q &g, static Q Inverse(const Q &g,
ChartJacobian H = boost::none) { ChartJacobian H = {}) {
if (H) *H = -g.toRotationMatrix(); if (H) *H = -g.toRotationMatrix();
return g.inverse(); return g.inverse();
} }
/// Exponential map, using the inlined code from Eigen's conversion from axis/angle /// Exponential map, using the inlined code from Eigen's conversion from axis/angle
static Q Expmap(const Eigen::Ref<const TangentVector>& omega, static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
ChartJacobian H = boost::none) { ChartJacobian H = {}) {
using std::cos; using std::cos;
using std::sin; using std::sin;
if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>()); if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>());
@ -95,7 +95,7 @@ struct traits<QUATERNION_TYPE> {
} }
/// We use our own Logmap, as there is a slight bug in Eigen /// We use our own Logmap, as there is a slight bug in Eigen
static TangentVector Logmap(const Q& q, ChartJacobian H = boost::none) { static TangentVector Logmap(const Q& q, ChartJacobian H = {}) {
using std::acos; using std::acos;
using std::sqrt; using std::sqrt;
@ -145,7 +145,7 @@ struct traits<QUATERNION_TYPE> {
/// @{ /// @{
static TangentVector Local(const Q& g, const Q& h, static TangentVector Local(const Q& g, const Q& h,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
Q b = Between(g, h, H1, H2); Q b = Between(g, h, H1, H2);
Matrix3 D_v_b; Matrix3 D_v_b;
TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0); TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0);
@ -155,7 +155,7 @@ struct traits<QUATERNION_TYPE> {
} }
static Q Retract(const Q& g, const TangentVector& v, static Q Retract(const Q& g, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
Matrix3 D_h_v; Matrix3 D_h_v;
Q b = Expmap(v,H2 ? &D_h_v : 0); Q b = Expmap(v,H2 ? &D_h_v : 0);
Q h = Compose(g, b, H1, H2); Q h = Compose(g, b, H1, H2);

18
gtsam/geometry/Rot2.h
View File

@ -80,7 +80,7 @@ namespace gtsam {
* @return 2D rotation \f$ \in SO(2) \f$ * @return 2D rotation \f$ \in SO(2) \f$
*/ */
static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H = static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H =
boost::none); {});
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */ /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
static Rot2 atan2(double y, double x); static Rot2 atan2(double y, double x);
@ -123,10 +123,10 @@ namespace gtsam {
/// @{ /// @{
/// Exponential map at identity - create a rotation from canonical coordinates /// Exponential map at identity - create a rotation from canonical coordinates
static Rot2 Expmap(const Vector1& v, ChartJacobian H = boost::none); static Rot2 Expmap(const Vector1& v, ChartJacobian H = {});
/// Log map at identity - return the canonical coordinates of this rotation /// Log map at identity - return the canonical coordinates of this rotation
static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none); static Vector1 Logmap(const Rot2& r, ChartJacobian H = {});
/** Calculate Adjoint map */ /** Calculate Adjoint map */
Matrix1 AdjointMap() const { return I_1x1; } Matrix1 AdjointMap() const { return I_1x1; }
@ -143,10 +143,10 @@ namespace gtsam {
// Chart at origin simply uses exponential map and its inverse // Chart at origin simply uses exponential map and its inverse
struct ChartAtOrigin { struct ChartAtOrigin {
static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) { static Rot2 Retract(const Vector1& v, ChartJacobian H = {}) {
return Expmap(v, H); return Expmap(v, H);
} }
static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) { static Vector1 Local(const Rot2& r, ChartJacobian H = {}) {
return Logmap(r, H); return Logmap(r, H);
} }
}; };
@ -160,8 +160,8 @@ namespace gtsam {
/** /**
* rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$
*/ */
Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = {},
OptionalJacobian<2, 2> H2 = boost::none) const; OptionalJacobian<2, 2> H2 = {}) const;
/** syntactic sugar for rotate */ /** syntactic sugar for rotate */
inline Point2 operator*(const Point2& p) const { inline Point2 operator*(const Point2& p) const {
@ -171,8 +171,8 @@ namespace gtsam {
/** /**
* rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
*/ */
Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = {},
OptionalJacobian<2, 2> H2 = boost::none) const; OptionalJacobian<2, 2> H2 = {}) const;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface

56
gtsam/geometry/Rot3.h
View File

@ -152,13 +152,13 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
/// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
static Rot3 RzRyRx(double x, double y, double z, static Rot3 RzRyRx(double x, double y, double z,
OptionalJacobian<3, 1> Hx = boost::none, OptionalJacobian<3, 1> Hx = {},
OptionalJacobian<3, 1> Hy = boost::none, OptionalJacobian<3, 1> Hy = {},
OptionalJacobian<3, 1> Hz = boost::none); OptionalJacobian<3, 1> Hz = {});
/// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
inline static Rot3 RzRyRx(const Vector& xyz, inline static Rot3 RzRyRx(const Vector& xyz,
OptionalJacobian<3, 3> H = boost::none) { OptionalJacobian<3, 3> H = {}) {
assert(xyz.size() == 3); assert(xyz.size() == 3);
Rot3 out; Rot3 out;
if (H) { if (H) {
@ -194,9 +194,9 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
* Positive roll is to right (decreasing yaw in aircraft). * Positive roll is to right (decreasing yaw in aircraft).
*/ */
static Rot3 Ypr(double y, double p, double r, static Rot3 Ypr(double y, double p, double r,
OptionalJacobian<3, 1> Hy = boost::none, OptionalJacobian<3, 1> Hy = {},
OptionalJacobian<3, 1> Hp = boost::none, OptionalJacobian<3, 1> Hp = {},
OptionalJacobian<3, 1> Hr = boost::none) { OptionalJacobian<3, 1> Hr = {}) {
return RzRyRx(r, p, y, Hr, Hp, Hy); return RzRyRx(r, p, y, Hr, Hp, Hy);
} }
@ -347,8 +347,8 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
// Cayley chart around origin // Cayley chart around origin
struct CayleyChart { struct CayleyChart {
static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = boost::none); static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = {});
static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = boost::none); static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = {});
}; };
/// Retraction from R^3 to Rot3 manifold using the Cayley transform /// Retraction from R^3 to Rot3 manifold using the Cayley transform
@ -371,7 +371,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
* Exponential map at identity - create a rotation from canonical coordinates * Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
*/ */
static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) { static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = {}) {
if(H) *H = Rot3::ExpmapDerivative(v); if(H) *H = Rot3::ExpmapDerivative(v);
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
return traits<gtsam::Quaternion>::Expmap(v); return traits<gtsam::Quaternion>::Expmap(v);
@ -384,7 +384,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
* Log map at identity - returns the canonical coordinates * Log map at identity - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation * \f$ [R_x,R_y,R_z] \f$ of this rotation
*/ */
static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none); static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = {});
/// Derivative of Expmap /// Derivative of Expmap
static Matrix3 ExpmapDerivative(const Vector3& x); static Matrix3 ExpmapDerivative(const Vector3& x);
@ -397,8 +397,8 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
// Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE
struct ChartAtOrigin { struct ChartAtOrigin {
static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none); static Rot3 Retract(const Vector3& v, ChartJacobian H = {});
static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none); static Vector3 Local(const Rot3& r, ChartJacobian H = {});
}; };
using LieGroup<Rot3, 3>::inverse; // version with derivative using LieGroup<Rot3, 3>::inverse; // version with derivative
@ -410,27 +410,27 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
/** /**
* rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$
*/ */
Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = {},
OptionalJacobian<3,3> H2 = boost::none) const; OptionalJacobian<3,3> H2 = {}) const;
/// rotate point from rotated coordinate frame to world = R*p /// rotate point from rotated coordinate frame to world = R*p
Point3 operator*(const Point3& p) const; Point3 operator*(const Point3& p) const;
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = {},
OptionalJacobian<3,3> H2=boost::none) const; OptionalJacobian<3,3> H2={}) const;
/// @} /// @}
/// @name Group Action on Unit3 /// @name Group Action on Unit3
/// @{ /// @{
/// rotate 3D direction from rotated coordinate frame to world frame /// rotate 3D direction from rotated coordinate frame to world frame
Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = {},
OptionalJacobian<2,2> Hp = boost::none) const; OptionalJacobian<2,2> Hp = {}) const;
/// unrotate 3D direction from world frame to rotated coordinate frame /// unrotate 3D direction from world frame to rotated coordinate frame
Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = {},
OptionalJacobian<2,2> Hp = boost::none) const; OptionalJacobian<2,2> Hp = {}) const;
/// rotate 3D direction from rotated coordinate frame to world frame /// rotate 3D direction from rotated coordinate frame to world frame
Unit3 operator*(const Unit3& p) const; Unit3 operator*(const Unit3& p) const;
@ -458,19 +458,19 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
* Use RQ to calculate xyz angle representation * Use RQ to calculate xyz angle representation
* @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z) * @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z)
*/ */
Vector3 xyz(OptionalJacobian<3, 3> H = boost::none) const; Vector3 xyz(OptionalJacobian<3, 3> H = {}) const;
/** /**
* Use RQ to calculate yaw-pitch-roll angle representation * Use RQ to calculate yaw-pitch-roll angle representation
* @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r) * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r)
*/ */
Vector3 ypr(OptionalJacobian<3, 3> H = boost::none) const; Vector3 ypr(OptionalJacobian<3, 3> H = {}) const;
/** /**
* Use RQ to calculate roll-pitch-yaw angle representation * Use RQ to calculate roll-pitch-yaw angle representation
* @return a vector containing rpy s.t. R = Rot3::Ypr(y,p,r) * @return a vector containing rpy s.t. R = Rot3::Ypr(y,p,r)
*/ */
Vector3 rpy(OptionalJacobian<3, 3> H = boost::none) const; Vector3 rpy(OptionalJacobian<3, 3> H = {}) const;
/** /**
* Accessor to get to component of angle representations * Accessor to get to component of angle representations
@ -478,7 +478,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
* you should instead use xyz() or ypr() * you should instead use xyz() or ypr()
* TODO: make this more efficient * TODO: make this more efficient
*/ */
double roll(OptionalJacobian<1, 3> H = boost::none) const; double roll(OptionalJacobian<1, 3> H = {}) const;
/** /**
* Accessor to get to component of angle representations * Accessor to get to component of angle representations
@ -486,7 +486,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
* you should instead use xyz() or ypr() * you should instead use xyz() or ypr()
* TODO: make this more efficient * TODO: make this more efficient
*/ */
double pitch(OptionalJacobian<1, 3> H = boost::none) const; double pitch(OptionalJacobian<1, 3> H = {}) const;
/** /**
* Accessor to get to component of angle representations * Accessor to get to component of angle representations
@ -494,7 +494,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
* you should instead use xyz() or ypr() * you should instead use xyz() or ypr()
* TODO: make this more efficient * TODO: make this more efficient
*/ */
double yaw(OptionalJacobian<1, 3> H = boost::none) const; double yaw(OptionalJacobian<1, 3> H = {}) const;
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface
@ -583,7 +583,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
* @return a vector [thetax, thetay, thetaz] in radians. * @return a vector [thetax, thetay, thetaz] in radians.
*/ */
GTSAM_EXPORT std::pair<Matrix3, Vector3> RQ( GTSAM_EXPORT std::pair<Matrix3, Vector3> RQ(
const Matrix3& A, OptionalJacobian<3, 9> H = boost::none); const Matrix3& A, OptionalJacobian<3, 9> H = {});
template<> template<>
struct traits<Rot3> : public internal::LieGroup<Rot3> {}; struct traits<Rot3> : public internal::LieGroup<Rot3> {};

10
gtsam/geometry/SO3.h
View File

@ -121,7 +121,7 @@ namespace so3 {
* We only provide the 9*9 derivative in the first argument M. * We only provide the 9*9 derivative in the first argument M.
*/ */
GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
OptionalJacobian<9, 9> H = boost::none); OptionalJacobian<9, 9> H = {});
/// (constant) Jacobian of compose wrpt M /// (constant) Jacobian of compose wrpt M
GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
@ -170,13 +170,13 @@ class DexpFunctor : public ExpmapFunctor {
const Matrix3& dexp() const { return dexp_; } const Matrix3& dexp() const { return dexp_; }
/// Multiplies with dexp(), with optional derivatives /// Multiplies with dexp(), with optional derivatives
GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = boost::none) const; OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with dexp().inverse(), with optional derivatives /// Multiplies with dexp().inverse(), with optional derivatives
GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v, GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = boost::none) const; OptionalJacobian<3, 3> H2 = {}) const;
}; };
} // namespace so3 } // namespace so3

4
gtsam/geometry/SO4.h
View File

@ -70,13 +70,13 @@ Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H);
/** /**
* Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3). * Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3).
*/ */
GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::none); GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = {});
/** /**
* Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) * Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q)
* -> \f$ S \in St(3,4) \f$. * -> \f$ S \in St(3,4) \f$.
*/ */
GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none); GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = {});
/** Serialization function */ /** Serialization function */
template <class Archive> template <class Archive>

10
gtsam/geometry/SOn.h
View File

@ -242,12 +242,12 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
* Retract uses Cayley map. See note about xi element order in Hat. * Retract uses Cayley map. See note about xi element order in Hat.
* Deafault implementation has no Jacobian implemented * Deafault implementation has no Jacobian implemented
*/ */
static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none); static SO Retract(const TangentVector& xi, ChartJacobian H = {});
/** /**
* Inverse of Retract. See note about xi element order in Hat. * Inverse of Retract. See note about xi element order in Hat.
*/ */
static TangentVector Local(const SO& R, ChartJacobian H = boost::none); static TangentVector Local(const SO& R, ChartJacobian H = {});
}; };
// Return dynamic identity DxD Jacobian for given SO(n) // Return dynamic identity DxD Jacobian for given SO(n)
@ -267,7 +267,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
/** /**
* Exponential map at identity - create a rotation from canonical coordinates * Exponential map at identity - create a rotation from canonical coordinates
*/ */
static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none); static SO Expmap(const TangentVector& omega, ChartJacobian H = {});
/// Derivative of Expmap, currently only defined for SO3 /// Derivative of Expmap, currently only defined for SO3
static MatrixDD ExpmapDerivative(const TangentVector& omega); static MatrixDD ExpmapDerivative(const TangentVector& omega);
@ -275,7 +275,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
/** /**
* Log map at identity - returns the canonical coordinates of this rotation * Log map at identity - returns the canonical coordinates of this rotation
*/ */
static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none); static TangentVector Logmap(const SO& R, ChartJacobian H = {});
/// Derivative of Logmap, currently only defined for SO3 /// Derivative of Logmap, currently only defined for SO3
static MatrixDD LogmapDerivative(const TangentVector& omega); static MatrixDD LogmapDerivative(const TangentVector& omega);
@ -293,7 +293,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
* X and fixed-size Jacobian if dimension is known at compile time. * X and fixed-size Jacobian if dimension is known at compile time.
* */ * */
VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H = VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
boost::none) const; {}) const;
/// Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N) /// Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
template <int N_ = N, typename = IsFixed<N_>> template <int N_ = N, typename = IsFixed<N_>>

8
gtsam/geometry/Similarity2.h
View File

@ -143,20 +143,20 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
* \f$ [t_x, t_y, \delta, \lambda] \f$ * \f$ [t_x, t_y, \delta, \lambda] \f$
*/ */
static Vector4 Logmap(const Similarity2& S, // static Vector4 Logmap(const Similarity2& S, //
OptionalJacobian<4, 4> Hm = boost::none); OptionalJacobian<4, 4> Hm = {});
/// Exponential map at the identity /// Exponential map at the identity
static Similarity2 Expmap(const Vector4& v, // static Similarity2 Expmap(const Vector4& v, //
OptionalJacobian<4, 4> Hm = boost::none); OptionalJacobian<4, 4> Hm = {});
/// Chart at the origin /// Chart at the origin
struct ChartAtOrigin { struct ChartAtOrigin {
static Similarity2 Retract(const Vector4& v, static Similarity2 Retract(const Vector4& v,
ChartJacobian H = boost::none) { ChartJacobian H = {}) {
return Similarity2::Expmap(v, H); return Similarity2::Expmap(v, H);
} }
static Vector4 Local(const Similarity2& other, static Vector4 Local(const Similarity2& other,
ChartJacobian H = boost::none) { ChartJacobian H = {}) {
return Similarity2::Logmap(other, H); return Similarity2::Logmap(other, H);
} }
}; };

12
gtsam/geometry/Similarity3.h
View File

@ -98,8 +98,8 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
/// Action on a point p is s*(R*p+t) /// Action on a point p is s*(R*p+t)
Point3 transformFrom(const Point3& p, // Point3 transformFrom(const Point3& p, //
OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 7> H1 = {}, //
OptionalJacobian<3, 3> H2 = boost::none) const; OptionalJacobian<3, 3> H2 = {}) const;
/** /**
* Action on a pose T. * Action on a pose T.
@ -142,21 +142,21 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
* \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$ * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$
*/ */
static Vector7 Logmap(const Similarity3& s, // static Vector7 Logmap(const Similarity3& s, //
OptionalJacobian<7, 7> Hm = boost::none); OptionalJacobian<7, 7> Hm = {});
/** Exponential map at the identity /** Exponential map at the identity
*/ */
static Similarity3 Expmap(const Vector7& v, // static Similarity3 Expmap(const Vector7& v, //
OptionalJacobian<7, 7> Hm = boost::none); OptionalJacobian<7, 7> Hm = {});
/// Chart at the origin /// Chart at the origin
struct ChartAtOrigin { struct ChartAtOrigin {
static Similarity3 Retract(const Vector7& v, static Similarity3 Retract(const Vector7& v,
ChartJacobian H = boost::none) { ChartJacobian H = {}) {
return Similarity3::Expmap(v, H); return Similarity3::Expmap(v, H);
} }
static Vector7 Local(const Similarity3& other, static Vector7 Local(const Similarity3& other,
ChartJacobian H = boost::none) { ChartJacobian H = {}) {
return Similarity3::Logmap(other, H); return Similarity3::Logmap(other, H);
} }
}; };

2
gtsam/geometry/SphericalCamera.cpp
View File

@ -96,7 +96,7 @@ Vector2 SphericalCamera::reprojectionError(
Matrix23 H_project_point; Matrix23 H_project_point;
Matrix22 H_error; Matrix22 H_error;
Unit3 projected = project2(point, H_project_pose, H_project_point); Unit3 projected = project2(point, H_project_pose, H_project_point);
Vector2 error = measured.errorVector(projected, boost::none, H_error); Vector2 error = measured.errorVector(projected, {}, H_error);
if (Dpose) *Dpose = H_error * H_project_pose; if (Dpose) *Dpose = H_error * H_project_pose;
if (Dpoint) *Dpoint = H_error * H_project_point; if (Dpoint) *Dpoint = H_error * H_project_point;
return error; return error;

16
gtsam/geometry/SphericalCamera.h
View File

@ -152,16 +152,16 @@ class GTSAM_EXPORT SphericalCamera {
* @param point 3D point in world coordinates * @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point * @return the intrinsic coordinates of the projected point
*/ */
Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 3> Dpoint = boost::none) const; OptionalJacobian<2, 3> Dpoint = {}) const;
/** Project point into the image /** Project point into the image
* (note: there is no CheiralityException for a spherical camera) * (note: there is no CheiralityException for a spherical camera)
* @param point 3D direction in world coordinates * @param point 3D direction in world coordinates
* @return the intrinsic coordinates of the projected point * @return the intrinsic coordinates of the projected point
*/ */
Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none, Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 2> Dpoint = boost::none) const; OptionalJacobian<2, 2> Dpoint = {}) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth /// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Unit3& p, const double depth) const; Point3 backproject(const Unit3& p, const double depth) const;
@ -174,16 +174,16 @@ class GTSAM_EXPORT SphericalCamera {
* @param point 3D point in world coordinates * @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point * @return the intrinsic coordinates of the projected point
*/ */
Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none, Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 3> Dpoint = boost::none) const; OptionalJacobian<2, 3> Dpoint = {}) const;
/** Compute reprojection error for a given 3D point in world coordinates /** Compute reprojection error for a given 3D point in world coordinates
* @param point 3D point in world coordinates * @param point 3D point in world coordinates
* @return the tangent space error between the projection and the measurement * @return the tangent space error between the projection and the measurement
*/ */
Vector2 reprojectionError(const Point3& point, const Unit3& measured, Vector2 reprojectionError(const Point3& point, const Unit3& measured,
OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 3> Dpoint = boost::none) const; OptionalJacobian<2, 3> Dpoint = {}) const;
/// @} /// @}
/// move a cameras according to d /// move a cameras according to d

10
gtsam/geometry/StereoCamera.h
View File

@ -143,7 +143,7 @@ public:
* @param H2 derivative with respect to point * @param H2 derivative with respect to point
*/ */
StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 = StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 =
boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; {}, OptionalJacobian<3, 3> H2 = {}) const;
/// back-project a measurement /// back-project a measurement
Point3 backproject(const StereoPoint2& z) const; Point3 backproject(const StereoPoint2& z) const;
@ -153,8 +153,8 @@ public:
* @param H2 derivative with respect to point * @param H2 derivative with respect to point
*/ */
Point3 backproject2(const StereoPoint2& z, Point3 backproject2(const StereoPoint2& z,
OptionalJacobian<3, 6> H1 = boost::none, OptionalJacobian<3, 6> H1 = {},
OptionalJacobian<3, 3> H2 = boost::none) const; OptionalJacobian<3, 3> H2 = {}) const;
/// @} /// @}
/// @name Deprecated /// @name Deprecated
@ -167,8 +167,8 @@ public:
* @param H3 IGNORED (for calibration) * @param H3 IGNORED (for calibration)
*/ */
StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1, StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1,
OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 = OptionalJacobian<3, 3> H2 = {}, OptionalJacobian<3, 0> H3 =
boost::none) const; {}) const;
/// for Nonlinear Triangulation /// for Nonlinear Triangulation
Vector defaultErrorWhenTriangulatingBehindCamera() const { Vector defaultErrorWhenTriangulatingBehindCamera() const {

4
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -57,7 +57,7 @@ Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3DS2, Duncalibrate1) { TEST(Cal3DS2, Duncalibrate1) {
Matrix computed; Matrix computed;
K.uncalibrate(p, computed, boost::none); K.uncalibrate(p, computed, {});
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical, computed, 1e-5)); CHECK(assert_equal(numerical, computed, 1e-5));
Matrix separate = K.D2d_calibration(p); Matrix separate = K.D2d_calibration(p);
@ -67,7 +67,7 @@ TEST(Cal3DS2, Duncalibrate1) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3DS2, Duncalibrate2) { TEST(Cal3DS2, Duncalibrate2) {
Matrix computed; Matrix computed;
K.uncalibrate(p, boost::none, computed); K.uncalibrate(p, {}, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical, computed, 1e-5)); CHECK(assert_equal(numerical, computed, 1e-5));
Matrix separate = K.D2d_intrinsic(p); Matrix separate = K.D2d_intrinsic(p);

4
gtsam/geometry/tests/testCal3Unified.cpp
View File

@ -68,7 +68,7 @@ Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Unified, Duncalibrate1) { TEST(Cal3Unified, Duncalibrate1) {
Matrix computed; Matrix computed;
K.uncalibrate(p, computed, boost::none); K.uncalibrate(p, computed, {});
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical, computed, 1e-6)); CHECK(assert_equal(numerical, computed, 1e-6));
} }
@ -76,7 +76,7 @@ TEST(Cal3Unified, Duncalibrate1) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Unified, Duncalibrate2) { TEST(Cal3Unified, Duncalibrate2) {
Matrix computed; Matrix computed;
K.uncalibrate(p, boost::none, computed); K.uncalibrate(p, {}, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical, computed, 1e-6)); CHECK(assert_equal(numerical, computed, 1e-6));
} }

8
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -64,7 +64,7 @@ Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) {
TEST(Cal3_S2, Duncalibrate1) { TEST(Cal3_S2, Duncalibrate1) {
Matrix25 computed; Matrix25 computed;
K.uncalibrate(p, computed, boost::none); K.uncalibrate(p, computed, {});
Matrix numerical = numericalDerivative21(uncalibrate_, K, p); Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
CHECK(assert_equal(numerical, computed, 1e-8)); CHECK(assert_equal(numerical, computed, 1e-8));
} }
@ -72,7 +72,7 @@ TEST(Cal3_S2, Duncalibrate1) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2, Duncalibrate2) { TEST(Cal3_S2, Duncalibrate2) {
Matrix computed; Matrix computed;
K.uncalibrate(p, boost::none, computed); K.uncalibrate(p, {}, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p); Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical, computed, 1e-9)); CHECK(assert_equal(numerical, computed, 1e-9));
} }
@ -84,7 +84,7 @@ Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate1) { TEST(Cal3_S2, Dcalibrate1) {
Matrix computed; Matrix computed;
Point2 expected = K.calibrate(p_uv, computed, boost::none); Point2 expected = K.calibrate(p_uv, computed, {});
Matrix numerical = numericalDerivative21(calibrate_, K, p_uv); Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8)); CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8)); CHECK(assert_equal(numerical, computed, 1e-8));
@ -93,7 +93,7 @@ TEST(Cal3_S2, Dcalibrate1) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate2) { TEST(Cal3_S2, Dcalibrate2) {
Matrix computed; Matrix computed;
Point2 expected = K.calibrate(p_uv, boost::none, computed); Point2 expected = K.calibrate(p_uv, {}, computed);
Matrix numerical = numericalDerivative22(calibrate_, K, p_uv); Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8)); CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8)); CHECK(assert_equal(numerical, computed, 1e-8));

2
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -54,7 +54,7 @@ TEST(CalibratedCamera, Create) {
// Check derivative // Check derivative
std::function<CalibratedCamera(Pose3)> f = // std::function<CalibratedCamera(Pose3)> f = //
std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none); std::bind(CalibratedCamera::Create, std::placeholders::_1, nullptr);
Matrix numericalH = numericalDerivative11<CalibratedCamera, Pose3>(f, kDefaultPose); Matrix numericalH = numericalDerivative11<CalibratedCamera, Pose3>(f, kDefaultPose);
EXPECT(assert_equal(numericalH, actualH, 1e-9)); EXPECT(assert_equal(numericalH, actualH, 1e-9));
} }

36
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -40,17 +40,21 @@ TEST(EssentialMatrix, FromRotationAndDirection) {
trueE, EssentialMatrix::FromRotationAndDirection(trueRotation, trueDirection, actualH1, actualH2), trueE, EssentialMatrix::FromRotationAndDirection(trueRotation, trueDirection, actualH1, actualH2),
1e-8)); 1e-8));
Matrix expectedH1 = numericalDerivative11<EssentialMatrix, Rot3>( {
std::bind(EssentialMatrix::FromRotationAndDirection, std::function<EssentialMatrix(const Rot3&)> fn = [](const Rot3& R) {
std::placeholders::_1, trueDirection, boost::none, boost::none), return EssentialMatrix::FromRotationAndDirection(R, trueDirection);
trueRotation); };
EXPECT(assert_equal(expectedH1, actualH1, 1e-7)); Matrix expectedH1 = numericalDerivative11<EssentialMatrix, Rot3>(fn, trueRotation);
EXPECT(assert_equal(expectedH1, actualH1, 1e-7));
}
Matrix expectedH2 = numericalDerivative11<EssentialMatrix, Unit3>( {
std::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, std::function<EssentialMatrix(const Unit3&)> fn = [](const Unit3& t) {
std::placeholders::_1, boost::none, boost::none), return EssentialMatrix::FromRotationAndDirection(trueRotation, t);
trueDirection); };
EXPECT(assert_equal(expectedH2, actualH2, 1e-7)); Matrix expectedH2 = numericalDerivative11<EssentialMatrix, Unit3>(fn, trueDirection);
EXPECT(assert_equal(expectedH2, actualH2, 1e-7));
}
} }
//************************************************************************* //*************************************************************************
@ -174,8 +178,10 @@ TEST (EssentialMatrix, FromPose3_a) {
Matrix actualH; Matrix actualH;
Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras
EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>( std::function<EssentialMatrix(const Pose3&)> fn = [](const Pose3& pose) {
std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose); return EssentialMatrix::FromPose3(pose);
};
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(fn, pose);
EXPECT(assert_equal(expectedH, actualH, 1e-7)); EXPECT(assert_equal(expectedH, actualH, 1e-7));
} }
@ -187,8 +193,10 @@ TEST (EssentialMatrix, FromPose3_b) {
EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); EssentialMatrix E(c1Rc2, Unit3(c1Tc2));
Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>( std::function<EssentialMatrix(const Pose3&)> fn = [](const Pose3& pose) {
std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose); return EssentialMatrix::FromPose3(pose);
};
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(fn, pose);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }

28
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -23,7 +23,6 @@
using namespace std::placeholders; using namespace std::placeholders;
using namespace gtsam; using namespace gtsam;
using boost::none;
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
@ -57,17 +56,17 @@ TEST(OrientedPlane3, transform) {
gtsam::Point3(2.0, 3.0, 4.0)); gtsam::Point3(2.0, 3.0, 4.0));
OrientedPlane3 plane(-1, 0, 0, 5); OrientedPlane3 plane(-1, 0, 0, 5);
OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3);
OrientedPlane3 transformedPlane = plane.transform(pose, none, none); OrientedPlane3 transformedPlane = plane.transform(pose, {}, {});
EXPECT(assert_equal(expectedPlane, transformedPlane, 1e-5)); EXPECT(assert_equal(expectedPlane, transformedPlane, 1e-5));
// Test the jacobians of transform // Test the jacobians of transform
Matrix actualH1, expectedH1, actualH2, expectedH2; Matrix actualH1, expectedH1, actualH2, expectedH2;
expectedH1 = numericalDerivative21(transform_, plane, pose); expectedH1 = numericalDerivative21(transform_, plane, pose);
plane.transform(pose, actualH1, none); plane.transform(pose, actualH1, {});
EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
expectedH2 = numericalDerivative22(transform_, plane, pose); expectedH2 = numericalDerivative22(transform_, plane, pose);
plane.transform(pose, none, actualH2); plane.transform(pose, {}, actualH2);
EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
} }
@ -135,8 +134,9 @@ TEST(OrientedPlane3, errorVector) {
EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2])); EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2]));
std::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f = std::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
std::bind(&OrientedPlane3::errorVector, std::placeholders::_1, [](const OrientedPlane3& p1, const OrientedPlane3& p2) {
std::placeholders::_2, boost::none, boost::none); return p1.errorVector(p2);
};
expectedH1 = numericalDerivative21(f, plane1, plane2); expectedH1 = numericalDerivative21(f, plane1, plane2);
expectedH2 = numericalDerivative22(f, plane1, plane2); expectedH2 = numericalDerivative22(f, plane1, plane2);
EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
@ -147,8 +147,10 @@ TEST(OrientedPlane3, errorVector) {
TEST(OrientedPlane3, jacobian_retract) { TEST(OrientedPlane3, jacobian_retract) {
OrientedPlane3 plane(-1, 0.1, 0.2, 5); OrientedPlane3 plane(-1, 0.1, 0.2, 5);
Matrix33 H_actual; Matrix33 H_actual;
std::function<OrientedPlane3(const Vector3&)> f = std::bind( std::function<OrientedPlane3(const Vector3&)> f = [&plane](const Vector3& v) {
&OrientedPlane3::retract, plane, std::placeholders::_1, boost::none); return plane.retract(v);
};
{ {
Vector3 v(-0.1, 0.2, 0.3); Vector3 v(-0.1, 0.2, 0.3);
plane.retract(v, H_actual); plane.retract(v, H_actual);
@ -168,8 +170,9 @@ TEST(OrientedPlane3, jacobian_normal) {
Matrix23 H_actual, H_expected; Matrix23 H_actual, H_expected;
OrientedPlane3 plane(-1, 0.1, 0.2, 5); OrientedPlane3 plane(-1, 0.1, 0.2, 5);
std::function<Unit3(const OrientedPlane3&)> f = std::bind( std::function<Unit3(const OrientedPlane3&)> f = [](const OrientedPlane3& p) {
&OrientedPlane3::normal, std::placeholders::_1, boost::none); return p.normal();
};
H_expected = numericalDerivative11(f, plane); H_expected = numericalDerivative11(f, plane);
plane.normal(H_actual); plane.normal(H_actual);
@ -181,8 +184,9 @@ TEST(OrientedPlane3, jacobian_distance) {
Matrix13 H_actual, H_expected; Matrix13 H_actual, H_expected;
OrientedPlane3 plane(-1, 0.1, 0.2, 5); OrientedPlane3 plane(-1, 0.1, 0.2, 5);
std::function<double(const OrientedPlane3&)> f = std::bind( std::function<double(const OrientedPlane3&)> f = [](const OrientedPlane3& p) {
&OrientedPlane3::distance, std::placeholders::_1, boost::none); return p.distance();
};
H_expected = numericalDerivative11(f, plane); H_expected = numericalDerivative11(f, plane);
plane.distance(H_actual); plane.distance(H_actual);

4
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -67,7 +67,7 @@ TEST(PinholeCamera, Create) {
// Check derivative // Check derivative
std::function<Camera(Pose3, Cal3_S2)> f = // std::function<Camera(Pose3, Cal3_S2)> f = //
std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2, std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none); nullptr, nullptr);
Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K); Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
EXPECT(assert_equal(numericalH1, actualH1, 1e-9)); EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K); Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
@ -82,7 +82,7 @@ TEST(PinholeCamera, Pose) {
// Check derivative // Check derivative
std::function<Pose3(Camera)> f = // std::function<Pose3(Camera)> f = //
std::bind(&Camera::getPose, std::placeholders::_1, boost::none); std::bind(&Camera::getPose, std::placeholders::_1, nullptr);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera); Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9)); EXPECT(assert_equal(numericalH, actualH, 1e-9));
} }

2
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -66,7 +66,7 @@ TEST(PinholeCamera, Pose) {
// Check derivative // Check derivative
std::function<Pose3(Camera)> f = // std::function<Pose3(Camera)> f = //
std::bind(&Camera::getPose,_1,boost::none); std::bind(&Camera::getPose,_1,{});
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera); Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9)); EXPECT(assert_equal(numericalH, actualH, 1e-9));
} }

12
gtsam/geometry/tests/testPoint3.cpp
View File

@ -121,9 +121,8 @@ TEST( Point3, dot) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Point3, cross) { TEST(Point3, cross) {
Matrix aH1, aH2; Matrix aH1, aH2;
std::function<Point3(const Point3&, const Point3&)> f = std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2, [](const Point3& p, const Point3& q) { return gtsam::cross(p, q); };
boost::none, boost::none);
const Point3 omega(0, 1, 0), theta(4, 6, 8); const Point3 omega(0, 1, 0), theta(4, 6, 8);
cross(omega, theta, aH1, aH2); cross(omega, theta, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
@ -142,8 +141,7 @@ TEST( Point3, cross2) {
// Use numerical derivatives to calculate the expected Jacobians // Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2; Matrix H1, H2;
std::function<Point3(const Point3&, const Point3&)> f = std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2, // [](const Point3& p, const Point3& q) { return gtsam::cross(p, q); };
boost::none, boost::none);
{ {
gtsam::cross(p, q, H1, H2); gtsam::cross(p, q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, q), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, q), H1, 1e-9));
@ -162,8 +160,8 @@ TEST (Point3, normalize) {
Point3 point(1, -2, 3); // arbitrary point Point3 point(1, -2, 3); // arbitrary point
Point3 expected(point / sqrt(14.0)); Point3 expected(point / sqrt(14.0));
EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8)); EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<Point3, Point3>( std::function<Point3(const Point3&)> fn = [](const Point3& p) { return normalize(p); };
std::bind(gtsam::normalize, std::placeholders::_1, boost::none), point); Matrix expectedH = numericalDerivative11<Point3, Point3>(fn, point);
EXPECT(assert_equal(expectedH, actualH, 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-8));
} }

8
gtsam/geometry/tests/testPose2.cpp
View File

@ -228,7 +228,7 @@ TEST( Pose2, ExpmapDerivative1) {
Vector3 w(0.1, 0.27, -0.3); Vector3 w(0.1, 0.27, -0.3);
Pose2::Expmap(w,actualH); Pose2::Expmap(w,actualH);
Matrix3 expectedH = numericalDerivative21<Pose2, Vector3, Matrix3 expectedH = numericalDerivative21<Pose2, Vector3,
OptionalJacobian<3, 3> >(&Pose2::Expmap, w, boost::none, 1e-2); OptionalJacobian<3, 3> >(&Pose2::Expmap, w, {}, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
@ -238,7 +238,7 @@ TEST( Pose2, ExpmapDerivative2) {
Vector3 w0(0.1, 0.27, 0.0); // alpha = 0 Vector3 w0(0.1, 0.27, 0.0); // alpha = 0
Pose2::Expmap(w0,actualH); Pose2::Expmap(w0,actualH);
Matrix3 expectedH = numericalDerivative21<Pose2, Vector3, Matrix3 expectedH = numericalDerivative21<Pose2, Vector3,
OptionalJacobian<3, 3> >(&Pose2::Expmap, w0, boost::none, 1e-2); OptionalJacobian<3, 3> >(&Pose2::Expmap, w0, {}, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
@ -249,7 +249,7 @@ TEST( Pose2, LogmapDerivative1) {
Pose2 p = Pose2::Expmap(w); Pose2 p = Pose2::Expmap(w);
EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5)); EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5));
Matrix3 expectedH = numericalDerivative21<Vector3, Pose2, Matrix3 expectedH = numericalDerivative21<Vector3, Pose2,
OptionalJacobian<3, 3> >(&Pose2::Logmap, p, boost::none, 1e-2); OptionalJacobian<3, 3> >(&Pose2::Logmap, p, {}, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
@ -260,7 +260,7 @@ TEST( Pose2, LogmapDerivative2) {
Pose2 p = Pose2::Expmap(w0); Pose2 p = Pose2::Expmap(w0);
EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5)); EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5));
Matrix3 expectedH = numericalDerivative21<Vector3, Pose2, Matrix3 expectedH = numericalDerivative21<Vector3, Pose2,
OptionalJacobian<3, 3> >(&Pose2::Logmap, p, boost::none, 1e-2); OptionalJacobian<3, 3> >(&Pose2::Logmap, p, {}, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }

2
gtsam/geometry/tests/testRot3.cpp
View File

@ -360,7 +360,7 @@ TEST( Rot3, rotate_derivatives)
{ {
Matrix actualDrotate1a, actualDrotate1b, actualDrotate2; Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
R.rotate(P, actualDrotate1a, actualDrotate2); R.rotate(P, actualDrotate1a, actualDrotate2);
R.inverse().rotate(P, actualDrotate1b, boost::none); R.inverse().rotate(P, actualDrotate1b, {});
Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3,Point3>, R, P); Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3,Point3>, R, P);
Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3,Point3>, R.inverse(), P); Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3,Point3>, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3,Point3>, R, P); Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3,Point3>, R, P);

2
gtsam/geometry/tests/testRot3Q.cpp
View File

@ -30,7 +30,7 @@ using namespace gtsam;
//****************************************************************************** //******************************************************************************
TEST(Rot3Q , Compare) { TEST(Rot3Q , Compare) {
using boost::none; using {};
// We set up expected values with quaternions // We set up expected values with quaternions
typedef Quaternion Q; typedef Quaternion Q;

10
gtsam/geometry/tests/testSO3.cpp
View File

@ -210,7 +210,7 @@ TEST(SO3, ExpmapDerivative) {
TEST(SO3, ExpmapDerivative2) { TEST(SO3, ExpmapDerivative2) {
const Vector3 theta(0.1, 0, 0.1); const Vector3 theta(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>( const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta); std::bind(&SO3::Expmap, std::placeholders::_1, nullptr), theta);
CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()), CHECK(assert_equal(Matrix3(Jexpected.transpose()),
@ -221,7 +221,7 @@ TEST(SO3, ExpmapDerivative2) {
TEST(SO3, ExpmapDerivative3) { TEST(SO3, ExpmapDerivative3) {
const Vector3 theta(10, 20, 30); const Vector3 theta(10, 20, 30);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>( const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta); std::bind(&SO3::Expmap, std::placeholders::_1, nullptr), theta);
CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()), CHECK(assert_equal(Matrix3(Jexpected.transpose()),
@ -276,7 +276,7 @@ TEST(SO3, ExpmapDerivative5) {
TEST(SO3, ExpmapDerivative6) { TEST(SO3, ExpmapDerivative6) {
const Vector3 thetahat(0.1, 0, 0.1); const Vector3 thetahat(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>( const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), thetahat); std::bind(&SO3::Expmap, std::placeholders::_1, nullptr), thetahat);
Matrix3 Jactual; Matrix3 Jactual;
SO3::Expmap(thetahat, Jactual); SO3::Expmap(thetahat, Jactual);
EXPECT(assert_equal(Jexpected, Jactual)); EXPECT(assert_equal(Jexpected, Jactual));
@ -287,7 +287,7 @@ TEST(SO3, LogmapDerivative) {
const Vector3 thetahat(0.1, 0, 0.1); const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>( const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R); std::bind(&SO3::Logmap, std::placeholders::_1, nullptr), R);
const Matrix3 Jactual = SO3::LogmapDerivative(thetahat); const Matrix3 Jactual = SO3::LogmapDerivative(thetahat);
EXPECT(assert_equal(Jexpected, Jactual)); EXPECT(assert_equal(Jexpected, Jactual));
} }
@ -297,7 +297,7 @@ TEST(SO3, JacobianLogmap) {
const Vector3 thetahat(0.1, 0, 0.1); const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>( const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R); std::bind(&SO3::Logmap, std::placeholders::_1, nullptr), R);
Matrix3 Jactual; Matrix3 Jactual;
SO3::Logmap(R, Jactual); SO3::Logmap(R, Jactual);
EXPECT(assert_equal(Jexpected, Jactual)); EXPECT(assert_equal(Jexpected, Jactual));

2
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -123,7 +123,7 @@ static Point2 project2(const Pose3& pose, const Point3& point) {
TEST( SimpleCamera, Dproject_point_pose) TEST( SimpleCamera, Dproject_point_pose)
{ {
Matrix Dpose, Dpoint; Matrix Dpose, Dpoint;
Point2 result = camera.project(point1, Dpose, Dpoint, boost::none); Point2 result = camera.project(point1, Dpose, Dpoint, {});
Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); Matrix numerical_pose = numericalDerivative21(project2, pose1, point1);
Matrix numerical_point = numericalDerivative22(project2, pose1, point1); Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
CHECK(assert_equal(result, Point2(-100, 100) )); CHECK(assert_equal(result, Point2(-100, 100) ));

4
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -96,11 +96,11 @@ static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_
TEST( StereoCamera, Dproject) TEST( StereoCamera, Dproject)
{ {
Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K); Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K);
Matrix actual1; stereoCam.project2(landmark, actual1, boost::none); Matrix actual1; stereoCam.project2(landmark, actual1, {});
CHECK(assert_equal(expected1,actual1,1e-7)); CHECK(assert_equal(expected1,actual1,1e-7));
Matrix expected2 = numericalDerivative32(project3, camPose, landmark, *K); Matrix expected2 = numericalDerivative32(project3, camPose, landmark, *K);
Matrix actual2; stereoCam.project2(landmark, boost::none, actual2); Matrix actual2; stereoCam.project2(landmark, {}, actual2);
CHECK(assert_equal(expected2,actual2,1e-7)); CHECK(assert_equal(expected2,actual2,1e-7));
} }

42
gtsam/geometry/tests/testUnit3.cpp
View File

@ -74,12 +74,12 @@ TEST(Unit3, rotate) {
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expectedH = numericalDerivative21(rotate_, R, p); expectedH = numericalDerivative21(rotate_, R, p);
R.rotate(p, actualH, boost::none); R.rotate(p, actualH, nullptr);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
{ {
expectedH = numericalDerivative22(rotate_, R, p); expectedH = numericalDerivative22(rotate_, R, p);
R.rotate(p, boost::none, actualH); R.rotate(p, nullptr, actualH);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
} }
@ -100,12 +100,12 @@ TEST(Unit3, unrotate) {
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expectedH = numericalDerivative21(unrotate_, R, p); expectedH = numericalDerivative21(unrotate_, R, p);
R.unrotate(p, actualH, boost::none); R.unrotate(p, actualH, nullptr);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
{ {
expectedH = numericalDerivative22(unrotate_, R, p); expectedH = numericalDerivative22(unrotate_, R, p);
R.unrotate(p, boost::none, actualH); R.unrotate(p, nullptr, actualH);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
} }
@ -124,7 +124,7 @@ TEST(Unit3, dot) {
Matrix H1, H2; Matrix H1, H2;
std::function<double(const Unit3&, const Unit3&)> f = std::function<double(const Unit3&, const Unit3&)> f =
std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2, // std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2, //
boost::none, boost::none); nullptr, nullptr);
{ {
p.dot(q, H1, H2); p.dot(q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-5)); EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-5));
@ -154,13 +154,13 @@ TEST(Unit3, error) {
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expected = numericalDerivative11<Vector2,Unit3>( expected = numericalDerivative11<Vector2,Unit3>(
std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), q); std::bind(&Unit3::error, &p, std::placeholders::_1, nullptr), q);
p.error(q, actual); p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
} }
{ {
expected = numericalDerivative11<Vector2,Unit3>( expected = numericalDerivative11<Vector2,Unit3>(
std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), r); std::bind(&Unit3::error, &p, std::placeholders::_1, nullptr), r);
p.error(r, actual); p.error(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
} }
@ -182,33 +182,33 @@ TEST(Unit3, error2) {
{ {
expected = numericalDerivative21<Vector2, Unit3, Unit3>( expected = numericalDerivative21<Vector2, Unit3, Unit3>(
std::bind(&Unit3::errorVector, std::placeholders::_1, std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none), std::placeholders::_2, nullptr, nullptr),
p, q); p, q);
p.errorVector(q, actual, boost::none); p.errorVector(q, actual, nullptr);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
{ {
expected = numericalDerivative21<Vector2, Unit3, Unit3>( expected = numericalDerivative21<Vector2, Unit3, Unit3>(
std::bind(&Unit3::errorVector, std::placeholders::_1, std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none), std::placeholders::_2, nullptr, nullptr),
p, r); p, r);
p.errorVector(r, actual, boost::none); p.errorVector(r, actual, nullptr);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
{ {
expected = numericalDerivative22<Vector2, Unit3, Unit3>( expected = numericalDerivative22<Vector2, Unit3, Unit3>(
std::bind(&Unit3::errorVector, std::placeholders::_1, std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none), std::placeholders::_2, nullptr, nullptr),
p, q); p, q);
p.errorVector(q, boost::none, actual); p.errorVector(q, nullptr, actual);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
{ {
expected = numericalDerivative22<Vector2, Unit3, Unit3>( expected = numericalDerivative22<Vector2, Unit3, Unit3>(
std::bind(&Unit3::errorVector, std::placeholders::_1, std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none), std::placeholders::_2, nullptr, nullptr),
p, r); p, r);
p.errorVector(r, boost::none, actual); p.errorVector(r, nullptr, actual);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
} }
@ -225,13 +225,13 @@ TEST(Unit3, distance) {
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expected = numericalGradient<Unit3>( expected = numericalGradient<Unit3>(
std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), q); std::bind(&Unit3::distance, &p, std::placeholders::_1, nullptr), q);
p.distance(q, actual); p.distance(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
} }
{ {
expected = numericalGradient<Unit3>( expected = numericalGradient<Unit3>(
std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), r); std::bind(&Unit3::distance, &p, std::placeholders::_1, nullptr), r);
p.distance(r, actual); p.distance(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
} }
@ -323,7 +323,7 @@ TEST(Unit3, basis) {
Matrix62 actualH; Matrix62 actualH;
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>( Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
std::bind(BasisTest, std::placeholders::_1, boost::none), p); std::bind(BasisTest, std::placeholders::_1, nullptr), p);
// without H, first time // without H, first time
EXPECT(assert_equal(expected, p.basis(), 1e-6)); EXPECT(assert_equal(expected, p.basis(), 1e-6));
@ -352,7 +352,7 @@ TEST(Unit3, basis_derivatives) {
p.basis(actualH); p.basis(actualH);
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>( Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
std::bind(BasisTest, std::placeholders::_1, boost::none), p); std::bind(BasisTest, std::placeholders::_1, nullptr), p);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
} }
@ -381,7 +381,7 @@ TEST (Unit3, jacobian_retract) {
Matrix22 H; Matrix22 H;
Unit3 p; Unit3 p;
std::function<Unit3(const Vector2&)> f = std::function<Unit3(const Vector2&)> f =
std::bind(&Unit3::retract, p, std::placeholders::_1, boost::none); std::bind(&Unit3::retract, p, std::placeholders::_1, nullptr);
{ {
Vector2 v (-0.2, 0.1); Vector2 v (-0.2, 0.1);
p.retract(v, H); p.retract(v, H);
@ -444,7 +444,7 @@ TEST (Unit3, FromPoint3) {
Unit3 expected(point); Unit3 expected(point);
EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5)); EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5));
Matrix expectedH = numericalDerivative11<Unit3, Point3>( Matrix expectedH = numericalDerivative11<Unit3, Point3>(
std::bind(Unit3::FromPoint3, std::placeholders::_1, boost::none), point); std::bind(Unit3::FromPoint3, std::placeholders::_1, nullptr), point);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }

5
gtsam/inference/EliminateableFactorGraph.h
View File

@ -19,6 +19,7 @@
#pragma once #pragma once
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <cstddef>
#include <functional> #include <functional>
#include <optional> #include <optional>
#include <boost/variant.hpp> #include <boost/variant.hpp>
@ -326,7 +327,7 @@ namespace gtsam {
/** @deprecated */ /** @deprecated */
boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED marginalMultifrontalBayesNet( boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> variables, boost::variant<const Ordering&, const KeyVector&> variables,
boost::none_t, std::nullptr_t,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) const { OptionalVariableIndex variableIndex = {}) const {
return marginalMultifrontalBayesNet(variables, function, variableIndex); return marginalMultifrontalBayesNet(variables, function, variableIndex);
@ -335,7 +336,7 @@ namespace gtsam {
/** @deprecated */ /** @deprecated */
boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED marginalMultifrontalBayesTree( boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> variables, boost::variant<const Ordering&, const KeyVector&> variables,
boost::none_t, std::nullptr_t,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) const { OptionalVariableIndex variableIndex = {}) const {
return marginalMultifrontalBayesTree(variables, function, variableIndex); return marginalMultifrontalBayesTree(variables, function, variableIndex);

3
gtsam/linear/GaussianFactorGraph.h
View File

@ -21,6 +21,7 @@
#pragma once #pragma once
#include <cstddef>
#include <gtsam/inference/EliminateableFactorGraph.h> #include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator #include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
@ -414,7 +415,7 @@ namespace gtsam {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/** @deprecated */ /** @deprecated */
VectorValues GTSAM_DEPRECATED VectorValues GTSAM_DEPRECATED
optimize(boost::none_t, const Eliminate& function = optimize(std::nullptr_t, const Eliminate& function =
EliminationTraitsType::DefaultEliminate) const { EliminationTraitsType::DefaultEliminate) const {
return optimize(function); return optimize(function);
} }

2
gtsam/navigation/AHRSFactor.h
View File

@ -103,7 +103,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
/// Predict bias-corrected incremental rotation /// Predict bias-corrected incremental rotation
/// TODO: The matrix Hbias is the derivative of predict? Unit-test? /// TODO: The matrix Hbias is the derivative of predict? Unit-test?
Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const; Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = {}) const;
// This function is only used for test purposes // This function is only used for test purposes
// (compare numerical derivatives wrt analytic ones) // (compare numerical derivatives wrt analytic ones)

2
gtsam/navigation/AttitudeFactor.h
View File

@ -54,7 +54,7 @@ public:
/** vector of errors */ /** vector of errors */
Vector attitudeError(const Rot3& p, Vector attitudeError(const Rot3& p,
OptionalJacobian<2,3> H = boost::none) const; OptionalJacobian<2,3> H = {}) const;
const Unit3& nZ() const { const Unit3& nZ() const {
return nZ_; return nZ_;

8
gtsam/navigation/ImuBias.h
View File

@ -72,8 +72,8 @@ public:
/** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
Vector3 correctAccelerometer(const Vector3& measurement, Vector3 correctAccelerometer(const Vector3& measurement,
OptionalJacobian<3, 6> H1 = boost::none, OptionalJacobian<3, 6> H1 = {},
OptionalJacobian<3, 3> H2 = boost::none) const { OptionalJacobian<3, 3> H2 = {}) const {
if (H1) (*H1) << -I_3x3, Z_3x3; if (H1) (*H1) << -I_3x3, Z_3x3;
if (H2) (*H2) << I_3x3; if (H2) (*H2) << I_3x3;
return measurement - biasAcc_; return measurement - biasAcc_;
@ -81,8 +81,8 @@ public:
/** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
Vector3 correctGyroscope(const Vector3& measurement, Vector3 correctGyroscope(const Vector3& measurement,
OptionalJacobian<3, 6> H1 = boost::none, OptionalJacobian<3, 6> H1 = {},
OptionalJacobian<3, 3> H2 = boost::none) const { OptionalJacobian<3, 3> H2 = {}) const {
if (H1) (*H1) << Z_3x3, -I_3x3; if (H1) (*H1) << Z_3x3, -I_3x3;
if (H2) (*H2) << I_3x3; if (H2) (*H2) << I_3x3;
return measurement - biasGyro_; return measurement - biasGyro_;

2
gtsam/navigation/MagFactor.h
View File

@ -65,7 +65,7 @@ public:
static Point3 unrotate(const Rot2& R, const Point3& p, static Point3 unrotate(const Rot2& R, const Point3& p,
OptionalMatrixType HR = OptionalNone) { OptionalMatrixType HR = OptionalNone) {
Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none); Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, {});
if (HR) { if (HR) {
// assign to temporary first to avoid error in Win-Debug mode // assign to temporary first to avoid error in Win-Debug mode
Matrix H = HR->col(2); Matrix H = HR->col(2);

2
gtsam/navigation/ManifoldPreintegration.cpp
View File

@ -113,7 +113,7 @@ Vector9 ManifoldPreintegration::biasCorrectedDelta(
const imuBias::ConstantBias biasIncr = bias_i - biasHat_; const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
Matrix3 D_correctedRij_bias; Matrix3 D_correctedRij_bias;
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, {},
H ? &D_correctedRij_bias : 0); H ? &D_correctedRij_bias : 0);
if (H) if (H)
D_correctedRij_bias *= delRdelBiasOmega_; D_correctedRij_bias *= delRdelBiasOmega_;

2
gtsam/navigation/ManifoldPreintegration.h
View File

@ -103,7 +103,7 @@ public:
/// summarizing the preintegrated IMU measurements so far /// summarizing the preintegrated IMU measurements so far
/// NOTE(frank): implementation is different in two versions /// NOTE(frank): implementation is different in two versions
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const override; OptionalJacobian<9, 6> H = {}) const override;
/** Dummy clone for MATLAB */ /** Dummy clone for MATLAB */
virtual boost::shared_ptr<ManifoldPreintegration> clone() const { virtual boost::shared_ptr<ManifoldPreintegration> clone() const {

12
gtsam/navigation/TangentPreintegration.h
View File

@ -91,9 +91,9 @@ public:
static Vector9 UpdatePreintegrated(const Vector3& a_body, static Vector9 UpdatePreintegrated(const Vector3& a_body,
const Vector3& w_body, const double dt, const Vector3& w_body, const double dt,
const Vector9& preintegrated, const Vector9& preintegrated,
OptionalJacobian<9, 9> A = boost::none, OptionalJacobian<9, 9> A = {},
OptionalJacobian<9, 3> B = boost::none, OptionalJacobian<9, 3> B = {},
OptionalJacobian<9, 3> C = boost::none); OptionalJacobian<9, 3> C = {});
/// Update preintegrated measurements and get derivatives /// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame /// It takes measured quantities in the j frame
@ -106,13 +106,13 @@ public:
/// summarizing the preintegrated IMU measurements so far /// summarizing the preintegrated IMU measurements so far
/// NOTE(frank): implementation is different in two versions /// NOTE(frank): implementation is different in two versions
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const override; OptionalJacobian<9, 6> H = {}) const override;
// Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
double deltaT12, double deltaT12,
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H1 = {},
OptionalJacobian<9, 9> H2 = boost::none); OptionalJacobian<9, 9> H2 = {});
/// Merge in a different set of measurements and update bias derivatives accordingly /// Merge in a different set of measurements and update bias derivatives accordingly
/// The derivatives apply to the preintegrated Vector9 /// The derivatives apply to the preintegrated Vector9

4
gtsam/navigation/tests/testImuBias.cpp
View File

@ -125,7 +125,7 @@ TEST(ImuBias, Correct1) {
const Vector3 measurement(1, 2, 3); const Vector3 measurement(1, 2, 3);
std::function<Vector3(const Bias&, const Vector3&)> f = std::function<Vector3(const Bias&, const Vector3&)> f =
std::bind(&Bias::correctAccelerometer, std::placeholders::_1, std::bind(&Bias::correctAccelerometer, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none); std::placeholders::_2, nullptr, nullptr);
bias1.correctAccelerometer(measurement, aH1, aH2); bias1.correctAccelerometer(measurement, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
@ -137,7 +137,7 @@ TEST(ImuBias, Correct2) {
const Vector3 measurement(1, 2, 3); const Vector3 measurement(1, 2, 3);
std::function<Vector3(const Bias&, const Vector3&)> f = std::function<Vector3(const Bias&, const Vector3&)> f =
std::bind(&Bias::correctGyroscope, std::placeholders::_1, std::bind(&Bias::correctGyroscope, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none); std::placeholders::_2, nullptr, nullptr);
bias1.correctGyroscope(measurement, aH1, aH2); bias1.correctGyroscope(measurement, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));

20
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -149,7 +149,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
std::function<Vector9(const NavState&, const NavState&, const Bias&)> f = std::function<Vector9(const NavState&, const NavState&, const Bias&)> f =
std::bind(&PreintegrationBase::computeError, actual, std::bind(&PreintegrationBase::computeError, actual,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
boost::none, boost::none, boost::none); nullptr, nullptr, nullptr);
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
@ -205,7 +205,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
pim.biasCorrectedDelta(kZeroBias, actualH); pim.biasCorrectedDelta(kZeroBias, actualH);
Matrix expectedH = numericalDerivative11<Vector9, Bias>( Matrix expectedH = numericalDerivative11<Vector9, Bias>(
std::bind(&PreintegrationBase::biasCorrectedDelta, pim, std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
std::placeholders::_1, boost::none), kZeroBias); std::placeholders::_1, nullptr), kZeroBias);
EXPECT(assert_equal(expectedH, actualH)); EXPECT(assert_equal(expectedH, actualH));
Matrix9 aH1; Matrix9 aH1;
@ -213,11 +213,11 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2); NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2);
Matrix eH1 = numericalDerivative11<NavState, NavState>( Matrix eH1 = numericalDerivative11<NavState, NavState>(
std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1, std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1,
kZeroBias, boost::none, boost::none), state1); kZeroBias, nullptr, nullptr), state1);
EXPECT(assert_equal(eH1, aH1)); EXPECT(assert_equal(eH1, aH1));
Matrix eH2 = numericalDerivative11<NavState, Bias>( Matrix eH2 = numericalDerivative11<NavState, Bias>(
std::bind(&PreintegrationBase::predict, pim, state1, std::bind(&PreintegrationBase::predict, pim, state1,
std::placeholders::_1, boost::none, boost::none), kZeroBias); std::placeholders::_1, nullptr, nullptr), kZeroBias);
EXPECT(assert_equal(eH2, aH2)); EXPECT(assert_equal(eH2, aH2));
} }
@ -334,7 +334,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
pim.biasCorrectedDelta(bias, actualH); pim.biasCorrectedDelta(bias, actualH);
Matrix expectedH = numericalDerivative11<Vector9, Bias>( Matrix expectedH = numericalDerivative11<Vector9, Bias>(
std::bind(&PreintegrationBase::biasCorrectedDelta, pim, std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
std::placeholders::_1, boost::none), bias); std::placeholders::_1, nullptr), bias);
EXPECT(assert_equal(expectedH, actualH)); EXPECT(assert_equal(expectedH, actualH));
// Create factor // Create factor
@ -520,7 +520,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
// Check updatedDeltaXij derivatives // Check updatedDeltaXij derivatives
Matrix3 D_correctedAcc_measuredOmega = Z_3x3; Matrix3 D_correctedAcc_measuredOmega = Z_3x3;
pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega, pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega,
boost::none, D_correctedAcc_measuredOmega, boost::none); nullptr, D_correctedAcc_measuredOmega, nullptr);
Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>( Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(
std::bind(correctedAcc, pim, measuredAcc, std::placeholders::_1), std::bind(correctedAcc, pim, measuredAcc, std::placeholders::_1),
measuredOmega, 1e-6); measuredOmega, 1e-6);
@ -531,19 +531,19 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
// TODO(frank): revive derivative tests // TODO(frank): revive derivative tests
// Matrix93 G1, G2; // Matrix93 G1, G2;
// Vector9 preint = // Vector9 preint =
// pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2); // pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, {}, G1, G2);
// //
// Matrix93 expectedG1 = numericalDerivative21<NavState, Vector3, Vector3>( // Matrix93 expectedG1 = numericalDerivative21<NavState, Vector3, Vector3>(
// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, // std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
// std::placeholders::_1, std::placeholders::_2, // std::placeholders::_1, std::placeholders::_2,
// dt, boost::none, boost::none, boost::none), measuredAcc, // dt, {}, {}, {}), measuredAcc,
// measuredOmega, 1e-6); // measuredOmega, 1e-6);
// EXPECT(assert_equal(expectedG1, G1, 1e-5)); // EXPECT(assert_equal(expectedG1, G1, 1e-5));
// //
// Matrix93 expectedG2 = numericalDerivative22<NavState, Vector3, Vector3>( // Matrix93 expectedG2 = numericalDerivative22<NavState, Vector3, Vector3>(
// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, // std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
// std::placeholders::_1, std::placeholders::_2, // std::placeholders::_1, std::placeholders::_2,
// dt, boost::none, boost::none, boost::none), measuredAcc, // dt, {}, {}, {}), measuredAcc,
// measuredOmega, 1e-6); // measuredOmega, 1e-6);
// EXPECT(assert_equal(expectedG2, G2, 1e-5)); // EXPECT(assert_equal(expectedG2, G2, 1e-5));
@ -658,7 +658,7 @@ TEST(ImuFactor, PredictArbitrary) {
p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise
PreintegratedImuMeasurements pim(p, biasHat); PreintegratedImuMeasurements pim(p, biasHat);
Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
// EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega, // EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, {}, measuredAcc, measuredOmega,
// Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000)); // Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000));
double dt = 0.001; double dt = 0.001;

4
gtsam/navigation/tests/testManifoldPreintegration.cpp
View File

@ -101,8 +101,8 @@ TEST(ManifoldPreintegration, computeError) {
const imuBias::ConstantBias&)> const imuBias::ConstantBias&)>
f = std::bind(&ManifoldPreintegration::computeError, pim, f = std::bind(&ManifoldPreintegration::computeError, pim,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, std::placeholders::_3, nullptr, nullptr,
boost::none); nullptr);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));

20
gtsam/navigation/tests/testNavState.cpp
View File

@ -41,7 +41,7 @@ static const Vector9 kZeroXi = Vector9::Zero();
TEST(NavState, Constructor) { TEST(NavState, Constructor) {
std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create = std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2, std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, boost::none); std::placeholders::_3, nullptr, nullptr, nullptr);
Matrix aH1, aH2, aH3; Matrix aH1, aH2, aH3;
EXPECT( EXPECT(
assert_equal(kState1, assert_equal(kState1,
@ -61,7 +61,7 @@ TEST(NavState, Constructor) {
TEST(NavState, Constructor2) { TEST(NavState, Constructor2) {
std::function<NavState(const Pose3&, const Vector3&)> construct = std::function<NavState(const Pose3&, const Vector3&)> construct =
std::bind(&NavState::FromPoseVelocity, std::placeholders::_1, std::bind(&NavState::FromPoseVelocity, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none); std::placeholders::_2, nullptr, nullptr);
Matrix aH1, aH2; Matrix aH1, aH2;
EXPECT( EXPECT(
assert_equal(kState1, assert_equal(kState1,
@ -76,7 +76,7 @@ TEST( NavState, Attitude) {
Rot3 actual = kState1.attitude(aH); Rot3 actual = kState1.attitude(aH);
EXPECT(assert_equal(actual, kAttitude)); EXPECT(assert_equal(actual, kAttitude));
eH = numericalDerivative11<Rot3, NavState>( eH = numericalDerivative11<Rot3, NavState>(
std::bind(&NavState::attitude, std::placeholders::_1, boost::none), kState1); std::bind(&NavState::attitude, std::placeholders::_1, nullptr), kState1);
EXPECT(assert_equal((Matrix )eH, aH)); EXPECT(assert_equal((Matrix )eH, aH));
} }
@ -86,7 +86,7 @@ TEST( NavState, Position) {
Point3 actual = kState1.position(aH); Point3 actual = kState1.position(aH);
EXPECT(assert_equal(actual, kPosition)); EXPECT(assert_equal(actual, kPosition));
eH = numericalDerivative11<Point3, NavState>( eH = numericalDerivative11<Point3, NavState>(
std::bind(&NavState::position, std::placeholders::_1, boost::none), std::bind(&NavState::position, std::placeholders::_1, nullptr),
kState1); kState1);
EXPECT(assert_equal((Matrix )eH, aH)); EXPECT(assert_equal((Matrix )eH, aH));
} }
@ -97,7 +97,7 @@ TEST( NavState, Velocity) {
Velocity3 actual = kState1.velocity(aH); Velocity3 actual = kState1.velocity(aH);
EXPECT(assert_equal(actual, kVelocity)); EXPECT(assert_equal(actual, kVelocity));
eH = numericalDerivative11<Velocity3, NavState>( eH = numericalDerivative11<Velocity3, NavState>(
std::bind(&NavState::velocity, std::placeholders::_1, boost::none), std::bind(&NavState::velocity, std::placeholders::_1, nullptr),
kState1); kState1);
EXPECT(assert_equal((Matrix )eH, aH)); EXPECT(assert_equal((Matrix )eH, aH));
} }
@ -108,7 +108,7 @@ TEST( NavState, BodyVelocity) {
Velocity3 actual = kState1.bodyVelocity(aH); Velocity3 actual = kState1.bodyVelocity(aH);
EXPECT(assert_equal<Velocity3>(actual, kAttitude.unrotate(kVelocity))); EXPECT(assert_equal<Velocity3>(actual, kAttitude.unrotate(kVelocity)));
eH = numericalDerivative11<Velocity3, NavState>( eH = numericalDerivative11<Velocity3, NavState>(
std::bind(&NavState::bodyVelocity, std::placeholders::_1, boost::none), std::bind(&NavState::bodyVelocity, std::placeholders::_1, nullptr),
kState1); kState1);
EXPECT(assert_equal((Matrix )eH, aH)); EXPECT(assert_equal((Matrix )eH, aH));
} }
@ -142,7 +142,7 @@ TEST( NavState, Manifold ) {
kState1.retract(xi, aH1, aH2); kState1.retract(xi, aH1, aH2);
std::function<NavState(const NavState&, const Vector9&)> retract = std::function<NavState(const NavState&, const Vector9&)> retract =
std::bind(&NavState::retract, std::placeholders::_1, std::bind(&NavState::retract, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none); std::placeholders::_2, nullptr, nullptr);
EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1));
EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
@ -155,7 +155,7 @@ TEST( NavState, Manifold ) {
// Check localCoordinates derivatives // Check localCoordinates derivatives
std::function<Vector9(const NavState&, const NavState&)> local = std::function<Vector9(const NavState&, const NavState&)> local =
std::bind(&NavState::localCoordinates, std::placeholders::_1, std::bind(&NavState::localCoordinates, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none); std::placeholders::_2, nullptr, nullptr);
// from state1 to state2 // from state1 to state2
kState1.localCoordinates(state2, aH1, aH2); kState1.localCoordinates(state2, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1)); EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1));
@ -174,7 +174,7 @@ TEST( NavState, Manifold ) {
static const double dt = 2.0; static const double dt = 2.0;
std::function<Vector9(const NavState&, const bool&)> coriolis = std::function<Vector9(const NavState&, const bool&)> coriolis =
std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis, std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis,
std::placeholders::_2, boost::none); std::placeholders::_2, nullptr);
TEST(NavState, Coriolis) { TEST(NavState, Coriolis) {
Matrix9 aH; Matrix9 aH;
@ -252,7 +252,7 @@ TEST(NavState, CorrectPIM) {
std::function<Vector9(const NavState&, const Vector9&)> correctPIM = std::function<Vector9(const NavState&, const Vector9&)> correctPIM =
std::bind(&NavState::correctPIM, std::placeholders::_1, std::bind(&NavState::correctPIM, std::placeholders::_1,
std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false, std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false,
boost::none, boost::none); nullptr, nullptr);
kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2); kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1));
EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2)); EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2));

4
gtsam/navigation/tests/testTangentPreintegration.cpp
View File

@ -108,8 +108,8 @@ TEST(TangentPreintegration, computeError) {
const imuBias::ConstantBias&)> const imuBias::ConstantBias&)>
f = std::bind(&TangentPreintegration::computeError, pim, f = std::bind(&TangentPreintegration::computeError, pim,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, std::placeholders::_3, nullptr, nullptr,
boost::none); nullptr);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));

4
gtsam/nonlinear/AdaptAutoDiff.h
View File

@ -47,8 +47,8 @@ class AdaptAutoDiff {
public: public:
VectorT operator()(const Vector1& v1, const Vector2& v2, VectorT operator()(const Vector1& v1, const Vector2& v2,
OptionalJacobian<M, N1> H1 = boost::none, OptionalJacobian<M, N1> H1 = {},
OptionalJacobian<M, N2> H2 = boost::none) { OptionalJacobian<M, N2> H2 = {}) {
using ceres::internal::AutoDiff; using ceres::internal::AutoDiff;
bool success; bool success;

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

@ -241,7 +241,7 @@ struct apply_compose {
typedef T result_type; typedef T result_type;
static const int Dim = traits<T>::dimension; static const int Dim = traits<T>::dimension;
T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 = T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const { {}, OptionalJacobian<Dim, Dim> H2 = {}) const {
return x.compose(y, H1, H2); return x.compose(y, H1, H2);
} }
}; };
@ -249,8 +249,8 @@ struct apply_compose {
template <> template <>
struct apply_compose<double> { struct apply_compose<double> {
double operator()(const double& x, const double& y, double operator()(const double& x, const double& y,
OptionalJacobian<1, 1> H1 = boost::none, OptionalJacobian<1, 1> H1 = {},
OptionalJacobian<1, 1> H2 = boost::none) const { OptionalJacobian<1, 1> H2 = {}) const {
if (H1) H1->setConstant(y); if (H1) H1->setConstant(y);
if (H2) H2->setConstant(x); if (H2) H2->setConstant(x);
return x * y; return x * y;

2
gtsam/nonlinear/FunctorizedFactor.h
View File

@ -42,7 +42,7 @@ namespace gtsam {
* public: * public:
* MultiplyFunctor(double m) : m_(m) {} * MultiplyFunctor(double m) : m_(m) {}
* Matrix operator()(const Matrix &X, * Matrix operator()(const Matrix &X,
* OptionalJacobian<-1, -1> H = boost::none) const { * OptionalJacobian<-1, -1> H = {}) const {
* if (H) * if (H)
* *H = m_ * Matrix::Identity(X.rows()*X.cols(), X.rows()*X.cols()); * *H = m_ * Matrix::Identity(X.rows()*X.cols(), X.rows()*X.cols());
* return m_ * X; * return m_ * X;

5
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -21,6 +21,7 @@
#pragma once #pragma once
#include <cstddef>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/GraphvizFormatting.h> #include <gtsam/nonlinear/GraphvizFormatting.h>
@ -268,11 +269,11 @@ namespace gtsam {
/// @{ /// @{
/** @deprecated */ /** @deprecated */
boost::shared_ptr<HessianFactor> GTSAM_DEPRECATED linearizeToHessianFactor( boost::shared_ptr<HessianFactor> GTSAM_DEPRECATED linearizeToHessianFactor(
const Values& values, boost::none_t, const Dampen& dampen = nullptr) const const Values& values, std::nullptr_t, const Dampen& dampen = nullptr) const
{return linearizeToHessianFactor(values, dampen);} {return linearizeToHessianFactor(values, dampen);}
/** @deprecated */ /** @deprecated */
Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t, Values GTSAM_DEPRECATED updateCholesky(const Values& values, std::nullptr_t,
const Dampen& dampen = nullptr) const const Dampen& dampen = nullptr) const
{return updateCholesky(values, dampen);} {return updateCholesky(values, dampen);}

10
gtsam/nonlinear/internal/ExpressionNode.h
View File

@ -255,7 +255,7 @@ public:
/// Return value /// Return value
T value(const Values& values) const override { T value(const Values& values) const override {
return function_(expression1_->value(values), boost::none); return function_(expression1_->value(values), {});
} }
/// Return keys that play in this expression /// Return keys that play in this expression
@ -365,9 +365,9 @@ public:
/// Return value /// Return value
T value(const Values& values) const override { T value(const Values& values) const override {
using boost::none; using std::nullopt;
return function_(expression1_->value(values), expression2_->value(values), return function_(expression1_->value(values), expression2_->value(values),
none, none); {}, {});
} }
/// Return keys that play in this expression /// Return keys that play in this expression
@ -473,9 +473,9 @@ public:
/// Return value /// Return value
T value(const Values& values) const override { T value(const Values& values) const override {
using boost::none; using std::nullopt;
return function_(expression1_->value(values), expression2_->value(values), return function_(expression1_->value(values), expression2_->value(values),
expression3_->value(values), none, none, none); expression3_->value(values), {}, {}, {});
} }
/// Return keys that play in this expression /// Return keys that play in this expression

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

@ -119,7 +119,7 @@ class Class : public Point3 {
using Point3::Point3; using Point3::Point3;
const Vector3& vector() const { return *this; } const Vector3& vector() const { return *this; }
inline static Class Identity() { return Class(0,0,0); } inline static Class Identity() { return Class(0,0,0); }
double norm(OptionalJacobian<1,3> H = boost::none) const { double norm(OptionalJacobian<1,3> H = {}) const {
return norm3(*this, H); return norm3(*this, H);
} }
bool equals(const Class &q, double tol) const { bool equals(const Class &q, double tol) const {

6
gtsam/nonlinear/tests/testFactorTesting.cpp
View File

@ -28,8 +28,8 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 bodyVelocity(const Pose3& w_t_b, Vector3 bodyVelocity(const Pose3& w_t_b,
const Vector3& vec_w, const Vector3& vec_w,
OptionalJacobian<3, 6> Hpose = boost::none, OptionalJacobian<3, 6> Hpose = {},
OptionalJacobian<3, 3> Hvel = boost::none) { OptionalJacobian<3, 3> Hvel = {}) {
Matrix36 Hrot__pose; Matrix36 Hrot__pose;
Rot3 w_R_b = w_t_b.rotation(Hrot__pose); Rot3 w_R_b = w_t_b.rotation(Hrot__pose);
Matrix33 Hvel__rot; Matrix33 Hvel__rot;
@ -51,7 +51,7 @@ class ScaledVelocityFunctor {
// and velocity scale factor. Also computes the corresponding jacobian // and velocity scale factor. Also computes the corresponding jacobian
// (w.r.t. the velocity scale). // (w.r.t. the velocity scale).
Vector3 operator()(double vscale, Vector3 operator()(double vscale,
OptionalJacobian<3, 1> H = boost::none) const { OptionalJacobian<3, 1> H = {}) const {
// The velocity scale factor value we are optimizing for is centered around // The velocity scale factor value we are optimizing for is centered around
// 0, so we need to add 1 to it before scaling the velocity. // 0, so we need to add 1 to it before scaling the velocity.
const Vector3 scaled_velocity = (vscale + 1.0) * measured_velocity_; const Vector3 scaled_velocity = (vscale + 1.0) * measured_velocity_;

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

@ -47,7 +47,7 @@ class MultiplyFunctor {
MultiplyFunctor(double m) : m_(m) {} MultiplyFunctor(double m) : m_(m) {}
Matrix operator()(const Matrix &X, Matrix operator()(const Matrix &X,
OptionalJacobian<-1, -1> H = boost::none) const { OptionalJacobian<-1, -1> H = {}) const {
if (H) *H = m_ * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols()); if (H) *H = m_ * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
return m_ * X; return m_ * X;
} }
@ -57,8 +57,8 @@ class MultiplyFunctor {
class ProjectionFunctor { class ProjectionFunctor {
public: public:
Vector operator()(const Matrix &A, const Vector &x, Vector operator()(const Matrix &A, const Vector &x,
OptionalJacobian<-1, -1> H1 = boost::none, OptionalJacobian<-1, -1> H1 = {},
OptionalJacobian<-1, -1> H2 = boost::none) const { OptionalJacobian<-1, -1> H2 = {}) const {
if (H1) { if (H1) {
H1->resize(x.size(), A.size()); H1->resize(x.size(), A.size());
*H1 << I_3x3, I_3x3, I_3x3; *H1 << I_3x3, I_3x3, I_3x3;
@ -178,7 +178,7 @@ TEST(FunctorizedFactor, Lambda) {
Matrix measurement = multiplier * Matrix::Identity(3, 3); Matrix measurement = multiplier * Matrix::Identity(3, 3);
auto lambda = [multiplier](const Matrix &X, auto lambda = [multiplier](const Matrix &X,
OptionalJacobian<-1, -1> H = boost::none) { OptionalJacobian<-1, -1> H = {}) {
if (H) if (H)
*H = multiplier * *H = multiplier *
Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols()); Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
@ -251,8 +251,8 @@ TEST(FunctorizedFactor, Lambda2) {
Matrix measurement = A * x; Matrix measurement = A * x;
auto lambda = [](const Matrix &A, const Vector &x, auto lambda = [](const Matrix &A, const Vector &x,
OptionalJacobian<-1, -1> H1 = boost::none, OptionalJacobian<-1, -1> H1 = {},
OptionalJacobian<-1, -1> H2 = boost::none) { OptionalJacobian<-1, -1> H2 = {}) {
if (H1) { if (H1) {
H1->resize(x.size(), A.size()); H1->resize(x.size(), A.size());
*H1 << I_3x3, I_3x3, I_3x3; *H1 << I_3x3, I_3x3, I_3x3;

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

@ -60,13 +60,13 @@ public:
bool equals(const TestValue& other, double tol = 1e-9) const { return true; } bool equals(const TestValue& other, double tol = 1e-9) const { return true; }
size_t dim() const { return 0; } size_t dim() const { return 0; }
TestValue retract(const Vector&, TestValue retract(const Vector&,
OptionalJacobian<dimension,dimension> H1=boost::none, OptionalJacobian<dimension,dimension> H1={},
OptionalJacobian<dimension,dimension> H2=boost::none) const { OptionalJacobian<dimension,dimension> H2={}) const {
return TestValue(); return TestValue();
} }
Vector localCoordinates(const TestValue&, Vector localCoordinates(const TestValue&,
OptionalJacobian<dimension,dimension> H1=boost::none, OptionalJacobian<dimension,dimension> H1={},
OptionalJacobian<dimension,dimension> H2=boost::none) const { OptionalJacobian<dimension,dimension> H2={}) const {
return Vector(); return Vector();
} }
}; };

6
gtsam/slam/ProjectionFactor.h
View File

@ -142,16 +142,16 @@ namespace gtsam {
if(H1) { if(H1) {
gtsam::Matrix H0; gtsam::Matrix H0;
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_); PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); Point2 reprojectionError(camera.project(point, H1, H2, {}) - measured_);
*H1 = *H1 * H0; *H1 = *H1 * H0;
return reprojectionError; return reprojectionError;
} else { } else {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_); PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
return camera.project(point, H1, H2, boost::none) - measured_; return camera.project(point, H1, H2, {}) - measured_;
} }
} else { } else {
PinholeCamera<CALIBRATION> camera(pose, *K_); PinholeCamera<CALIBRATION> camera(pose, *K_);
return camera.project(point, H1, H2, boost::none) - measured_; return camera.project(point, H1, H2, {}) - measured_;
} }
} catch( CheiralityException& e) { } catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6); if (H1) *H1 = Matrix::Zero(2,6);

2
gtsam/slam/TriangulationFactor.h
View File

@ -163,7 +163,7 @@ public:
// Would be even better if we could pass blocks to project // Would be even better if we could pass blocks to project
const Point3& point = x.at<Point3>(key()); const Point3& point = x.at<Point3>(key());
b = traits<Measurement>::Local(camera_.project2(point, boost::none, A), measured_); b = traits<Measurement>::Local(camera_.project2(point, {}, A), measured_);
if (noiseModel_) if (noiseModel_)
this->noiseModel_->WhitenSystem(A, b); this->noiseModel_->WhitenSystem(A, b);

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

@ -91,7 +91,7 @@ TEST( triangulation, TriangulationFactorStereo ) {
// compare same problem against expression factor // compare same problem against expression factor
Expression<StereoPoint2>::UnaryFunction<Point3>::type f = Expression<StereoPoint2>::UnaryFunction<Point3>::type f =
std::bind(&StereoCamera::project2, camera2, std::placeholders::_1, std::bind(&StereoCamera::project2, camera2, std::placeholders::_1,
boost::none, std::placeholders::_2); nullptr, std::placeholders::_2);
Expression<Point3> point_(pointKey); Expression<Point3> point_(pointKey);
Expression<StereoPoint2> project2_(f, point_); Expression<StereoPoint2> project2_(f, point_);

8
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -88,8 +88,8 @@ public:
/** range between translations */ /** range between translations */
double range(const PoseRTV& other, double range(const PoseRTV& other,
OptionalJacobian<1,9> H1=boost::none, OptionalJacobian<1,9> H1={},
OptionalJacobian<1,9> H2=boost::none) const; OptionalJacobian<1,9> H2={}) const;
/// @} /// @}
/// @name IMU-specific /// @name IMU-specific
@ -138,8 +138,8 @@ public:
* Note: the transform jacobian convention is flipped * Note: the transform jacobian convention is flipped
*/ */
PoseRTV transformed_from(const Pose3& trans, PoseRTV transformed_from(const Pose3& trans,
ChartJacobian Dglobal = boost::none, ChartJacobian Dglobal = {},
OptionalJacobian<9, 6> Dtrans = boost::none) const; OptionalJacobian<9, 6> Dtrans = {}) const;
/// @} /// @}
/// @name Utility functions /// @name Utility functions

2
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -130,7 +130,7 @@ public:
Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1); Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1);
Matrix D_gravityBody_gk; Matrix D_gravityBody_gk;
Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, boost::none); Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, {});
Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()).finished(); Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()).finished();
Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;

6
gtsam_unstable/geometry/Event.h
View File

@ -55,7 +55,7 @@ class Event {
Point3 location() const { return location_; } Point3 location() const { return location_; }
// TODO(frank) we really have to think of a better way to do linear arguments // TODO(frank) we really have to think of a better way to do linear arguments
double height(OptionalJacobian<1, 4> H = boost::none) const { double height(OptionalJacobian<1, 4> H = {}) const {
static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished(); static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished();
if (H) *H = JacobianZ; if (H) *H = JacobianZ;
return location_.z(); return location_.z();
@ -101,8 +101,8 @@ class TimeOfArrival {
/// Calculate time of arrival, with derivatives /// Calculate time of arrival, with derivatives
double operator()(const Event& event, const Point3& sensor, // double operator()(const Event& event, const Point3& sensor, //
OptionalJacobian<1, 4> H1 = boost::none, // OptionalJacobian<1, 4> H1 = {}, //
OptionalJacobian<1, 3> H2 = boost::none) const { OptionalJacobian<1, 3> H2 = {}) const {
Matrix13 D1, D2; Matrix13 D1, D2;
double distance = gtsam::distance3(event.location(), sensor, D1, D2); double distance = gtsam::distance3(event.location(), sensor, D1, D2);
if (H1) if (H1)

2
gtsam_unstable/geometry/InvDepthCamera3.h
View File

@ -101,7 +101,7 @@ public:
} }
else { else {
gtsam::Matrix J2; gtsam::Matrix J2;
gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none); gtsam::Point2 uv= camera.project(landmark,H1, J2, {});
if (H1) { if (H1) {
*H1 = (*H1) * I_6x6; *H1 = (*H1) * I_6x6;
} }

10
gtsam_unstable/geometry/Pose3Upright.h
View File

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

4
gtsam_unstable/geometry/tests/testEvent.cpp
View File

@ -65,11 +65,11 @@ TEST(Event, Derivatives) {
Matrix13 actualH2; Matrix13 actualH2;
kToa(exampleEvent, microphoneAt0, actualH1, actualH2); kToa(exampleEvent, microphoneAt0, actualH1, actualH2);
Matrix expectedH1 = numericalDerivative11<double, Event>( Matrix expectedH1 = numericalDerivative11<double, Event>(
std::bind(kToa, std::placeholders::_1, microphoneAt0, boost::none, boost::none), std::bind(kToa, std::placeholders::_1, microphoneAt0, nullptr, nullptr),
exampleEvent); exampleEvent);
EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
Matrix expectedH2 = numericalDerivative11<double, Point3>( Matrix expectedH2 = numericalDerivative11<double, Point3>(
std::bind(kToa, exampleEvent, std::placeholders::_1, boost::none, boost::none), std::bind(kToa, exampleEvent, std::placeholders::_1, nullptr, nullptr),
microphoneAt0); microphoneAt0);
EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
} }

2
gtsam_unstable/gtsam_unstable.i
View File

@ -179,7 +179,7 @@ class SimWall2D {
gtsam::Point2 midpoint() const; gtsam::Point2 midpoint() const;
bool intersects(const gtsam::SimWall2D& wall) const; bool intersects(const gtsam::SimWall2D& wall) const;
// bool intersects(const gtsam::SimWall2D& wall, boost::optional<gtsam::Point2&> pt=boost::none) const; // bool intersects(const gtsam::SimWall2D& wall, boost::optional<gtsam::Point2&> pt={}) const;
gtsam::Point2 norm() const; gtsam::Point2 norm() const;
gtsam::Rot2 reflection(const gtsam::Point2& init, const gtsam::Point2& intersection) const; gtsam::Rot2 reflection(const gtsam::Point2& init, const gtsam::Point2& intersection) const;

6
tests/testSimulated3D.cpp
View File

@ -46,7 +46,7 @@ TEST( simulated3D, Values )
TEST( simulated3D, Dprior ) TEST( simulated3D, Dprior )
{ {
Point3 x(1,-9, 7); Point3 x(1,-9, 7);
Matrix numerical = numericalDerivative11<Point3, Point3>(std::bind(simulated3D::prior, std::placeholders::_1, boost::none),x); Matrix numerical = numericalDerivative11<Point3, Point3>(std::bind(simulated3D::prior, std::placeholders::_1, nullptr),x);
Matrix computed; Matrix computed;
simulated3D::prior(x,computed); simulated3D::prior(x,computed);
EXPECT(assert_equal(numerical,computed,1e-9)); EXPECT(assert_equal(numerical,computed,1e-9));
@ -60,12 +60,12 @@ TEST( simulated3D, DOdo )
simulated3D::odo(x1, x2, H1, H2); simulated3D::odo(x1, x2, H1, H2);
Matrix A1 = numericalDerivative21<Point3, Point3, Point3>( Matrix A1 = numericalDerivative21<Point3, Point3, Point3>(
std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2, std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none), nullptr, nullptr),
x1, x2); x1, x2);
EXPECT(assert_equal(A1, H1, 1e-9)); EXPECT(assert_equal(A1, H1, 1e-9));
Matrix A2 = numericalDerivative22<Point3, Point3, Point3>( Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(
std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2, std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none), nullptr, nullptr),
x1, x2); x1, x2);
EXPECT(assert_equal(A2, H2, 1e-9)); EXPECT(assert_equal(A2, H2, 1e-9));
} }