From 374632c6484de75ad0db0ebd8281391c534dd19e Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 1 Oct 2025 23:02:50 -0700 Subject: [PATCH] New FastNoiseModelFactor with more efficient linearization --- gtsam/base/OptionalJacobian.h | 22 +- gtsam/linear/GaussianConditional.cpp | 2 +- gtsam/linear/JacobianFactor-inl.h | 21 +- gtsam/linear/JacobianFactor.h | 16 +- gtsam/linear/NoiseModel.h | 10 +- gtsam/navigation/ImuBias.cpp | 4 +- gtsam/nonlinear/FastNoiseModelFactorN.h | 496 ++++++++++++++++++++++++ gtsam/nonlinear/PriorFactor.h | 12 +- gtsam/slam/tests/testPriorFactor.cpp | 2 +- 9 files changed, 546 insertions(+), 39 deletions(-) create mode 100644 gtsam/nonlinear/FastNoiseModelFactorN.h diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 22a17e131b..b2eae1a7dc 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -120,7 +120,27 @@ class OptionalJacobian { "Expected: ") + "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")"); } - } + } + + /** + * @brief Constructor from an Eigen::Ref *value*. Will not usurp if dimension is wrong + * @note This is important so we don't overwrite someone else's memory! + */ + template + OptionalJacobian(Eigen::Ref* dynamic_optional) : + map_(nullptr) { + if (!dynamic_optional) return; // stay empty + Eigen::Ref& dynamic_ref = *dynamic_optional; + if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) { + usurp(dynamic_ref.data()); + } else { + throw std::invalid_argument( + std::string("OptionalJacobian called with wrong dimensions or " + "storage order.\n" + "Expected: ") + + "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")"); + } + } /// Constructor with std::nullopt just makes empty OptionalJacobian(std::nullopt_t /*none*/) : diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 8d3b0fa61e..dc2adc1b72 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -322,7 +322,7 @@ namespace gtsam { for (auto&& key : parents()) newKeys.push_back(key); // Hopefully second newAb copy below is optimized out... - return std::make_shared(newKeys, newAb, model_); + return std::make_shared(newKeys, std::move(newAb), model_); } /* **************************************************************************/ diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index e616091e84..2e70a2a789 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -34,20 +34,15 @@ namespace gtsam { } /* ************************************************************************* */ - template - JacobianFactor::JacobianFactor(const KEYS& keys, - const VerticalBlockMatrix& augmentedMatrix, + template < + typename KEYS, typename MATRIX, + std::enable_if_t< + std::is_same, VerticalBlockMatrix>::value, bool>> + JacobianFactor::JacobianFactor(KEYS&& keys, MATRIX&& augmentedMatrix, const SharedDiagonal& model) - : Base(keys), Ab_(augmentedMatrix), model_(model) { - checkAb(model, augmentedMatrix); - } - - /* ************************************************************************* */ - template - JacobianFactor::JacobianFactor(const KEYS& keys, - VerticalBlockMatrix&& augmentedMatrix, - const SharedDiagonal& model) - : Base(keys), Ab_(std::move(augmentedMatrix)), model_(model) { + : Base(std::forward(keys)), + Ab_(std::forward(augmentedMatrix)), + model_(model) { checkAb(model, Ab_); } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 33f7183a61..c48469e347 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -145,17 +145,13 @@ namespace gtsam { template JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - /** Constructor with arbitrary number keys, and where the augmented matrix - * is given all together instead of in block terms. - */ - template - JacobianFactor(const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, - const SharedDiagonal& sigmas = SharedDiagonal()); - /** Construct with an rvalue VerticalBlockMatrix, to allow std::move. */ - template - JacobianFactor(const KEYS& keys, VerticalBlockMatrix&& augmentedMatrix, - const SharedDiagonal& model); + template , + VerticalBlockMatrix>::value, + bool> = true> + JacobianFactor(KEYS&& keys, MATRIX&& augmentedMatrix, + const SharedDiagonal& model = SharedDiagonal()); /** * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 47a84654a1..3efee67b90 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -123,12 +123,12 @@ namespace gtsam { } /** in-place whiten, override if can be done more efficiently */ - virtual void whitenInPlace(Eigen::Block& v) const { - v = whiten(v); + virtual void whitenInPlace(Eigen::Block v) const { + v = Whiten(v); } /** in-place unwhiten, override if can be done more efficiently */ - virtual void unwhitenInPlace(Eigen::Block& v) const { + virtual void unwhitenInPlace(Eigen::Block v) const { v = unwhiten(v); } @@ -644,8 +644,8 @@ namespace gtsam { void WhitenInPlace(Eigen::Block /*H*/) const override {} void whitenInPlace(Vector& /*v*/) const override {} void unwhitenInPlace(Vector& /*v*/) const override {} - void whitenInPlace(Eigen::Block& /*v*/) const override {} - void unwhitenInPlace(Eigen::Block& /*v*/) const override {} + void whitenInPlace(Eigen::Block /*v*/) const override {} + void unwhitenInPlace(Eigen::Block /*v*/) const override {} private: #if GTSAM_ENABLE_BOOST_SERIALIZATION diff --git a/gtsam/navigation/ImuBias.cpp b/gtsam/navigation/ImuBias.cpp index 49a364ca03..34085f6b22 100644 --- a/gtsam/navigation/ImuBias.cpp +++ b/gtsam/navigation/ImuBias.cpp @@ -66,8 +66,8 @@ namespace imuBias { // } /// ostream operator std::ostream& operator<<(std::ostream& os, const ConstantBias& bias) { - os << "acc = " << bias.accelerometer().transpose(); - os << " gyro = " << bias.gyroscope().transpose(); + os << "acc = " << bias.accelerometer().transpose().eval(); + os << " gyro = " << bias.gyroscope().transpose().eval(); return os; } diff --git a/gtsam/nonlinear/FastNoiseModelFactorN.h b/gtsam/nonlinear/FastNoiseModelFactorN.h new file mode 100644 index 0000000000..3505b6515a --- /dev/null +++ b/gtsam/nonlinear/FastNoiseModelFactorN.h @@ -0,0 +1,496 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include "gtsam/base/GenericValue.h" +#include "gtsam/base/utilities.h" + +namespace gtsam { +/* ************************************************************************* */ +/** + * A convenient base class for creating your own NoiseModelFactor + * with n variables. To derive from this class, implement evaluateError(). + * + * For example, a 2-way factor that computes the difference in x-translation + * between a Pose3 and Point3 could be implemented like so: + * + * ~~~~~~~~~~~~~~~~~~~~{.cpp} + * class MyFactor : public FastNoiseModelFactorN { + * public: + * using Base = FastNoiseModelFactorN; + * + * MyFactor(Key pose_key, Key point_key, const SharedNoiseModel& noiseModel) + * : Base(noiseModel, pose_key, point_key) {} + * + * Vector evaluateError( + * const Pose3& T, const Point3& p, + * OptionalMatrixType H_T = OptionalNone, + * OptionalMatrixType H_p = OptionalNone) const override { + * Matrix36 t_H_T; // partial derivative of translation w.r.t. pose T + * + * // Only compute t_H_T if needed: + * Point3 t = T.translation(H_T ? &t_H_T : 0); + * double a = t(0); // a_H_t = [1, 0, 0] + * double b = p(0); // b_H_p = [1, 0, 0] + * double error = a - b; // H_a = 1, H_b = -1 + * + * // H_T = H_a * a_H_t * t_H_T = the first row of t_H_T + * if (H_T) *H_T = (Matrix(1, 6) << t_H_T.row(0)).finished(); + * // H_p = H_b * b_H_p = -1 * [1, 0, 0] + * if (H_p) *H_p = (Matrix(1, 3) << -1., 0., 0.).finished(); + * + * return Vector1(error); + * } + * }; + * + * // Unit Test + * TEST(NonlinearFactor, MyFactor) { + * MyFactor f(X(1), X(2), noiseModel::Unit::Create(1)); + * EXPECT_DOUBLES_EQUAL(-8., f.evaluateError(Pose3(), Point3(8., 7., 6.))(0), + * 1e-9); + * Values values; + * values.insert(X(1), Pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1, 2, 3))); + * values.insert(X(2), Point3(1, 2, 3)); + * EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); + * } + * ~~~~~~~~~~~~~~~~~~~~ + * + * These factors are templated on a values structure type. The values structures + * are typically more general than just vectors, e.g., Rot3 or Pose3, which are + * objects in non-linear manifolds (Lie groups). + */ +template +class FastNoiseModelFactorN + : public NoiseModelFactor, + public detail::NoiseModelFactorAliases { + public: + /// N is the number of variables (N-way factor) + inline constexpr static auto N = sizeof...(ValueTypes); + + using NoiseModelFactor::unwhitenedError; + + protected: + using Base = NoiseModelFactor; + using This = FastNoiseModelFactorN; + + /// @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>; + + /** A helper alias to check if a list of args + * are all references to a matrix or not. It will be used + * to choose the right overload of evaluateError. + */ + template + using AreAllMatrixRefs = + std::enable_if_t<(... && std::is_convertible::value), Ret>; + + template + using IsMatrixPointer = std::is_same, Matrix*>; + + template + using IsNullpointer = + std::is_same, std::nullptr_t>; + + /** A helper alias to check if a list of args + * are all pointers to a matrix or not. It will be used + * to choose the right overload of evaluateError. + */ + template + using AreAllMatrixPtrs = + std::enable_if_t<(... && (IsMatrixPointer::value || + IsNullpointer::value)), + Ret>; + + /// @} + + /* Like std::void_t, except produces `OptionalMatrixType` instead of + * `void`. Used to expand fixed-type parameter-packs with same length as + * ValueTypes. */ + template + using OptionalMatrixTypeT = Matrix*; + + /* Like std::void_t, except produces `Eigen::Ref` instead of `void`. + */ + template + using OptionalMatrixRefTypeT = Eigen::Ref*; + + /* 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; + + /* Like std::void_t, except produces `Matrix` instead of + * `void`. Used to expand fixed-type parameter-packs with same length as + * ValueTypes. This helps in creating an evaluateError overload that accepts + * Matrices instead of pointers to matrices */ + template + using MatrixTypeT = Matrix; + + 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 = FastNoiseModelFactorN; + * Factor::ValueType<1> // Pose3 + * Factor::ValueType<2> // Point3 + * // Factor::ValueType<0> // ERROR! Will not compile. + * // Factor::ValueType<3> // ERROR! Will not compile. + * ``` + * + * You can also use the shortcuts `X1`, ..., `X6` which are the same as + * `ValueType<1>`, ..., `ValueType<6>` respectively (see + * detail::NoiseModelFactorAliases). + * + * Note that, if your class is templated AND you want to use `ValueType<1>` + * inside your class, due to dependent types you need the `template` keyword: + * `typename MyFactor::template ValueType<1>`. + */ + template > + using ValueType = + typename std::tuple_element>::type; + + public: + /// @name Constructors + /// @{ + + /// Default Constructor for I/O + FastNoiseModelFactorN() {} + + /** + * Constructor. + * Example usage: FastNoiseModelFactorN(noise, key1, key2, ..., keyN) + * @param noiseModel Shared pointer to noise model. + * @param keys Keys for the variables in this factor, passed in as separate + * arguments. + */ + FastNoiseModelFactorN(const SharedNoiseModel& noiseModel, + KeyType... keys) + : Base(noiseModel, std::array{keys...}) {} + + /** + * Constructor. + * Example usage: `FastNoiseModelFactorN(noise, {key1, key2, ..., keyN})` + * Example usage: `FastNoiseModelFactorN(noise, keys)` where keys is a + * vector + * @param noiseModel Shared pointer to noise model. + * @param keys A container of keys for the variables in this factor. + */ + template , + typename = IsContainerOfKeys> + FastNoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) + : Base(noiseModel, keys) { + if (keys.size() != N) { + throw std::invalid_argument( + "FastNoiseModelFactorN: wrong number of keys given"); + } + } + + /// @} + + ~FastNoiseModelFactorN() override {} + + /** Returns a key. Usage: `key()` returns the I'th key. + * I is 1-indexed for backwards compatibility/consistency! So for example, + * ``` + * FastNoiseModelFactorN factor(noise, key1, key2); + * key<1>() // = key1 + * key<2>() // = key2 + * // key<0>() // ERROR! Will not compile + * // key<3>() // ERROR! Will not compile + * ``` + * + * Note that, if your class is templated AND you are trying to call `key<1>` + * inside your class, due to dependent types you need the `template` keyword: + * `this->key1()`. + */ + template + inline Key key() const { + static_assert(I <= N, "Index out of bounds"); + return keys_[I - 1]; + } + + /// @name NoiseModelFactor methods + /// @{ + + /** This implements the `unwhitenedError` virtual function by calling the + * n-key specific version of evaluateError, which is pure virtual so must be + * implemented in the derived class. + * + * Example usage: + * ``` + * gtsam::Values values; + * values.insert(...) // populate values + * std::vector Hs(2); // this will be an optional output argument + * const Vector error = factor.unwhitenedError(values, Hs); + * ``` + * @param[in] x A Values object containing the values of all the variables + * used in this factor + * @param[out] H A vector of (dynamic) matrices whose size should be equal to + * n. The Jacobians w.r.t. each variable will be output in this parameter. + */ + Vector unwhitenedError(const Values& x, + OptionalMatrixVecType H = nullptr) const override { + return unwhitenedError(gtsam::index_sequence_for{}, x, H); + } + + /// @} + /// @name Virtual methods + /// @{ + + /** + * 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, + * OptionalMatrixType H1 = OptionalNone, + * OptionalMatrixType H2 = OptionalNone) 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, + OptionalMatrixRefTypeT... H) const = 0; + + /// @} + /// @name Convenience method overloads + /// @{ + + /** Some (but not all) optional Jacobians are omitted (function overload) + * and the jacobians are l-value references to matrices. + * e.g. `const Vector error = factor.evaluateError(pose, point, Hpose);` + */ + template > + inline AreAllMatrixRefs evaluateError( + const ValueTypes&... x, OptionalJacArgs&&... H) const { + return evaluateError(x..., (&H)...); + } + + /** Some (but not all) optional Jacobians are omitted (function overload) + * and the jacobians are pointers to matrices. + * e.g. `const Vector error = factor.evaluateError(pose, point, &Hpose);` + */ + template > + inline AreAllMatrixPtrs evaluateError( + const ValueTypes&... x, OptionalJacArgs&&... H) const { + // If they are pointer version, ensure to cast them all to be Matrix* types + // This will ensure any arguments inferred as std::nonetype_t are cast to + // (Matrix*) nullptr This guides the compiler to the correct overload which + // is the one that takes pointers + return evaluateError(x..., std::forward(H)..., + static_cast(OptionalNone)); + } + + /* ************************************************************************* + */ + std::shared_ptr linearize(const Values& x) const override { + // Only linearize if the factor is active + if (!active(x)) return std::shared_ptr(); + + const std::tuple valueReferences = + getValueReferences(std::make_index_sequence{}, x); + + // Fill in terms, needed to create JacobianFactor below + KeyVector keys_ = keys(); + std::array dimensions; + fillDimensions(valueReferences, dimensions, std::make_index_sequence{}); + + const auto b_dim = this->dim(); + + VerticalBlockMatrix Ab_ = VerticalBlockMatrix(dimensions, b_dim, true); + + // Compute Jacobians directly into Ab_ blocks and set RHS vector b using + // a tuple expansion over the variable pack. + evalIntoAb(std::make_index_sequence{}, valueReferences, Ab_); + + // Whiten the corresponding system now + if (noiseModel_) { + for (size_t j = 0; j < size(); ++j) { + noiseModel_->whitenInPlace(Ab_(j)); + } + Vector b = Ab_(size()); + Ab_(size()) = noiseModel_->whiten(b); + } + + // TODO pass unwhitened + noise model to Gaussian factor + using noiseModel::Constrained; + if (noiseModel_ && noiseModel_->isConstrained()) + return GaussianFactor::shared_ptr(new JacobianFactor( + std::move(keys_), std::move(Ab_), + std::static_pointer_cast(noiseModel_)->unit())); + else { + return GaussianFactor::shared_ptr( + new JacobianFactor(std::move(keys_), std::move(Ab_))); + } + } + + /// @} + + private: + /// Helper: get the reference to the I'th value + template + inline const T& getValueReference(const Values& values) const { + const Value& value = values.at(keys_[I]); + + if (dynamic_cast*>(&value) == nullptr) { + throw std::invalid_argument( + "FastNoiseModelFactorN: wrong type of value for key"); + } + + return static_cast&>(value).value(); + } + + /// Helper: get references to values as a tuple + /// @param[in] sequence Index sequence for template parameter expansion + /// @param[in] values Values object containing variable values + /// @return Tuple of const references to the variable values + template + inline std::tuple&...> getValueReferences( + std::index_sequence, const Values& values) const { + return std::tuple&...>( + getValueReference, I>(values)...); + } + + /// Helper: fill dimensions array from value references tuple + /// @param[in] valueReferences Tuple of const references to variable values + /// @param[out] dimensions Array to fill with dimension values + /// @param[in] sequence Index sequence for template parameter expansion + template + inline void fillDimensions( + const std::tuple&...>& valueReferences, + std::array& dimensions, + std::index_sequence) const { + ((dimensions[I] = traits>::GetDimension( + std::get(valueReferences))), + ...); + } + + // Helper: expand indices to call evaluateError with values and pointers to + // Eigen::Ref blocks that alias Ab_'s Jacobian columns. Also writes the + // returned error into the last block (b column). + template + inline void evalIntoAb(std::index_sequence, + const std::tuple&...>& x, + VerticalBlockMatrix& Ab) const { + // Create refs that alias each Jacobian block in Ab. + auto Htuple = std::make_tuple(Eigen::Ref(Ab(I))...); + + // Call evaluateError with values and pointers to the refs. + Vector e = evaluateError(std::get(x)..., (&std::get(Htuple))...); + + // Store RHS vector b in the last block's single column. + Ab(size()).col(0) = -e; // b = -error + } + + /** Pack expansion with index_sequence template pattern, used to index into + * `keys_` and `H`. + * + * Example: For `FastNoiseModelFactorN`, the call would look + * like: `const Vector error = unwhitenedError(0, 1, values, H);` + */ + template + inline Vector unwhitenedError(gtsam::index_sequence, // + const Values& x, + OptionalMatrixVecType H = nullptr) const { + if (this->active(x)) { + if (H) { + // Preallocate each Jacobian matrix if not already allocated + if (H->size() != N) H->resize(N); + // C++17 fold expression to resize all matrices + ((*H)[Indices].resize( + this->dim(), traits>::GetDimension( + x.at>(keys_[Indices]))), + ...); + std::tuple>...> + Htuple{Eigen::Ref((*H)[Indices])...}; + + return evaluateError(x.at>(keys_[Indices])..., + &std::get(Htuple)...); + } else { + return evaluateError(x.at(keys_[Indices])..., + OptionalMatrixRefTypeT(nullptr)...); + } + } else { + return Vector::Zero(this->dim()); + } + } + +#if GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "FastNoiseModelFactor", boost::serialization::base_object(*this)); + } +#endif + + public: + /// @name Shortcut functions `key1()` -> `key<1>()` + /// @{ + + inline Key key1() const { return key<1>(); } + template + inline Key key2() const { + static_assert(I <= N, "Index out of bounds"); + return key<2>(); + } + template + inline Key key3() const { + static_assert(I <= N, "Index out of bounds"); + return key<3>(); + } + template + inline Key key4() const { + static_assert(I <= N, "Index out of bounds"); + return key<4>(); + } + template + inline Key key5() const { + static_assert(I <= N, "Index out of bounds"); + return key<5>(); + } + template + inline Key key6() const { + static_assert(I <= N, "Index out of bounds"); + return key<6>(); + } + + /// @} + +}; // \class FastNoiseModelFactorN + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 0979be3df8..ebb8142478 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -15,7 +15,7 @@ **/ #pragma once -#include +#include #include #include @@ -27,18 +27,18 @@ namespace gtsam { * @ingroup nonlinear */ template - class PriorFactor: public NoiseModelFactorN { + class PriorFactor: public FastNoiseModelFactorN { public: typedef VALUE T; // Provide access to the Matrix& version of evaluateError: - using NoiseModelFactor1::evaluateError; + using FastNoiseModelFactorN::evaluateError; private: - typedef NoiseModelFactorN Base; + typedef FastNoiseModelFactorN Base; VALUE prior_; /** The measurement */ @@ -95,7 +95,7 @@ namespace gtsam { /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const T& x, OptionalMatrixType H) const override { + Vector evaluateError(const T& x, Eigen::Ref* H) const override { if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); // manifold equivalent of z-x -> Local(x,z) return -traits::Local(x, prior_); @@ -110,7 +110,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility + // NoiseModelFactor1 instead of FastNoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp index d1a60e3461..03b51a7511 100644 --- a/gtsam/slam/tests/testPriorFactor.cpp +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -40,7 +40,7 @@ TEST(PriorFactor, ConstructorDynamicSizeVector) { Vector callEvaluateError(const PriorFactor& factor, const ConstantBias& bias) { - return factor.evaluateError(bias); + return factor.evaluateError(bias, nullptr); } // Test for imuBias::ConstantBias