diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 2f4b27d39..1d2bff8a7 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -308,26 +308,58 @@ class NoiseModelFactorN : public NoiseModelFactor { /// N is the number of variables (N-way factor) enum { N = sizeof...(ValueTypes) }; - /// The type of the i'th template param can be obtained as ValueType - template ::type = true> - using ValueType = - typename std::tuple_element>::type; - protected: using Base = NoiseModelFactor; using This = NoiseModelFactorN; - /* Like std::void_t, except produces `boost::optional` instead. Used - * to expand fixed-type parameter-packs with same length as ValueTypes */ + /// @name SFINAE aliases + /// @{ + + template + using IsConvertible = + typename std::enable_if::value, void>::type; + + template + using IndexIsValid = typename std::enable_if<(I >= 1) && (I <= N), + void>::type; // 1-indexed! + + template + using ContainerElementType = + typename std::decay().begin())>::type; + template + using IsContainerOfKeys = IsConvertible, Key>; + + /// @} + + /* Like std::void_t, except produces `boost::optional` instead of + * `void`. Used to expand fixed-type parameter-packs with same length as + * ValueTypes. */ template using OptionalMatrix = boost::optional; - /* Like std::void_t, except produces `Key` instead. Used to expand fixed-type - * parameter-packs with same length as ValueTypes */ + /* Like std::void_t, except produces `Key` instead of `void`. Used to expand + * fixed-type parameter-packs with same length as ValueTypes. */ template using KeyType = Key; public: + /** + * The type of the I'th template param can be obtained as ValueType. + * I is 1-indexed for backwards compatibility/consistency! So for example, + * ``` + * using Factor = NoiseModelFactorN; + * Factor::ValueType<1> // Pose3 + * Factor::ValueType<2> // Point3 + * // Factor::ValueType<0> // ERROR! Will not compile. + * // Factor::ValueType<3> // ERROR! Will not compile. + * ``` + */ + template > + using ValueType = + typename std::tuple_element>::type; + + public: + /// @name Constructors /// @{ @@ -353,11 +385,7 @@ class NoiseModelFactorN : public NoiseModelFactor { * @param keys A container of keys for the variables in this factor. */ template , - // check that CONTAINER is a container of Keys: - typename T = typename std::decay< - decltype(*std::declval().begin())>::type, - typename std::enable_if::value, - bool>::type = true> + typename = IsContainerOfKeys> NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) : Base(noiseModel, keys) { assert(keys.size() == N); @@ -367,10 +395,19 @@ class NoiseModelFactorN : public NoiseModelFactor { ~NoiseModelFactorN() override {} - /// Returns a key. Usage: `key()` returns the I'th key. - template - inline typename std::enable_if<(I < N), Key>::type key() const { - return keys_[I]; + /** Returns a key. Usage: `key()` returns the I'th key. + * I is 1-indexed for backwards compatibility/consistency! So for example, + * ``` + * NoiseModelFactorN factor(noise, key1, key2); + * key<1>() // = key1 + * key<2>() // = key2 + * // key<0>() // ERROR! Will not compile + * // key<3>() // ERROR! Will not compile + * ``` + */ + template > + inline Key key() const { + return keys_[I - 1]; } /// @name NoiseModelFactor methods @@ -433,9 +470,7 @@ class NoiseModelFactorN : public NoiseModelFactor { /** Some optional jacobians omitted function overload */ template 0) && - (sizeof...(OptionalJacArgs) < N), - bool>::type = true> + typename = IndexIsValid> inline Vector evaluateError(const ValueTypes&... x, OptionalJacArgs&&... H) const { return evaluateError(x..., std::forward(H)..., @@ -448,16 +483,17 @@ class NoiseModelFactorN : public NoiseModelFactor { /** Pack expansion with index_sequence template pattern, used to index into * `keys_` and `H` */ - template + template inline Vector unwhitenedError( - boost::mp11::index_sequence, // + boost::mp11::index_sequence, // const Values& x, boost::optional&> H = boost::none) const { if (this->active(x)) { if (H) { - return evaluateError(x.at(keys_[Inds])..., (*H)[Inds]...); + return evaluateError(x.at(keys_[Indices])..., + (*H)[Indices]...); } else { - return evaluateError(x.at(keys_[Inds])...); + return evaluateError(x.at(keys_[Indices])...); } } else { return Vector::Zero(this->dim());