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(); Values currentEstimate = isam.calculateEstimate();
currentEstimate.print("Current estimate: "); currentEstimate.print("Current estimate: ");
boost::optional<Point3> pointEstimate = smartFactor->point(currentEstimate); auto pointEstimate = smartFactor->point(currentEstimate);
if (pointEstimate) { if (pointEstimate) {
cout << *pointEstimate << endl; cout << *pointEstimate << endl;
} else { } 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 // 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]); SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) { 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. // the triangulation operation inside smart factor will encounter degeneracy.
boost::optional<Point3> point = smart->point(result); auto point = smart->point(result);
if (point) // ignore if boost::optional return nullptr if (point) // ignore if std::optional return nullptr
landmark_result.insert(j, *point); 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) { for (size_t j = 0; j < points.size(); ++j) {
auto smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]); auto smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) { if (smart) {
boost::optional<Point3> point = smart->point(result); std::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return nullptr if (point) // ignore if std::optional return nullptr
landmark_result.insert(j, *point); landmark_result.insert(j, *point);
} }
} }

13
examples/TriangulationLOSTExample.cpp
View File

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

30
gtsam/base/Lie.h
View File

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

12
gtsam/base/Matrix.h
View File

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

51
gtsam/base/OptionalJacobian.h
View File

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

14
gtsam/base/ProductLieGroup.h
View File

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

26
gtsam/base/TestableAssertions.h
View File

