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

Merge pull request #1403 from kartikarcot/verdant/replace-boost-optional-vals

release/4.3a0
Frank Dellaert 2023-01-22 16:56:41 -08:00 committed by GitHub
parent 0811cd9e6f d251803334
commit aa4657c7d8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
228 changed files with 1408 additions and 1098 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/ISAM2Example_SmartFactor.cpp
View File

@ -105,7 +105,7 @@ int main(int argc, char* argv[]) {
Values currentEstimate = isam.calculateEstimate();
currentEstimate.print("Current estimate: ");
boost::optional<Point3> pointEstimate = smartFactor->point(currentEstimate);
auto pointEstimate = smartFactor->point(currentEstimate);
if (pointEstimate) {
cout << *pointEstimate << endl;
} else {

6
examples/SFMExample_SmartFactor.cpp
View File

@ -114,10 +114,10 @@ int main(int argc, char* argv[]) {
// The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first
SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
// The output of point() is in boost::optional<Point3>, as sometimes
// The output of point() is in std::optional<Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy.
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return nullptr
auto point = smart->point(result);
if (point) // ignore if std::optional return nullptr
landmark_result.insert(j, *point);
}
}

4
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -110,8 +110,8 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
auto smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return nullptr
std::optional<Point3> point = smart->point(result);
if (point) // ignore if std::optional return nullptr
landmark_result.insert(j, *point);
}
}

13
examples/TriangulationLOSTExample.cpp
View File

@ -29,6 +29,7 @@
#include <chrono>
#include <iostream>
#include <random>
#include <optional>
using namespace std;
using namespace gtsam;
@ -131,23 +132,23 @@ int main(int argc, char* argv[]) {
AddNoiseToMeasurements(measurements, measurementSigma);
auto lostStart = std::chrono::high_resolution_clock::now();
boost::optional<Point3> estimateLOST = triangulatePoint3<Cal3_S2>(
auto estimateLOST = triangulatePoint3<Cal3_S2>(
cameras, noisyMeasurements, rank_tol, false, measurementNoise, true);
durationLOST += std::chrono::high_resolution_clock::now() - lostStart;
auto dltStart = std::chrono::high_resolution_clock::now();
boost::optional<Point3> estimateDLT = triangulatePoint3<Cal3_S2>(
auto estimateDLT = triangulatePoint3<Cal3_S2>(
cameras, noisyMeasurements, rank_tol, false, measurementNoise, false);
durationDLT += std::chrono::high_resolution_clock::now() - dltStart;
auto dltOptStart = std::chrono::high_resolution_clock::now();
boost::optional<Point3> estimateDLTOpt = triangulatePoint3<Cal3_S2>(
auto estimateDLTOpt = triangulatePoint3<Cal3_S2>(
cameras, noisyMeasurements, rank_tol, true, measurementNoise, false);
durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart;
errorsLOST.row(i) = *estimateLOST - landmark;
errorsDLT.row(i) = *estimateDLT - landmark;
errorsDLTOpt.row(i) = *estimateDLTOpt - landmark;
errorsLOST.row(i) = estimateLOST - landmark;
errorsDLT.row(i) = estimateDLT - landmark;
errorsDLTOpt.row(i) = estimateDLTOpt - landmark;
}
PrintCovarianceStats(errorsLOST, "LOST");
PrintCovarianceStats(errorsDLT, "DLT");

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,20 @@
*/
#pragma once
#include <cstddef>
#include <functional>
#include <gtsam/config.h> // Configuration from CMake
#include <Eigen/Dense>
#include <optional>
#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 +63,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) {
@ -116,26 +120,21 @@ public:
"Expected: ") +
"(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")");
}
}
}
#ifndef OPTIONALJACOBIAN_NOBOOST
/// Constructor with boost::none just makes empty
OptionalJacobian(boost::none_t /*none*/) :
/// Constructor with std::nullopt just makes empty
OptionalJacobian(std::nullopt_t /*none*/) :
map_(nullptr) {
}
/// Constructor compatible with old-style derivatives
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
OptionalJacobian(const std::optional<std::reference_wrapper<Eigen::MatrixXd>> optional) :
map_(nullptr) {
if (optional) {
optional->resize(Rows, Cols);
usurp(optional->data());
optional->get().resize(Rows, Cols);
usurp(optional->get().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 +199,7 @@ private:
public:
/// Default constructor acts like boost::none
/// Default constructor
OptionalJacobian() :
pointer_(nullptr) {
}
@ -211,21 +210,17 @@ 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*/) :
/// Constructor with std::nullopt just makes empty
OptionalJacobian(std::nullopt_t /*none*/) :
pointer_(nullptr) {
}
/// Constructor compatible with old-style derivatives
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
/// Constructor for optional matrix reference
OptionalJacobian(const std::optional<std::reference_wrapper<Eigen::MatrixXd>> optional) :
pointer_(nullptr) {
if (optional) pointer_ = &(*optional);
if (optional) pointer_ = &((*optional).get());
}
#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 {

26
gtsam/base/TestableAssertions.h
View File

@ -20,7 +20,8 @@
#include <gtsam/base/Testable.h>
#include <gtsam/global_includes.h>
#include <boost/optional.hpp>
#include <functional>
#include <optional>
#include <map>
#include <iostream>
#include <sstream>
@ -40,21 +41,21 @@ inline bool assert_equal(const Key& expected, const Key& actual, double tol = 0.
}
/**
* Comparisons for boost.optional objects that checks whether objects exist
* Comparisons for std.optional objects that checks whether objects exist
* before comparing their values. First version allows for both to be
* boost::none, but the second, with expected given rather than optional
* std::nullopt, but the second, with expected given rather than optional
*
* Concept requirement: V is testable
*/
template<class V>
bool assert_equal(const boost::optional<V>& expected,
const boost::optional<V>& actual, double tol = 1e-9) {
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)
@ -63,21 +64,22 @@ bool assert_equal(const boost::optional<V>& expected,
}
template<class V>
bool assert_equal(const V& expected, const boost::optional<V>& actual, double tol = 1e-9) {
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);
}
template<class V>
bool assert_equal(const V& expected, const boost::optional<const V&>& actual, double tol = 1e-9) {
bool assert_equal(const V& expected,
const std::optional<std::reference_wrapper<const V>>& actual, double tol = 1e-9) {
if (!actual) {
std::cout << "actual is boost::none" << std::endl;
std::cout << "actual is std::nullopt" << std::endl;
return false;
}
return assert_equal(expected, *actual, tol);
return assert_equal(expected, *actual.get(), tol);
}
/**

4
gtsam/base/ThreadsafeException.h
View File

@ -21,11 +21,11 @@
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <boost/optional/optional.hpp>
#include <gtsam/dllexport.h>
#include <string>
#include <typeinfo>
#include <exception>
#include <optional>
#ifdef GTSAM_USE_TBB
#include <tbb/tbb_allocator.h>
@ -53,7 +53,7 @@ protected:
protected:
bool dynamic_; ///< Whether this object was moved
mutable boost::optional<String> description_; ///< Optional description
mutable std::optional<String> description_; ///< Optional description
/// Default constructor is protected - may only be created from derived classes
ThreadsafeException() :

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>

1
gtsam/base/Vector.cpp
View File

@ -18,7 +18,6 @@
*/
#include <gtsam/base/Vector.h>
#include <boost/optional.hpp>
#include <stdexcept>
#include <cstdarg>
#include <limits>

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)

1
gtsam/base/serializationTestHelpers.h
View File

@ -24,6 +24,7 @@
#include <string>
#include <gtsam/base/serialization.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/serialization/serialization.hpp>
#include <boost/filesystem.hpp>

95
gtsam/base/std_optional_serialization.h Normal file
View File

@ -0,0 +1,95 @@
// Functionality to serialize std::optional<T> to boost::archive
// Following this:
// PR: https://github.com/boostorg/serialization/pull/148/files#
#pragma once
#include <optional>
#include <boost/config.hpp>
#include <boost/archive/detail/basic_iarchive.hpp>
#include <boost/move/utility_core.hpp>
#include <boost/serialization/item_version_type.hpp>
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/level.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/version.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/serialization/detail/stack_constructor.hpp>
#include <boost/serialization/detail/is_default_constructible.hpp>
/** A bunch of declarations to deal with gcc bug
* The compiler has a difficult time distinguisihing between:
*
* template<template <Archive, class U> class SPT> void load(Archive, SPT<U>&, const unsigned int) : <boost/serialization/shared_ptr.hpp>
*
* and
*
* template<T> void load(Archive, std::optional<T>&, const unsigned int) : <std_optional_serialization.h>
*
* The compiler will try to instantiate an object of the type of std::optional<boost::serialization::U> which is not valid since U is not a type and
* thus leading to a whole series of errros.
*
* This is a well known bug in gcc documented here: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=84075
* A minimal reproducible example here: https://godbolt.org/z/anj9YjnPY
*
* For std::optional the workaround is just provide the traits needed to make std::optional<boost::serialization::U> possible
* This is not an issue since there is no actual type boost::serialization::U and we are not falsely providing a specialization
* for std::optional<boost::serialization::U>
*/
#ifdef __GNUC__
#if __GNUC__ >= 7 && __cplusplus >= 201703L
namespace boost { namespace serialization { struct U; } }
namespace std { template<> struct is_trivially_default_constructible<boost::serialization::U> : std::false_type {}; }
namespace std { template<> struct is_trivially_copy_constructible<boost::serialization::U> : std::false_type {}; }
namespace std { template<> struct is_trivially_move_constructible<boost::serialization::U> : std::false_type {}; }
#endif
#endif
// function specializations must be defined in the appropriate
// namespace - boost::serialization
namespace boost {
namespace serialization {
template <class Archive, class T>
void save(Archive& ar, const std::optional<T>& t, const unsigned int /*version*/
) {
// It is an inherent limitation to the serialization of optional.hpp
// that the underlying type must be either a pointer or must have a
// default constructor.
BOOST_STATIC_ASSERT(boost::serialization::detail::is_default_constructible<T>::value || boost::is_pointer<T>::value);
const bool tflag = t.has_value();
ar << boost::serialization::make_nvp("initialized", tflag);
if (tflag) {
ar << boost::serialization::make_nvp("value", *t);
}
}
template <class Archive, class T>
void load(Archive& ar, std::optional<T>& t, const unsigned int /*version*/
) {
bool tflag;
ar >> boost::serialization::make_nvp("initialized", tflag);
if (!tflag) {
t.reset();
return;
}
if (!t.has_value()) {
// Need to be default constructible
t.emplace();
}
ar >> boost::serialization::make_nvp("value", *t);
}
template <class Archive, class T>
void serialize(Archive& ar, std::optional<T>& t, const unsigned int version) {
boost::serialization::split_free(ar, t, version);
}
// derive boost::xml_archive_impl for archiving std::optional<T> with xml
} // namespace serialization
} // namespace boost

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

@ -23,6 +23,8 @@
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <sstream>
#include <optional>
#include <functional>
using namespace std;
using namespace gtsam;
@ -1176,6 +1178,17 @@ TEST(Matrix, AbsoluteError) {
EXPECT(isEqual);
}
// A test to check if a matrix and an optional reference_wrapper to
// a matrix are equal.
TEST(Matrix, MatrixRef) {
Matrix A = Matrix::Random(3, 3);
Matrix B = Matrix::Random(3, 3);
EXPECT(assert_equal(A, A));
EXPECT(assert_equal(A, std::cref(A)));
EXPECT(!assert_equal(A, std::cref(B)));
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -20,6 +20,9 @@
#include <gtsam/base/OptionalJacobian.h>
#include <CppUnitLite/TestHarness.h>
#include <optional>
#include <functional>
using namespace std;
using namespace gtsam;
@ -32,7 +35,7 @@ using namespace gtsam;
TEST( OptionalJacobian, Constructors ) {
Matrix23 fixed;
Matrix dynamic;
boost::optional<Matrix&> optional(dynamic);
std::optional<std::reference_wrapper<Matrix>> optionalRef(std::ref(dynamic));
OptionalJacobian<2, 3> H;
EXPECT(!H);
@ -41,22 +44,23 @@ 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_CONSTRUCTOR(2, 3, std::nullopt, false);
TEST_CONSTRUCTOR(2, 3, optionalRef, 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);
TEST_CONSTRUCTOR(-1, -1, std::nullopt, false);
TEST_CONSTRUCTOR(-1, -1, optionalRef, 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 +120,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 +149,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));

1
gtsam/base/tests/testSerializationBase.cpp
View File

@ -28,6 +28,7 @@
#include <gtsam/base/FastVector.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/base/std_optional_serialization.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;

157
gtsam/base/tests/testStdOptionalSerialization.cpp Normal file
View File

@ -0,0 +1,157 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include "gtsam/base/serializationTestHelpers.h"
#include "gtsam/base/std_optional_serialization.h"
using namespace gtsam;
// A test to check that the serialization of std::optional works with boost
TEST(StdOptionalSerialization, SerializeIntOptional) {
// Create an optional
std::optional<int> opt(42);
// Serialize it
std::stringstream ss;
boost::archive::text_oarchive oa(ss);
oa << opt;
// Deserialize it
std::optional<int> opt2;
boost::archive::text_iarchive ia(ss);
ia >> opt2;
// Check that it worked
EXPECT(opt2.has_value());
EXPECT(*opt2 == 42);
}
// A test to check if we serialize an empty optional, it is deserialized as an
// empty optional
TEST(StdOptionalSerialization, SerializeEmptyOptional) {
// Create an optional
std::optional<int> opt;
// Serialize it
std::stringstream ss;
boost::archive::text_oarchive oa(ss);
oa << opt;
// Deserialize it
std::optional<int> opt2 = 43;
boost::archive::text_iarchive ia(ss);
ia >> opt2;
// Check that it worked
EXPECT(!opt2.has_value());
}
/* ************************************************************************* */
// Implement the equals trait for TestOptionalStruct
namespace gtsam {
// A struct which contains an optional
class TestOptionalStruct {
public:
std::optional<int> opt;
TestOptionalStruct() = default;
TestOptionalStruct(const int& opt)
: opt(opt) {}
// A copy constructor is needed for serialization
TestOptionalStruct(const TestOptionalStruct& other) = default;
bool operator==(const TestOptionalStruct& other) const {
// check the values are equal
return *opt == *other.opt;
}
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(opt);
}
};
template <>
struct traits<TestOptionalStruct> {
static bool Equals(const TestOptionalStruct& q1, const TestOptionalStruct& q2, double) {
// if both have values then compare the values
if (q1.opt.has_value() && q2.opt.has_value()) {
return *q1.opt == *q2.opt;
}
// otherwise check that both are empty
return q1.opt == q2.opt;
}
static void Print(const TestOptionalStruct& m, const std::string& s = "") {
/* std::cout << s << "opt: " << m.opt << std::endl; */
if (m.opt.has_value()) {
std::cout << s << "opt: " << *m.opt << std::endl;
} else {
std::cout << s << "opt: empty" << std::endl;
}
}
};
} // namespace gtsam
// Test for a struct with an initialized optional
TEST(StdOptionalSerialization, SerializTestOptionalStruct) {
TestOptionalStruct optStruct(10);
EXPECT(serializationTestHelpers::equalsObj(optStruct));
EXPECT(serializationTestHelpers::equalsXML(optStruct));
EXPECT(serializationTestHelpers::equalsBinary(optStruct));
}
// Test for a struct with an uninitialized optional
TEST(StdOptionalSerialization, SerializTestOptionalStructUninitialized) {
TestOptionalStruct optStruct;
EXPECT(serializationTestHelpers::equalsObj(optStruct));
EXPECT(serializationTestHelpers::equalsXML(optStruct));
EXPECT(serializationTestHelpers::equalsBinary(optStruct));
}
// Test for serialization of std::optional<TestOptionalStruct>
TEST(StdOptionalSerialization, SerializTestOptionalStructPointer) {
// Create an optional
std::optional<TestOptionalStruct> opt(TestOptionalStruct(42));
// Serialize it
std::stringstream ss;
boost::archive::text_oarchive oa(ss);
oa << opt;
// Deserialize it
std::optional<TestOptionalStruct> opt2;
boost::archive::text_iarchive ia(ss);
ia >> opt2;
// Check that it worked
EXPECT(opt2.has_value());
EXPECT(*opt2 == TestOptionalStruct(42));
}
// Test for serialization of std::optional<TestOptionalStruct*>
TEST(StdOptionalSerialization, SerializTestOptionalStructPointerPointer) {
// Create an optional
std::optional<TestOptionalStruct*> opt(new TestOptionalStruct(42));
// Serialize it
std::stringstream ss;
boost::archive::text_oarchive oa(ss);
oa << opt;
// Deserialize it
std::optional<TestOptionalStruct*> opt2;
boost::archive::text_iarchive ia(ss);
ia >> opt2;
// Check that it worked
EXPECT(opt2.has_value());
EXPECT(**opt2 == TestOptionalStruct(42));
}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}

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

