From f7c683a7942e22888def5b7e4550026bc3710017 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 5 Jan 2023 18:02:10 -0800 Subject: [PATCH] some changes that get testPriorFactor compiling --- gtsam/navigation/CombinedImuFactor.cpp | 6 ++--- gtsam/navigation/GPSFactor.cpp | 2 +- gtsam/navigation/GPSFactor.h | 2 +- gtsam/nonlinear/CustomFactor.cpp | 2 +- gtsam/nonlinear/CustomFactor.h | 2 +- gtsam/nonlinear/ExpressionFactor.h | 2 +- gtsam/nonlinear/NonlinearFactor.h | 28 +++++++++++++++------ gtsam/sfm/ShonanFactor.cpp | 8 +++--- gtsam/sfm/ShonanFactor.h | 8 +++--- gtsam/slam/OrientedPlane3Factor.h | 6 ++--- gtsam_unstable/slam/MultiProjectionFactor.h | 2 +- 11 files changed, 40 insertions(+), 28 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 38b3dc763..7958cd3d5 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -222,9 +222,9 @@ bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const { Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, - boost::optional H1, boost::optional H2, - boost::optional H3, boost::optional H4, - boost::optional H5, boost::optional H6) const { + OptionalMatrixType H1, OptionalMatrixType H2, + OptionalMatrixType H3, OptionalMatrixType H4, + OptionalMatrixType H5, OptionalMatrixType H6) const { // error wrt bias evolution model (random walk) Matrix6 Hbias_i, Hbias_j; diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 7a559bb4e..47de385ef 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -80,7 +80,7 @@ bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const { //*************************************************************************** Vector GPSFactor2::evaluateError(const NavState& p, - boost::optional H) const { + OptionalMatrixType H) const { return p.position(H) -nT_; } diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 1f488579d..73ec7d5a7 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -152,7 +152,7 @@ public: /// vector of errors Vector evaluateError(const NavState& p, - boost::optional H = boost::none) const override; + OptionalMatrixType H = OptionalNone) const override; inline const Point3 & measurementIn() const { return nT_; diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp index 6f0ad3a8b..ee5f9eae5 100644 --- a/gtsam/nonlinear/CustomFactor.cpp +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -43,7 +43,7 @@ Vector CustomFactor::unwhitenedError(const Values& x, OptionalMatrixVecType H) c * return error * ``` */ - return this->error_function_(*this, x, H.get_ptr()); + return this->error_function_(*this, x, &(*H)); } else { /* * In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python. diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index f01a73414..ef4090a92 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -75,7 +75,7 @@ public: * Calls the errorFunction closure, which is a std::function object * One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array */ - Vector unwhitenedError(const Values &x, OptionalMatrixVecType H = boost::none) const override; + Vector unwhitenedError(const Values &x, OptionalMatrixVecType H = OptionalMatrixVecNone) const override; /** print */ void print(const std::string &s, diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 0c59a6cb2..77add0012 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -97,7 +97,7 @@ protected: * both the function evaluation and its derivative(s) in H. */ Vector unwhitenedError(const Values& x, - OptionalMatrixVecType H = OptionalNone) const override { + OptionalMatrixVecType H = OptionalMatrixVecNone) const override { if (H) { const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index f7e3f67fe..8bb965ef5 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -41,13 +41,17 @@ namespace gtsam { // These typedefs and aliases will help with making the evaluateError interface // independent of boost using OptionalNoneType = std::nullptr_t; -#define OptionalNone = nullptr +// TODO: Change this to OptionalMatrixNone +#define OptionalNone static_cast(nullptr) template using OptionalMatrixTypeT = Matrix*; +template +using MatrixTypeT = Matrix; using OptionalMatrixType = Matrix*; // These typedefs and aliases will help with making the unwhitenedError interface // independent of boost using OptionalMatrixVecType = std::vector*; +#define OptionalMatrixVecNone static_cast*>(nullptr) #else // creating a none value to use when declaring our interfaces using OptionalNoneType = boost::none_t; @@ -56,6 +60,7 @@ template using OptionalMatrixTypeT = boost::optional; using OptionalMatrixType = boost::optional; using OptionalMatrixVecType = boost::optional&>; +#define OptionalMatrixVecNone boost::none #endif /** * Nonlinear factor base class @@ -252,10 +257,10 @@ public: * If the optional arguments is specified, it should compute * both the function evaluation and its derivative(s) in H. */ - virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalNone) const = 0; -#ifdef NO_BOOST_C17 + 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& H) { + Vector unwhitenedError(const Values& x, std::vector& H) const { return unwhitenedError(x, &H); } #endif @@ -565,7 +570,7 @@ protected: */ Vector unwhitenedError( const Values& x, - OptionalMatrixVecType H = OptionalNone) const override { + OptionalMatrixVecType H = OptionalMatrixVecNone) const override { return unwhitenedError(boost::mp11::index_sequence_for{}, x, H); } @@ -599,8 +604,15 @@ protected: virtual Vector evaluateError(const ValueTypes&... x, OptionalMatrixTypeT... 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&... H) const { + return evaluateError(x..., (&H)...); + } +#endif + /// @} /// @name Convenience method overloads /// @{ @@ -630,7 +642,7 @@ protected: "or Matrix*"); // if they pass all matrices then we want to pass their pointers instead if constexpr (are_all_mat) { - return evaluateError(x..., (&OptionalJacArgs)...); + return evaluateError(x..., (&H)...); } else { return evaluateError(x..., std::forward(H)..., static_cast(OptionalNone)); } @@ -659,7 +671,7 @@ protected: inline Vector unwhitenedError( boost::mp11::index_sequence, // const Values& x, - OptionalMatrixVecType H = OptionalNone) const { + OptionalMatrixVecType H = OptionalMatrixVecNone) const { if (this->active(x)) { if (H) { return evaluateError(x.at(keys_[Indices])..., diff --git a/gtsam/sfm/ShonanFactor.cpp b/gtsam/sfm/ShonanFactor.cpp index a48b6e6fa..288bd573e 100644 --- a/gtsam/sfm/ShonanFactor.cpp +++ b/gtsam/sfm/ShonanFactor.cpp @@ -75,8 +75,8 @@ bool ShonanFactor::equals(const NonlinearFactor &expected, //****************************************************************************** template void ShonanFactor::fillJacobians(const Matrix &M1, const Matrix &M2, - boost::optional H1, - boost::optional H2) const { + OptionalMatrixType H1, + OptionalMatrixType H2) const { gttic(ShonanFactor_Jacobians); const size_t dim = p_ * d; // Stiefel manifold dimension @@ -106,8 +106,8 @@ void ShonanFactor::fillJacobians(const Matrix &M1, const Matrix &M2, //****************************************************************************** template Vector ShonanFactor::evaluateError(const SOn &Q1, const SOn &Q2, - boost::optional H1, - boost::optional H2) const { + OptionalMatrixType H1, + OptionalMatrixType H2) const { gttic(ShonanFactor_evaluateError); const Matrix &M1 = Q1.matrix(); diff --git a/gtsam/sfm/ShonanFactor.h b/gtsam/sfm/ShonanFactor.h index 78cc39765..faa88879f 100644 --- a/gtsam/sfm/ShonanFactor.h +++ b/gtsam/sfm/ShonanFactor.h @@ -73,15 +73,15 @@ public: /// projects down from SO(p) to the Stiefel manifold of px3 matrices. Vector evaluateError(const SOn &Q1, const SOn &Q2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override; + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override; /// @} private: /// Calculate Jacobians if asked, Only implemented for d=2 and 3 in .cpp void fillJacobians(const Matrix &M1, const Matrix &M2, - boost::optional H1, - boost::optional H2) const; + OptionalMatrixType H1, + OptionalMatrixType H2) const; }; // Explicit instantiation for d=2 and d=3 in .cpp file: diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index 1550201ec..750215b72 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -44,8 +44,8 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN H1 = boost::none, - boost::optional H2 = boost::none) const override; + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override; }; // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior @@ -73,7 +73,7 @@ class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN H = boost::none) const override; + OptionalMatrixType H = OptionalNone) const override; }; } // gtsam diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 4f3bccd0a..dbc4bc907 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -129,7 +129,7 @@ namespace gtsam { } /// Evaluate error h(x)-z and optionally derivatives - Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalNone) const override { + Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const override { Vector a; return a;