@ -20,7 +20,8 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/global_includes.h> #include <gtsam/global_includes.h>
#include <boost/optional.hpp> #include <functional>
#include <optional>
#include <map> #include <map>
#include <iostream> #include <iostream>
#include <sstream> #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 * 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 * Concept requirement: V is testable
*/ */
template<class V> template<class V>
bool assert_equal(const boost::optional<V>& expected, bool assert_equal(const std::optional<V>& expected,
const boost::optional<V>& actual, double tol = 1e-9) { const std::optional<V>& actual, double tol = 1e-9) {
if (!expected && actual) { if (!expected && actual) {
std::cout << "expected is boost::none, while actual is not" << std::endl; std::cout << "expected is {}, while actual is not" << std::endl;
return false; return false;
} }
if (expected && !actual) { if (expected && !actual) {
std::cout << "actual is boost::none, while expected is not" << std::endl; std::cout << "actual is {}, while expected is not" << std::endl;
return false; return false;
} }
if (!expected && !actual) if (!expected && !actual)
@ -63,21 +64,22 @@ bool assert_equal(const boost::optional<V>& expected,
} }
template<class V> 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) { if (!actual) {
std::cout << "actual is boost::none" << std::endl; std::cout << "actual is {}" << std::endl;
return false; return false;
} }
return assert_equal(expected, *actual, tol); return assert_equal(expected, *actual, tol);
} }
template<class V> 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) { if (!actual) {
std::cout << "actual is boost::none" << std::endl; std::cout << "actual is std::nullopt" << std::endl;
return false; 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 <gtsam/config.h> // for GTSAM_USE_TBB
#include <boost/optional/optional.hpp>
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#include <string> #include <string>
#include <typeinfo> #include <typeinfo>
#include <exception> #include <exception>
#include <optional>
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
#include <tbb/tbb_allocator.h> #include <tbb/tbb_allocator.h>
@ -53,7 +53,7 @@ protected:
protected: protected:
bool dynamic_; ///< Whether this object was moved 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 /// Default constructor is protected - may only be created from derived classes
ThreadsafeException() : ThreadsafeException() :

1
gtsam/base/Value.h
View File

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

1
gtsam/base/Vector.cpp
View File

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

60
gtsam/base/VectorSpace.h
View File

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

4
gtsam/base/numericalDerivative.h
View File

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

1
gtsam/base/serializationTestHelpers.h
View File

@ -24,6 +24,7 @@
#include <string> #include <string>
#include <gtsam/base/serialization.h> #include <gtsam/base/serialization.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/serialization/serialization.hpp> #include <boost/serialization/serialization.hpp>
#include <boost/filesystem.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 <boost/tuple/tuple.hpp>
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>
#include <optional>
#include <functional>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -1176,6 +1178,17 @@ TEST(Matrix, AbsoluteError) {
EXPECT(isEqual); 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() { int main() {
TestResult tr; TestResult tr;

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

@ -20,6 +20,9 @@
#include <gtsam/base/OptionalJacobian.h> #include <gtsam/base/OptionalJacobian.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <optional>
#include <functional>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -32,7 +35,7 @@ using namespace gtsam;
TEST( OptionalJacobian, Constructors ) { TEST( OptionalJacobian, Constructors ) {
Matrix23 fixed; Matrix23 fixed;
Matrix dynamic; Matrix dynamic;
boost::optional<Matrix&> optional(dynamic); std::optional<std::reference_wrapper<Matrix>> optionalRef(std::ref(dynamic));
OptionalJacobian<2, 3> H; OptionalJacobian<2, 3> H;
EXPECT(!H); EXPECT(!H);
@ -41,22 +44,23 @@ TEST( OptionalJacobian, Constructors ) {
TEST_CONSTRUCTOR(2, 3, &fixed, true); TEST_CONSTRUCTOR(2, 3, &fixed, true);
TEST_CONSTRUCTOR(2, 3, dynamic, true); TEST_CONSTRUCTOR(2, 3, dynamic, true);
TEST_CONSTRUCTOR(2, 3, &dynamic, true); TEST_CONSTRUCTOR(2, 3, &dynamic, true);
TEST_CONSTRUCTOR(2, 3, boost::none, false); TEST_CONSTRUCTOR(2, 3, std::nullopt, false);
TEST_CONSTRUCTOR(2, 3, optional, true); TEST_CONSTRUCTOR(2, 3, optionalRef, true);
// Test dynamic // Test dynamic
OptionalJacobian<-1, -1> H7; OptionalJacobian<-1, -1> H7;
EXPECT(!H7); EXPECT(!H7);
TEST_CONSTRUCTOR(-1, -1, dynamic, true); TEST_CONSTRUCTOR(-1, -1, dynamic, true);
TEST_CONSTRUCTOR(-1, -1, boost::none, false); TEST_CONSTRUCTOR(-1, -1, std::nullopt, false);
TEST_CONSTRUCTOR(-1, -1, optional, true); TEST_CONSTRUCTOR(-1, -1, optionalRef, true);
} }
//****************************************************************************** //******************************************************************************
Matrix kTestMatrix = (Matrix23() << 11,12,13,21,22,23).finished(); Matrix kTestMatrix = (Matrix23() << 11,12,13,21,22,23).finished();
void test(OptionalJacobian<2, 3> H = boost::none) { void test(OptionalJacobian<2, 3> H = {}) {
if (H) if (H)
*H = kTestMatrix; *H = kTestMatrix;
} }
@ -116,7 +120,7 @@ TEST( OptionalJacobian, Fixed) {
} }
//****************************************************************************** //******************************************************************************
void test2(OptionalJacobian<-1,-1> H = boost::none) { void test2(OptionalJacobian<-1,-1> H = {}) {
if (H) if (H)
*H = kTestMatrix; // resizes *H = kTestMatrix; // resizes
} }
@ -145,12 +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; if (H) *H << add + 10, add + 20;
} }
// This function calls the above function three times, one for each column // This function calls the above function three times, one for each column
void test4(OptionalJacobian<2, 3> H = boost::none) { void test4(OptionalJacobian<2, 3> H = {}) {
if (H) { if (H) {
test3(1, H.cols<1>(0)); test3(1, H.cols<1>(0));
test3(2, H.cols<1>(1)); test3(2, H.cols<1>(1));

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

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

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

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

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

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

10
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -70,8 +70,8 @@ template<> struct EliminationTraits<DiscreteFactorGraph>
/// The default ordering generation function /// The default ordering generation function
static Ordering DefaultOrderingFunc( static Ordering DefaultOrderingFunc(
const FactorGraphType& graph, const FactorGraphType& graph,
boost::optional<const VariableIndex&> variableIndex) { std::optional<std::reference_wrapper<const VariableIndex>> variableIndex) {
return Ordering::Colamd(*variableIndex); return Ordering::Colamd((*variableIndex).get());
} }
}; };
@ -155,7 +155,7 @@ class GTSAM_EXPORT DiscreteFactorGraph
* @return DiscreteBayesNet encoding posterior P(X|Z) * @return DiscreteBayesNet encoding posterior P(X|Z)
*/ */
DiscreteBayesNet sumProduct( DiscreteBayesNet sumProduct(
OptionalOrderingType orderingType = boost::none) const; OptionalOrderingType orderingType = {}) const;
/** /**
* @brief Implement the sum-product algorithm * @brief Implement the sum-product algorithm
@ -172,7 +172,7 @@ class GTSAM_EXPORT DiscreteFactorGraph
* @return DiscreteLookupDAG DAG with lookup tables * @return DiscreteLookupDAG DAG with lookup tables
*/ */
DiscreteLookupDAG maxProduct( DiscreteLookupDAG maxProduct(
OptionalOrderingType orderingType = boost::none) const; OptionalOrderingType orderingType = {}) const;
/** /**
* @brief Implement the max-product algorithm * @brief Implement the max-product algorithm
@ -189,7 +189,7 @@ class GTSAM_EXPORT DiscreteFactorGraph
* @return DiscreteValues : MPE * @return DiscreteValues : MPE
*/ */
DiscreteValues optimize( DiscreteValues optimize(
OptionalOrderingType orderingType = boost::none) const; OptionalOrderingType orderingType = {}) const;
/** /**
* @brief Find the maximum probable explanation (MPE) by doing max-product. * @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) { Signature& Signature::operator=(const string& spec) {
spec_.reset(spec); spec_ = spec;
Table table; Table table;
parser::It f = spec.begin(), l = spec.end(); parser::It f = spec.begin(), l = spec.end();
bool success = bool success =
qi::phrase_parse(f, l, parser::grammar.table, qi::space, table); qi::phrase_parse(f, l, parser::grammar.table, qi::space, table);
if (success) { if (success) {
for (Row& row : table) normalize(row); for (Row& row : table) normalize(row);
table_.reset(table); table_ = table;
} }
return *this; return *this;
} }
@ -153,7 +153,7 @@ namespace gtsam {
Table table = t; Table table = t;
for(Row& row: table) for(Row& row: table)
normalize(row); normalize(row);
table_.reset(table); table_ = table;
return *this; return *this;
} }

8
gtsam/discrete/Signature.h
View File

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

12
gtsam/geometry/BearingRange.h
View File

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

4
gtsam/geometry/Cal3.h
View File

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

4
gtsam/geometry/Cal3Bundler.cpp
View File

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

8
gtsam/geometry/Cal3Bundler.h
View File

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

8
gtsam/geometry/Cal3DS2_Base.h
View File

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

2
gtsam/geometry/Cal3Fisheye.cpp
View File

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

8
gtsam/geometry/Cal3Fisheye.h
View File

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

8
gtsam/geometry/Cal3Unified.h
View File

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

12
gtsam/geometry/Cal3_S2.h
View File

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

8
gtsam/geometry/Cal3_S2Stereo.h
View File

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

2
gtsam/geometry/CalibratedCamera.cpp
View File

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

36
gtsam/geometry/CalibratedCamera.h
View File

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

14
gtsam/geometry/EssentialMatrix.h
View File

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

14
gtsam/geometry/Line3.h
View File

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

14
gtsam/geometry/OrientedPlane3.h
View File

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

22
gtsam/geometry/PinholeCamera.h
View File

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

48
gtsam/geometry/PinholePose.h
View File

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

1
gtsam/geometry/PinholeSet.h
View File

@ -19,7 +19,6 @@
#include <gtsam/geometry/CameraSet.h> #include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/triangulation.h> #include <gtsam/geometry/triangulation.h>
#include <boost/optional.hpp>
namespace gtsam { 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/ // 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 tol) {
double R2_d2 = R_d*R_d; // Yes, RD-D2 ! 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)) // h^2<0 is equivalent to (d > (R + r) || d < (R - r))
// Hence, there are only solutions if >=0 // 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 if (h2<tol) return Point2(f,0.0); // one solution
else return Point2(f,std::sqrt(h2)); // two solutions else return Point2(f,std::sqrt(h2)); // two solutions
} }
/* ************************************************************************* */ /* ************************************************************************* */
list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, list<Point2> circleCircleIntersection(Point2 c1, Point2 c2,
boost::optional<Point2> fh) { std::optional<Point2> fh) {
list<Point2> solutions; 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) { if (fh) {
// vector between circle centers // vector between circle centers
Point2 c12 = c2-c1; Point2 c12 = c2-c1;
@ -107,7 +107,7 @@ list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
// Calculate f and h given normalized radii // Calculate f and h given normalized radii
double _d = 1.0/d, R_d = r1*_d, r_d=r2*_d; 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 // Call version that takes fh
return circleCircleIntersection(c1, c2, fh); return circleCircleIntersection(c1, c2, fh);

21
gtsam/geometry/Point2.h
View File

@ -18,8 +18,11 @@
#pragma once #pragma once
#include <gtsam/base/VectorSpace.h> #include <gtsam/base/VectorSpace.h>
#include <gtsam/base/std_optional_serialization.h>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <optional>
namespace gtsam { namespace gtsam {
/// As of GTSAM 4, in order to make GTSAM more lean, /// 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>; using Point2Pairs = std::vector<Point2Pair>;
/// Distance of the point from the origin, with Jacobian /// Distance of the point from the origin, with Jacobian
GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = {});
/// distance between two points /// distance between two points
GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q, GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
OptionalJacobian<1, 2> H1 = boost::none, OptionalJacobian<1, 2> H1 = {},
OptionalJacobian<1, 2> H2 = boost::none); OptionalJacobian<1, 2> H2 = {});
// For MATLAB wrapper // For MATLAB wrapper
typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector; typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
@ -53,16 +56,16 @@ inline Point2 operator*(double s, const Point2& p) {
* Calculate f and h, respectively the parallel and perpendicular distance of * Calculate f and h, respectively the parallel and perpendicular distance of
* the intersections of two circles along and from the line connecting the centers. * the intersections of two circles along and from the line connecting the centers.
* Both are dimensionless fractions of the distance d between the circle centers. * Both are dimensionless fractions of the distance d between the circle centers.
* If the circles do not intersect or they are identical, returns boost::none. * If the circles do not intersect or they are identical, returns {}.
* If one solution (touching circles, as determined by tol), h will be exactly zero. * If one solution (touching circles, as determined by tol), h will be exactly zero.
* h is a good measure for how accurate the intersection will be, as when circles touch * h is a good measure for how accurate the intersection will be, as when circles touch
* or nearly touch, the intersection is ill-defined with noisy radius measurements. * or nearly touch, the intersection is ill-defined with noisy radius measurements.
* @param R_d : R/d, ratio of radius of first circle to distance between centers * @param R_d : R/d, ratio of radius of first circle to distance between centers
* @param r_d : r/d, ratio of radius of second circle to distance between centers * @param r_d : r/d, ratio of radius of second circle to distance between centers
* @param tol: absolute tolerance below which we consider touching circles * @param tol: absolute tolerance below which we consider touching circles
* @return optional Point2 with f and h, boost::none if no solution. * @return optional Point2 with f and h, {} if no solution.
*/ */
GTSAM_EXPORT 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. * @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 * @param c2 center of second circle
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. * @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 /// Calculate the two means of a set of Point2 pairs
GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs); GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs);
@ -94,8 +97,8 @@ template <>
struct Range<Point2, Point2> { struct Range<Point2, Point2> {
typedef double result_type; typedef double result_type;
double operator()(const Point2& p, const Point2& q, double operator()(const Point2& p, const Point2& q,
OptionalJacobian<1, 2> H1 = boost::none, OptionalJacobian<1, 2> H1 = {},
OptionalJacobian<1, 2> H2 = boost::none) { OptionalJacobian<1, 2> H2 = {}) {
return distance2(p, q, H1, H2); return distance2(p, q, H1, H2);
} }
}; };

20
gtsam/geometry/Point3.h
View File

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

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 * 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(); const size_t n = ab_pairs.size();
if (n < 2) { if (n < 2) {
return boost::none; // we need at least 2 pairs return {}; // we need at least 2 pairs
} }
// calculate centroids // calculate centroids
@ -359,7 +359,7 @@ boost::optional<Pose2> Pose2::Align(const Point2Pairs &ab_pairs) {
return Pose2(R, t); 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()) { if (a.rows() != 2 || b.rows() != 2 || a.cols() != b.cols()) {
throw std::invalid_argument( throw std::invalid_argument(
"Pose2:Align expects 2*N matrices of equal shape."); "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/geometry/Rot2.h>
#include <gtsam/base/Lie.h> #include <gtsam/base/Lie.h>
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#include <gtsam/base/std_optional_serialization.h>
#include <optional>
namespace gtsam { namespace gtsam {
@ -99,10 +102,10 @@ public:
* Note this allows for noise on the points but in that case the mapping * Note this allows for noise on the points but in that case the mapping
* will not be exact. * 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. // 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 /// @name Testable
@ -134,10 +137,10 @@ public:
/// @{ /// @{
///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ ///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 ///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 * Calculate Adjoint map
@ -196,8 +199,8 @@ public:
// Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
struct ChartAtOrigin { struct ChartAtOrigin {
GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none); GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none); GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {});
}; };
using LieGroup<Pose2, 3>::inverse; // version with derivative using LieGroup<Pose2, 3>::inverse; // version with derivative
@ -208,8 +211,8 @@ public:
/** Return point coordinates in pose coordinate frame */ /** Return point coordinates in pose coordinate frame */
GTSAM_EXPORT Point2 transformTo(const Point2& point, GTSAM_EXPORT Point2 transformTo(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none, OptionalJacobian<2, 3> Dpose = {},
OptionalJacobian<2, 2> Dpoint = boost::none) const; OptionalJacobian<2, 2> Dpoint = {}) const;
/** /**
* @brief transform many points in world coordinates and transform to Pose. * @brief transform many points in world coordinates and transform to Pose.
@ -220,8 +223,8 @@ public:
/** Return point coordinates in global frame */ /** Return point coordinates in global frame */
GTSAM_EXPORT Point2 transformFrom(const Point2& point, GTSAM_EXPORT Point2 transformFrom(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none, OptionalJacobian<2, 3> Dpose = {},
OptionalJacobian<2, 2> Dpoint = boost::none) const; OptionalJacobian<2, 2> Dpoint = {}) const;
/** /**
* @brief transform many points in Pose coordinates and transform to world. * @brief transform many points in Pose coordinates and transform to world.
@ -269,7 +272,7 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$ * @return 2D rotation \f$ \in SO(2) \f$
*/ */
GTSAM_EXPORT Rot2 bearing(const Point2& point, 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 * Calculate bearing to another pose
@ -277,7 +280,7 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$ * @return 2D rotation \f$ \in SO(2) \f$
*/ */
GTSAM_EXPORT Rot2 bearing(const Pose2& pose, 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 * Calculate range to a landmark
@ -285,8 +288,8 @@ public:
* @return range (double) * @return range (double)
*/ */
GTSAM_EXPORT double range(const Point2& point, GTSAM_EXPORT double range(const Point2& point,
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H1={},
OptionalJacobian<1, 2> H2=boost::none) const; OptionalJacobian<1, 2> H2={}) const;
/** /**
* Calculate range to another pose * Calculate range to another pose
@ -294,8 +297,8 @@ public:
* @return range (double) * @return range (double)
*/ */
GTSAM_EXPORT double range(const Pose2& point, GTSAM_EXPORT double range(const Pose2& point,
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H1={},
OptionalJacobian<1, 3> H2=boost::none) const; OptionalJacobian<1, 3> H2={}) const;
/// @} /// @}
/// @name Advanced Interface /// @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(); Hpose->setZero();
return bearing(pose.translation(), Hself, Hpose.cols<3>(3)); 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(); const size_t n = abPointPairs.size();
if (n < 3) { if (n < 3) {
return boost::none; // we need at least three pairs return {}; // we need at least three pairs
} }
// calculate centroids // calculate centroids
@ -466,7 +466,7 @@ boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
return Pose3(aRb, aTb); 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()) { if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) {
throw std::invalid_argument( throw std::invalid_argument(
"Pose3:Align expects 3*N matrices of equal shape."); "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 /// Named constructor with derivatives
static Pose3 Create(const Rot3& R, const Point3& t, static Pose3 Create(const Rot3& R, const Point3& t,
OptionalJacobian<6, 3> HR = boost::none, OptionalJacobian<6, 3> HR = {},
OptionalJacobian<6, 3> Ht = boost::none); OptionalJacobian<6, 3> Ht = {});
/** /**
* Create Pose3 by aligning two point pairs * 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 * 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. * 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. // 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 /// @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$ /// 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 /// 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 * 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() * Note that H_xib = AdjointMap()
*/ */
Vector6 Adjoint(const Vector6& xi_b, Vector6 Adjoint(const Vector6& xi_b,
OptionalJacobian<6, 6> H_this = boost::none, OptionalJacobian<6, 6> H_this = {},
OptionalJacobian<6, 6> H_xib = boost::none) const; OptionalJacobian<6, 6> H_xib = {}) const;
/// The dual version of Adjoint /// The dual version of Adjoint
Vector6 AdjointTranspose(const Vector6& x, Vector6 AdjointTranspose(const Vector6& x,
OptionalJacobian<6, 6> H_this = boost::none, OptionalJacobian<6, 6> H_this = {},
OptionalJacobian<6, 6> H_x = boost::none) const; OptionalJacobian<6, 6> H_x = {}) const;
/** /**
* Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 * 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 * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/ */
static Vector6 adjoint(const Vector6& xi, const Vector6& y, static Vector6 adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none, OptionalJacobian<6, 6> Hxi = {},
OptionalJacobian<6, 6> H_y = boost::none); OptionalJacobian<6, 6> H_y = {});
// temporary fix for wrappers until case issue is resolved // temporary fix for wrappers until case issue is resolved
static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} 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. * 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, static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none, OptionalJacobian<6, 6> Hxi = {},
OptionalJacobian<6, 6> H_y = boost::none); OptionalJacobian<6, 6> H_y = {});
/// Derivative of Expmap /// Derivative of Expmap
static Matrix6 ExpmapDerivative(const Vector6& xi); static Matrix6 ExpmapDerivative(const Vector6& xi);
@ -208,8 +208,8 @@ public:
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
struct ChartAtOrigin { struct ChartAtOrigin {
static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none); static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {});
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {});
}; };
/** /**
@ -250,7 +250,7 @@ public:
* @return point in world coordinates * @return point in world coordinates
*/ */
Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself = 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. * @brief transform many points in Pose coordinates and transform to world.
@ -272,7 +272,7 @@ public:
* @return point in Pose coordinates * @return point in Pose coordinates
*/ */
Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself = 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. * @brief transform many points in world coordinates and transform to Pose.
@ -286,10 +286,10 @@ public:
/// @{ /// @{
/// get rotation /// get rotation
const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const; const Rot3& rotation(OptionalJacobian<3, 6> Hself = {}) const;
/// get translation /// get translation
const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const; const Point3& translation(OptionalJacobian<3, 6> Hself = {}) const;
/// get x /// get x
double x() const { double x() const {
@ -314,39 +314,39 @@ public:
* and transforms it to world coordinates wTb = wTa * aTb. * and transforms it to world coordinates wTb = wTa * aTb.
* This is identical to compose. * This is identical to compose.
*/ */
Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none, Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = {},
OptionalJacobian<6, 6> HaTb = boost::none) const; OptionalJacobian<6, 6> HaTb = {}) const;
/** /**
* Assuming self == wTa, takes a pose wTb in world coordinates * Assuming self == wTa, takes a pose wTb in world coordinates
* and transforms it to local coordinates aTb = inv(wTa) * wTb * and transforms it to local coordinates aTb = inv(wTa) * wTb
*/ */
Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none, Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = {},
OptionalJacobian<6, 6> HwTb = boost::none) const; OptionalJacobian<6, 6> HwTb = {}) const;
/** /**
* Calculate range to a landmark * Calculate range to a landmark
* @param point 3D location of landmark * @param point 3D location of landmark
* @return range (double) * @return range (double)
*/ */
double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none, double range(const Point3& point, OptionalJacobian<1, 6> Hself = {},
OptionalJacobian<1, 3> Hpoint = boost::none) const; OptionalJacobian<1, 3> Hpoint = {}) const;
/** /**
* Calculate range to another pose * Calculate range to another pose
* @param pose Other SO(3) pose * @param pose Other SO(3) pose
* @return range (double) * @return range (double)
*/ */
double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none, double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = {},
OptionalJacobian<1, 6> Hpose = boost::none) const; OptionalJacobian<1, 6> Hpose = {}) const;
/** /**
* Calculate bearing to a landmark * Calculate bearing to a landmark
* @param point 3D location of landmark * @param point 3D location of landmark
* @return bearing (Unit3) * @return bearing (Unit3)
*/ */
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none, Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = {},
OptionalJacobian<2, 3> Hpoint = boost::none) const; OptionalJacobian<2, 3> Hpoint = {}) const;
/** /**
* Calculate bearing to another pose * Calculate bearing to another pose
@ -354,8 +354,8 @@ public:
* information is ignored. * information is ignored.
* @return bearing (Unit3) * @return bearing (Unit3)
*/ */
Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none, Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = {},
OptionalJacobian<2, 6> Hpose = boost::none) const; OptionalJacobian<2, 6> Hpose = {}) const;
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface
@ -384,8 +384,8 @@ public:
* @param s a value between 0 and 1.5 * @param s a value between 0 and 1.5
* @param other final point of interpolation geodesic on manifold * @param other final point of interpolation geodesic on manifold
*/ */
Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = {},
OptionalJacobian<6, 6> Hy = boost::none) const; OptionalJacobian<6, 6> Hy = {}) const;
/// Output stream operator /// Output stream operator
GTSAM_EXPORT GTSAM_EXPORT