6
gtsam/discrete/DecisionTree-inl.h
View File

@ -24,7 +24,6 @@
#include <algorithm>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/optional.hpp>
#include <cmath>
#include <fstream>
@ -34,6 +33,7 @@
#include <sstream>
#include <string>
#include <vector>
#include <optional>
namespace gtsam {
@ -563,7 +563,7 @@ namespace gtsam {
typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::compose(
Iterator begin, Iterator end, const L& label) const {
// find highest label among branches
boost::optional<L> highestLabel;
std::optional<L> highestLabel;
size_t nrChoices = 0;
for (Iterator it = begin; it != end; it++) {
if (it->root_->isLeaf())
@ -571,7 +571,7 @@ namespace gtsam {
boost::shared_ptr<const Choice> c =
boost::dynamic_pointer_cast<const Choice>(it->root_);
if (!highestLabel || c->label() > *highestLabel) {
highestLabel.reset(c->label());
highestLabel = c->label();
nrChoices = c->nrChoices();
}
}

10
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -70,8 +70,8 @@ template<> struct EliminationTraits<DiscreteFactorGraph>
/// The default ordering generation function
static Ordering DefaultOrderingFunc(
const FactorGraphType& graph,
boost::optional<const VariableIndex&> variableIndex) {
return Ordering::Colamd(*variableIndex);
std::optional<std::reference_wrapper<const VariableIndex>> variableIndex) {
return Ordering::Colamd((*variableIndex).get());
}
};
@ -155,7 +155,7 @@ class GTSAM_EXPORT DiscreteFactorGraph
* @return DiscreteBayesNet encoding posterior P(X|Z)
*/
DiscreteBayesNet sumProduct(
OptionalOrderingType orderingType = boost::none) const;
OptionalOrderingType orderingType = {}) const;
/**
* @brief Implement the sum-product algorithm
@ -172,7 +172,7 @@ class GTSAM_EXPORT DiscreteFactorGraph
* @return DiscreteLookupDAG DAG with lookup tables
*/
DiscreteLookupDAG maxProduct(
OptionalOrderingType orderingType = boost::none) const;
OptionalOrderingType orderingType = {}) const;
/**
* @brief Implement the max-product algorithm
@ -189,7 +189,7 @@ class GTSAM_EXPORT DiscreteFactorGraph
* @return DiscreteValues : MPE
*/
DiscreteValues optimize(
OptionalOrderingType orderingType = boost::none) const;
OptionalOrderingType orderingType = {}) const;
/**
* @brief Find the maximum probable explanation (MPE) by doing max-product.

6
gtsam/discrete/Signature.cpp
View File

@ -137,14 +137,14 @@ namespace gtsam {
}
Signature& Signature::operator=(const string& spec) {
spec_.reset(spec);
spec_ = spec;
Table table;
parser::It f = spec.begin(), l = spec.end();
bool success =
qi::phrase_parse(f, l, parser::grammar.table, qi::space, table);
if (success) {
for (Row& row : table) normalize(row);
table_.reset(table);
table_ = table;
}
return *this;
}
@ -153,7 +153,7 @@ namespace gtsam {
Table table = t;
for(Row& row: table)
normalize(row);
table_.reset(table);
table_ = table;
return *this;
}

8
gtsam/discrete/Signature.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <string>
#include <vector>
#include <boost/optional.hpp>
#include <optional>
#include <gtsam/discrete/DiscreteKey.h>
namespace gtsam {
@ -68,10 +68,10 @@ namespace gtsam {
DiscreteKeys parents_;
// the given CPT specification string
boost::optional<std::string> spec_;
std::optional<std::string> spec_;
// the CPT as parsed, if successful
boost::optional<Table> table_;
std::optional<Table> table_;
public:
/**
@ -124,7 +124,7 @@ namespace gtsam {
KeyVector indices() const;
// the CPT as parsed, if successful
const boost::optional<Table>& table() const { return table_; }
const std::optional<Table>& table() const { return table_; }
// the CPT as a vector of doubles, with key's values most rapidly changing
std::vector<double> cpt() const;

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

@ -108,8 +108,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
@ -119,8 +119,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);
}

1
gtsam/geometry/PinholeSet.h
View File

@ -19,7 +19,6 @@
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/triangulation.h>
#include <boost/optional.hpp>
namespace gtsam {

10
gtsam/geometry/Point2.cpp
View File

@ -52,7 +52,7 @@ double distance2(const Point2& p, const Point2& q, OptionalJacobian<1, 2> H1,
/* ************************************************************************* */
// Math inspired by http://paulbourke.net/geometry/circlesphere/
boost::optional<Point2> circleCircleIntersection(double R_d, double r_d,
std::optional<Point2> circleCircleIntersection(double R_d, double r_d,
double tol) {
double R2_d2 = R_d*R_d; // Yes, RD-D2 !
@ -61,17 +61,17 @@ boost::optional<Point2> circleCircleIntersection(double R_d, double r_d,
// h^2<0 is equivalent to (d > (R + r) || d < (R - r))
// Hence, there are only solutions if >=0
if (h2<-tol) return boost::none; // allow *slightly* negative
if (h2<-tol) return {}; // allow *slightly* negative
else if (h2<tol) return Point2(f,0.0); // one solution
else return Point2(f,std::sqrt(h2)); // two solutions
}
/* ************************************************************************* */
list<Point2> circleCircleIntersection(Point2 c1, Point2 c2,
boost::optional<Point2> fh) {
std::optional<Point2> fh) {
list<Point2> solutions;
// If fh==boost::none, there are no solutions, i.e., d > (R + r) || d < (R - r)
// If fh==std::nullopt, there are no solutions, i.e., d > (R + r) || d < (R - r)
if (fh) {
// vector between circle centers
Point2 c12 = c2-c1;
@ -107,7 +107,7 @@ list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
// Calculate f and h given normalized radii
double _d = 1.0/d, R_d = r1*_d, r_d=r2*_d;
boost::optional<Point2> fh = circleCircleIntersection(R_d,r_d);
std::optional<Point2> fh = circleCircleIntersection(R_d,r_d);
// Call version that takes fh
return circleCircleIntersection(c1, c2, fh);

21
gtsam/geometry/Point2.h
View File

@ -18,8 +18,11 @@
#pragma once
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/std_optional_serialization.h>
#include <boost/serialization/nvp.hpp>
#include <optional>
namespace gtsam {
/// As of GTSAM 4, in order to make GTSAM more lean,
@ -33,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;
@ -53,16 +56,16 @@ 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 boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
GTSAM_EXPORT std::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
/*
* @brief Circle-circle intersection, from the normalized radii solution.
@ -70,7 +73,7 @@ GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double
* @param c2 center of second circle
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
*/
GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, std::optional<Point2> fh);
/// Calculate the two means of a set of Point2 pairs
GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs);
@ -94,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);
}
};

