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,
ChartJacobian H2 = boost::none) const {
ChartJacobian H2 = {}) const {
if (H1) *H1 = g.inverse().AdjointMap();
if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
return derived() * g;
}
Class between(const Class& g, ChartJacobian H1,
ChartJacobian H2 = boost::none) const {
ChartJacobian H2 = {}) const {
Class result = derived().inverse() * g;
if (H1) *H1 = - result.inverse().AdjointMap();
if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
@ -87,7 +87,7 @@ struct LieGroup {
/// expmap with optional derivatives
Class expmap(const TangentVector& v, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
ChartJacobian H1, ChartJacobian H2 = {}) const {
Jacobian D_g_v;
Class g = Class::Expmap(v,H2 ? &D_g_v : 0);
Class h = compose(g); // derivatives inlined below
@ -98,7 +98,7 @@ struct LieGroup {
/// logmap with optional derivatives
TangentVector logmap(const Class& g, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
ChartJacobian H1, ChartJacobian H2 = {}) const {
Class h = between(g); // derivatives inlined below
Jacobian D_v_h;
TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0);
@ -139,7 +139,7 @@ struct LieGroup {
/// retract with optional derivatives
Class retract(const TangentVector& v, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
ChartJacobian H1, ChartJacobian H2 = {}) const {
Jacobian D_g_v;
Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0);
Class h = compose(g); // derivatives inlined below
@ -150,7 +150,7 @@ struct LieGroup {
/// localCoordinates with optional derivatives
TangentVector localCoordinates(const Class& g, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
ChartJacobian H1, ChartJacobian H2 = {}) const {
Class h = between(g); // derivatives inlined below
Jacobian D_v_h;
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;
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);
}
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);
}
/// @}
/// @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);
}
static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) {
return Class::Expmap(v, Hv);
}
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);
}
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);
}
static Class Inverse(const Class& m, //
ChartJacobian H = boost::none) {
ChartJacobian H = {}) {
return m.inverse(H);
}
/// @}
@ -325,8 +325,8 @@ T expm(const Vector& x, int K=7) {
*/
template <typename T>
T interpolate(const T& X, const T& Y, double t,
typename MakeOptionalJacobian<T, T>::type Hx = boost::none,
typename MakeOptionalJacobian<T, T>::type Hy = boost::none) {
typename MakeOptionalJacobian<T, T>::type Hx = {},
typename MakeOptionalJacobian<T, T>::type Hy = {}) {
if (Hx || Hy) {
typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
const T between =

12
gtsam/base/Matrix.h
View File

@ -459,8 +459,8 @@ struct MultiplyWithInverse {
/// A.inverse() * b, with optional derivatives
VectorN operator()(const MatrixN& A, const VectorN& b,
OptionalJacobian<N, N* N> H1 = boost::none,
OptionalJacobian<N, N> H2 = boost::none) const {
OptionalJacobian<N, N* N> H1 = {},
OptionalJacobian<N, N> H2 = {}) const {
const MatrixN invA = A.inverse();
const VectorN c = invA * b;
// 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
VectorN operator()(const T& a, const VectorN& b,
OptionalJacobian<N, M> H1 = boost::none,
OptionalJacobian<N, N> H2 = boost::none) const {
OptionalJacobian<N, M> H1 = {},
OptionalJacobian<N, N> H2 = {}) const {
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 VectorN c = invA * b;
if (H1) {
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;
}
if (H2) *H2 = invA;

53
gtsam/base/OptionalJacobian.h
View File

@ -18,23 +18,18 @@
*/
#pragma once
#include <cstddef>
#include <gtsam/config.h> // Configuration from CMake
#include <Eigen/Dense>
#include <stdexcept>
#include <string>
#ifndef OPTIONALJACOBIAN_NOBOOST
#include <boost/optional.hpp>
#endif
namespace gtsam {
/**
* 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
* matrix will be resized. Finally, there is a constructor that takes
* boost::none, the default constructor acts like boost::none, and
* boost::optional<Eigen::MatrixXd&> is also supported for backwards compatibility.
* matrix will be resized.
* Below this class, a dynamic version is also implemented.
*/
template<int Rows, int Cols>
@ -66,11 +61,18 @@ private:
public:
/// Default constructor acts like boost::none
/// Default constructor
OptionalJacobian() :
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
OptionalJacobian(Jacobian& fixed) :
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
/// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible
// template <typename Derived, bool InnerPanel>
@ -200,7 +184,7 @@ private:
public:
/// Default constructor acts like boost::none
/// Default constructor acts like
OptionalJacobian() :
pointer_(nullptr) {
}
@ -211,21 +195,6 @@ public:
/// Construct from refrence to dynamic matrix
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
operator bool() const {
return pointer_!=nullptr;

14
gtsam/base/ProductLieGroup.h
View File

@ -75,14 +75,14 @@ public:
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
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");
G g = traits<G>::Retract(this->first, v.template head<dimension1>());
H h = traits<H>::Retract(this->second, v.template tail<dimension2>());
return ProductLieGroup(g,h);
}
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");
typename traits<G>::TangentVector v1 = traits<G>::Local(this->first, g.first);
typename traits<H>::TangentVector v2 = traits<H>::Local(this->second, g.second);
@ -101,7 +101,7 @@ protected:
public:
ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1,
ChartJacobian H2 = boost::none) const {
ChartJacobian H2 = {}) const {
Jacobian1 D_g_first; Jacobian2 D_h_second;
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);
@ -114,7 +114,7 @@ public:
return ProductLieGroup(g,h);
}
ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1,
ChartJacobian H2 = boost::none) const {
ChartJacobian H2 = {}) const {
Jacobian1 D_g_first; Jacobian2 D_h_second;
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);
@ -137,7 +137,7 @@ public:
}
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;
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);
@ -148,7 +148,7 @@ public:
}
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;
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);
@ -161,7 +161,7 @@ public:
}
return v;
}
static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = {}) {
return Logmap(p, Hp);
}
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,
const std::optional<V>& actual, double tol = 1e-9) {
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;
}
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;
}
if (!expected && !actual)
@ -65,7 +65,7 @@ bool assert_equal(const std::optional<V>& expected,
template<class V>
bool assert_equal(const V& expected, const std::optional<V>& actual, double tol = 1e-9) {
if (!actual) {
std::cout << "actual is boost::none" << std::endl;
std::cout << "actual is {}" << std::endl;
return false;
}
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/base/Vector.h>
#include <boost/make_shared.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/assume_abstract.hpp>
#include <memory>

60
gtsam/base/VectorSpace.h
View File

@ -32,7 +32,7 @@ struct VectorSpaceImpl {
static int GetDimension(const Class&) { return N;}
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 (H2) *H2 = Jacobian::Identity();
Class v = other-origin;
@ -40,7 +40,7 @@ struct VectorSpaceImpl {
}
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 (H2) *H2 = Jacobian::Identity();
return origin + v;
@ -51,31 +51,31 @@ struct VectorSpaceImpl {
/// @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();
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();
return Class(v);
}
static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
ChartJacobian H2 = boost::none) {
static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = {},
ChartJacobian H2 = {}) {
if (H1) *H1 = Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity();
return v1 + v2;
}
static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
ChartJacobian H2 = boost::none) {
static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = {},
ChartJacobian H2 = {}) {
if (H1) *H1 = - Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity();
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();
return -v;
}
@ -106,7 +106,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
}
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 (H2) *H2 = Eye(other);
Class v = other-origin;
@ -114,7 +114,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
}
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 (H2) *H2 = Eye(origin);
return origin + v;
@ -125,12 +125,12 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
/// @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);
return m.vector();
}
static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) {
Class result(v);
if (Hv)
*Hv = Eye(v);
@ -138,14 +138,14 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
}
static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1,
ChartJacobian H2 = boost::none) {
ChartJacobian H2 = {}) {
if (H1) *H1 = Eye(v1);
if (H2) *H2 = Eye(v2);
return v1 + v2;
}
static Class Between(const Class& v1, const Class& v2, ChartJacobian H1,
ChartJacobian H2 = boost::none) {
ChartJacobian H2 = {}) {
if (H1) *H1 = - Eye(v1);
if (H2) *H2 = Eye(v2);
return v2 - v1;
@ -237,7 +237,7 @@ struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
typedef OptionalJacobian<1, 1> ChartJacobian;
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 (H2) (*H2)[0] = 1.0;
TangentVector result;
@ -246,7 +246,7 @@ struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
}
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 (H2) (*H2)[0] = 1.0;
return origin + v[0];
@ -255,12 +255,12 @@ struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
/// @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;
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;
return v[0];
}
@ -312,7 +312,7 @@ struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
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 (H2) (*H2) = Jacobian::Identity();
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,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
if (H1) (*H1) = Jacobian::Identity();
if (H2) (*H2) = Jacobian::Identity();
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
/// @{
static TangentVector Logmap(const Fixed& m, ChartJacobian H = boost::none) {
static TangentVector Logmap(const Fixed& m, ChartJacobian H = {}) {
if (H) *H = Jacobian::Identity();
TangentVector result;
Eigen::Map<Fixed>(result.data()) = m;
return result;
}
static Fixed Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
static Fixed Expmap(const TangentVector& v, ChartJacobian H = {}) {
Fixed m;
m.setZero();
if (H) *H = Jacobian::Identity();
@ -393,7 +393,7 @@ struct DynamicTraits {
}
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 (H2) *H2 = Eye(m);
TangentVector v(GetDimension(m));
@ -402,7 +402,7 @@ struct DynamicTraits {
}
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 (H2) *H2 = Eye(m);
return m + Eigen::Map<const Dynamic>(v.data(), m.rows(), m.cols());
@ -411,32 +411,32 @@ struct DynamicTraits {
/// @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);
TangentVector result(GetDimension(m));
Eigen::Map<Dynamic>(result.data(), m.cols(), m.rows()) = m;
return result;
}
static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost::none) {
static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = {}) {
static_cast<void>(H);
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);
return -m;
}
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 (H2) *H2 = Eye(v1);
return v1 + 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 (H2) *H2 = Eye(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:
* Foo bar(const Obj& a, OptionalMatrixType H1)
* Use boost.bind to restructure:
* std::bind(bar, std::placeholders::_1, boost::none)
* This syntax will fix the optional argument to boost::none, while using the first argument provided
* std::bind(bar, std::placeholders::_1, {})
* 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
* Foo SomeClass::bar(const Obj& a)

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

@ -32,7 +32,6 @@ using namespace gtsam;
TEST( OptionalJacobian, Constructors ) {
Matrix23 fixed;
Matrix dynamic;
boost::optional<Matrix&> optional(dynamic);
OptionalJacobian<2, 3> H;
EXPECT(!H);
@ -41,22 +40,18 @@ TEST( OptionalJacobian, Constructors ) {
TEST_CONSTRUCTOR(2, 3, &fixed, 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
OptionalJacobian<-1, -1> H7;
EXPECT(!H7);
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();
void test(OptionalJacobian<2, 3> H = boost::none) {
void test(OptionalJacobian<2, 3> H = {}) {
if (H)
*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)
*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;
}
// 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) {
test3(1, H.cols<1>(0));
test3(2, H.cols<1>(1));

28
gtsam/basis/Basis.h
View File

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

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

@ -17,6 +17,7 @@
* methods
*/
#include <cstddef>
#include <gtsam/basis/Chebyshev2.h>
#include <gtsam/basis/FitBasis.h>
#include <gtsam/geometry/Pose2.h>
@ -119,7 +120,7 @@ TEST(Chebyshev2, InterpolateVector) {
// Check derivative
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 =
numericalDerivative11<Vector2, ParameterMatrix<2>, 2 * N>(f, X);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
@ -377,7 +378,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor) {
// Test Jacobian
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));
}
@ -409,7 +410,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) {
VecD vecd(N, points(0), 0, T);
vecd(X, actualH);
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));
}
@ -426,7 +427,7 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) {
EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);
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));
}

