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

removed NO_BOOST definitions and evaluateErrorInterface from expressionfactor

release/4.3a0
kartik arcot 2023-01-10 11:23:21 -08:00
parent b7073e3224
commit 6aed555eef
12 changed files with 28 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
CMakeLists.txt
View File

@ -6,14 +6,10 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH)
set(CMAKE_MACOSX_RPATH 0)
endif()
option(GTSAM_NO_BOOST_CPP17 "Require and use boost" ON)
add_definitions(-Wno-deprecated-declarations)
add_definitions(-ftemplate-backtrace-limit=0)
set(CMAKE_CXX_STANDARD 17)
if (GTSAM_NO_BOOST_CPP17)
add_definitions(-DNO_BOOST_CPP17)
endif()
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 4)
@ -60,10 +56,7 @@ endif()
include(cmake/HandleGeneralOptions.cmake) # CMake build options
# Libraries:
# if (NOT GTSAM_NO_BOOST_CPP17)
include(cmake/HandleBoost.cmake) # Boost
# endif()
include(cmake/HandleBoost.cmake) # Boost
include(cmake/HandleCCache.cmake) # ccache
include(cmake/HandleCPack.cmake) # CPack
include(cmake/HandleEigen.cmake) # Eigen3

98
gtsam/nonlinear/ExpressionFactor.h
View File