6
gtsam/geometry/Pose2.cpp
View File

@ -327,10 +327,10 @@ double Pose2::range(const Pose2& pose,
* as they also satisfy ca = t + R*cb, hence t = ca - R*cb
*/
boost::optional<Pose2> Pose2::Align(const Point2Pairs &ab_pairs) {
std::optional<Pose2> Pose2::Align(const Point2Pairs &ab_pairs) {
const size_t n = ab_pairs.size();
if (n < 2) {
return boost::none; // we need at least 2 pairs
return {}; // we need at least 2 pairs
}
// calculate centroids
@ -359,7 +359,7 @@ boost::optional<Pose2> Pose2::Align(const Point2Pairs &ab_pairs) {
return Pose2(R, t);
}
boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
std::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
if (a.rows() != 2 || b.rows() != 2 || a.cols() != b.cols()) {
throw std::invalid_argument(
"Pose2:Align expects 2*N matrices of equal shape.");

35
gtsam/geometry/Pose2.h
View File

@ -25,6 +25,9 @@
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Lie.h>
#include <gtsam/dllexport.h>
#include <gtsam/base/std_optional_serialization.h>
#include <optional>
namespace gtsam {
@ -99,10 +102,10 @@ public:
* Note this allows for noise on the points but in that case the mapping
* will not be exact.
*/
static boost::optional<Pose2> Align(const Point2Pairs& abPointPairs);
static std::optional<Pose2> Align(const Point2Pairs& abPointPairs);
// Version of Pose2::Align that takes 2 matrices.
static boost::optional<Pose2> Align(const Matrix& a, const Matrix& b);
static std::optional<Pose2> Align(const Matrix& a, const Matrix& b);
/// @}
/// @name Testable
@ -134,10 +137,10 @@ public:
/// @{
///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);
GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {});
///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = {});
/**
* Calculate Adjoint map
@ -196,8 +199,8 @@ public:
// Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
struct ChartAtOrigin {
GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {});
};
using LieGroup<Pose2, 3>::inverse; // version with derivative
@ -208,8 +211,8 @@ public:
/** Return point coordinates in pose coordinate frame */
GTSAM_EXPORT Point2 transformTo(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const;
OptionalJacobian<2, 3> Dpose = {},
OptionalJacobian<2, 2> Dpoint = {}) const;
/**
* @brief transform many points in world coordinates and transform to Pose.
@ -220,8 +223,8 @@ public:
/** Return point coordinates in global frame */
GTSAM_EXPORT Point2 transformFrom(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const;
OptionalJacobian<2, 3> Dpose = {},
OptionalJacobian<2, 2> Dpoint = {}) const;
/**
* @brief transform many points in Pose coordinates and transform to world.
@ -269,7 +272,7 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$
*/
GTSAM_EXPORT Rot2 bearing(const Point2& point,
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const;
/**
* Calculate bearing to another pose
@ -277,7 +280,7 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$
*/
GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const;
/**
* Calculate range to a landmark
@ -285,8 +288,8 @@ public:
* @return range (double)
*/
GTSAM_EXPORT double range(const Point2& point,
OptionalJacobian<1, 3> H1=boost::none,
OptionalJacobian<1, 2> H2=boost::none) const;
OptionalJacobian<1, 3> H1={},
OptionalJacobian<1, 2> H2={}) const;
/**
* Calculate range to another pose
@ -294,8 +297,8 @@ public:
* @return range (double)
*/
GTSAM_EXPORT double range(const Pose2& point,
OptionalJacobian<1, 3> H1=boost::none,
OptionalJacobian<1, 3> H2=boost::none) const;
OptionalJacobian<1, 3> H1={},
OptionalJacobian<1, 3> H2={}) const;
/// @}
/// @name Advanced Interface

8
gtsam/geometry/Pose3.cpp
View File

@ -439,14 +439,14 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
Hpose->setZero();
return bearing(pose.translation(), Hself, Hpose.cols<3>(3));
}
return bearing(pose.translation(), Hself, boost::none);
return bearing(pose.translation(), Hself, {});
}
/* ************************************************************************* */
boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
std::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
const size_t n = abPointPairs.size();
if (n < 3) {
return boost::none; // we need at least three pairs
return {}; // we need at least three pairs
}
// calculate centroids
@ -466,7 +466,7 @@ boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
return Pose3(aRb, aTb);
}
boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
std::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) {
throw std::invalid_argument(
"Pose3:Align expects 3*N matrices of equal shape.");

68
gtsam/geometry/Pose3.h
View File

@ -75,18 +75,18 @@ public:
/// Named constructor with derivatives
static Pose3 Create(const Rot3& R, const Point3& t,
OptionalJacobian<6, 3> HR = boost::none,
OptionalJacobian<6, 3> Ht = boost::none);
OptionalJacobian<6, 3> HR = {},
OptionalJacobian<6, 3> Ht = {});
/**
* Create Pose3 by aligning two point pairs
* A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point
* Note this allows for noise on the points but in that case the mapping will not be exact.
*/
static boost::optional<Pose3> Align(const Point3Pairs& abPointPairs);
static std::optional<Pose3> Align(const Point3Pairs& abPointPairs);
// Version of Pose3::Align that takes 2 matrices.
static boost::optional<Pose3> Align(const Matrix& a, const Matrix& b);
static std::optional<Pose3> Align(const Matrix& a, const Matrix& b);
/// @}
/// @name Testable
@ -139,10 +139,10 @@ public:
/// @{
/// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = boost::none);
static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {});
/// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation
static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = {});
/**
* Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame
@ -157,13 +157,13 @@ public:
* Note that H_xib = AdjointMap()
*/
Vector6 Adjoint(const Vector6& xi_b,
OptionalJacobian<6, 6> H_this = boost::none,
OptionalJacobian<6, 6> H_xib = boost::none) const;
OptionalJacobian<6, 6> H_this = {},
OptionalJacobian<6, 6> H_xib = {}) const;
/// The dual version of Adjoint
Vector6 AdjointTranspose(const Vector6& x,
OptionalJacobian<6, 6> H_this = boost::none,
OptionalJacobian<6, 6> H_x = boost::none) const;
OptionalJacobian<6, 6> H_this = {},
OptionalJacobian<6, 6> H_x = {}) const;
/**
* Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
@ -186,8 +186,8 @@ public:
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/
static Vector6 adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none,
OptionalJacobian<6, 6> H_y = boost::none);
OptionalJacobian<6, 6> Hxi = {},
OptionalJacobian<6, 6> H_y = {});
// temporary fix for wrappers until case issue is resolved
static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
@ -197,8 +197,8 @@ public:
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
*/
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none,
OptionalJacobian<6, 6> H_y = boost::none);
OptionalJacobian<6, 6> Hxi = {},
OptionalJacobian<6, 6> H_y = {});
/// Derivative of Expmap
static Matrix6 ExpmapDerivative(const Vector6& xi);
@ -208,8 +208,8 @@ public:
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
struct ChartAtOrigin {
static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none);
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {});
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {});
};
/**
@ -250,7 +250,7 @@ public:
* @return point in world coordinates
*/
Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
{}, OptionalJacobian<3, 3> Hpoint = {}) const;
/**
* @brief transform many points in Pose coordinates and transform to world.
@ -272,7 +272,7 @@ public:
* @return point in Pose coordinates
*/
Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
{}, OptionalJacobian<3, 3> Hpoint = {}) const;
/**
* @brief transform many points in world coordinates and transform to Pose.
@ -286,10 +286,10 @@ public:
/// @{
/// get rotation
const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const;
const Rot3& rotation(OptionalJacobian<3, 6> Hself = {}) const;
/// get translation
const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const;
const Point3& translation(OptionalJacobian<3, 6> Hself = {}) const;
/// get x
double x() const {
@ -314,39 +314,39 @@ public:
* and transforms it to world coordinates wTb = wTa * aTb.
* This is identical to compose.
*/
Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none,
OptionalJacobian<6, 6> HaTb = boost::none) const;
Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = {},
OptionalJacobian<6, 6> HaTb = {}) const;
/**
* Assuming self == wTa, takes a pose wTb in world coordinates
* and transforms it to local coordinates aTb = inv(wTa) * wTb
*/
Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none,
OptionalJacobian<6, 6> HwTb = boost::none) const;
Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = {},
OptionalJacobian<6, 6> HwTb = {}) const;
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @return range (double)
*/
double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none,
OptionalJacobian<1, 3> Hpoint = boost::none) const;
double range(const Point3& point, OptionalJacobian<1, 6> Hself = {},
OptionalJacobian<1, 3> Hpoint = {}) const;
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
* @return range (double)
*/
double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none,
OptionalJacobian<1, 6> Hpose = boost::none) const;
double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = {},
OptionalJacobian<1, 6> Hpose = {}) const;
/**
* Calculate bearing to a landmark
* @param point 3D location of landmark
* @return bearing (Unit3)
*/
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none,
OptionalJacobian<2, 3> Hpoint = boost::none) const;
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = {},
OptionalJacobian<2, 3> Hpoint = {}) const;
/**
* Calculate bearing to another pose
@ -354,8 +354,8 @@ public:
* information is ignored.
* @return bearing (Unit3)
*/
Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none,
OptionalJacobian<2, 6> Hpose = boost::none) const;
Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = {},
OptionalJacobian<2, 6> Hpose = {}) const;
/// @}
/// @name Advanced Interface
@ -384,8 +384,8 @@ public:
* @param s a value between 0 and 1.5
* @param other final point of interpolation geodesic on manifold
*/
Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none,
OptionalJacobian<6, 6> Hy = boost::none) const;
Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = {},
OptionalJacobian<6, 6> Hy = {}) const;
/// Output stream operator
GTSAM_EXPORT

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