12
gtsam/geometry/BearingRange.h
View File

@ -77,8 +77,8 @@ public:
/// Prediction function that stacks measurements
static BearingRange Measure(
const A1& a1, const A2& a2,
OptionalJacobian<dimension, traits<A1>::dimension> H1 = boost::none,
OptionalJacobian<dimension, traits<A2>::dimension> H2 = boost::none) {
OptionalJacobian<dimension, traits<A1>::dimension> H1 = {},
OptionalJacobian<dimension, traits<A2>::dimension> H2 = {}) {
typename MakeJacobian<B, A1>::type HB1;
typename MakeJacobian<B, A2>::type HB2;
typename MakeJacobian<R, A1>::type HR1;
@ -181,8 +181,8 @@ struct HasBearing {
typedef RT result_type;
RT operator()(
const A1& a1, const A2& a2,
OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1=boost::none,
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2=boost::none) {
OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1={},
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2={}) {
return a1.bearing(a2, H1, H2);
}
};
@ -195,8 +195,8 @@ struct HasRange {
typedef RT result_type;
RT operator()(
const A1& a1, const A2& a2,
OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1=boost::none,
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2=boost::none) {
OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1={},
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> 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>
void calibrateJacobians(const Cal& calibration, const Point2& pn,
OptionalJacobian<2, Dim> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) {
OptionalJacobian<2, Dim> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) {
if (Dcal || Dp) {
Eigen::Matrix<double, 2, Dim> H_uncal_K;
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 Dp;
uncalibrate(p, boost::none, Dp);
uncalibrate(p, {}, Dp);
return Dp;
}
/* ************************************************************************* */
Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const {
Matrix23 Dcal;
uncalibrate(p, Dcal, boost::none);
uncalibrate(p, 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
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/**
* 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
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/// @deprecated might be removed in next release, use uncalibrate
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
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
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;
// 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
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)
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/**
* 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)
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/// @}
/// @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
*/
Point2 uncalibrate(const Point2& p,
OptionalJacobian<2, 10> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
OptionalJacobian<2, 10> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/// Convert a 3D point to normalized unit plane
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
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/**
* 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
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/**
* 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)
inline Cal3_S2 between(const Cal3_S2& q,
OptionalJacobian<5, 5> H1 = boost::none,
OptionalJacobian<5, 5> H2 = boost::none) const {
OptionalJacobian<5, 5> H1 = {},
OptionalJacobian<5, 5> H2 = {}) const {
if (H1) *H1 = -I_5x5;
if (H2) *H2 = I_5x5;
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
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/**
* 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
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/**
* 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 {
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
if (q.z() <= 0)
throw CheiralityException();

36
gtsam/geometry/CalibratedCamera.h
View File

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

14
gtsam/geometry/EssentialMatrix.h
View File

@ -49,12 +49,12 @@ class EssentialMatrix {
/// Named constructor with derivatives
GTSAM_EXPORT static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
OptionalJacobian<5, 3> H1 = boost::none,
OptionalJacobian<5, 2> H2 = boost::none);
OptionalJacobian<5, 3> H1 = {},
OptionalJacobian<5, 2> H2 = {});
/// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
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
template<typename Engine>
@ -139,8 +139,8 @@ class EssentialMatrix {
* @return point in pose coordinates
*/
GTSAM_EXPORT Point3 transformTo(const Point3& p,
OptionalJacobian<3, 5> DE = boost::none,
OptionalJacobian<3, 3> Dpoint = boost::none) const;
OptionalJacobian<3, 5> DE = {},
OptionalJacobian<3, 3> Dpoint = {}) const;
/**
* 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
*/
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
@ -161,7 +161,7 @@ class EssentialMatrix {
/// epipolar error, algebraic
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
*/
GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
OptionalJacobian<4, 6> Dpose = boost::none,
OptionalJacobian<4, 4> Dline = boost::none);
OptionalJacobian<4, 6> Dpose = {},
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
*/
Line3 retract(const Vector4 &v,
OptionalJacobian<4, 4> Dp = boost::none,
OptionalJacobian<4, 4> Dv = boost::none) const;
OptionalJacobian<4, 4> Dp = {},
OptionalJacobian<4, 4> Dv = {}) const;
/**
* 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
*/
Vector4 localCoordinates(const Line3 &q,
OptionalJacobian<4, 4> Dp = boost::none,
OptionalJacobian<4, 4> Dq = boost::none) const;
OptionalJacobian<4, 4> Dp = {},
OptionalJacobian<4, 4> Dq = {}) const;
/**
* Print R, a, b
@ -105,7 +105,7 @@ class GTSAM_EXPORT Line3 {
* @return Unit3 - projected line in image plane, in homogenous coordinates.
* 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

14
gtsam/geometry/OrientedPlane3.h
View File

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

22
gtsam/geometry/PinholeCamera.h
View File

@ -109,8 +109,8 @@ public:
// Create PinholeCamera, with derivatives
static PinholeCamera Create(const Pose3& pose, const Calibration &K,
OptionalJacobian<dimension, 6> H1 = boost::none, //
OptionalJacobian<dimension, DimK> H2 = boost::none) {
OptionalJacobian<dimension, 6> H1 = {}, //
OptionalJacobian<dimension, DimK> H2 = {}) {
typedef Eigen::Matrix<double, DimK, 6> MatrixK6;
if (H1)
*H1 << I_6x6, MatrixK6::Zero();
@ -237,19 +237,19 @@ public:
*Dcamera << Dpose, Dcal;
return pi;
} 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
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);
}
/// project a point at infinity from world coordinates into the image
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);
}
@ -259,7 +259,7 @@ public:
* @return range (double)
*/
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_;
double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint);
if (Dcamera)
@ -273,7 +273,7 @@ public:
* @return range (double)
*/
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_;
double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose);
if (Dcamera)
@ -288,8 +288,8 @@ public:
*/
template<class CalibrationB>
double range(const PinholeCamera<CalibrationB>& camera,
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const {
OptionalJacobian<1, dimension> Dcamera = {},
OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = {}) const {
Matrix16 Dcamera_, Dother_;
double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
Dother ? &Dother_ : 0);
@ -309,8 +309,8 @@ public:
* @return range (double)
*/
double range(const CalibratedCamera& camera,
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6> Dother = boost::none) const {
OptionalJacobian<1, dimension> Dcamera = {},
OptionalJacobian<1, 6> Dother = {}) const {
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
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 3> Dpoint = {},
OptionalJacobian<2, DimK> Dcal = {}) const {
return _project(pw, Dpose, Dpoint, Dcal);
}
/// project a 3D point from world coordinates into the image
Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 3> Dpoint = {},
OptionalJacobian<2, DimK> Dcal = {}) const {
return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured);
}
/// project a point at infinity from world coordinates into the image
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 2> Dpoint = {},
OptionalJacobian<2, DimK> Dcal = {}) const {
return _project(pw, Dpose, Dpoint, Dcal);
}
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& p, double depth,
OptionalJacobian<3, 6> Dresult_dpose = boost::none,
OptionalJacobian<3, 2> Dresult_dp = boost::none,
OptionalJacobian<3, 1> Dresult_ddepth = boost::none,
OptionalJacobian<3, DimK> Dresult_dcal = boost::none) const {
OptionalJacobian<3, 6> Dresult_dpose = {},
OptionalJacobian<3, 2> Dresult_dp = {},
OptionalJacobian<3, 1> Dresult_ddepth = {},
OptionalJacobian<3, DimK> Dresult_dcal = {}) const {
typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
Matrix2K Dpn_dcal;
Matrix22 Dpn_dp;
@ -179,8 +179,8 @@ public:
* @return range (double)
*/
double range(const Point3& point,
OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 3> Dpoint = boost::none) const {
OptionalJacobian<1, 6> Dcamera = {},
OptionalJacobian<1, 3> Dpoint = {}) const {
return pose().range(point, Dcamera, Dpoint);
}
@ -189,8 +189,8 @@ public:
* @param pose Other SO(3) pose
* @return range (double)
*/
double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 6> Dpose = boost::none) const {
double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = {},
OptionalJacobian<1, 6> Dpose = {}) const {
return this->pose().range(pose, Dcamera, Dpose);
}
@ -200,7 +200,7 @@ public:
* @return range (double)
*/
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);
}
@ -211,8 +211,8 @@ public:
*/
template<class CalibrationB>
double range(const PinholeBaseK<CalibrationB>& camera,
OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 6> Dother = boost::none) const {
OptionalJacobian<1, 6> Dcamera = {},
OptionalJacobian<1, 6> Dother = {}) const {
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 Dpoint is the Jacobian w.r.t. point3
*/
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 3> Dpoint = {}) const {
return Base::project(pw, Dpose, Dpoint);
}
/// project2 version for point at infinity
Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const {
Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 2> Dpoint = {}) const {
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>;
/// 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
GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
OptionalJacobian<1, 2> H1 = boost::none,
OptionalJacobian<1, 2> H2 = boost::none);
OptionalJacobian<1, 2> H1 = {},
OptionalJacobian<1, 2> H2 = {});
// For MATLAB wrapper
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
* 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.
* 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.
* 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.
* @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 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);
@ -97,8 +97,8 @@ template <>
struct Range<Point2, Point2> {
typedef double result_type;
double operator()(const Point2& p, const Point2& q,
OptionalJacobian<1, 2> H1 = boost::none,
OptionalJacobian<1, 2> H2 = boost::none) {
OptionalJacobian<1, 2> H1 = {},
OptionalJacobian<1, 2> 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
GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none);
OptionalJacobian<1, 3> H1 = {},
OptionalJacobian<1, 3> H2 = {});
/// 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
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
GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
OptionalJacobian<3, 3> H_p = boost::none,
OptionalJacobian<3, 3> H_q = boost::none);
OptionalJacobian<3, 3> H_p = {},
OptionalJacobian<3, 3> H_q = {});
/// dot product
GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H_p = boost::none,
OptionalJacobian<1, 3> H_q = boost::none);
OptionalJacobian<1, 3> H_p = {},
OptionalJacobian<1, 3> H_q = {});
/// mean
template <class CONTAINER>
@ -82,8 +82,8 @@ template <>
struct Range<Point3, Point3> {
typedef double result_type;
double operator()(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
OptionalJacobian<1, 3> H1 = {},
OptionalJacobian<1, 3> 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
/// @{
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 (Hh) *Hh = I_3x3;
return g * 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;
if (Hg) *Hg = -d.toRotationMatrix().transpose();
if (Hh) *Hh = I_3x3;
@ -70,14 +70,14 @@ struct traits<QUATERNION_TYPE> {
}
static Q Inverse(const Q &g,
ChartJacobian H = boost::none) {
ChartJacobian H = {}) {
if (H) *H = -g.toRotationMatrix();
return g.inverse();
}
/// Exponential map, using the inlined code from Eigen's conversion from axis/angle
static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
ChartJacobian H = boost::none) {
ChartJacobian H = {}) {
using std::cos;
using std::sin;
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
static TangentVector Logmap(const Q& q, ChartJacobian H = boost::none) {
static TangentVector Logmap(const Q& q, ChartJacobian H = {}) {
using std::acos;
using std::sqrt;
@ -145,7 +145,7 @@ struct traits<QUATERNION_TYPE> {
/// @{
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);
Matrix3 D_v_b;
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,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
Matrix3 D_h_v;
Q b = Expmap(v,H2 ? &D_h_v : 0);
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$
*/
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 */
static Rot2 atan2(double y, double x);
@ -123,10 +123,10 @@ namespace gtsam {
/// @{
/// 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
static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none);
static Vector1 Logmap(const Rot2& r, ChartJacobian H = {});
/** Calculate Adjoint map */
Matrix1 AdjointMap() const { return I_1x1; }
@ -143,10 +143,10 @@ namespace gtsam {
// Chart at origin simply uses exponential map and its inverse
struct ChartAtOrigin {
static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) {
static Rot2 Retract(const Vector1& v, ChartJacobian 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);
}
};
@ -160,8 +160,8 @@ namespace gtsam {
/**
* 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,
OptionalJacobian<2, 2> H2 = boost::none) const;
Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = {},
OptionalJacobian<2, 2> H2 = {}) const;
/** syntactic sugar for rotate */
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$
*/
Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = {},
OptionalJacobian<2, 2> H2 = {}) const;
/// @}
/// @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.
static Rot3 RzRyRx(double x, double y, double z,
OptionalJacobian<3, 1> Hx = boost::none,
OptionalJacobian<3, 1> Hy = boost::none,
OptionalJacobian<3, 1> Hz = boost::none);
OptionalJacobian<3, 1> Hx = {},
OptionalJacobian<3, 1> Hy = {},
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.
inline static Rot3 RzRyRx(const Vector& xyz,
OptionalJacobian<3, 3> H = boost::none) {
OptionalJacobian<3, 3> H = {}) {
assert(xyz.size() == 3);
Rot3 out;
if (H) {
@ -194,9 +194,9 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
* Positive roll is to right (decreasing yaw in aircraft).
*/
static Rot3 Ypr(double y, double p, double r,
OptionalJacobian<3, 1> Hy = boost::none,
OptionalJacobian<3, 1> Hp = boost::none,
OptionalJacobian<3, 1> Hr = boost::none) {
OptionalJacobian<3, 1> Hy = {},
OptionalJacobian<3, 1> Hp = {},
OptionalJacobian<3, 1> Hr = {}) {
return RzRyRx(r, p, y, Hr, Hp, Hy);
}
@ -347,8 +347,8 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
// Cayley chart around origin
struct CayleyChart {
static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = boost::none);
static Vector3 Local(const Rot3& r, 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 = {});
};
/// 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
* \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);
#ifdef GTSAM_USE_QUATERNIONS
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
* \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
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
struct ChartAtOrigin {
static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none);
static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none);
static Rot3 Retract(const Vector3& v, ChartJacobian H = {});
static Vector3 Local(const Rot3& r, ChartJacobian H = {});
};
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$
*/
Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2 = boost::none) const;
Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = {},
OptionalJacobian<3,3> H2 = {}) const;
/// rotate point from rotated coordinate frame to world = R*p
Point3 operator*(const Point3& p) const;
/// 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,
OptionalJacobian<3,3> H2=boost::none) const;
Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = {},
OptionalJacobian<3,3> H2={}) const;
/// @}
/// @name Group Action on Unit3
/// @{
/// rotate 3D direction from rotated coordinate frame to world frame
Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
OptionalJacobian<2,2> Hp = boost::none) const;
Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = {},
OptionalJacobian<2,2> Hp = {}) const;
/// unrotate 3D direction from world frame to rotated coordinate frame
Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
OptionalJacobian<2,2> Hp = boost::none) const;
Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = {},
OptionalJacobian<2,2> Hp = {}) const;
/// rotate 3D direction from rotated coordinate frame to world frame
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
* @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
* @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
* @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
@ -478,7 +478,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
* you should instead use xyz() or ypr()
* 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
@ -486,7 +486,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
* you should instead use xyz() or ypr()
* 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
@ -494,7 +494,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
* you should instead use xyz() or ypr()
* TODO: make this more efficient
*/
double yaw(OptionalJacobian<1, 3> H = boost::none) const;
double yaw(OptionalJacobian<1, 3> H = {}) const;
/// @}
/// @name Advanced Interface
@ -583,7 +583,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
* @return a vector [thetax, thetay, thetaz] in radians.
*/
GTSAM_EXPORT std::pair<Matrix3, Vector3> RQ(
const Matrix3& A, OptionalJacobian<3, 9> H = boost::none);
const Matrix3& A, OptionalJacobian<3, 9> H = {});
template<>
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.
*/
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
GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
@ -170,13 +170,13 @@ class DexpFunctor : public ExpmapFunctor {
const Matrix3& dexp() const { return dexp_; }
/// Multiplies with dexp(), with optional derivatives
GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with dexp().inverse(), with optional derivatives
GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
};
} // 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).
*/
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)
* -> \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 */
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.
* 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.
*/
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)
@ -267,7 +267,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
/**
* 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
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
*/
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
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.
* */
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)
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$
*/
static Vector4 Logmap(const Similarity2& S, //
OptionalJacobian<4, 4> Hm = boost::none);
OptionalJacobian<4, 4> Hm = {});
/// Exponential map at the identity
static Similarity2 Expmap(const Vector4& v, //
OptionalJacobian<4, 4> Hm = boost::none);
OptionalJacobian<4, 4> Hm = {});
/// Chart at the origin
struct ChartAtOrigin {
static Similarity2 Retract(const Vector4& v,
ChartJacobian H = boost::none) {
ChartJacobian H = {}) {
return Similarity2::Expmap(v, H);
}
static Vector4 Local(const Similarity2& other,
ChartJacobian H = boost::none) {
ChartJacobian 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)
Point3 transformFrom(const Point3& p, //
OptionalJacobian<3, 7> H1 = boost::none, //
OptionalJacobian<3, 3> H2 = boost::none) const;
OptionalJacobian<3, 7> H1 = {}, //
OptionalJacobian<3, 3> H2 = {}) const;
/**
* 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$
*/
static Vector7 Logmap(const Similarity3& s, //
OptionalJacobian<7, 7> Hm = boost::none);
OptionalJacobian<7, 7> Hm = {});
/** Exponential map at the identity
*/
static Similarity3 Expmap(const Vector7& v, //
OptionalJacobian<7, 7> Hm = boost::none);
OptionalJacobian<7, 7> Hm = {});
/// Chart at the origin
struct ChartAtOrigin {
static Similarity3 Retract(const Vector7& v,
ChartJacobian H = boost::none) {
ChartJacobian H = {}) {
return Similarity3::Expmap(v, H);
}
static Vector7 Local(const Similarity3& other,
ChartJacobian H = boost::none) {
ChartJacobian H = {}) {
return Similarity3::Logmap(other, H);
}
};