@ -28,97 +28,6 @@
namespace gtsam {
template <typename... ValueTypes>
class EvaluateErrorInterface {
public:
enum { N = sizeof...(ValueTypes) };
private:
template <int I>
using IndexIsValid = typename std::enable_if<(I >= 1) && (I <= N),
void>::type; // 1-indexed!
public:
/**
* Override `evaluateError` to finish implementing an n-way factor.
*
* Both the `x` and `H` arguments are written here as parameter packs, but
* when overriding this method, you probably want to explicitly write them
* out. For example, for a 2-way factor with variable types Pose3 and Point3,
* you should implement:
* ```
* Vector evaluateError(
* const Pose3& x1, const Point3& x2,
* boost::optional<Matrix&> H1 = boost::none,
* boost::optional<Matrix&> H2 = boost::none) const override { ... }
* ```
*
* If any of the optional Matrix reference arguments are specified, it should
* compute both the function evaluation and its derivative(s) in the requested
* variables.
*
* @param x The values of the variables to evaluate the error for. Passed in
* as separate arguments.
* @param[out] H The Jacobian with respect to each variable (optional).
*/
virtual Vector evaluateError(const ValueTypes&... x, OptionalMatrixTypeT<ValueTypes>... H) const = 0;
#ifdef NO_BOOST_CPP17
// if someone uses the evaluateError function by supplying all the optional
// arguments then redirect the call to the one which takes pointers
Vector evaluateError(const ValueTypes&... x, MatrixTypeT<ValueTypes>&... H) const {
return evaluateError(x..., (&H)...);
}
#endif
/// @}
/// @name Convenience method overloads
/// @{
/** No-Jacobians requested function overload.
* This specializes the version below to avoid recursive calls since this is
* commonly used.
*
* e.g. `const Vector error = factor.evaluateError(pose, point);`
*/
inline Vector evaluateError(const ValueTypes&... x) const {
return evaluateError(x..., OptionalMatrixTypeT<ValueTypes>()...);
}
/** Some (but not all) optional Jacobians are omitted (function overload)
*
* e.g. `const Vector error = factor.evaluateError(pose, point, Hpose);`
*/
template <typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>
inline Vector evaluateError(const ValueTypes&... x, OptionalJacArgs&&... H) const {
#ifdef NO_BOOST_CPP17
// A check to ensure all arguments passed are all either matrices or are all pointers to matrices
constexpr bool are_all_mat = (... && (std::is_same<Matrix, std::decay_t<OptionalJacArgs>>::value));
constexpr bool are_all_ptrs = (... && (std::is_same<OptionalMatrixType, std::decay_t<OptionalJacArgs>>::value ||
std::is_same<OptionalNoneType, std::decay_t<OptionalJacArgs>>::value));
static_assert((are_all_mat || are_all_ptrs),
"Arguments that are passed to the evaluateError function can only be of following the types: Matrix, "
"or Matrix*");
// if they pass all matrices then we want to pass their pointers instead
if constexpr (are_all_mat) {
return evaluateError(x..., (&H)...);
} else {
return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., static_cast<OptionalMatrixType>(OptionalNone));
}
#else
// A check to ensure all arguments passed are all either matrices or are optionals of matrix references
constexpr bool are_all_mat = (... && (std::is_same<Matrix&, OptionalJacArgs>::value ||
std::is_same<OptionalMatrixType, std::decay_t<OptionalJacArgs>>::value ||
std::is_same<OptionalNoneType, std::decay_t<OptionalJacArgs>>::value));
static_assert(
are_all_mat,
"Arguments that are passed to the evaluateError function can only be of following the types: Matrix&, "
"boost::optional<Matrix&>, or boost::none_t");
return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., OptionalNone);
#endif
}
};
/**
* Factor that supports arbitrary expressions via AD.
*
@ -398,14 +307,14 @@ struct traits<ExpressionFactorN<T, Args...>>
* @deprecated Prefer the more general ExpressionFactorN<>.
*/
template <typename T, typename A1, typename A2>
class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN<T, A1, A2>, public EvaluateErrorInterface<A1, A2> {
class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
public:
/// Destructor
~ExpressionFactor2() override {}
/// Backwards compatible evaluateError, to make existing tests compile
virtual Vector evaluateError(const A1& a1, const A2& a2, OptionalMatrixType H1,
OptionalMatrixType H2) const override {
Vector evaluateError(const A1& a1, const A2& a2, OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const {
Values values;
values.insert(this->keys_[0], a1);
values.insert(this->keys_[1], a2);
@ -416,6 +325,7 @@ public:
return error;
}
/// Recreate expression from given keys_ and measured_, used in load
/// Needed to deserialize a derived factor
virtual Expression<T> expression(Key key1, Key key2) const {

34
gtsam/nonlinear/NonlinearFactor.h
View File

@ -33,11 +33,7 @@
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
using OptionalNoneType = std::nullptr_t;
@ -52,16 +48,7 @@ using OptionalMatrixType = Matrix*;
// independent of boost
using OptionalMatrixVecType = std::vector<Matrix>*;
#define OptionalMatrixVecNone static_cast<std::vector<Matrix>*>(nullptr)
#else
// creating a none value to use when declaring our interfaces
using OptionalNoneType = boost::none_t;
#define OptionalNone boost::none
template <typename T = void>
using OptionalMatrixTypeT = boost::optional<Matrix&>;
using OptionalMatrixType = boost::optional<Matrix&>;
using OptionalMatrixVecType = boost::optional<std::vector<Matrix>&>;
#define OptionalMatrixVecNone boost::none
#endif
/**
* Nonlinear factor base class
*
@ -258,12 +245,12 @@ public:
* both the function evaluation and its derivative(s) in H.
*/
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const = 0;
#ifdef NO_BOOST_CPP17
// support taking in the actual vector instead of the pointer as well
Vector unwhitenedError(const Values& x, std::vector<Matrix>& H) const {
return unwhitenedError(x, &H);
}
#endif
/**
* Vector of errors, whitened
* This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian
@ -606,13 +593,11 @@ protected:
virtual Vector evaluateError(const ValueTypes&... x,
OptionalMatrixTypeT<ValueTypes>... H) const = 0;
#ifdef NO_BOOST_CPP17
// if someone uses the evaluateError function by supplying all the optional
// arguments then redirect the call to the one which takes pointers
Vector evaluateError(const ValueTypes&... x, MatrixTypeT<ValueTypes>&... H) const {
return evaluateError(x..., (&H)...);
}
#endif
/// @}
/// @name Convenience method overloads
@ -634,7 +619,6 @@ protected:
*/
template <typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>
inline Vector evaluateError(const ValueTypes&... x, OptionalJacArgs&&... H) const {
#ifdef NO_BOOST_CPP17
// A check to ensure all arguments passed are all either matrices or are all pointers to matrices
constexpr bool are_all_mat = (... && (std::is_same<Matrix, std::decay_t<OptionalJacArgs>>::value));
constexpr bool are_all_ptrs = (... && (std::is_same<OptionalMatrixType, std::decay_t<OptionalJacArgs>>::value ||
@ -648,16 +632,6 @@ protected:
} else {
return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., static_cast<OptionalMatrixType>(OptionalNone));
}
#else
// A check to ensure all arguments passed are all either matrices or are optionals of matrix references
constexpr bool are_all_mat = (... && (std::is_same<Matrix&, OptionalJacArgs>::value ||
std::is_same<OptionalMatrixType, std::decay_t<OptionalJacArgs>>::value ||
std::is_same<OptionalNoneType, std::decay_t<OptionalJacArgs>>::value));
static_assert(are_all_mat,
"Arguments that are passed to the evaluateError function can only be of following the types: Matrix&, "
"boost::optional<Matrix&>, or boost::none_t");
return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., OptionalNone);
#endif
}
/// @}

8
gtsam/sam/BearingFactor.h
View File

@ -34,10 +34,9 @@ struct Bearing;
*/
template <typename A1, typename A2,
typename T = typename Bearing<A1, A2>::result_type>
struct BearingFactor : public ExpressionFactorN<T, A1, A2>, public EvaluateErrorInterface<A1, A2>{
struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
typedef ExpressionFactorN<T, A1, A2> Base;
using EvaluateErrorInterface<A1,A2>::evaluateError;
/// default constructor
BearingFactor() {}
@ -62,9 +61,8 @@ struct BearingFactor : public ExpressionFactorN<T, A1, A2>, public EvaluateError
Base::print(s, kf);
}
virtual Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1, OptionalMatrixType H2) const override
{
Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const {
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = this->unwhitenedError(

8
gtsam/sam/BearingRangeFactor.h
View File

@ -33,7 +33,7 @@ template <typename A1, typename A2,
typename B = typename Bearing<A1, A2>::result_type,
typename R = typename Range<A1, A2>::result_type>
class BearingRangeFactor
: public ExpressionFactorN<BearingRange<A1, A2>, A1, A2>, public EvaluateErrorInterface<A1, A2>{
: public ExpressionFactorN<BearingRange<A1, A2>, A1, A2> {
private:
typedef BearingRange<A1, A2> T;
typedef ExpressionFactorN<T, A1, A2> Base;
@ -41,7 +41,6 @@ class BearingRangeFactor
public:
typedef boost::shared_ptr<This> shared_ptr;
using EvaluateErrorInterface<A1, A2>::evaluateError;
/// Default constructor
BearingRangeFactor() {}
@ -74,9 +73,8 @@ class BearingRangeFactor
Expression<A2>(keys[1]));
}
virtual Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1, OptionalMatrixType H2) const override
{
Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const {
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = this->unwhitenedError(

21
gtsam/sam/RangeFactor.h
View File

@ -32,13 +32,12 @@ struct Range;
* @ingroup sam
*/
template <typename A1, typename A2 = A1, typename T = double>
class RangeFactor : public ExpressionFactorN<T, A1, A2> , public EvaluateErrorInterface<A1, A2>{
class RangeFactor : public ExpressionFactorN<T, A1, A2> {
private:
typedef RangeFactor<A1, A2> This;
typedef ExpressionFactorN<T, A1, A2> Base;
public:
using EvaluateErrorInterface<A1,A2>::evaluateError;
/// default constructor
RangeFactor() {}
@ -60,8 +59,8 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> , public EvaluateErrorIn
return Expression<T>(Range<A1, A2>(), a1_, a2_);
}
virtual Vector evaluateError(const A1& a1, const A2& a2, OptionalMatrixType H1,
OptionalMatrixType H2) const override {
Vector evaluateError(const A1& a1, const A2& a2, OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const {
std::vector<Matrix> Hs(2);
const auto& keys = Factor::keys();
const Vector error = Base::unwhitenedError({{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, Hs);
@ -97,7 +96,7 @@ struct traits<RangeFactor<A1, A2, T> >
*/
template <typename A1, typename A2 = A1,
typename T = typename Range<A1, A2>::result_type>
class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> , public EvaluateErrorInterface<A1, A2>{
class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
private:
typedef RangeFactorWithTransform<A1, A2> This;
typedef ExpressionFactorN<T, A1, A2> Base;
@ -105,7 +104,6 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> , public Ev
A1 body_T_sensor_; ///< The pose of the sensor in the body frame
public:
using EvaluateErrorInterface<A1, A2>::evaluateError;
//// Default constructor
RangeFactorWithTransform() {}
@ -134,9 +132,8 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> , public Ev
return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
}
virtual Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1, OptionalMatrixType H2) const override
{
Vector evaluateError(const A1& a1, const A2& a2,
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const {
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = Base::unwhitenedError(
@ -147,6 +144,12 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> , public Ev
return error;
}
// An evaluateError overload to accept matrices (Matrix&) and pass it to the
// OptionalMatrixType evaluateError overload
Vector evaluateError(const A1& a1, const A2& a2, Matrix& H1, Matrix& H2) const {
return evaluateError(a1, a2, &H1, &H2);
}
/** print contents */
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {

2
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -214,7 +214,7 @@ TEST( RangeFactor, Jacobian2D ) {
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
factor.evaluateError(pose, point, &H1Actual, &H2Actual);
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;

4
gtsam/slam/EssentialMatrixFactor.h
View File

@ -295,15 +295,11 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
// Version with derivatives
Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
#ifdef NO_BOOST_CPP17
// Had to do this since the only overloaded function EssentialMatrixFactor2
// uses the type OptionalMatrixType. Which would be a pointer when we are
// not using boost. There is no way to redirect that call to the top (NoiseModelFactorN)
// dereference it and bring it back to the Base (EssentialMatrixFactor2)
Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd);
#else
Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd);
#endif
*DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5)
return e;
}

2
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -228,11 +228,9 @@ public:
return err_wh_eq;
}
#ifdef NO_BOOST_CPP17
Vector whitenedError(const Values& x, std::vector<Matrix>& H) const {
return whitenedError(x, &H);
}
#endif
/* ************************************************************************* */
Vector calcIndicatorProb(const Values& x) const {

2
gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
View File

@ -186,11 +186,9 @@ namespace gtsam {
}
/* ************************************************************************* */
#ifdef NO_BOOST_CPP17
gtsam::Vector whitenedError(const gtsam::Values& x, std::vector<Matrix>& H) const {
return whitenedError(x, &H);
}
#endif
/* ************************************************************************* */

2
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -245,11 +245,9 @@ namespace gtsam {
}
/* ************************************************************************* */
#ifdef NO_BOOST_CPP17
Vector whitenedError(const Values& x, std::vector<Matrix>& H) const {
return whitenedError(x, &H);
}
#endif
/* ************************************************************************* */
Vector calcIndicatorProb(const Values& x) const {

4
tests/simulated3D.h
View File

@ -99,12 +99,8 @@ struct PointPrior3D: public NoiseModelFactor1<Point3> {
/**
* Models a linear 3D measurement between 3D points
*/
<<<<<<< HEAD
struct Simulated3DMeasurement: public NoiseModelFactorN<Point3, Point3> {
=======
struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> {
using NoiseModelFactor2<Point3, Point3>::evaluateError;
>>>>>>> 7b3c40e92 (everything compiles but tests fail in no boost mode)
Point3 measured_; ///< Linear displacement between a pose and landmark