19
gtsam/geometry/Rot2.h
View File

@ -21,7 +21,6 @@
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Lie.h>
#include <boost/optional.hpp>
#include <random>
@ -80,7 +79,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 +122,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 +142,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 +159,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 +170,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
@ -572,7 +572,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 {

6
gtsam/geometry/Unit3.cpp
View File

@ -109,8 +109,8 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
// Cache the result and jacobian
H_B_.reset(jacobian);
B_.reset(B);
H_B_ = (jacobian);
B_ = (B);
}
// Return cached jacobian, possibly computed just above
@ -126,7 +126,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
const Point3 B1 = gtsam::cross(n, axis);
B.col(0) = normalize(B1);
B.col(1) = gtsam::cross(n, B.col(0));
B_.reset(B);
B_ = (B);
}
return *B_;

27
gtsam/geometry/Unit3.h
View File

@ -28,7 +28,6 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/dllexport.h>
#include <boost/optional.hpp>
#include <random>
#include <string>
@ -45,8 +44,8 @@ class GTSAM_EXPORT Unit3 {
private:
Vector3 p_; ///< The location of the point on the unit sphere
mutable boost::optional<Matrix32> B_; ///< Cached basis
mutable boost::optional<Matrix62> H_B_; ///< Cached basis derivative
mutable std::optional<Matrix32> B_; ///< Cached basis
mutable std::optional<Matrix62> H_B_; ///< Cached basis derivative
#ifdef GTSAM_USE_TBB
mutable std::mutex B_mutex_; ///< Mutex to protect the cached basis.
@ -98,7 +97,7 @@ public:
/// Named constructor from Point3 with optional Jacobian
static Unit3 FromPoint3(const Point3& point, //
OptionalJacobian<2, 3> H = boost::none);
OptionalJacobian<2, 3> H = {});
/**
* Random direction, using boost::uniform_on_sphere
@ -133,16 +132,16 @@ public:
* tangent to the sphere at the current direction.
* Provides derivatives of the basis with the two basis vectors stacked up as a 6x1.
*/
const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
const Matrix32& basis(OptionalJacobian<6, 2> H = {}) const;
/// Return skew-symmetric associated with 3D point on unit sphere
Matrix3 skew() const;
/// Return unit-norm Point3
Point3 point3(OptionalJacobian<3, 2> H = boost::none) const;
Point3 point3(OptionalJacobian<3, 2> H = {}) const;
/// Return unit-norm Vector
Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const;
Vector3 unitVector(OptionalJacobian<3, 2> H = {}) const;
/// Return scaled direction as Point3
friend Point3 operator*(double s, const Unit3& d) {
@ -150,20 +149,20 @@ public:
}
/// Return dot product with q
double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
OptionalJacobian<1,2> H2 = boost::none) const;
double dot(const Unit3& q, OptionalJacobian<1,2> H1 = {}, //
OptionalJacobian<1,2> H2 = {}) const;
/// Signed, vector-valued error between two directions
/// @deprecated, errorVector has the proper derivatives, this confusingly has only the second.
Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = {}) const;
/// Signed, vector-valued error between two directions
/// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal.
Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
OptionalJacobian<2, 2> H_q = boost::none) const;
Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = {}, //
OptionalJacobian<2, 2> H_q = {}) const;
/// Distance between two directions
double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
double distance(const Unit3& q, OptionalJacobian<1, 2> H = {}) const;
/// Cross-product between two Unit3s
Unit3 cross(const Unit3& q) const {
@ -196,7 +195,7 @@ public:
};
/// The retract function
Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const;
Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = {}) const;
/// The local coordinates function
Vector2 localCoordinates(const Unit3& s) const;

1
gtsam/geometry/concepts.h
View File

@ -25,7 +25,6 @@
#pragma once
#include <gtsam/base/Matrix.h>
#include <boost/optional.hpp>
namespace gtsam {

8
gtsam/geometry/geometry.i
View File

@ -376,8 +376,8 @@ class Pose2 {
Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
Pose2(Vector v);
static boost::optional<gtsam::Pose2> Align(const gtsam::Point2Pairs& abPointPairs);
static boost::optional<gtsam::Pose2> Align(const gtsam::Matrix& a, const gtsam::Matrix& b);
static std::optional<gtsam::Pose2> Align(const gtsam::Point2Pairs& abPointPairs);
static std::optional<gtsam::Pose2> Align(const gtsam::Matrix& a, const gtsam::Matrix& b);
// Testable
void print(string s = "") const;
@ -440,8 +440,8 @@ class Pose3 {
Pose3(const gtsam::Pose2& pose2);
Pose3(Matrix mat);
static boost::optional<gtsam::Pose3> Align(const gtsam::Point3Pairs& abPointPairs);
static boost::optional<gtsam::Pose3> Align(const gtsam::Matrix& a, const gtsam::Matrix& b);
static std::optional<gtsam::Pose3> Align(const gtsam::Point3Pairs& abPointPairs);
static std::optional<gtsam::Pose3> Align(const gtsam::Matrix& a, const gtsam::Matrix& b);
// Testable
void print(string s = "") 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));
}

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