14
gtsam/geometry/Quaternion.h
View File

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

19
gtsam/geometry/Rot2.h
View File

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

56
gtsam/geometry/Rot3.h
View File

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

10
gtsam/geometry/SO3.h
View File

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

4
gtsam/geometry/SO4.h
View File

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

10
gtsam/geometry/SOn.h
View File

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

8
gtsam/geometry/Similarity2.h
View File

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

12
gtsam/geometry/Similarity3.h
View File

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

2
gtsam/geometry/SphericalCamera.cpp
View File

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

16
gtsam/geometry/SphericalCamera.h
View File

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

10
gtsam/geometry/StereoCamera.h
View File

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

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; jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
// Cache the result and jacobian // Cache the result and jacobian
H_B_.reset(jacobian); H_B_ = (jacobian);
B_.reset(B); B_ = (B);
} }
// Return cached jacobian, possibly computed just above // 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); const Point3 B1 = gtsam::cross(n, axis);
B.col(0) = normalize(B1); B.col(0) = normalize(B1);
B.col(1) = gtsam::cross(n, B.col(0)); B.col(1) = gtsam::cross(n, B.col(0));
B_.reset(B); B_ = (B);
} }
return *B_; return *B_;

27
gtsam/geometry/Unit3.h
View File

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

1
gtsam/geometry/concepts.h
View File

