changed interface for both unwhitenedError and evaluateError
parent
97269afe4b
commit
fcf339a31a
|
@ -7,8 +7,10 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
option(GTSAM_NO_BOOST_CPP17 "Require and use boost" ON)
|
option(GTSAM_NO_BOOST_CPP17 "Require and use boost" ON)
|
||||||
|
add_definitions(-Wno-deprecated-declarations)
|
||||||
|
|
||||||
if (GTSAM_NO_BOOST_CPP17)
|
if (GTSAM_NO_BOOST_CPP17)
|
||||||
|
add_definitions(-DNO_BOOST_CPP17)
|
||||||
set(CMAKE_CXX_STANDARD 17)
|
set(CMAKE_CXX_STANDARD 17)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
|
@ -57,9 +59,8 @@ endif()
|
||||||
include(cmake/HandleGeneralOptions.cmake) # CMake build options
|
include(cmake/HandleGeneralOptions.cmake) # CMake build options
|
||||||
|
|
||||||
# Libraries:
|
# Libraries:
|
||||||
if (GTSAM_NO_BOOST_CPP17)
|
if (NOT GTSAM_NO_BOOST_CPP17)
|
||||||
include(cmake/HandleBoost.cmake) # Boost
|
include(cmake/HandleBoost.cmake) # Boost
|
||||||
add_definitions(-DNO_BOOST_CPP17)
|
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
include(cmake/HandleCCache.cmake) # ccache
|
include(cmake/HandleCCache.cmake) # ccache
|
||||||
|
|
|
@ -33,7 +33,28 @@
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
/*
|
||||||
|
* Some typedef based aliases to compile these interfaces without boost if
|
||||||
|
* the NO_BOOST_C17 flag is enabled
|
||||||
|
*/
|
||||||
|
#ifdef NO_BOOST_CPP17
|
||||||
|
// These typedefs and aliases will help with making the evaluateError interface
|
||||||
|
// independent of boost
|
||||||
|
#define OptionalNone nullptr
|
||||||
|
template <typename T = void>
|
||||||
|
using OptionalMatrixT = Matrix*;
|
||||||
|
using OptionalMatrix = Matrix*;
|
||||||
|
// These typedefs and aliases will help with making the unwhitenedError interface
|
||||||
|
// independent of boost
|
||||||
|
using OptionalMatrixVec = std::vector<Matrix>*;
|
||||||
|
#else
|
||||||
|
// creating a none value to use when declaring our interfaces
|
||||||
|
#define OptionalNone boost::none
|
||||||
|
template <typename T = void>
|
||||||
|
using OptionalMatrixT = boost::optional<Matrix&>;
|
||||||
|
using OptionalMatrix = boost::optional<Matrix&>;
|
||||||
|
using OptionalMatrixVec = boost::optional<std::vector<Matrix>&>;
|
||||||
|
#endif
|
||||||
/**
|
/**
|
||||||
* Nonlinear factor base class
|
* Nonlinear factor base class
|
||||||
*
|
*
|
||||||
|
@ -206,7 +227,6 @@ protected:
|
||||||
NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {}
|
NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
/** Print */
|
/** Print */
|
||||||
void print(const std::string& s = "",
|
void print(const std::string& s = "",
|
||||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
|
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
|
||||||
|
@ -230,8 +250,7 @@ public:
|
||||||
* If the optional arguments is specified, it should compute
|
* If the optional arguments is specified, it should compute
|
||||||
* both the function evaluation and its derivative(s) in H.
|
* both the function evaluation and its derivative(s) in H.
|
||||||
*/
|
*/
|
||||||
virtual Vector unwhitenedError(const Values& x,
|
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVec H = OptionalNone) const = 0;
|
||||||
boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Vector of errors, whitened
|
* Vector of errors, whitened
|
||||||
|
@ -402,7 +421,7 @@ class NoiseModelFactorN
|
||||||
/// N is the number of variables (N-way factor)
|
/// N is the number of variables (N-way factor)
|
||||||
enum { N = sizeof...(ValueTypes) };
|
enum { N = sizeof...(ValueTypes) };
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
using Base = NoiseModelFactor;
|
using Base = NoiseModelFactor;
|
||||||
using This = NoiseModelFactorN<ValueTypes...>;
|
using This = NoiseModelFactorN<ValueTypes...>;
|
||||||
|
|
||||||
|
@ -428,8 +447,6 @@ class NoiseModelFactorN
|
||||||
/* Like std::void_t, except produces `boost::optional<Matrix&>` instead of
|
/* Like std::void_t, except produces `boost::optional<Matrix&>` instead of
|
||||||
* `void`. Used to expand fixed-type parameter-packs with same length as
|
* `void`. Used to expand fixed-type parameter-packs with same length as
|
||||||
* ValueTypes. */
|
* ValueTypes. */
|
||||||
template <typename T>
|
|
||||||
using OptionalMatrix = boost::optional<Matrix&>;
|
|
||||||
|
|
||||||
/* Like std::void_t, except produces `Key` instead of `void`. Used to expand
|
/* Like std::void_t, except produces `Key` instead of `void`. Used to expand
|
||||||
* fixed-type parameter-packs with same length as ValueTypes. */
|
* fixed-type parameter-packs with same length as ValueTypes. */
|
||||||
|
@ -541,7 +558,7 @@ class NoiseModelFactorN
|
||||||
*/
|
*/
|
||||||
Vector unwhitenedError(
|
Vector unwhitenedError(
|
||||||
const Values& x,
|
const Values& x,
|
||||||
boost::optional<std::vector<Matrix>&> H = boost::none) const override {
|
OptionalMatrixVec H = OptionalNone) const override {
|
||||||
return unwhitenedError(boost::mp11::index_sequence_for<ValueTypes...>{}, x,
|
return unwhitenedError(boost::mp11::index_sequence_for<ValueTypes...>{}, x,
|
||||||
H);
|
H);
|
||||||
}
|
}
|
||||||
|
@ -573,7 +590,7 @@ class NoiseModelFactorN
|
||||||
* @param[out] H The Jacobian with respect to each variable (optional).
|
* @param[out] H The Jacobian with respect to each variable (optional).
|
||||||
*/
|
*/
|
||||||
virtual Vector evaluateError(const ValueTypes&... x,
|
virtual Vector evaluateError(const ValueTypes&... x,
|
||||||
OptionalMatrix<ValueTypes>... H) const = 0;
|
OptionalMatrixT<ValueTypes>... H) const = 0;
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
|
||||||
|
@ -587,7 +604,7 @@ class NoiseModelFactorN
|
||||||
* e.g. `const Vector error = factor.evaluateError(pose, point);`
|
* e.g. `const Vector error = factor.evaluateError(pose, point);`
|
||||||
*/
|
*/
|
||||||
inline Vector evaluateError(const ValueTypes&... x) const {
|
inline Vector evaluateError(const ValueTypes&... x) const {
|
||||||
return evaluateError(x..., OptionalMatrix<ValueTypes>()...);
|
return evaluateError(x..., OptionalMatrixT<ValueTypes>()...);
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Some (but not all) optional Jacobians are omitted (function overload)
|
/** Some (but not all) optional Jacobians are omitted (function overload)
|
||||||
|
@ -599,7 +616,7 @@ class NoiseModelFactorN
|
||||||
inline Vector evaluateError(const ValueTypes&... x,
|
inline Vector evaluateError(const ValueTypes&... x,
|
||||||
OptionalJacArgs&&... H) const {
|
OptionalJacArgs&&... H) const {
|
||||||
return evaluateError(x..., std::forward<OptionalJacArgs>(H)...,
|
return evaluateError(x..., std::forward<OptionalJacArgs>(H)...,
|
||||||
boost::none);
|
OptionalNone);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
@ -615,7 +632,7 @@ class NoiseModelFactorN
|
||||||
inline Vector unwhitenedError(
|
inline Vector unwhitenedError(
|
||||||
boost::mp11::index_sequence<Indices...>, //
|
boost::mp11::index_sequence<Indices...>, //
|
||||||
const Values& x,
|
const Values& x,
|
||||||
boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
OptionalMatrixVec H = OptionalNone) const {
|
||||||
if (this->active(x)) {
|
if (this->active(x)) {
|
||||||
if (H) {
|
if (H) {
|
||||||
return evaluateError(x.at<ValueTypes>(keys_[Indices])...,
|
return evaluateError(x.at<ValueTypes>(keys_[Indices])...,
|
||||||
|
|
Loading…
Reference in New Issue