@ -23,7 +23,7 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Rot2.h>
#include <boost/optional.hpp>
#include <optional>
#include <cmath>
#include <iostream>
@ -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));
}
@ -718,7 +718,7 @@ TEST(Pose2, align_1) {
Pose2 expected(Rot2::fromAngle(0), Point2(10, 10));
Point2Pairs ab_pairs {{Point2(10, 10), Point2(0, 0)},
{Point2(30, 20), Point2(20, 10)}};
boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
std::optional<Pose2> aTb = Pose2::Align(ab_pairs);
EXPECT(assert_equal(expected, *aTb));
}
@ -731,7 +731,7 @@ TEST(Pose2, align_2) {
Point2Pairs ab_pairs {{expected.transformFrom(b1), b1},
{expected.transformFrom(b2), b2}};
boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
std::optional<Pose2> aTb = Pose2::Align(ab_pairs);
EXPECT(assert_equal(expected, *aTb));
}
@ -752,7 +752,7 @@ TEST(Pose2, align_3) {
Point2Pair ab3(make_pair(a3, b3));
const Point2Pairs ab_pairs{ab1, ab2, ab3};
boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
std::optional<Pose2> aTb = Pose2::Align(ab_pairs);
EXPECT(assert_equal(expected, *aTb));
}
@ -762,7 +762,7 @@ namespace {
/* ************************************************************************* */
struct Triangle { size_t i_, j_, k_;};
boost::optional<Pose2> align2(const Point2Vector& as, const Point2Vector& bs,
std::optional<Pose2> align2(const Point2Vector& as, const Point2Vector& bs,
const pair<Triangle, Triangle>& trianglePair) {
const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
Point2Pairs ab_pairs = {{as[t1.i_], bs[t2.i_]},
@ -780,7 +780,7 @@ TEST(Pose2, align_4) {
Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2;
Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0;
boost::optional<Pose2> actual = align2(as, bs, {t1, t2});
std::optional<Pose2> actual = align2(as, bs, {t1, t2});
EXPECT(assert_equal(expected, *actual));
}

39
gtsam/geometry/tests/testPose3.cpp
View File

@ -23,6 +23,7 @@
#include <CppUnitLite/TestHarness.h>
#include <cmath>
#include <functional>
using namespace std;
using namespace gtsam;
@ -286,8 +287,8 @@ TEST(Pose3, translation) {
Matrix actualH;
EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8));
Matrix numericalH = numericalDerivative11<Point3, Pose3>(
std::bind(&Pose3::translation, std::placeholders::_1, boost::none), T);
std::function<Point3(const Pose3&)> f = [](const Pose3& T) { return T.translation(); };
Matrix numericalH = numericalDerivative11<Point3, Pose3>(f, T);
EXPECT(assert_equal(numericalH, actualH, 1e-6));
}
@ -297,8 +298,8 @@ TEST(Pose3, rotation) {
Matrix actualH;
EXPECT(assert_equal(R, T.rotation(actualH), 1e-8));
Matrix numericalH = numericalDerivative11<Rot3, Pose3>(
std::bind(&Pose3::rotation, std::placeholders::_1, boost::none), T);
std::function<Rot3(const Pose3&)> f = [](const Pose3& T) { return T.rotation(); };
Matrix numericalH = numericalDerivative11<Rot3, Pose3>(f, T);
EXPECT(assert_equal(numericalH, actualH, 1e-6));
}
@ -396,7 +397,7 @@ Point3 transformFrom_(const Pose3& pose, const Point3& point) {
}
TEST(Pose3, Dtransform_from1_a) {
Matrix actualDtransform_from1;
T.transformFrom(P, actualDtransform_from1, boost::none);
T.transformFrom(P, actualDtransform_from1, {});
Matrix numerical = numericalDerivative21(transformFrom_, T, P);
EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
}
@ -404,7 +405,7 @@ TEST(Pose3, Dtransform_from1_a) {
TEST(Pose3, Dtransform_from1_b) {
Pose3 origin;
Matrix actualDtransform_from1;
origin.transformFrom(P, actualDtransform_from1, boost::none);
origin.transformFrom(P, actualDtransform_from1, {});
Matrix numerical = numericalDerivative21(transformFrom_, origin, P);
EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
}
@ -413,7 +414,7 @@ TEST(Pose3, Dtransform_from1_c) {
Point3 origin(0, 0, 0);
Pose3 T0(R, origin);
Matrix actualDtransform_from1;
T0.transformFrom(P, actualDtransform_from1, boost::none);
T0.transformFrom(P, actualDtransform_from1, {});
Matrix numerical = numericalDerivative21(transformFrom_, T0, P);
EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
}
@ -423,7 +424,7 @@ TEST(Pose3, Dtransform_from1_d) {
Point3 t0(100, 0, 0);
Pose3 T0(I, t0);
Matrix actualDtransform_from1;
T0.transformFrom(P, actualDtransform_from1, boost::none);
T0.transformFrom(P, actualDtransform_from1, {});
// print(computed, "Dtransform_from1_d computed:");
Matrix numerical = numericalDerivative21(transformFrom_, T0, P);
// print(numerical, "Dtransform_from1_d numerical:");
@ -433,7 +434,7 @@ TEST(Pose3, Dtransform_from1_d) {
/* ************************************************************************* */
TEST(Pose3, Dtransform_from2) {
Matrix actualDtransform_from2;
T.transformFrom(P, boost::none, actualDtransform_from2);
T.transformFrom(P, {}, actualDtransform_from2);
Matrix numerical = numericalDerivative22(transformFrom_, T, P);
EXPECT(assert_equal(numerical, actualDtransform_from2, 1e-8));
}
@ -444,7 +445,7 @@ Point3 transform_to_(const Pose3& pose, const Point3& point) {
}
TEST(Pose3, Dtransform_to1) {
Matrix computed;
T.transformTo(P, computed, boost::none);
T.transformTo(P, computed, {});
Matrix numerical = numericalDerivative21(transform_to_, T, P);
EXPECT(assert_equal(numerical, computed, 1e-8));
}
@ -452,7 +453,7 @@ TEST(Pose3, Dtransform_to1) {
/* ************************************************************************* */
TEST(Pose3, Dtransform_to2) {
Matrix computed;
T.transformTo(P, boost::none, computed);
T.transformTo(P, {}, computed);
Matrix numerical = numericalDerivative22(transform_to_, T, P);
EXPECT(assert_equal(numerical, computed, 1e-8));
}
@ -812,7 +813,7 @@ TEST(Pose3, Align1) {
Point3Pair ab3(Point3(20,30,0), Point3(10,20,0));
const vector<Point3Pair> correspondences{ab1, ab2, ab3};
boost::optional<Pose3> actual = Pose3::Align(correspondences);
std::optional<Pose3> actual = Pose3::Align(correspondences);
EXPECT(assert_equal(expected, *actual));
}
@ -829,7 +830,7 @@ TEST(Pose3, Align2) {
const Point3Pair ab1{q1, p1}, ab2{q2, p2}, ab3{q3, p3};
const vector<Point3Pair> correspondences{ab1, ab2, ab3};
boost::optional<Pose3> actual = Pose3::Align(correspondences);
std::optional<Pose3> actual = Pose3::Align(correspondences);
EXPECT(assert_equal(expected, *actual, 1e-5));
}
@ -839,7 +840,7 @@ TEST( Pose3, ExpmapDerivative1) {
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3::Expmap(w,actualH);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none);
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
EXPECT(assert_equal(expectedH, actualH));
}
@ -884,7 +885,7 @@ TEST( Pose3, ExpmapDerivativeQr) {
w.head<3>() = w.head<3>() * 0.9e-2;
Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none);
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
}
@ -896,7 +897,7 @@ TEST( Pose3, LogmapDerivative) {
Pose3 p = Pose3::Expmap(w);
EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
Matrix expectedH = numericalDerivative21<Vector6, Pose3,
OptionalJacobian<6, 6> >(&Pose3::Logmap, p, boost::none);
OptionalJacobian<6, 6> >(&Pose3::Logmap, p, {});
EXPECT(assert_equal(expectedH, actualH));
}
@ -1189,9 +1190,9 @@ TEST(Pose3, Create) {
Matrix63 actualH1, actualH2;
Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2);
EXPECT(assert_equal(T, actual));
std::function<Pose3(Rot3, Point3)> create =
std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
std::function<Pose3(Rot3, Point3)> create = [](Rot3 R, Point3 t) {
return Pose3::Create(R, t);
};
EXPECT(assert_equal(numericalDerivative21<Pose3,Rot3,Point3>(create, R, P2), actualH1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
}

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

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

@ -1,4 +1,4 @@
/* ----------------------------------------------------------------------------
/* ------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
@ -30,7 +30,6 @@ using namespace gtsam;
//******************************************************************************
TEST(Rot3Q , Compare) {
using boost::none;
// We set up expected values with quaternions
typedef Quaternion Q;
@ -47,8 +46,8 @@ TEST(Rot3Q , Compare) {
R R1(q1), R2(q2);
// Check Compose
Q q3 = TQ::Compose(q1, q2, none, none);
R R3 = TR::Compose(R1, R2, none, none);
Q q3 = TQ::Compose(q1, q2, {}, {});
R R3 = TR::Compose(R1, R2, {}, {});
EXPECT(assert_equal(R(q3), R3));
// Check Retract
@ -86,8 +85,8 @@ TEST(Rot3Q , Compare) {
// Check Compose Derivatives
Matrix HQ, HR;
HQ = numericalDerivative42<Q, Q, Q, OJ, OJ>(TQ::Compose, q1, q2, none, none);
HR = numericalDerivative42<R, R, R, OJ, OJ>(TR::Compose, R1, R2, none, none);
HQ = numericalDerivative42<Q, Q, Q, OJ, OJ>(TQ::Compose, q1, q2, {}, {});
HR = numericalDerivative42<R, R, R, OJ, OJ>(TR::Compose, R1, R2, {}, {});
EXPECT(assert_equal(HQ, HR));
}

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

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

82
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -67,13 +67,13 @@ TEST(triangulation, twoPoses) {
// 1. Test simple DLT, perfect in no noise situation
bool optimize = false;
boost::optional<Point3> actual1 = //
std::optional<Point3> actual1 = //
triangulatePoint3<Cal3_S2>(kPoses, kSharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
// 2. test with optimization on, same answer
optimize = true;
boost::optional<Point3> actual2 = //
std::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(kPoses, kSharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
@ -82,13 +82,13 @@ TEST(triangulation, twoPoses) {
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
optimize = false;
boost::optional<Point3> actual3 = //
std::optional<Point3> actual3 = //
triangulatePoint3<Cal3_S2>(kPoses, kSharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4));
// 4. Now with optimization on
optimize = true;
boost::optional<Point3> actual4 = //
std::optional<Point3> actual4 = //
triangulatePoint3<Cal3_S2>(kPoses, kSharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
}
@ -103,7 +103,7 @@ TEST(triangulation, twoCamerasUsingLOST) {
double rank_tol = 1e-9;
// 1. Test simple triangulation, perfect in no noise situation
boost::optional<Point3> actual1 = //
std::optional<Point3> actual1 = //
triangulatePoint3<PinholeCamera<Cal3_S2>>(cameras, measurements, rank_tol,
/*optimize=*/false,
measurementNoise,
@ -114,7 +114,7 @@ TEST(triangulation, twoCamerasUsingLOST) {
// 0.499167, 1.19814)
measurements[0] += Point2(0.1, 0.5);
measurements[1] += Point2(-0.2, 0.3);
boost::optional<Point3> actual2 = //
std::optional<Point3> actual2 = //
triangulatePoint3<PinholeCamera<Cal3_S2>>(
cameras, measurements, rank_tol,
/*optimize=*/false, measurementNoise,
@ -140,7 +140,7 @@ TEST(triangulation, twoCamerasLOSTvsDLT) {
const double rank_tol = 1e-9;
SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2);
boost::optional<Point3> estimateLOST = //
std::optional<Point3> estimateLOST = //
triangulatePoint3<PinholeCamera<Cal3_S2>>(cameras, measurements, rank_tol,
/*optimize=*/false,
measurementNoise,
@ -149,7 +149,7 @@ TEST(triangulation, twoCamerasLOSTvsDLT) {
// These values are from a MATLAB implementation.
EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3));
boost::optional<Point3> estimateDLT =
std::optional<Point3> estimateDLT =
triangulatePoint3<Cal3_S2>(cameras, measurements, rank_tol, false);
// The LOST estimate should have a smaller error.
@ -177,14 +177,14 @@ TEST(triangulation, twoPosesCal3DS2) {
// 1. Test simple DLT, perfect in no noise situation
bool optimize = false;
boost::optional<Point3> actual1 = //
std::optional<Point3> actual1 = //
triangulatePoint3<Cal3DS2>(kPoses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
// 2. test with optimization on, same answer
optimize = true;
boost::optional<Point3> actual2 = //
std::optional<Point3> actual2 = //
triangulatePoint3<Cal3DS2>(kPoses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
@ -194,14 +194,14 @@ TEST(triangulation, twoPosesCal3DS2) {
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
optimize = false;
boost::optional<Point3> actual3 = //
std::optional<Point3> actual3 = //
triangulatePoint3<Cal3DS2>(kPoses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
// 4. Now with optimization on
optimize = true;
boost::optional<Point3> actual4 = //
std::optional<Point3> actual4 = //
triangulatePoint3<Cal3DS2>(kPoses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
@ -230,14 +230,14 @@ TEST(triangulation, twoPosesFisheye) {
// 1. Test simple DLT, perfect in no noise situation
bool optimize = false;
boost::optional<Point3> actual1 = //
std::optional<Point3> actual1 = //
triangulatePoint3<Calibration>(kPoses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
// 2. test with optimization on, same answer
optimize = true;
boost::optional<Point3> actual2 = //
std::optional<Point3> actual2 = //
triangulatePoint3<Calibration>(kPoses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
@ -247,14 +247,14 @@ TEST(triangulation, twoPosesFisheye) {
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
optimize = false;
boost::optional<Point3> actual3 = //
std::optional<Point3> actual3 = //
triangulatePoint3<Calibration>(kPoses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
// 4. Now with optimization on
optimize = true;
boost::optional<Point3> actual4 = //
std::optional<Point3> actual4 = //
triangulatePoint3<Calibration>(kPoses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
@ -277,7 +277,7 @@ TEST(triangulation, twoPosesBundler) {
bool optimize = true;
double rank_tol = 1e-9;
boost::optional<Point3> actual = //
std::optional<Point3> actual = //
triangulatePoint3<Cal3Bundler>(kPoses, bundlerCal, measurements, rank_tol,
optimize);
EXPECT(assert_equal(kLandmark, *actual, 1e-7));
@ -286,7 +286,7 @@ TEST(triangulation, twoPosesBundler) {
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> actual2 = //
std::optional<Point3> actual2 = //
triangulatePoint3<Cal3Bundler>(kPoses, bundlerCal, measurements, rank_tol,
optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3));
@ -296,7 +296,7 @@ TEST(triangulation, twoPosesBundler) {
TEST(triangulation, fourPoses) {
Pose3Vector poses = kPoses;
Point2Vector measurements = kMeasurements;
boost::optional<Point3> actual =
std::optional<Point3> actual =
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
EXPECT(assert_equal(kLandmark, *actual, 1e-2));
@ -305,7 +305,7 @@ TEST(triangulation, fourPoses) {
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> actual2 = //
std::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
EXPECT(assert_equal(kLandmark, *actual2, 1e-2));
@ -317,12 +317,12 @@ TEST(triangulation, fourPoses) {
poses.push_back(pose3);
measurements.push_back(z3 + Point2(0.1, -0.1));
boost::optional<Point3> triangulated_3cameras = //
std::optional<Point3> triangulated_3cameras = //
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt =
std::optional<Point3> triangulated_3cameras_opt =
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, 1e-9, true);
EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2));
@ -352,7 +352,7 @@ TEST(triangulation, threePoses_robustNoiseModel) {
Point2Vector measurements{kZ1, kZ2, z3};
// noise free, so should give exactly the landmark
boost::optional<Point3> actual =
std::optional<Point3> actual =
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
EXPECT(assert_equal(kLandmark, *actual, 1e-2));
@ -360,14 +360,14 @@ TEST(triangulation, threePoses_robustNoiseModel) {
measurements.at(0) += Point2(100, 120); // very large pixel noise!
// now estimate does not match landmark
boost::optional<Point3> actual2 = //
std::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
// DLT is surprisingly robust, but still off (actual error is around 0.26m):
EXPECT( (kLandmark - *actual2).norm() >= 0.2);
EXPECT( (kLandmark - *actual2).norm() <= 0.5);
// Again with nonlinear optimization
boost::optional<Point3> actual3 =
std::optional<Point3> actual3 =
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, 1e-9, true);
// result from nonlinear (but non-robust optimization) is close to DLT and still off
EXPECT(assert_equal(*actual2, *actual3, 0.1));
@ -375,7 +375,7 @@ TEST(triangulation, threePoses_robustNoiseModel) {
// Again with nonlinear optimization, this time with robust loss
auto model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
std::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
poses, kSharedCal, measurements, 1e-9, true, model);
// using the Huber loss we now have a quite small error!! nice!
EXPECT(assert_equal(kLandmark, *actual4, 0.05));
@ -393,7 +393,7 @@ TEST(triangulation, fourPoses_robustNoiseModel) {
Point2Vector measurements{kZ1, kZ1, kZ2, z3};
// noise free, so should give exactly the landmark
boost::optional<Point3> actual =
std::optional<Point3> actual =
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
EXPECT(assert_equal(kLandmark, *actual, 1e-2));
@ -405,14 +405,14 @@ TEST(triangulation, fourPoses_robustNoiseModel) {
measurements.at(3) += Point2(0.3, 0.1);
// now estimate does not match landmark
boost::optional<Point3> actual2 = //
std::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
// DLT is surprisingly robust, but still off (actual error is around 0.17m):
EXPECT( (kLandmark - *actual2).norm() >= 0.1);
EXPECT( (kLandmark - *actual2).norm() <= 0.5);
// Again with nonlinear optimization
boost::optional<Point3> actual3 =
std::optional<Point3> actual3 =
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, 1e-9, true);
// result from nonlinear (but non-robust optimization) is close to DLT and still off
EXPECT(assert_equal(*actual2, *actual3, 0.1));
@ -420,7 +420,7 @@ TEST(triangulation, fourPoses_robustNoiseModel) {
// Again with nonlinear optimization, this time with robust loss
auto model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
std::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
poses, kSharedCal, measurements, 1e-9, true, model);
// using the Huber loss we now have a quite small error!! nice!
EXPECT(assert_equal(kLandmark, *actual4, 0.05));
@ -443,7 +443,7 @@ TEST(triangulation, fourPoses_distinct_Ks) {
CameraSet<PinholeCamera<Cal3_S2>> cameras{camera1, camera2};
Point2Vector measurements{z1, z2};
boost::optional<Point3> actual = //
std::optional<Point3> actual = //
triangulatePoint3<Cal3_S2>(cameras, measurements);
EXPECT(assert_equal(kLandmark, *actual, 1e-2));
@ -452,7 +452,7 @@ TEST(triangulation, fourPoses_distinct_Ks) {
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> actual2 = //
std::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(cameras, measurements);
EXPECT(assert_equal(kLandmark, *actual2, 1e-2));
@ -465,12 +465,12 @@ TEST(triangulation, fourPoses_distinct_Ks) {
cameras.push_back(camera3);
measurements.push_back(z3 + Point2(0.1, -0.1));
boost::optional<Point3> triangulated_3cameras = //
std::optional<Point3> triangulated_3cameras = //
triangulatePoint3<Cal3_S2>(cameras, measurements);
EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt =
std::optional<Point3> triangulated_3cameras_opt =
triangulatePoint3<Cal3_S2>(cameras, measurements, 1e-9, true);
EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2));
@ -506,7 +506,7 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) {
const CameraSet<PinholeCamera<Cal3DS2>> cameras{camera1, camera2};
const Point2Vector measurements{z1, z2};
boost::optional<Point3> actual = //
std::optional<Point3> actual = //
triangulatePoint3<Cal3DS2>(cameras, measurements);
EXPECT(assert_equal(kLandmark, *actual, 1e-2));
}
@ -727,14 +727,14 @@ TEST(triangulation, twoPoses_sphericalCamera) {
// 3. Test simple DLT, now within triangulatePoint3
bool optimize = false;
boost::optional<Point3> actual1 = //
std::optional<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
// 4. test with optimization on, same answer
optimize = true;
boost::optional<Point3> actual2 = //
std::optional<Point3> actual2 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
@ -745,14 +745,14 @@ TEST(triangulation, twoPoses_sphericalCamera) {
u1.retract(Vector2(0.01, 0.05)); // note: perturbation smaller for Unit3
measurements.at(1) = u2.retract(Vector2(-0.02, 0.03));
optimize = false;
boost::optional<Point3> actual3 = //
std::optional<Point3> actual3 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(Point3(5.9432, 0.654319, 1.48192), *actual3, 1e-3));
// 6. Now with optimization on
optimize = true;
boost::optional<Point3> actual4 = //
std::optional<Point3> actual4 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(Point3(5.9432, 0.654334, 1.48192), *actual4, 1e-3));
@ -795,7 +795,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) {
rank_tol, optimize),
TriangulationCheiralityException);
#else // otherwise project should not throw the exception
boost::optional<Point3> actual1 = //
std::optional<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(landmarkL, *actual1, 1e-7));
@ -809,7 +809,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) {
rank_tol, optimize),
TriangulationCheiralityException);
#else // otherwise project should not throw the exception
boost::optional<Point3> actual1 = //
std::optional<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(landmarkL, *actual1, 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, {});
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
{
expectedH = numericalDerivative22(rotate_, R, p);
R.rotate(p, boost::none, actualH);
R.rotate(p, {}, 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, {});
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
{
expectedH = numericalDerivative22(unrotate_, R, p);
R.unrotate(p, boost::none, actualH);
R.unrotate(p, {}, 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, {});
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, {});
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, {}, 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, {}, 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));
}

13
gtsam/geometry/triangulation.h
View File

@ -20,6 +20,7 @@
#pragma once
#include "gtsam/geometry/Point3.h"
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
@ -33,6 +34,8 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/TriangulationFactor.h>
#include <optional>
namespace gtsam {
/// Exception thrown by triangulateDLT when SVD returns rank < 3
@ -260,7 +263,7 @@ Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
template <class CALIBRATION, class MEASUREMENT>
MEASUREMENT undistortMeasurementInternal(
const CALIBRATION& cal, const MEASUREMENT& measurement,
boost::optional<Cal3_S2> pinholeCal = boost::none) {
std::optional<Cal3_S2> pinholeCal = {}) {
if (!pinholeCal) {
pinholeCal = createPinholeCalibration(cal);
}
@ -623,7 +626,7 @@ private:
* TriangulationResult is an optional point, along with the reasons why it is
* invalid.
*/
class TriangulationResult : public boost::optional<Point3> {
class TriangulationResult : public std::optional<Point3> {
public:
enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
Status status;
@ -640,7 +643,7 @@ class TriangulationResult : public boost::optional<Point3> {
/**
* Constructor
*/
TriangulationResult(const Point3& p) : status(VALID) { reset(p); }
TriangulationResult(const Point3& p) : status(VALID) { emplace(p); }
static TriangulationResult Degenerate() {
return TriangulationResult(DEGENERATE);
}
@ -656,6 +659,10 @@ class TriangulationResult : public boost::optional<Point3> {
bool outlier() const { return status == OUTLIER; }
bool farPoint() const { return status == FAR_POINT; }
bool behindCamera() const { return status == BEHIND_CAMERA; }
const gtsam::Point3& get() const {
if (!has_value()) throw std::runtime_error("TriangulationResult has no value");
return value();
}
// stream to output
friend std::ostream& operator<<(std::ostream& os,
const TriangulationResult& result) {

5
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -27,6 +27,9 @@
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <functional>
#include <optional>
namespace gtsam {
// Forward declarations
@ -85,7 +88,7 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
/// The default ordering generation function
static Ordering DefaultOrderingFunc(
const FactorGraphType& graph,
boost::optional<const VariableIndex&> variableIndex) {
std::optional<std::reference_wrapper<const VariableIndex>>) {
return HybridOrdering(graph);
}
};

10
gtsam/hybrid/HybridGaussianISAM.cpp
View File

@ -71,8 +71,8 @@ Ordering HybridGaussianISAM::GetOrdering(
void HybridGaussianISAM::updateInternal(
const HybridGaussianFactorGraph& newFactors,
HybridBayesTree::Cliques* orphans,
const boost::optional<size_t>& maxNrLeaves,
const boost::optional<Ordering>& ordering,
const std::optional<size_t>& maxNrLeaves,
const std::optional<Ordering>& ordering,
const HybridBayesTree::Eliminate& function) {
// Remove the contaminated part of the Bayes tree
BayesNetType bn;
@ -102,7 +102,7 @@ void HybridGaussianISAM::updateInternal(
// eliminate all factors (top, added, orphans) into a new Bayes tree
HybridBayesTree::shared_ptr bayesTree =
factors.eliminateMultifrontal(elimination_ordering, function, index);
factors.eliminateMultifrontal(elimination_ordering, function, std::cref(index));
if (maxNrLeaves) {
bayesTree->prune(*maxNrLeaves);
@ -116,8 +116,8 @@ void HybridGaussianISAM::updateInternal(
/* ************************************************************************* */
void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors,
const boost::optional<size_t>& maxNrLeaves,
const boost::optional<Ordering>& ordering,
const std::optional<size_t>& maxNrLeaves,
const std::optional<Ordering>& ordering,
const HybridBayesTree::Eliminate& function) {
Cliques orphans;
this->updateInternal(newFactors, &orphans, maxNrLeaves, ordering, function);

10
gtsam/hybrid/HybridGaussianISAM.h
View File

@ -24,6 +24,8 @@
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/ISAM.h>
#include <optional>
namespace gtsam {
/**
@ -53,8 +55,8 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
void updateInternal(
const HybridGaussianFactorGraph& newFactors,
HybridBayesTree::Cliques* orphans,
const boost::optional<size_t>& maxNrLeaves = boost::none,
const boost::optional<Ordering>& ordering = boost::none,
const std::optional<size_t>& maxNrLeaves = {},
const std::optional<Ordering>& ordering = {},
const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate);
@ -68,8 +70,8 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
* @param function Elimination function.
*/
void update(const HybridGaussianFactorGraph& newFactors,
const boost::optional<size_t>& maxNrLeaves = boost::none,
const boost::optional<Ordering>& ordering = boost::none,
const std::optional<size_t>& maxNrLeaves = {},
const std::optional<Ordering>& ordering = {},
const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate);

6
gtsam/hybrid/HybridNonlinearISAM.cpp
View File

@ -34,8 +34,8 @@ void HybridNonlinearISAM::saveGraph(const string& s,
/* ************************************************************************* */
void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors,
const Values& initialValues,
const boost::optional<size_t>& maxNrLeaves,
const boost::optional<Ordering>& ordering) {
const std::optional<size_t>& maxNrLeaves,
const std::optional<Ordering>& ordering) {
if (newFactors.size() > 0) {
// Reorder and relinearize every reorderInterval updates
if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
@ -70,7 +70,7 @@ void HybridNonlinearISAM::reorder_relinearize() {
// Just recreate the whole BayesTree
// TODO: allow for constrained ordering here
// TODO: decouple relinearization and reordering to avoid
isam_.update(*factors_.linearize(newLinPoint), boost::none, boost::none,
isam_.update(*factors_.linearize(newLinPoint), {}, {},
eliminationFunction_);
// Update linearization point

5
gtsam/hybrid/HybridNonlinearISAM.h
View File

@ -19,6 +19,7 @@
#include <gtsam/hybrid/HybridGaussianISAM.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <optional>
namespace gtsam {
/**
@ -119,8 +120,8 @@ class GTSAM_EXPORT HybridNonlinearISAM {
/** Add new factors along with their initial linearization points */
void update(const HybridNonlinearFactorGraph& newFactors,
const Values& initialValues,
const boost::optional<size_t>& maxNrLeaves = boost::none,
const boost::optional<Ordering>& ordering = boost::none);
const std::optional<size_t>& maxNrLeaves = {},
const std::optional<Ordering>& ordering = {});
/** Relinearization and reordering of variables */
void reorder_relinearize();

2
gtsam/hybrid/HybridSmoother.cpp
View File

@ -26,7 +26,7 @@ namespace gtsam {
/* ************************************************************************* */
void HybridSmoother::update(HybridGaussianFactorGraph graph,
const Ordering &ordering,
boost::optional<size_t> maxNrLeaves) {
std::optional<size_t> maxNrLeaves) {
// Add the necessary conditionals from the previous timestep(s).
std::tie(graph, hybridBayesNet_) =
addConditionals(graph, hybridBayesNet_, ordering);

4
gtsam/hybrid/HybridSmoother.h
View File

@ -20,6 +20,8 @@
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <optional>
namespace gtsam {
class HybridSmoother {
@ -48,7 +50,7 @@ class HybridSmoother {
* if applicable
*/
void update(HybridGaussianFactorGraph graph, const Ordering& ordering,
boost::optional<size_t> maxNrLeaves = boost::none);
std::optional<size_t> maxNrLeaves = {});
/**
* @brief Add conditionals from previous timestep as part of liquefication.

2
gtsam/hybrid/tests/TinyHybridExample.h
View File

@ -71,7 +71,7 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1,
*/
inline HybridGaussianFactorGraph createHybridGaussianFactorGraph(
size_t num_measurements = 1,
boost::optional<VectorValues> measurements = boost::none,
std::optional<VectorValues> measurements = {},
bool manyModes = false) {
auto bayesNet = createHybridBayesNet(num_measurements, manyModes);
if (measurements) {

1
gtsam/inference/BayesTree-inst.h
View File

@ -25,7 +25,6 @@
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/base/timing.h>
#include <boost/optional.hpp>
#include <fstream>
namespace gtsam {

4
gtsam/inference/BayesTreeCliqueBase-inst.h
View File

@ -178,7 +178,7 @@ namespace gtsam {
this->conditional()->endParents());
auto separatorMarginal =
p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function);
cachedSeparatorMarginal_.reset(*separatorMarginal);
cachedSeparatorMarginal_ = *separatorMarginal;
}
}
@ -217,7 +217,7 @@ namespace gtsam {
}
//Delete CachedShortcut for this clique
cachedSeparatorMarginal_ = boost::none;
cachedSeparatorMarginal_ = {};
}
}

8
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -21,10 +21,10 @@
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>
#include <boost/optional.hpp>
#include <string>
#include <mutex>
#include <optional>
namespace gtsam {
@ -102,7 +102,7 @@ namespace gtsam {
/// @}
/// This stores the Cached separator marginal P(S)
mutable boost::optional<FactorGraphType> cachedSeparatorMarginal_;
mutable std::optional<FactorGraphType> cachedSeparatorMarginal_;
/// This protects Cached seperator marginal P(S) from concurrent read/writes
/// as many the functions which access it are const (hence the mutable)
/// leading to the false impression that these const functions are thread-safe
@ -174,7 +174,7 @@ namespace gtsam {
*/
void deleteCachedShortcuts();
const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const {
const std::optional<FactorGraphType>& cachedSeparatorMarginal() const {
std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
return cachedSeparatorMarginal_;
}
@ -194,7 +194,7 @@ namespace gtsam {
/** Non-recursive delete cached shortcuts and marginals - internal only. */
void deleteCachedShortcutsNonRecursive() {
std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
cachedSeparatorMarginal_ = boost::none;
cachedSeparatorMarginal_ = {};
}
private:

20
gtsam/inference/DotWriter.cpp
View File

@ -40,7 +40,7 @@ void DotWriter::digraphPreamble(ostream* os) const {
}
void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position,
const std::optional<Vector2>& position,
ostream* os) const {
// Label the node with the label from the KeyFormatter
*os << " var" << key << "[label=\"" << keyFormatter(key)
@ -54,7 +54,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter,
*os << "];\n";
}
void DotWriter::DrawFactor(size_t i, const boost::optional<Vector2>& position,
void DotWriter::DrawFactor(size_t i, const std::optional<Vector2>& position,
ostream* os) {
*os << " factor" << i << "[label=\"\", shape=point";
if (position) {
@ -76,26 +76,28 @@ static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter,
}
/// Return variable position or none
boost::optional<Vector2> DotWriter::variablePos(Key key) const {
boost::optional<Vector2> result = boost::none;
std::optional<Vector2> DotWriter::variablePos(Key key) const {
std::optional<Vector2> result = {};
// Check position hint
Symbol symbol(key);
auto hint = positionHints.find(symbol.chr());
if (hint != positionHints.end())
result.reset(Vector2(symbol.index(), hint->second));
if (hint != positionHints.end()) {
result = Vector2(symbol.index(), hint->second);
}
// Override with explicit position, if given.
auto pos = variablePositions.find(key);
if (pos != variablePositions.end())
result.reset(pos->second);
if (pos != variablePositions.end()) {
result = pos->second;
}
return result;
}
void DotWriter::processFactor(size_t i, const KeyVector& keys,
const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position,
const std::optional<Vector2>& position,
ostream* os) const {
if (plotFactorPoints) {
if (binaryEdges && keys.size() == 2) {

9
gtsam/inference/DotWriter.h
View File

@ -25,6 +25,7 @@
#include <iosfwd>
#include <map>
#include <set>
#include <optional>
namespace gtsam {
@ -80,20 +81,20 @@ struct GTSAM_EXPORT DotWriter {
/// Create a variable dot fragment.
void drawVariable(Key key, const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position,
const std::optional<Vector2>& position,
std::ostream* os) const;
/// Create factor dot.
static void DrawFactor(size_t i, const boost::optional<Vector2>& position,
static void DrawFactor(size_t i, const std::optional<Vector2>& position,
std::ostream* os);
/// Return variable position or none
boost::optional<Vector2> variablePos(Key key) const;
std::optional<Vector2> variablePos(Key key) const;
/// Draw a single factor, specified by its index i and its variable keys.
void processFactor(size_t i, const KeyVector& keys,
const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position,
const std::optional<Vector2>& position,
std::ostream* os) const;
};

60
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -36,7 +36,7 @@ namespace gtsam {
// no Ordering is provided. When removing optional from VariableIndex, create VariableIndex
// before creating ordering.
VariableIndex computedVariableIndex(asDerived());
return eliminateSequential(orderingType, function, computedVariableIndex);
return eliminateSequential(orderingType, function, std::cref(computedVariableIndex));
}
else {
// Compute an ordering and call this function again. We are guaranteed to have a
@ -45,14 +45,14 @@ namespace gtsam {
Ordering computedOrdering = Ordering::Metis(asDerived());
return eliminateSequential(computedOrdering, function, variableIndex);
} else if (orderingType == Ordering::COLAMD) {
Ordering computedOrdering = Ordering::Colamd(*variableIndex);
Ordering computedOrdering = Ordering::Colamd((*variableIndex).get());
return eliminateSequential(computedOrdering, function, variableIndex);
} else if (orderingType == Ordering::NATURAL) {
Ordering computedOrdering = Ordering::Natural(asDerived());
return eliminateSequential(computedOrdering, function, variableIndex);
} else {
Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
asDerived(), variableIndex);
asDerived(), *variableIndex);
return eliminateSequential(computedOrdering, function, variableIndex);
}
}
@ -68,11 +68,11 @@ namespace gtsam {
if(!variableIndex) {
// If no VariableIndex provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived());
return eliminateSequential(ordering, function, computedVariableIndex);
return eliminateSequential(ordering, function, std::cref(computedVariableIndex));
} else {
gttic(eliminateSequential);
// Do elimination
EliminationTreeType etree(asDerived(), *variableIndex, ordering);
EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
boost::shared_ptr<BayesNetType> bayesNet;
boost::shared_ptr<FactorGraphType> factorGraph;
boost::tie(bayesNet,factorGraph) = etree.eliminate(function);
@ -99,7 +99,7 @@ namespace gtsam {
// creating ordering.
VariableIndex computedVariableIndex(asDerived());
return eliminateMultifrontal(orderingType, function,
computedVariableIndex);
std::cref(computedVariableIndex));
} else {
// Compute an ordering and call this function again. We are guaranteed to
// have a VariableIndex already here because we computed one if needed in
@ -108,14 +108,14 @@ namespace gtsam {
Ordering computedOrdering = Ordering::Metis(asDerived());
return eliminateMultifrontal(computedOrdering, function, variableIndex);
} else if (orderingType == Ordering::COLAMD) {
Ordering computedOrdering = Ordering::Colamd(*variableIndex);
Ordering computedOrdering = Ordering::Colamd((*variableIndex).get());
return eliminateMultifrontal(computedOrdering, function, variableIndex);
} else if (orderingType == Ordering::NATURAL) {
Ordering computedOrdering = Ordering::Natural(asDerived());
return eliminateMultifrontal(computedOrdering, function, variableIndex);
} else {
Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
asDerived(), variableIndex);
asDerived(), *variableIndex);
return eliminateMultifrontal(computedOrdering, function, variableIndex);
}
}
@ -131,11 +131,11 @@ namespace gtsam {
if(!variableIndex) {
// If no VariableIndex provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived());
return eliminateMultifrontal(ordering, function, computedVariableIndex);
return eliminateMultifrontal(ordering, function, std::cref(computedVariableIndex));
} else {
gttic(eliminateMultifrontal);
// Do elimination with given ordering
EliminationTreeType etree(asDerived(), *variableIndex, ordering);
EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
JunctionTreeType junctionTree(etree);
boost::shared_ptr<BayesTreeType> bayesTree;
boost::shared_ptr<FactorGraphType> factorGraph;
@ -157,12 +157,12 @@ namespace gtsam {
if(variableIndex) {
gttic(eliminatePartialSequential);
// Do elimination
EliminationTreeType etree(asDerived(), *variableIndex, ordering);
EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
return etree.eliminate(function);
} else {
// If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived());
return eliminatePartialSequential(ordering, function, computedVariableIndex);
return eliminatePartialSequential(ordering, function, std::cref(computedVariableIndex));
}
}
@ -175,7 +175,7 @@ namespace gtsam {
if(variableIndex) {
gttic(eliminatePartialSequential);
// Compute full ordering
Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables);
Ordering fullOrdering = Ordering::ColamdConstrainedFirst((*variableIndex).get(), variables);
// Split off the part of the ordering for the variables being eliminated
Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
@ -183,7 +183,7 @@ namespace gtsam {
} else {
// If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived());
return eliminatePartialSequential(variables, function, computedVariableIndex);
return eliminatePartialSequential(variables, function, std::cref(computedVariableIndex));
}
}
@ -196,13 +196,13 @@ namespace gtsam {
if(variableIndex) {
gttic(eliminatePartialMultifrontal);
// Do elimination
EliminationTreeType etree(asDerived(), *variableIndex, ordering);
EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
JunctionTreeType junctionTree(etree);
return junctionTree.eliminate(function);
} else {
// If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived());
return eliminatePartialMultifrontal(ordering, function, computedVariableIndex);
return eliminatePartialMultifrontal(ordering, function, std::cref(computedVariableIndex));
}
}
@ -215,7 +215,7 @@ namespace gtsam {
if(variableIndex) {
gttic(eliminatePartialMultifrontal);
// Compute full ordering
Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables);
Ordering fullOrdering = Ordering::ColamdConstrainedFirst((*variableIndex).get(), variables);
// Split off the part of the ordering for the variables being eliminated
Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
@ -223,7 +223,7 @@ namespace gtsam {
} else {
// If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived());
return eliminatePartialMultifrontal(variables, function, computedVariableIndex);
return eliminatePartialMultifrontal(variables, function, std::cref(computedVariableIndex));
}
}
@ -237,7 +237,7 @@ namespace gtsam {
if(!variableIndex) {
// If no variable index is provided, compute one and call this function again
VariableIndex index(asDerived());
return marginalMultifrontalBayesNet(variables, function, index);
return marginalMultifrontalBayesNet(variables, function, std::cref(index));
} else {
// No ordering was provided for the marginalized variables, so order them using constrained
// COLAMD.
@ -247,7 +247,7 @@ namespace gtsam {
boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
Ordering totalOrdering =
Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered);
// Split up ordering
const size_t nVars = variablesOrOrdering->size();
@ -255,7 +255,7 @@ namespace gtsam {
Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
// Call this function again with the computed orderings
return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, variableIndex);
}
}
@ -278,7 +278,7 @@ namespace gtsam {
boost::shared_ptr<BayesTreeType> bayesTree;
boost::shared_ptr<FactorGraphType> factorGraph;
boost::tie(bayesTree,factorGraph) =
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex);
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
{
@ -304,7 +304,7 @@ namespace gtsam {
if(!variableIndex) {
// If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived());
return marginalMultifrontalBayesTree(variables, function, computedVariableIndex);
return marginalMultifrontalBayesTree(variables, function, std::cref(computedVariableIndex));
} else {
// No ordering was provided for the marginalized variables, so order them using constrained
// COLAMD.
@ -314,7 +314,7 @@ namespace gtsam {
boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
Ordering totalOrdering =
Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered);
// Split up ordering
const size_t nVars = variablesOrOrdering->size();
@ -322,7 +322,7 @@ namespace gtsam {
Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
// Call this function again with the computed orderings
return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, variableIndex);
}
}
@ -337,7 +337,7 @@ namespace gtsam {
if(!variableIndex) {
// If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived());
return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex);
return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, std::cref(computedVariableIndex));
} else {
gttic(marginalMultifrontalBayesTree);
// An ordering was provided for the marginalized variables, so we can first eliminate them
@ -345,7 +345,7 @@ namespace gtsam {
boost::shared_ptr<BayesTreeType> bayesTree;
boost::shared_ptr<FactorGraphType> factorGraph;
boost::tie(bayesTree,factorGraph) =
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex);
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
{
@ -371,19 +371,19 @@ namespace gtsam {
if(variableIndex)
{
// Compute a total ordering for all variables
Ordering totalOrdering = Ordering::ColamdConstrainedLast(*variableIndex, variables);
Ordering totalOrdering = Ordering::ColamdConstrainedLast((*variableIndex).get(), variables);
// Split out the part for the marginalized variables
Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size());
// Eliminate and return the remaining factor graph
return eliminatePartialMultifrontal(marginalizationOrdering, function, *variableIndex).second;
return eliminatePartialMultifrontal(marginalizationOrdering, function, variableIndex).second;
}
else
{
// If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived());
return marginal(variables, function, computedVariableIndex);
return marginal(variables, function, std::cref(computedVariableIndex));
}
}

44
gtsam/inference/EliminateableFactorGraph.h
View File

@ -19,9 +19,10 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <cstddef>
#include <functional>
#include <optional>
#include <boost/variant.hpp>
#include <boost/optional.hpp>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndex.h>
@ -89,10 +90,11 @@ namespace gtsam {
typedef std::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
/// Typedef for an optional variable index as an argument to elimination functions
typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
/// It is an optional to a constant reference
typedef std::optional<std::reference_wrapper<const VariableIndex>> OptionalVariableIndex;
/// Typedef for an optional ordering type
typedef boost::optional<Ordering::OrderingType> OptionalOrderingType;
typedef std::optional<Ordering::OrderingType> OptionalOrderingType;
/** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not
* provided, the ordering provided by COLAMD will be used.
@ -111,13 +113,13 @@ namespace gtsam {
* \code
* VariableIndex varIndex(graph); // Build variable index
* Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, varIndex, boost::none);
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, varIndex, std::nullopt);
* \endcode
* */
boost::shared_ptr<BayesNetType> eliminateSequential(
OptionalOrderingType orderingType = boost::none,
OptionalOrderingType orderingType = {},
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
OptionalVariableIndex variableIndex = {}) const;
/** Do sequential elimination of all variables to produce a Bayes net.
*
@ -130,13 +132,13 @@ namespace gtsam {
* \code
* VariableIndex varIndex(graph); // Build variable index
* Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(myOrdering, EliminateQR, varIndex, boost::none);
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(myOrdering, EliminateQR, varIndex, std::nullopt);
* \endcode
* */
boost::shared_ptr<BayesNetType> eliminateSequential(
const Ordering& ordering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
OptionalVariableIndex variableIndex = {}) const;
/** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not
* provided, the ordering will be computed using either COLAMD or METIS, dependeing on
@ -151,13 +153,13 @@ namespace gtsam {
* \code
* VariableIndex varIndex(graph); // Build variable index
* Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
* boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, boost::none, varIndex);
* boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, {}, varIndex);
* \endcode
* */
boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
OptionalOrderingType orderingType = boost::none,
OptionalOrderingType orderingType = {},
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
OptionalVariableIndex variableIndex = {}) const;
/** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not
* provided, the ordering will be computed using either COLAMD or METIS, dependeing on
@ -171,7 +173,7 @@ namespace gtsam {
boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
const Ordering& ordering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
OptionalVariableIndex variableIndex = {}) const;
/** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net
* and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$,
@ -181,7 +183,7 @@ namespace gtsam {
eliminatePartialSequential(
const Ordering& ordering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
OptionalVariableIndex variableIndex = {}) const;
/** Do sequential elimination of the given \c variables in an ordering computed by COLAMD to
* produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X)
@ -191,7 +193,7 @@ namespace gtsam {
eliminatePartialSequential(
const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
OptionalVariableIndex variableIndex = {}) const;
/** Do multifrontal elimination of some variables, in \c ordering provided, to produce a Bayes
* tree and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B)
@ -201,7 +203,7 @@ namespace gtsam {
eliminatePartialMultifrontal(
const Ordering& ordering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
OptionalVariableIndex variableIndex = {}) const;
/** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to
* produce a Bayes tree and a remaining factor graph. This computes the factorization \f$ p(X)
@ -211,7 +213,7 @@ namespace gtsam {
eliminatePartialMultifrontal(
const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
OptionalVariableIndex variableIndex = {}) const;
/** Compute the marginal of the requested variables and return the result as a Bayes net. Uses
* COLAMD marginalization ordering by default
@ -225,7 +227,7 @@ namespace gtsam {
boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
OptionalVariableIndex variableIndex = {}) const;
/** Compute the marginal of the requested variables and return the result as a Bayes net.
* @param variables Determines the variables whose marginal to compute, if provided as an
@ -241,7 +243,7 @@ namespace gtsam {
boost::variant<const Ordering&, const KeyVector&> variables,
const Ordering& marginalizedVariableOrdering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
OptionalVariableIndex variableIndex = {}) const;
/** Compute the marginal of the requested variables and return the result as a Bayes tree. Uses
* COLAMD marginalization order by default
@ -255,7 +257,7 @@ namespace gtsam {
boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
OptionalVariableIndex variableIndex = {}) const;
/** Compute the marginal of the requested variables and return the result as a Bayes tree.
* @param variables Determines the variables whose marginal to compute, if provided as an
@ -271,13 +273,13 @@ namespace gtsam {
boost::variant<const Ordering&, const KeyVector&> variables,
const Ordering& marginalizedVariableOrdering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
OptionalVariableIndex variableIndex = {}) const;
/** Compute the marginal factor graph of the requested variables. */
boost::shared_ptr<FactorGraphType> marginal(
const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
OptionalVariableIndex variableIndex = {}) const;
private:

2
gtsam/inference/FactorGraph-inst.h
View File

@ -155,7 +155,7 @@ void FactorGraph<FACTOR>::dot(std::ostream& os,
const auto& factor = at(i);
if (factor) {
const KeyVector& factorKeys = factor->keys();
writer.processFactor(i, factorKeys, keyFormatter, boost::none, &os);
writer.processFactor(i, factorKeys, keyFormatter, {}, &os);
}
}

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