2
gtsam/geometry/SphericalCamera.cpp
View File

@ -96,7 +96,7 @@ Vector2 SphericalCamera::reprojectionError(
Matrix23 H_project_point;
Matrix22 H_error;
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 (Dpoint) *Dpoint = H_error * H_project_point;
return error;

16
gtsam/geometry/SphericalCamera.h
View File

@ -152,16 +152,16 @@ class GTSAM_EXPORT SphericalCamera {
* @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point
*/
Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 3> Dpoint = {}) const;
/** Project point into the image
* (note: there is no CheiralityException for a spherical camera)
* @param point 3D direction in world coordinates
* @return the intrinsic coordinates of the projected point
*/
Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const;
Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 2> Dpoint = {}) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Unit3& p, const double depth) const;
@ -174,16 +174,16 @@ class GTSAM_EXPORT SphericalCamera {
* @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point
*/
Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 3> Dpoint = {}) const;
/** Compute reprojection error for a given 3D point in world coordinates
* @param point 3D point in world coordinates
* @return the tangent space error between the projection and the measurement
*/
Vector2 reprojectionError(const Point3& point, const Unit3& measured,
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
OptionalJacobian<2, 6> Dpose = {},
OptionalJacobian<2, 3> Dpoint = {}) const;
/// @}
/// 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
*/
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
Point3 backproject(const StereoPoint2& z) const;
@ -153,8 +153,8 @@ public:
* @param H2 derivative with respect to point
*/
Point3 backproject2(const StereoPoint2& z,
OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
OptionalJacobian<3, 6> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
/// @}
/// @name Deprecated
@ -167,8 +167,8 @@ public:
* @param H3 IGNORED (for calibration)
*/
StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1,
OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
boost::none) const;
OptionalJacobian<3, 3> H2 = {}, OptionalJacobian<3, 0> H3 =
{}) const;
/// for Nonlinear Triangulation
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) {
Matrix computed;
K.uncalibrate(p, computed, boost::none);
K.uncalibrate(p, computed, {});
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical, computed, 1e-5));
Matrix separate = K.D2d_calibration(p);
@ -67,7 +67,7 @@ TEST(Cal3DS2, Duncalibrate1) {
/* ************************************************************************* */
TEST(Cal3DS2, Duncalibrate2) {
Matrix computed;
K.uncalibrate(p, boost::none, computed);
K.uncalibrate(p, {}, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical, computed, 1e-5));
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) {
Matrix computed;
K.uncalibrate(p, computed, boost::none);
K.uncalibrate(p, computed, {});
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical, computed, 1e-6));
}
@ -76,7 +76,7 @@ TEST(Cal3Unified, Duncalibrate1) {
/* ************************************************************************* */
TEST(Cal3Unified, Duncalibrate2) {
Matrix computed;
K.uncalibrate(p, boost::none, computed);
K.uncalibrate(p, {}, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
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) {
Matrix25 computed;
K.uncalibrate(p, computed, boost::none);
K.uncalibrate(p, computed, {});
Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
CHECK(assert_equal(numerical, computed, 1e-8));
}
@ -72,7 +72,7 @@ TEST(Cal3_S2, Duncalibrate1) {
/* ************************************************************************* */
TEST(Cal3_S2, Duncalibrate2) {
Matrix computed;
K.uncalibrate(p, boost::none, computed);
K.uncalibrate(p, {}, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical, computed, 1e-9));
}
@ -84,7 +84,7 @@ Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {
/* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate1) {
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);
CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8));
@ -93,7 +93,7 @@ TEST(Cal3_S2, Dcalibrate1) {
/* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate2) {
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);
CHECK(assert_equal(expected, p_xy, 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
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);
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),
1e-8));
Matrix expectedH1 = numericalDerivative11<EssentialMatrix, Rot3>(
std::bind(EssentialMatrix::FromRotationAndDirection,
std::placeholders::_1, trueDirection, boost::none, boost::none),
trueRotation);
EXPECT(assert_equal(expectedH1, actualH1, 1e-7));
{
std::function<EssentialMatrix(const Rot3&)> fn = [](const Rot3& R) {
return EssentialMatrix::FromRotationAndDirection(R, trueDirection);
};
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::placeholders::_1, boost::none, boost::none),
trueDirection);
EXPECT(assert_equal(expectedH2, actualH2, 1e-7));
{
std::function<EssentialMatrix(const Unit3&)> fn = [](const Unit3& t) {
return EssentialMatrix::FromRotationAndDirection(trueRotation, t);
};
Matrix expectedH2 = numericalDerivative11<EssentialMatrix, Unit3>(fn, trueDirection);
EXPECT(assert_equal(expectedH2, actualH2, 1e-7));
}
}
//*************************************************************************
@ -174,8 +178,10 @@ TEST (EssentialMatrix, FromPose3_a) {
Matrix actualH;
Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras
EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
std::function<EssentialMatrix(const Pose3&)> fn = [](const Pose3& pose) {
return EssentialMatrix::FromPose3(pose);
};
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(fn, pose);
EXPECT(assert_equal(expectedH, actualH, 1e-7));
}
@ -187,8 +193,10 @@ TEST (EssentialMatrix, FromPose3_b) {
EssentialMatrix E(c1Rc2, Unit3(c1Tc2));
Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
std::function<EssentialMatrix(const Pose3&)> fn = [](const Pose3& pose) {
return EssentialMatrix::FromPose3(pose);
};
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(fn, pose);
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 gtsam;
using boost::none;
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
@ -57,17 +56,17 @@ TEST(OrientedPlane3, transform) {
gtsam::Point3(2.0, 3.0, 4.0));
OrientedPlane3 plane(-1, 0, 0, 5);
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));
// Test the jacobians of transform
Matrix actualH1, expectedH1, actualH2, expectedH2;
expectedH1 = numericalDerivative21(transform_, plane, pose);
plane.transform(pose, actualH1, none);
plane.transform(pose, actualH1, {});
EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
expectedH2 = numericalDerivative22(transform_, plane, pose);
plane.transform(pose, none, actualH2);
plane.transform(pose, {}, actualH2);
EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
}
@ -135,8 +134,9 @@ TEST(OrientedPlane3, errorVector) {
EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2]));
std::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
std::bind(&OrientedPlane3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
[](const OrientedPlane3& p1, const OrientedPlane3& p2) {
return p1.errorVector(p2);
};
expectedH1 = numericalDerivative21(f, plane1, plane2);
expectedH2 = numericalDerivative22(f, plane1, plane2);
EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
@ -147,8 +147,10 @@ TEST(OrientedPlane3, errorVector) {
TEST(OrientedPlane3, jacobian_retract) {
OrientedPlane3 plane(-1, 0.1, 0.2, 5);
Matrix33 H_actual;
std::function<OrientedPlane3(const Vector3&)> f = std::bind(
&OrientedPlane3::retract, plane, std::placeholders::_1, boost::none);
std::function<OrientedPlane3(const Vector3&)> f = [&plane](const Vector3& v) {
return plane.retract(v);
};
{
Vector3 v(-0.1, 0.2, 0.3);
plane.retract(v, H_actual);
@ -168,8 +170,9 @@ TEST(OrientedPlane3, jacobian_normal) {
Matrix23 H_actual, H_expected;
OrientedPlane3 plane(-1, 0.1, 0.2, 5);
std::function<Unit3(const OrientedPlane3&)> f = std::bind(
&OrientedPlane3::normal, std::placeholders::_1, boost::none);
std::function<Unit3(const OrientedPlane3&)> f = [](const OrientedPlane3& p) {
return p.normal();
};
H_expected = numericalDerivative11(f, plane);
plane.normal(H_actual);
@ -181,8 +184,9 @@ TEST(OrientedPlane3, jacobian_distance) {
Matrix13 H_actual, H_expected;
OrientedPlane3 plane(-1, 0.1, 0.2, 5);
std::function<double(const OrientedPlane3&)> f = std::bind(
&OrientedPlane3::distance, std::placeholders::_1, boost::none);
std::function<double(const OrientedPlane3&)> f = [](const OrientedPlane3& p) {
return p.distance();
};
H_expected = numericalDerivative11(f, plane);
plane.distance(H_actual);

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

@ -67,7 +67,7 @@ TEST(PinholeCamera, Create) {
// Check derivative
std::function<Camera(Pose3, Cal3_S2)> f = //
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);
EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
@ -82,7 +82,7 @@ TEST(PinholeCamera, Pose) {
// Check derivative
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);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

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

@ -66,7 +66,7 @@ TEST(PinholeCamera, Pose) {
// Check derivative
std::function<Pose3(Camera)> f = //
std::bind(&Camera::getPose,_1,boost::none);
std::bind(&Camera::getPose,_1,{});
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
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) {
Matrix aH1, aH2;
std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
std::function<Point3(const Point3&, const Point3&)> f =
[](const Point3& p, const Point3& q) { return gtsam::cross(p, q); };
const Point3 omega(0, 1, 0), theta(4, 6, 8);
cross(omega, theta, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
@ -142,8 +141,7 @@ TEST( Point3, cross2) {
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2, //
boost::none, boost::none);
[](const Point3& p, const Point3& q) { return gtsam::cross(p, q); };
{
gtsam::cross(p, q, H1, H2);
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 expected(point / sqrt(14.0));
EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<Point3, Point3>(
std::bind(gtsam::normalize, std::placeholders::_1, boost::none), point);
std::function<Point3(const Point3&)> fn = [](const Point3& p) { return normalize(p); };
Matrix expectedH = numericalDerivative11<Point3, Point3>(fn, point);
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);
Pose2::Expmap(w,actualH);
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));
}
@ -238,7 +238,7 @@ TEST( Pose2, ExpmapDerivative2) {
Vector3 w0(0.1, 0.27, 0.0); // alpha = 0
Pose2::Expmap(w0,actualH);
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));
}
@ -249,7 +249,7 @@ TEST( Pose2, LogmapDerivative1) {
Pose2 p = Pose2::Expmap(w);
EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5));
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));
}
@ -260,7 +260,7 @@ TEST( Pose2, LogmapDerivative2) {
Pose2 p = Pose2::Expmap(w0);
EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5));
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));
}

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

@ -360,7 +360,7 @@ TEST( Rot3, rotate_derivatives)
{
Matrix actualDrotate1a, actualDrotate1b, 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 numerical2 = numericalDerivative21(testing::rotate<Rot3,Point3>, R.inverse(), 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) {
using boost::none;
using {};
// We set up expected values with quaternions
typedef Quaternion Q;

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

@ -210,7 +210,7 @@ TEST(SO3, ExpmapDerivative) {
TEST(SO3, ExpmapDerivative2) {
const Vector3 theta(0.1, 0, 0.1);
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(Matrix3(Jexpected.transpose()),
@ -221,7 +221,7 @@ TEST(SO3, ExpmapDerivative2) {
TEST(SO3, ExpmapDerivative3) {
const Vector3 theta(10, 20, 30);
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(Matrix3(Jexpected.transpose()),
@ -276,7 +276,7 @@ TEST(SO3, ExpmapDerivative5) {
TEST(SO3, ExpmapDerivative6) {
const Vector3 thetahat(0.1, 0, 0.1);
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;
SO3::Expmap(thetahat, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
@ -287,7 +287,7 @@ TEST(SO3, LogmapDerivative) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
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);
EXPECT(assert_equal(Jexpected, Jactual));
}
@ -297,7 +297,7 @@ TEST(SO3, JacobianLogmap) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
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;
SO3::Logmap(R, 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)
{
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_point = numericalDerivative22(project2, pose1, point1);
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)
{
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));
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));
}

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

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

5
gtsam/inference/EliminateableFactorGraph.h
View File

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

3
gtsam/linear/GaussianFactorGraph.h
View File

@ -21,6 +21,7 @@
#pragma once
#include <cstddef>
#include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/FactorGraph.h>
#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
/** @deprecated */
VectorValues GTSAM_DEPRECATED
optimize(boost::none_t, const Eliminate& function =
optimize(std::nullptr_t, const Eliminate& function =
EliminationTraitsType::DefaultEliminate) const {
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
/// 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
// (compare numerical derivatives wrt analytic ones)

2
gtsam/navigation/AttitudeFactor.h
View File

@ -54,7 +54,7 @@ public:
/** vector of errors */
Vector attitudeError(const Rot3& p,
OptionalJacobian<2,3> H = boost::none) const;
OptionalJacobian<2,3> H = {}) const;
const Unit3& nZ() const {
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 */
Vector3 correctAccelerometer(const Vector3& measurement,
OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
OptionalJacobian<3, 6> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const {
if (H1) (*H1) << -I_3x3, Z_3x3;
if (H2) (*H2) << I_3x3;
return measurement - biasAcc_;
@ -81,8 +81,8 @@ public:
/** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
Vector3 correctGyroscope(const Vector3& measurement,
OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
OptionalJacobian<3, 6> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const {
if (H1) (*H1) << Z_3x3, -I_3x3;
if (H2) (*H2) << I_3x3;
return measurement - biasGyro_;

2
gtsam/navigation/MagFactor.h
View File

@ -65,7 +65,7 @@ public:
static Point3 unrotate(const Rot2& R, const Point3& p,
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) {
// assign to temporary first to avoid error in Win-Debug mode
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_;
Matrix3 D_correctedRij_bias;
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);
if (H)
D_correctedRij_bias *= delRdelBiasOmega_;

2
gtsam/navigation/ManifoldPreintegration.h
View File

@ -103,7 +103,7 @@ public:
/// summarizing the preintegrated IMU measurements so far
/// NOTE(frank): implementation is different in two versions
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 */
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,
const Vector3& w_body, const double dt,
const Vector9& preintegrated,
OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none);
OptionalJacobian<9, 9> A = {},
OptionalJacobian<9, 3> B = {},
OptionalJacobian<9, 3> C = {});
/// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame
@ -106,13 +106,13 @@ public:
/// summarizing the preintegrated IMU measurements so far
/// NOTE(frank): implementation is different in two versions
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
static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
double deltaT12,
OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none);
OptionalJacobian<9, 9> H1 = {},
OptionalJacobian<9, 9> H2 = {});
/// Merge in a different set of measurements and update bias derivatives accordingly
/// 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);
std::function<Vector3(const Bias&, const Vector3&)> f =
std::bind(&Bias::correctAccelerometer, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
std::placeholders::_2, nullptr, nullptr);
bias1.correctAccelerometer(measurement, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
@ -137,7 +137,7 @@ TEST(ImuBias, Correct2) {
const Vector3 measurement(1, 2, 3);
std::function<Vector3(const Bias&, const Vector3&)> f =
std::bind(&Bias::correctGyroscope, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
std::placeholders::_2, nullptr, nullptr);
bias1.correctGyroscope(measurement, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
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::bind(&PreintegrationBase::computeError, actual,
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(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
@ -205,7 +205,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
pim.biasCorrectedDelta(kZeroBias, actualH);
Matrix expectedH = numericalDerivative11<Vector9, Bias>(
std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
std::placeholders::_1, boost::none), kZeroBias);
std::placeholders::_1, nullptr), kZeroBias);
EXPECT(assert_equal(expectedH, actualH));
Matrix9 aH1;
@ -213,11 +213,11 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2);
Matrix eH1 = numericalDerivative11<NavState, NavState>(
std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1,
kZeroBias, boost::none, boost::none), state1);
kZeroBias, nullptr, nullptr), state1);
EXPECT(assert_equal(eH1, aH1));
Matrix eH2 = numericalDerivative11<NavState, Bias>(
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));
}
@ -334,7 +334,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
pim.biasCorrectedDelta(bias, actualH);
Matrix expectedH = numericalDerivative11<Vector9, Bias>(
std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
std::placeholders::_1, boost::none), bias);
std::placeholders::_1, nullptr), bias);
EXPECT(assert_equal(expectedH, actualH));
// Create factor
@ -520,7 +520,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
// Check updatedDeltaXij derivatives
Matrix3 D_correctedAcc_measuredOmega = Z_3x3;
pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega,
boost::none, D_correctedAcc_measuredOmega, boost::none);
nullptr, D_correctedAcc_measuredOmega, nullptr);
Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(
std::bind(correctedAcc, pim, measuredAcc, std::placeholders::_1),
measuredOmega, 1e-6);
@ -531,19 +531,19 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
// TODO(frank): revive derivative tests
// Matrix93 G1, G2;
// Vector9 preint =
// pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2);
// pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, {}, G1, G2);
//
// Matrix93 expectedG1 = numericalDerivative21<NavState, Vector3, Vector3>(
// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
// std::placeholders::_1, std::placeholders::_2,
// dt, boost::none, boost::none, boost::none), measuredAcc,
// dt, {}, {}, {}), measuredAcc,
// measuredOmega, 1e-6);
// EXPECT(assert_equal(expectedG1, G1, 1e-5));
//
// Matrix93 expectedG2 = numericalDerivative22<NavState, Vector3, Vector3>(
// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
// std::placeholders::_1, std::placeholders::_2,
// dt, boost::none, boost::none, boost::none), measuredAcc,
// dt, {}, {}, {}), measuredAcc,
// measuredOmega, 1e-6);
// EXPECT(assert_equal(expectedG2, G2, 1e-5));
@ -658,7 +658,7 @@ TEST(ImuFactor, PredictArbitrary) {
p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise
PreintegratedImuMeasurements pim(p, biasHat);
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));
double dt = 0.001;

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

