diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index a814fea87..4521cbb82 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -106,8 +106,8 @@ class CameraSet : public std::vector> { */ template ZVector project2(const POINT& point, // - boost::optional Fs = boost::none, // - boost::optional E = boost::none) const { + FBlocks* Fs = nullptr, // + Matrix* E = nullptr) const { static const int N = FixedDimension::value; // Allocate result @@ -131,14 +131,35 @@ class CameraSet : public std::vector> { return z; } + /** An overload o the project2 function to accept + * full matrices and vectors and pass it to the pointer + * version of the function + */ + template + ZVector project2(const POINT& point, OptArgs&... args) const { + // pass it to the pointer version of the function + return project2(point, (&args)...); + } + /// Calculate vector [project2(point)-z] of re-projection errors template Vector reprojectionError(const POINT& point, const ZVector& measured, - boost::optional Fs = boost::none, // - boost::optional E = boost::none) const { + FBlocks* Fs = nullptr, // + Matrix* E = nullptr) const { return ErrorVector(project2(point, Fs, E), measured); } + /** An overload o the reprojectionError function to accept + * full matrices and vectors and pass it to the pointer + * version of the function + */ + template + Vector reprojectionError(const POINT& point, const ZVector& measured, + OptArgs&... args) const { + // pass it to the pointer version of the function + return reprojectionError(point, measured, (&args)...); + } + /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index b2ef6f055..06088c79b 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -138,8 +138,7 @@ void Expression::print(const std::string& s) const { template T Expression::value(const Values& values, - boost::optional&> H) const { - + std::vector* H) const { if (H) { // Call private version that returns derivatives in H KeyVector keys; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index eb828760d..758101d36 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -155,8 +155,15 @@ public: * Notes: this is not terribly efficient, and H should have correct size. * The order of the Jacobians is same as keys in either keys() or dims() */ - T value(const Values& values, boost::optional&> H = - boost::none) const; + T value(const Values& values, std::vector* H = nullptr) const; + + /** + * An overload of the value function to accept reference to vector of matrices instead of + * a pointer to vector of matrices. + */ + T value(const Values& values, std::vector& H) const { + return value(values, &H); + } /** * @return a "deep" copy of this Expression diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index f56b23777..d5e25cef8 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -478,8 +478,8 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, /* ************************************************************************* */ void ISAM2::marginalizeLeaves( const FastList& leafKeysList, - boost::optional marginalFactorsIndices, - boost::optional deletedFactorsIndices) { + FactorIndices* marginalFactorsIndices, + FactorIndices* deletedFactorsIndices) { // Convert to ordered set KeySet leafKeys(leafKeysList.begin(), leafKeysList.end()); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 6e88079b1..dd246617a 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -198,8 +198,20 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { */ void marginalizeLeaves( const FastList& leafKeys, - boost::optional marginalFactorsIndices = boost::none, - boost::optional deletedFactorsIndices = boost::none); + FactorIndices* marginalFactorsIndices = nullptr, + FactorIndices* deletedFactorsIndices = nullptr); + + /** An overload of marginalizeLeaves that takes references + * to vectors instead of pointers to vectors and passes + * it to the pointer version of the function. + */ + template + void marginalizeLeaves(const FastList& leafKeys, + OptArgs&&... optArgs) { + // dereference the optional arguments and pass + // it to the pointer version + marginalizeLeaves(leafKeys, (&optArgs)...); + } /// Access the current linearization point const Values& getLinearizationPoint() const { return theta_; } diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 598963761..283f8d3a0 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -373,7 +373,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::optional Values::exists(Key j) const { + const ValueType * Values::exists(Key j) const { // Find the item KeyValueMap::const_iterator item = values_.find(j); @@ -381,14 +381,14 @@ namespace gtsam { // dynamic cast the type and throw exception if incorrect auto ptr = dynamic_cast*>(item->second); if (ptr) { - return ptr->value(); + return &ptr->value(); } else { // NOTE(abe): clang warns about potential side effects if done in typeid const Value* value = item->second; throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); } } else { - return boost::none; + return nullptr; } } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 79ddb0267..d75967eef 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -184,12 +184,12 @@ namespace gtsam { * exists. */ bool exists(Key j) const; - /** Check if a value with key \c j exists, returns the value with type - * \c Value if the key does exist, or boost::none if it does not exist. + /** Check if a value with key \c j exists, returns a pointer to the const version of the value + * \c Value if the key does exist, or nullptr if it does not exist. * Throws DynamicValuesIncorrectType if the value type associated with the * requested key does not match the stored value type. */ template - boost::optional exists(Key j) const; + const ValueType * exists(Key j) const; /** Find an element by key, returning an iterator, or end() if the key was * not found. */ diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 85dd2f4b3..ee7fcd189 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -308,7 +308,7 @@ TEST(Values, exists_) config0.insert(key1, 1.0); config0.insert(key2, 2.0); - boost::optional v = config0.exists(key1); + const double* v = config0.exists(key1); DOUBLES_EQUAL(1.0,*v,1e-9); } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index ca158cc1d..b53235ab3 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -198,13 +198,16 @@ protected: } } - /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and - /// derivatives. This is the error before the noise model is applied. + /** Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and + * derivatives. This is the error before the noise model is applied. + * The templated version described above must finally get resolved to this + * function. + */ template Vector unwhitenedError( const Cameras& cameras, const POINT& point, - boost::optional Fs = boost::none, // - boost::optional E = boost::none) const { + typename Cameras::FBlocks* Fs = nullptr, // + Matrix* E = nullptr) const { // Reproject, with optional derivatives. Vector error = cameras.reprojectionError(point, measured_, Fs, E); @@ -233,6 +236,19 @@ protected: return error; } + /** + * An overload of unwhitenedError. This allows + * end users to provide optional arguments that are l-value references + * to the matrices and vectors that will be used to store the results instead + * of pointers. + */ + template + Vector unwhitenedError( + const Cameras& cameras, const POINT& point, + OptArgs&&... optArgs) const { + return unwhitenedError(cameras, point, (&optArgs)...); + } + /** * This corrects the Jacobians for the case in which some 2D measurement is * missing (nan). In practice, this does not do anything in the monocular @@ -240,8 +256,21 @@ protected: */ virtual void correctForMissingMeasurements( const Cameras& cameras, Vector& ue, - boost::optional Fs = boost::none, - boost::optional E = boost::none) const {} + typename Cameras::FBlocks* Fs = nullptr, + Matrix* E = nullptr) const {} + + /** + * An overload of correctForMissingMeasurements. This allows + * end users to provide optional arguments that are l-value references + * to the matrices and vectors that will be used to store the results instead + * of pointers. + */ + template + void correctForMissingMeasurements( + const Cameras& cameras, Vector& ue, + OptArgs&&... optArgs) const { + correctForMissingMeasurements(cameras, ue, (&optArgs)...); + } /** * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - @@ -288,7 +317,7 @@ protected: // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar) // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z| // = |A*dx - (z-h(x_bar))| - b = -unwhitenedError(cameras, point, Fs, E); + b = -unwhitenedError(cameras, point, &Fs, &E); } /** diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index f9c101cb8..d09f42509 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -337,8 +337,9 @@ protected: */ bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const { bool nonDegenerate = triangulateForLinearize(cameras); - if (nonDegenerate) - cameras.project2(*result_, boost::none, E); + if (nonDegenerate) { + cameras.project2(*result_, nullptr, &E); + } return nonDegenerate; } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 61f110d3a..a9caf0c4c 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -340,7 +340,7 @@ public: bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const { bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - cameras.project2(*result_, boost::none, E); + cameras.project2(*result_, nullptr, &E); return nonDegenerate; } @@ -453,8 +453,8 @@ public: */ void correctForMissingMeasurements( const Cameras& cameras, Vector& ue, - boost::optional Fs = boost::none, - boost::optional E = boost::none) const override { + typename Cameras::FBlocks* Fs = nullptr, + Matrix* E = nullptr) const override { // when using stereo cameras, some of the measurements might be missing: for (size_t i = 0; i < cameras.size(); i++) { const StereoPoint2& z = measured_.at(i);