@ -25,7 +25,6 @@
#pragma once #pragma once
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <boost/optional.hpp>
namespace gtsam { 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(const gtsam::Rot2& r, const gtsam::Point2& t);
Pose2(Vector v); Pose2(Vector v);
static boost::optional<gtsam::Pose2> Align(const gtsam::Point2Pairs& abPointPairs); static std::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::Matrix& a, const gtsam::Matrix& b);
// Testable // Testable
void print(string s = "") const; void print(string s = "") const;
@ -440,8 +440,8 @@ class Pose3 {
Pose3(const gtsam::Pose2& pose2); Pose3(const gtsam::Pose2& pose2);
Pose3(Matrix mat); Pose3(Matrix mat);
static boost::optional<gtsam::Pose3> Align(const gtsam::Point3Pairs& abPointPairs); static std::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::Matrix& a, const gtsam::Matrix& b);
// Testable // Testable
void print(string s = "") const; 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) { TEST(Cal3DS2, Duncalibrate1) {
Matrix computed; Matrix computed;
K.uncalibrate(p, computed, boost::none); K.uncalibrate(p, computed, {});
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical, computed, 1e-5)); CHECK(assert_equal(numerical, computed, 1e-5));
Matrix separate = K.D2d_calibration(p); Matrix separate = K.D2d_calibration(p);
@ -67,7 +67,7 @@ TEST(Cal3DS2, Duncalibrate1) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3DS2, Duncalibrate2) { TEST(Cal3DS2, Duncalibrate2) {
Matrix computed; Matrix computed;
K.uncalibrate(p, boost::none, computed); K.uncalibrate(p, {}, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical, computed, 1e-5)); CHECK(assert_equal(numerical, computed, 1e-5));
Matrix separate = K.D2d_intrinsic(p); Matrix separate = K.D2d_intrinsic(p);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -23,7 +23,7 @@
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Rot2.h> #include <gtsam/geometry/Rot2.h>
#include <boost/optional.hpp> #include <optional>
#include <cmath> #include <cmath>
#include <iostream> #include <iostream>
@ -228,7 +228,7 @@ TEST( Pose2, ExpmapDerivative1) {
Vector3 w(0.1, 0.27, -0.3); Vector3 w(0.1, 0.27, -0.3);
Pose2::Expmap(w,actualH); Pose2::Expmap(w,actualH);
Matrix3 expectedH = numericalDerivative21<Pose2, Vector3, Matrix3 expectedH = numericalDerivative21<Pose2, Vector3,
OptionalJacobian<3, 3> >(&Pose2::Expmap, w, boost::none, 1e-2); OptionalJacobian<3, 3> >(&Pose2::Expmap, w, {}, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
@ -238,7 +238,7 @@ TEST( Pose2, ExpmapDerivative2) {
Vector3 w0(0.1, 0.27, 0.0); // alpha = 0 Vector3 w0(0.1, 0.27, 0.0); // alpha = 0
Pose2::Expmap(w0,actualH); Pose2::Expmap(w0,actualH);
Matrix3 expectedH = numericalDerivative21<Pose2, Vector3, Matrix3 expectedH = numericalDerivative21<Pose2, Vector3,
OptionalJacobian<3, 3> >(&Pose2::Expmap, w0, boost::none, 1e-2); OptionalJacobian<3, 3> >(&Pose2::Expmap, w0, {}, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
@ -249,7 +249,7 @@ TEST( Pose2, LogmapDerivative1) {
Pose2 p = Pose2::Expmap(w); Pose2 p = Pose2::Expmap(w);
EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5)); EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5));
Matrix3 expectedH = numericalDerivative21<Vector3, Pose2, Matrix3 expectedH = numericalDerivative21<Vector3, Pose2,
OptionalJacobian<3, 3> >(&Pose2::Logmap, p, boost::none, 1e-2); OptionalJacobian<3, 3> >(&Pose2::Logmap, p, {}, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
@ -260,7 +260,7 @@ TEST( Pose2, LogmapDerivative2) {
Pose2 p = Pose2::Expmap(w0); Pose2 p = Pose2::Expmap(w0);
EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5)); EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5));
Matrix3 expectedH = numericalDerivative21<Vector3, Pose2, Matrix3 expectedH = numericalDerivative21<Vector3, Pose2,
OptionalJacobian<3, 3> >(&Pose2::Logmap, p, boost::none, 1e-2); OptionalJacobian<3, 3> >(&Pose2::Logmap, p, {}, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
@ -718,7 +718,7 @@ TEST(Pose2, align_1) {
Pose2 expected(Rot2::fromAngle(0), Point2(10, 10)); Pose2 expected(Rot2::fromAngle(0), Point2(10, 10));
Point2Pairs ab_pairs {{Point2(10, 10), Point2(0, 0)}, Point2Pairs ab_pairs {{Point2(10, 10), Point2(0, 0)},
{Point2(30, 20), Point2(20, 10)}}; {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)); EXPECT(assert_equal(expected, *aTb));
} }
@ -731,7 +731,7 @@ TEST(Pose2, align_2) {
Point2Pairs ab_pairs {{expected.transformFrom(b1), b1}, Point2Pairs ab_pairs {{expected.transformFrom(b1), b1},
{expected.transformFrom(b2), b2}}; {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)); EXPECT(assert_equal(expected, *aTb));
} }
@ -752,7 +752,7 @@ TEST(Pose2, align_3) {
Point2Pair ab3(make_pair(a3, b3)); Point2Pair ab3(make_pair(a3, b3));
const Point2Pairs ab_pairs{ab1, ab2, ab3}; 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)); EXPECT(assert_equal(expected, *aTb));
} }
@ -762,7 +762,7 @@ namespace {
/* ************************************************************************* */ /* ************************************************************************* */
struct Triangle { size_t i_, j_, k_;}; 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 pair<Triangle, Triangle>& trianglePair) {
const Triangle& t1 = trianglePair.first, t2 = trianglePair.second; const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
Point2Pairs ab_pairs = {{as[t1.i_], bs[t2.i_]}, 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 t1; t1.i_=0; t1.j_=1; t1.k_=2;
Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0; 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)); EXPECT(assert_equal(expected, *actual));
} }

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

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

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

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

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

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

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

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

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

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

