diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 06a25ac23..6c6c155c7 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -135,7 +135,7 @@ T Expression::value(const Values& values, if (H) { // Call private version that returns derivatives in H KeysAndDims pair = keysAndDims(); - return value(values, pair.first, pair.second, *H); + return valueAndDerivatives(values, pair.first, pair.second, *H); } else // no derivatives needed, just return value return root_->value(values); @@ -154,8 +154,9 @@ size_t Expression::traceSize() const { // Private methods: template -T Expression::value(const Values& values, const FastVector& keys, - const FastVector& dims, std::vector& H) const { +T Expression::valueAndDerivatives(const Values& values, + const FastVector& keys, const FastVector& dims, + std::vector& H) const { // H should be pre-allocated assert(H.size()==keys.size()); @@ -167,7 +168,7 @@ T Expression::value(const Values& values, const FastVector& keys, internal::JacobianMap jacobianMap(keys, Ab); // Call unsafe version - T result = value(values, jacobianMap); + T result = valueAndJacobianMap(values, jacobianMap); // Copy blocks into the vector of jacobians passed in for (DenseIndex i = 0; i < static_cast(keys.size()); i++) @@ -184,7 +185,8 @@ T Expression::traceExecution(const Values& values, } template -T Expression::value(const Values& values, internal::JacobianMap& jacobians) const { +T Expression::valueAndJacobianMap(const Values& values, + internal::JacobianMap& jacobians) const { // The following piece of code is absolutely crucial for performance. // We allocate a block of memory on the stack, which can be done at runtime // with modern C++ compilers. The traceExecution then fills this memory diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index faa884562..3985d8a58 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -170,7 +170,7 @@ private: KeysAndDims keysAndDims() const; /// private version that takes keys and dimensions, returns derivatives - T value(const Values& values, const FastVector& keys, + T valueAndDerivatives(const Values& values, const FastVector& keys, const FastVector& dims, std::vector& H) const; /// trace execution, very unsafe @@ -178,10 +178,11 @@ private: void* traceStorage) const; /// brief Return value and derivatives, reverse AD version - T value(const Values& values, internal::JacobianMap& jacobians) const; + T valueAndJacobianMap(const Values& values, + internal::JacobianMap& jacobians) const; // be very selective on who can access these private methods: - friend class ExpressionFactor; + friend class ExpressionFactor ; friend class internal::ExpressionNode; // and add tests diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index afb487ff4..63e2b0ae8 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -65,8 +65,8 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if (H) { - const T value = expression_.value(x, keys_, dims_, *H); + if (H) { + const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); return traits::Local(measurement_, value); } else { const T value = expression_.value(x); @@ -95,7 +95,7 @@ public: Ab.matrix().setZero(); // Get value and Jacobians, writing directly into JacobianFactor - T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here ! + T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here ! // Evaluate error and set RHS vector b Ab(size()).col(0) = -traits::Local(measurement_, value); @@ -109,5 +109,5 @@ public: }; // ExpressionFactor -} // \ namespace gtsam +}// \ namespace gtsam