@ -101,8 +101,8 @@ TEST(ManifoldPreintegration, computeError) {
const imuBias::ConstantBias&)>
f = std::bind(&ManifoldPreintegration::computeError, pim,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none);
std::placeholders::_3, nullptr, nullptr,
nullptr);
// 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(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) {
std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
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;
EXPECT(
assert_equal(kState1,
@ -61,7 +61,7 @@ TEST(NavState, Constructor) {
TEST(NavState, Constructor2) {
std::function<NavState(const Pose3&, const Vector3&)> construct =
std::bind(&NavState::FromPoseVelocity, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
std::placeholders::_2, nullptr, nullptr);
Matrix aH1, aH2;
EXPECT(
assert_equal(kState1,
@ -76,7 +76,7 @@ TEST( NavState, Attitude) {
Rot3 actual = kState1.attitude(aH);
EXPECT(assert_equal(actual, kAttitude));
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));
}
@ -86,7 +86,7 @@ TEST( NavState, Position) {
Point3 actual = kState1.position(aH);
EXPECT(assert_equal(actual, kPosition));
eH = numericalDerivative11<Point3, NavState>(
std::bind(&NavState::position, std::placeholders::_1, boost::none),
std::bind(&NavState::position, std::placeholders::_1, nullptr),
kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
@ -97,7 +97,7 @@ TEST( NavState, Velocity) {
Velocity3 actual = kState1.velocity(aH);
EXPECT(assert_equal(actual, kVelocity));
eH = numericalDerivative11<Velocity3, NavState>(
std::bind(&NavState::velocity, std::placeholders::_1, boost::none),
std::bind(&NavState::velocity, std::placeholders::_1, nullptr),
kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
@ -108,7 +108,7 @@ TEST( NavState, BodyVelocity) {
Velocity3 actual = kState1.bodyVelocity(aH);
EXPECT(assert_equal<Velocity3>(actual, kAttitude.unrotate(kVelocity)));
eH = numericalDerivative11<Velocity3, NavState>(
std::bind(&NavState::bodyVelocity, std::placeholders::_1, boost::none),
std::bind(&NavState::bodyVelocity, std::placeholders::_1, nullptr),
kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
@ -142,7 +142,7 @@ TEST( NavState, Manifold ) {
kState1.retract(xi, aH1, aH2);
std::function<NavState(const NavState&, const Vector9&)> retract =
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(numericalDerivative22(retract, kState1, xi), aH2));
@ -155,7 +155,7 @@ TEST( NavState, Manifold ) {
// Check localCoordinates derivatives
std::function<Vector9(const NavState&, const NavState&)> local =
std::bind(&NavState::localCoordinates, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
std::placeholders::_2, nullptr, nullptr);
// from state1 to state2
kState1.localCoordinates(state2, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1));
@ -174,7 +174,7 @@ TEST( NavState, Manifold ) {
static const double dt = 2.0;
std::function<Vector9(const NavState&, const bool&)> coriolis =
std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis,
std::placeholders::_2, boost::none);
std::placeholders::_2, nullptr);
TEST(NavState, Coriolis) {
Matrix9 aH;
@ -252,7 +252,7 @@ TEST(NavState, CorrectPIM) {
std::function<Vector9(const NavState&, const Vector9&)> correctPIM =
std::bind(&NavState::correctPIM, std::placeholders::_1,
std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false,
boost::none, boost::none);
nullptr, nullptr);
kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1));
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&)>
f = std::bind(&TangentPreintegration::computeError, pim,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none);
std::placeholders::_3, nullptr, nullptr,
nullptr);
// 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(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));

4
gtsam/nonlinear/AdaptAutoDiff.h
View File

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

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

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

2
gtsam/nonlinear/FunctorizedFactor.h
View File

@ -42,7 +42,7 @@ namespace gtsam {
* public:
* MultiplyFunctor(double m) : m_(m) {}
* 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());
* return m_ * X;

5
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -21,6 +21,7 @@
#pragma once
#include <cstddef>
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/GraphvizFormatting.h>
@ -268,11 +269,11 @@ namespace gtsam {
/// @{
/** @deprecated */
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);}
/** @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
{return updateCholesky(values, dampen);}

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

@ -255,7 +255,7 @@ public:
/// Return value
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
@ -365,9 +365,9 @@ public:
/// Return value
T value(const Values& values) const override {
using boost::none;
using std::nullopt;
return function_(expression1_->value(values), expression2_->value(values),
none, none);
{}, {});
}
/// Return keys that play in this expression
@ -473,9 +473,9 @@ public:
/// Return value
T value(const Values& values) const override {
using boost::none;
using std::nullopt;
return function_(expression1_->value(values), expression2_->value(values),
expression3_->value(values), none, none, none);
expression3_->value(values), {}, {}, {});
}
/// 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;
const Vector3& vector() const { return *this; }
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);
}
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,
const Vector3& vec_w,
OptionalJacobian<3, 6> Hpose = boost::none,
OptionalJacobian<3, 3> Hvel = boost::none) {
OptionalJacobian<3, 6> Hpose = {},
OptionalJacobian<3, 3> Hvel = {}) {
Matrix36 Hrot__pose;
Rot3 w_R_b = w_t_b.rotation(Hrot__pose);
Matrix33 Hvel__rot;
@ -51,7 +51,7 @@ class ScaledVelocityFunctor {
// and velocity scale factor. Also computes the corresponding jacobian
// (w.r.t. the velocity scale).
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
// 0, so we need to add 1 to it before scaling the 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) {}
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());
return m_ * X;
}
@ -57,8 +57,8 @@ class MultiplyFunctor {
class ProjectionFunctor {
public:
Vector operator()(const Matrix &A, const Vector &x,
OptionalJacobian<-1, -1> H1 = boost::none,
OptionalJacobian<-1, -1> H2 = boost::none) const {
OptionalJacobian<-1, -1> H1 = {},
OptionalJacobian<-1, -1> H2 = {}) const {
if (H1) {
H1->resize(x.size(), A.size());
*H1 << I_3x3, I_3x3, I_3x3;
@ -178,7 +178,7 @@ TEST(FunctorizedFactor, Lambda) {
Matrix measurement = multiplier * Matrix::Identity(3, 3);
auto lambda = [multiplier](const Matrix &X,
OptionalJacobian<-1, -1> H = boost::none) {
OptionalJacobian<-1, -1> H = {}) {
if (H)
*H = multiplier *
Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
@ -251,8 +251,8 @@ TEST(FunctorizedFactor, Lambda2) {
Matrix measurement = A * x;
auto lambda = [](const Matrix &A, const Vector &x,
OptionalJacobian<-1, -1> H1 = boost::none,
OptionalJacobian<-1, -1> H2 = boost::none) {
OptionalJacobian<-1, -1> H1 = {},
OptionalJacobian<-1, -1> H2 = {}) {
if (H1) {
H1->resize(x.size(), A.size());
*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; }
size_t dim() const { return 0; }
TestValue retract(const Vector&,
OptionalJacobian<dimension,dimension> H1=boost::none,
OptionalJacobian<dimension,dimension> H2=boost::none) const {
OptionalJacobian<dimension,dimension> H1={},
OptionalJacobian<dimension,dimension> H2={}) const {
return TestValue();
}
Vector localCoordinates(const TestValue&,
OptionalJacobian<dimension,dimension> H1=boost::none,
OptionalJacobian<dimension,dimension> H2=boost::none) const {
OptionalJacobian<dimension,dimension> H1={},
OptionalJacobian<dimension,dimension> H2={}) const {
return Vector();
}
};

6
gtsam/slam/ProjectionFactor.h
View File

@ -142,16 +142,16 @@ namespace gtsam {
if(H1) {
gtsam::Matrix H0;
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;
return reprojectionError;
} else {
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 {
PinholeCamera<CALIBRATION> camera(pose, *K_);
return camera.project(point, H1, H2, boost::none) - measured_;
return camera.project(point, H1, H2, {}) - measured_;
}
} catch( CheiralityException& e) {
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
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_)
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
Expression<StereoPoint2>::UnaryFunction<Point3>::type f =
std::bind(&StereoCamera::project2, camera2, std::placeholders::_1,
boost::none, std::placeholders::_2);
nullptr, std::placeholders::_2);
Expression<Point3> point_(pointKey);
Expression<StereoPoint2> project2_(f, point_);

8
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -88,8 +88,8 @@ public:
/** range between translations */
double range(const PoseRTV& other,
OptionalJacobian<1,9> H1=boost::none,
OptionalJacobian<1,9> H2=boost::none) const;
OptionalJacobian<1,9> H1={},
OptionalJacobian<1,9> H2={}) const;
/// @}
/// @name IMU-specific
@ -138,8 +138,8 @@ public:
* Note: the transform jacobian convention is flipped
*/
PoseRTV transformed_from(const Pose3& trans,
ChartJacobian Dglobal = boost::none,
OptionalJacobian<9, 6> Dtrans = boost::none) const;
ChartJacobian Dglobal = {},
OptionalJacobian<9, 6> Dtrans = {}) const;
/// @}
/// @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);
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 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_; }
// 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();
if (H) *H = JacobianZ;
return location_.z();
@ -101,8 +101,8 @@ class TimeOfArrival {
/// Calculate time of arrival, with derivatives
double operator()(const Event& event, const Point3& sensor, //
OptionalJacobian<1, 4> H1 = boost::none, //
OptionalJacobian<1, 3> H2 = boost::none) const {
OptionalJacobian<1, 4> H1 = {}, //
OptionalJacobian<1, 3> H2 = {}) const {
Matrix13 D1, D2;
double distance = gtsam::distance3(event.location(), sensor, D1, D2);
if (H1)

2
gtsam_unstable/geometry/InvDepthCamera3.h
View File

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

10
gtsam_unstable/geometry/Pose3Upright.h
View File

@ -98,12 +98,12 @@ public:
static Pose3Upright Identity() { return Pose3Upright(); }
/// 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)
Pose3Upright compose(const Pose3Upright& p2,
OptionalJacobian<4,4> H1=boost::none,
OptionalJacobian<4,4> H2=boost::none) const;
OptionalJacobian<4,4> H1={},
OptionalJacobian<4,4> H2={}) const;
/// compose syntactic sugar
inline Pose3Upright operator*(const Pose3Upright& T) const { return compose(T); }
@ -113,8 +113,8 @@ public:
* as well as optionally the derivatives
*/
Pose3Upright between(const Pose3Upright& p2,
OptionalJacobian<4,4> H1=boost::none,
OptionalJacobian<4,4> H2=boost::none) const;
OptionalJacobian<4,4> H1={},
OptionalJacobian<4,4> H2={}) const;
/// @}
/// @name Lie Group

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

@ -65,11 +65,11 @@ TEST(Event, Derivatives) {
Matrix13 actualH2;
kToa(exampleEvent, microphoneAt0, actualH1, actualH2);
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);
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
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);
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;
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::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 )
{
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;
simulated3D::prior(x,computed);
EXPECT(assert_equal(numerical,computed,1e-9));
@ -60,12 +60,12 @@ TEST( simulated3D, DOdo )
simulated3D::odo(x1, x2, H1, H2);
Matrix A1 = numericalDerivative21<Point3, Point3, Point3>(
std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none),
nullptr, nullptr),
x1, x2);
EXPECT(assert_equal(A1, H1, 1e-9));
Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(
std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none),
nullptr, nullptr),
x1, x2);
EXPECT(assert_equal(A2, H2, 1e-9));
}