13
gtsam/geometry/triangulation.h
View File

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

5
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

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

10
gtsam/hybrid/HybridGaussianISAM.cpp
View File

@ -71,8 +71,8 @@ Ordering HybridGaussianISAM::GetOrdering(
void HybridGaussianISAM::updateInternal( void HybridGaussianISAM::updateInternal(
const HybridGaussianFactorGraph& newFactors, const HybridGaussianFactorGraph& newFactors,
HybridBayesTree::Cliques* orphans, HybridBayesTree::Cliques* orphans,
const boost::optional<size_t>& maxNrLeaves, const std::optional<size_t>& maxNrLeaves,
const boost::optional<Ordering>& ordering, const std::optional<Ordering>& ordering,
const HybridBayesTree::Eliminate& function) { const HybridBayesTree::Eliminate& function) {
// Remove the contaminated part of the Bayes tree // Remove the contaminated part of the Bayes tree
BayesNetType bn; BayesNetType bn;
@ -102,7 +102,7 @@ void HybridGaussianISAM::updateInternal(
// eliminate all factors (top, added, orphans) into a new Bayes tree // eliminate all factors (top, added, orphans) into a new Bayes tree
HybridBayesTree::shared_ptr bayesTree = HybridBayesTree::shared_ptr bayesTree =
factors.eliminateMultifrontal(elimination_ordering, function, index); factors.eliminateMultifrontal(elimination_ordering, function, std::cref(index));
if (maxNrLeaves) { if (maxNrLeaves) {
bayesTree->prune(*maxNrLeaves); bayesTree->prune(*maxNrLeaves);
@ -116,8 +116,8 @@ void HybridGaussianISAM::updateInternal(
/* ************************************************************************* */ /* ************************************************************************* */
void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors, void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors,
const boost::optional<size_t>& maxNrLeaves, const std::optional<size_t>& maxNrLeaves,
const boost::optional<Ordering>& ordering, const std::optional<Ordering>& ordering,
const HybridBayesTree::Eliminate& function) { const HybridBayesTree::Eliminate& function) {
Cliques orphans; Cliques orphans;
this->updateInternal(newFactors, &orphans, maxNrLeaves, ordering, function); 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/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/ISAM.h> #include <gtsam/inference/ISAM.h>
#include <optional>
namespace gtsam { namespace gtsam {
/** /**
@ -53,8 +55,8 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
void updateInternal( void updateInternal(
const HybridGaussianFactorGraph& newFactors, const HybridGaussianFactorGraph& newFactors,
HybridBayesTree::Cliques* orphans, HybridBayesTree::Cliques* orphans,
const boost::optional<size_t>& maxNrLeaves = boost::none, const std::optional<size_t>& maxNrLeaves = {},
const boost::optional<Ordering>& ordering = boost::none, const std::optional<Ordering>& ordering = {},
const HybridBayesTree::Eliminate& function = const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate); HybridBayesTree::EliminationTraitsType::DefaultEliminate);
@ -68,8 +70,8 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
* @param function Elimination function. * @param function Elimination function.
*/ */
void update(const HybridGaussianFactorGraph& newFactors, void update(const HybridGaussianFactorGraph& newFactors,
const boost::optional<size_t>& maxNrLeaves = boost::none, const std::optional<size_t>& maxNrLeaves = {},
const boost::optional<Ordering>& ordering = boost::none, const std::optional<Ordering>& ordering = {},
const HybridBayesTree::Eliminate& function = const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate); 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, void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors,
const Values& initialValues, const Values& initialValues,
const boost::optional<size_t>& maxNrLeaves, const std::optional<size_t>& maxNrLeaves,
const boost::optional<Ordering>& ordering) { const std::optional<Ordering>& ordering) {
if (newFactors.size() > 0) { if (newFactors.size() > 0) {
// Reorder and relinearize every reorderInterval updates // Reorder and relinearize every reorderInterval updates
if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
@ -70,7 +70,7 @@ void HybridNonlinearISAM::reorder_relinearize() {
// Just recreate the whole BayesTree // Just recreate the whole BayesTree
// TODO: allow for constrained ordering here // TODO: allow for constrained ordering here
// TODO: decouple relinearization and reordering to avoid // TODO: decouple relinearization and reordering to avoid
isam_.update(*factors_.linearize(newLinPoint), boost::none, boost::none, isam_.update(*factors_.linearize(newLinPoint), {}, {},
eliminationFunction_); eliminationFunction_);
// Update linearization point // Update linearization point

5
gtsam/hybrid/HybridNonlinearISAM.h
View File

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

2
gtsam/hybrid/HybridSmoother.cpp
View File

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

4
gtsam/hybrid/HybridSmoother.h
View File

@ -20,6 +20,8 @@
#include <gtsam/hybrid/HybridBayesNet.h> #include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h> #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <optional>
namespace gtsam { namespace gtsam {
class HybridSmoother { class HybridSmoother {
@ -48,7 +50,7 @@ class HybridSmoother {
* if applicable * if applicable
*/ */
void update(HybridGaussianFactorGraph graph, const Ordering& ordering, 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. * @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( inline HybridGaussianFactorGraph createHybridGaussianFactorGraph(
size_t num_measurements = 1, size_t num_measurements = 1,
boost::optional<VectorValues> measurements = boost::none, std::optional<VectorValues> measurements = {},
bool manyModes = false) { bool manyModes = false) {
auto bayesNet = createHybridBayesNet(num_measurements, manyModes); auto bayesNet = createHybridBayesNet(num_measurements, manyModes);
if (measurements) { if (measurements) {

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

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

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

@ -178,7 +178,7 @@ namespace gtsam {
this->conditional()->endParents()); this->conditional()->endParents());
auto separatorMarginal = auto separatorMarginal =
p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function); p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function);
cachedSeparatorMarginal_.reset(*separatorMarginal); cachedSeparatorMarginal_ = *separatorMarginal;
} }
} }
@ -217,7 +217,7 @@ namespace gtsam {
} }
//Delete CachedShortcut for this clique //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/inference/Ordering.h>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h> #include <gtsam/base/FastVector.h>
#include <boost/optional.hpp>
#include <string> #include <string>
#include <mutex> #include <mutex>
#include <optional>
namespace gtsam { namespace gtsam {
@ -102,7 +102,7 @@ namespace gtsam {
/// @} /// @}
/// This stores the Cached separator marginal P(S) /// 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 /// This protects Cached seperator marginal P(S) from concurrent read/writes
/// as many the functions which access it are const (hence the mutable) /// as many the functions which access it are const (hence the mutable)
/// leading to the false impression that these const functions are thread-safe /// leading to the false impression that these const functions are thread-safe
@ -174,7 +174,7 @@ namespace gtsam {
*/ */
void deleteCachedShortcuts(); void deleteCachedShortcuts();
const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const { const std::optional<FactorGraphType>& cachedSeparatorMarginal() const {
std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_); std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
return cachedSeparatorMarginal_; return cachedSeparatorMarginal_;
} }
@ -194,7 +194,7 @@ namespace gtsam {
/** Non-recursive delete cached shortcuts and marginals - internal only. */ /** Non-recursive delete cached shortcuts and marginals - internal only. */
void deleteCachedShortcutsNonRecursive() { void deleteCachedShortcutsNonRecursive() {
std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_); std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
cachedSeparatorMarginal_ = boost::none; cachedSeparatorMarginal_ = {};
} }
private: 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, void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position, const std::optional<Vector2>& position,
ostream* os) const { ostream* os) const {
// Label the node with the label from the KeyFormatter // Label the node with the label from the KeyFormatter
*os << " var" << key << "[label=\"" << keyFormatter(key) *os << " var" << key << "[label=\"" << keyFormatter(key)
@ -54,7 +54,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter,
*os << "];\n"; *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) { ostream* os) {
*os << " factor" << i << "[label=\"\", shape=point"; *os << " factor" << i << "[label=\"\", shape=point";
if (position) { if (position) {
@ -76,26 +76,28 @@ static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter,
} }
/// Return variable position or none /// Return variable position or none
boost::optional<Vector2> DotWriter::variablePos(Key key) const { std::optional<Vector2> DotWriter::variablePos(Key key) const {
boost::optional<Vector2> result = boost::none; std::optional<Vector2> result = {};
// Check position hint // Check position hint
Symbol symbol(key); Symbol symbol(key);
auto hint = positionHints.find(symbol.chr()); auto hint = positionHints.find(symbol.chr());
if (hint != positionHints.end()) if (hint != positionHints.end()) {
result.reset(Vector2(symbol.index(), hint->second)); result = Vector2(symbol.index(), hint->second);
}
// Override with explicit position, if given. // Override with explicit position, if given.
auto pos = variablePositions.find(key); auto pos = variablePositions.find(key);
if (pos != variablePositions.end()) if (pos != variablePositions.end()) {
result.reset(pos->second); result = pos->second;
}
return result; return result;
} }
void DotWriter::processFactor(size_t i, const KeyVector& keys, void DotWriter::processFactor(size_t i, const KeyVector& keys,
const KeyFormatter& keyFormatter, const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position, const std::optional<Vector2>& position,
ostream* os) const { ostream* os) const {
if (plotFactorPoints) { if (plotFactorPoints) {
if (binaryEdges && keys.size() == 2) { if (binaryEdges && keys.size() == 2) {

9
gtsam/inference/DotWriter.h
View File

@ -25,6 +25,7 @@
#include <iosfwd> #include <iosfwd>
#include <map> #include <map>
#include <set> #include <set>
#include <optional>
namespace gtsam { namespace gtsam {
@ -80,20 +81,20 @@ struct GTSAM_EXPORT DotWriter {
/// Create a variable dot fragment. /// Create a variable dot fragment.
void drawVariable(Key key, const KeyFormatter& keyFormatter, void drawVariable(Key key, const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position, const std::optional<Vector2>& position,
std::ostream* os) const; std::ostream* os) const;
/// Create factor dot. /// 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); std::ostream* os);
/// Return variable position or none /// 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. /// Draw a single factor, specified by its index i and its variable keys.
void processFactor(size_t i, const KeyVector& keys, void processFactor(size_t i, const KeyVector& keys,
const KeyFormatter& keyFormatter, const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position, const std::optional<Vector2>& position,
std::ostream* os) const; 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 // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex
// before creating ordering. // before creating ordering.
VariableIndex computedVariableIndex(asDerived()); VariableIndex computedVariableIndex(asDerived());
return eliminateSequential(orderingType, function, computedVariableIndex); return eliminateSequential(orderingType, function, std::cref(computedVariableIndex));
} }
else { else {
// Compute an ordering and call this function again. We are guaranteed to have a // 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()); Ordering computedOrdering = Ordering::Metis(asDerived());
return eliminateSequential(computedOrdering, function, variableIndex); return eliminateSequential(computedOrdering, function, variableIndex);
} else if (orderingType == Ordering::COLAMD) { } else if (orderingType == Ordering::COLAMD) {
Ordering computedOrdering = Ordering::Colamd(*variableIndex); Ordering computedOrdering = Ordering::Colamd((*variableIndex).get());
return eliminateSequential(computedOrdering, function, variableIndex); return eliminateSequential(computedOrdering, function, variableIndex);
} else if (orderingType == Ordering::NATURAL) { } else if (orderingType == Ordering::NATURAL) {
Ordering computedOrdering = Ordering::Natural(asDerived()); Ordering computedOrdering = Ordering::Natural(asDerived());
return eliminateSequential(computedOrdering, function, variableIndex); return eliminateSequential(computedOrdering, function, variableIndex);
} else { } else {
Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc( Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
asDerived(), variableIndex); asDerived(), *variableIndex);
return eliminateSequential(computedOrdering, function, variableIndex); return eliminateSequential(computedOrdering, function, variableIndex);
} }
} }
@ -68,11 +68,11 @@ namespace gtsam {
if(!variableIndex) { if(!variableIndex) {
// If no VariableIndex provided, compute one and call this function again // If no VariableIndex provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived()); VariableIndex computedVariableIndex(asDerived());
return eliminateSequential(ordering, function, computedVariableIndex); return eliminateSequential(ordering, function, std::cref(computedVariableIndex));
} else { } else {
gttic(eliminateSequential); gttic(eliminateSequential);
// Do elimination // Do elimination
EliminationTreeType etree(asDerived(), *variableIndex, ordering); EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
boost::shared_ptr<BayesNetType> bayesNet; boost::shared_ptr<BayesNetType> bayesNet;
boost::shared_ptr<FactorGraphType> factorGraph; boost::shared_ptr<FactorGraphType> factorGraph;
boost::tie(bayesNet,factorGraph) = etree.eliminate(function); boost::tie(bayesNet,factorGraph) = etree.eliminate(function);
@ -99,7 +99,7 @@ namespace gtsam {
// creating ordering. // creating ordering.
VariableIndex computedVariableIndex(asDerived()); VariableIndex computedVariableIndex(asDerived());
return eliminateMultifrontal(orderingType, function, return eliminateMultifrontal(orderingType, function,
computedVariableIndex); std::cref(computedVariableIndex));
} else { } else {
// Compute an ordering and call this function again. We are guaranteed to // Compute an ordering and call this function again. We are guaranteed to
// have a VariableIndex already here because we computed one if needed in // have a VariableIndex already here because we computed one if needed in
@ -108,14 +108,14 @@ namespace gtsam {
Ordering computedOrdering = Ordering::Metis(asDerived()); Ordering computedOrdering = Ordering::Metis(asDerived());
return eliminateMultifrontal(computedOrdering, function, variableIndex); return eliminateMultifrontal(computedOrdering, function, variableIndex);
} else if (orderingType == Ordering::COLAMD) { } else if (orderingType == Ordering::COLAMD) {
Ordering computedOrdering = Ordering::Colamd(*variableIndex); Ordering computedOrdering = Ordering::Colamd((*variableIndex).get());
return eliminateMultifrontal(computedOrdering, function, variableIndex); return eliminateMultifrontal(computedOrdering, function, variableIndex);
} else if (orderingType == Ordering::NATURAL) { } else if (orderingType == Ordering::NATURAL) {
Ordering computedOrdering = Ordering::Natural(asDerived()); Ordering computedOrdering = Ordering::Natural(asDerived());
return eliminateMultifrontal(computedOrdering, function, variableIndex); return eliminateMultifrontal(computedOrdering, function, variableIndex);
} else { } else {
Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc( Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
asDerived(), variableIndex); asDerived(), *variableIndex);
return eliminateMultifrontal(computedOrdering, function, variableIndex); return eliminateMultifrontal(computedOrdering, function, variableIndex);
} }
} }
@ -131,11 +131,11 @@ namespace gtsam {
if(!variableIndex) { if(!variableIndex) {
// If no VariableIndex provided, compute one and call this function again // If no VariableIndex provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived()); VariableIndex computedVariableIndex(asDerived());
return eliminateMultifrontal(ordering, function, computedVariableIndex); return eliminateMultifrontal(ordering, function, std::cref(computedVariableIndex));
} else { } else {
gttic(eliminateMultifrontal); gttic(eliminateMultifrontal);
// Do elimination with given ordering // Do elimination with given ordering
EliminationTreeType etree(asDerived(), *variableIndex, ordering); EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
JunctionTreeType junctionTree(etree); JunctionTreeType junctionTree(etree);
boost::shared_ptr<BayesTreeType> bayesTree; boost::shared_ptr<BayesTreeType> bayesTree;
boost::shared_ptr<FactorGraphType> factorGraph; boost::shared_ptr<FactorGraphType> factorGraph;
@ -157,12 +157,12 @@ namespace gtsam {
if(variableIndex) { if(variableIndex) {
gttic(eliminatePartialSequential); gttic(eliminatePartialSequential);
// Do elimination // Do elimination
EliminationTreeType etree(asDerived(), *variableIndex, ordering); EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
return etree.eliminate(function); return etree.eliminate(function);
} else { } else {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived()); VariableIndex computedVariableIndex(asDerived());
return eliminatePartialSequential(ordering, function, computedVariableIndex); return eliminatePartialSequential(ordering, function, std::cref(computedVariableIndex));
} }
} }
@ -175,7 +175,7 @@ namespace gtsam {
if(variableIndex) { if(variableIndex) {
gttic(eliminatePartialSequential); gttic(eliminatePartialSequential);
// Compute full ordering // 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 // Split off the part of the ordering for the variables being eliminated
Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
@ -183,7 +183,7 @@ namespace gtsam {
} else { } else {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived()); VariableIndex computedVariableIndex(asDerived());
return eliminatePartialSequential(variables, function, computedVariableIndex); return eliminatePartialSequential(variables, function, std::cref(computedVariableIndex));
} }
} }
@ -196,13 +196,13 @@ namespace gtsam {
if(variableIndex) { if(variableIndex) {
gttic(eliminatePartialMultifrontal); gttic(eliminatePartialMultifrontal);
// Do elimination // Do elimination
EliminationTreeType etree(asDerived(), *variableIndex, ordering); EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
JunctionTreeType junctionTree(etree); JunctionTreeType junctionTree(etree);
return junctionTree.eliminate(function); return junctionTree.eliminate(function);
} else { } else {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived()); VariableIndex computedVariableIndex(asDerived());
return eliminatePartialMultifrontal(ordering, function, computedVariableIndex); return eliminatePartialMultifrontal(ordering, function, std::cref(computedVariableIndex));
} }
} }
@ -215,7 +215,7 @@ namespace gtsam {
if(variableIndex) { if(variableIndex) {
gttic(eliminatePartialMultifrontal); gttic(eliminatePartialMultifrontal);
// Compute full ordering // 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 // Split off the part of the ordering for the variables being eliminated
Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
@ -223,7 +223,7 @@ namespace gtsam {
} else { } else {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived()); VariableIndex computedVariableIndex(asDerived());
return eliminatePartialMultifrontal(variables, function, computedVariableIndex); return eliminatePartialMultifrontal(variables, function, std::cref(computedVariableIndex));
} }
} }
@ -237,7 +237,7 @@ namespace gtsam {
if(!variableIndex) { if(!variableIndex) {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
VariableIndex index(asDerived()); VariableIndex index(asDerived());
return marginalMultifrontalBayesNet(variables, function, index); return marginalMultifrontalBayesNet(variables, function, std::cref(index));
} else { } else {
// No ordering was provided for the marginalized variables, so order them using constrained // No ordering was provided for the marginalized variables, so order them using constrained
// COLAMD. // COLAMD.
@ -247,7 +247,7 @@ namespace gtsam {
boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables); boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
Ordering totalOrdering = Ordering totalOrdering =
Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered);
// Split up ordering // Split up ordering
const size_t nVars = variablesOrOrdering->size(); const size_t nVars = variablesOrOrdering->size();
@ -255,7 +255,7 @@ namespace gtsam {
Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
// Call this function again with the computed orderings // 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<BayesTreeType> bayesTree;
boost::shared_ptr<FactorGraphType> factorGraph; boost::shared_ptr<FactorGraphType> factorGraph;
boost::tie(bayesTree,factorGraph) = boost::tie(bayesTree,factorGraph) =
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables)) if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
{ {
@ -304,7 +304,7 @@ namespace gtsam {
if(!variableIndex) { if(!variableIndex) {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived()); VariableIndex computedVariableIndex(asDerived());
return marginalMultifrontalBayesTree(variables, function, computedVariableIndex); return marginalMultifrontalBayesTree(variables, function, std::cref(computedVariableIndex));
} else { } else {
// No ordering was provided for the marginalized variables, so order them using constrained // No ordering was provided for the marginalized variables, so order them using constrained
// COLAMD. // COLAMD.
@ -314,7 +314,7 @@ namespace gtsam {
boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables); boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
Ordering totalOrdering = Ordering totalOrdering =
Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered);
// Split up ordering // Split up ordering
const size_t nVars = variablesOrOrdering->size(); const size_t nVars = variablesOrOrdering->size();
@ -322,7 +322,7 @@ namespace gtsam {
Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
// Call this function again with the computed orderings // 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(!variableIndex) {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived()); VariableIndex computedVariableIndex(asDerived());
return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex); return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, std::cref(computedVariableIndex));
} else { } else {
gttic(marginalMultifrontalBayesTree); gttic(marginalMultifrontalBayesTree);
// An ordering was provided for the marginalized variables, so we can first eliminate them // 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<BayesTreeType> bayesTree;
boost::shared_ptr<FactorGraphType> factorGraph; boost::shared_ptr<FactorGraphType> factorGraph;
boost::tie(bayesTree,factorGraph) = boost::tie(bayesTree,factorGraph) =
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables)) if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
{ {
@ -371,19 +371,19 @@ namespace gtsam {
if(variableIndex) if(variableIndex)
{ {
// Compute a total ordering for all variables // 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 // Split out the part for the marginalized variables
Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size()); Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size());
// Eliminate and return the remaining factor graph // Eliminate and return the remaining factor graph
return eliminatePartialMultifrontal(marginalizationOrdering, function, *variableIndex).second; return eliminatePartialMultifrontal(marginalizationOrdering, function, variableIndex).second;
} }
else else
{ {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
VariableIndex computedVariableIndex(asDerived()); 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 #pragma once
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <cstddef>
#include <functional> #include <functional>
#include <optional>
#include <boost/variant.hpp> #include <boost/variant.hpp>
#include <boost/optional.hpp>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndex.h>
@ -89,10 +90,11 @@ namespace gtsam {
typedef std::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate; typedef std::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
/// Typedef for an optional variable index as an argument to elimination functions /// 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 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 /** 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. * provided, the ordering provided by COLAMD will be used.
@ -111,13 +113,13 @@ namespace gtsam {
* \code * \code
* VariableIndex varIndex(graph); // Build variable index * VariableIndex varIndex(graph); // Build variable index
* Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses 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 * \endcode
* */ * */
boost::shared_ptr<BayesNetType> eliminateSequential( boost::shared_ptr<BayesNetType> eliminateSequential(
OptionalOrderingType orderingType = boost::none, OptionalOrderingType orderingType = {},
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = {}) const;
/** Do sequential elimination of all variables to produce a Bayes net. /** Do sequential elimination of all variables to produce a Bayes net.
* *
@ -130,13 +132,13 @@ namespace gtsam {
* \code * \code
* VariableIndex varIndex(graph); // Build variable index * VariableIndex varIndex(graph); // Build variable index
* Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses 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 * \endcode
* */ * */
boost::shared_ptr<BayesNetType> eliminateSequential( boost::shared_ptr<BayesNetType> eliminateSequential(
const Ordering& ordering, const Ordering& ordering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, 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 /** 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 * provided, the ordering will be computed using either COLAMD or METIS, dependeing on
@ -151,13 +153,13 @@ namespace gtsam {
* \code * \code
* VariableIndex varIndex(graph); // Build variable index * VariableIndex varIndex(graph); // Build variable index
* Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses 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 * \endcode
* */ * */
boost::shared_ptr<BayesTreeType> eliminateMultifrontal( boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
OptionalOrderingType orderingType = boost::none, OptionalOrderingType orderingType = {},
const Eliminate& function = EliminationTraitsType::DefaultEliminate, 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 /** 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 * provided, the ordering will be computed using either COLAMD or METIS, dependeing on
@ -171,7 +173,7 @@ namespace gtsam {
boost::shared_ptr<BayesTreeType> eliminateMultifrontal( boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
const Ordering& ordering, const Ordering& ordering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, 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 /** 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$, * 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( eliminatePartialSequential(
const Ordering& ordering, const Ordering& ordering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, 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 /** 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) * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X)
@ -191,7 +193,7 @@ namespace gtsam {
eliminatePartialSequential( eliminatePartialSequential(
const KeyVector& variables, const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, 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 /** 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) * 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( eliminatePartialMultifrontal(
const Ordering& ordering, const Ordering& ordering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, 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 /** 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) * produce a Bayes tree and a remaining factor graph. This computes the factorization \f$ p(X)
@ -211,7 +213,7 @@ namespace gtsam {
eliminatePartialMultifrontal( eliminatePartialMultifrontal(
const KeyVector& variables, const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, 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 /** Compute the marginal of the requested variables and return the result as a Bayes net. Uses
* COLAMD marginalization ordering by default * COLAMD marginalization ordering by default
@ -225,7 +227,7 @@ namespace gtsam {
boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet( boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> variables, boost::variant<const Ordering&, const KeyVector&> variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, 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. /** 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 * @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, boost::variant<const Ordering&, const KeyVector&> variables,
const Ordering& marginalizedVariableOrdering, const Ordering& marginalizedVariableOrdering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, 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 /** Compute the marginal of the requested variables and return the result as a Bayes tree. Uses
* COLAMD marginalization order by default * COLAMD marginalization order by default
@ -255,7 +257,7 @@ namespace gtsam {
boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree( boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> variables, boost::variant<const Ordering&, const KeyVector&> variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, 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. /** 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 * @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, boost::variant<const Ordering&, const KeyVector&> variables,
const Ordering& marginalizedVariableOrdering, const Ordering& marginalizedVariableOrdering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = {}) const;
/** Compute the marginal factor graph of the requested variables. */ /** Compute the marginal factor graph of the requested variables. */
boost::shared_ptr<FactorGraphType> marginal( boost::shared_ptr<FactorGraphType> marginal(
const KeyVector& variables, const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = {}) const;
private: 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); const auto& factor = at(i);
if (factor) { if (factor) {
const KeyVector& factorKeys = factor->keys(); 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