diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 08a0e0bc6..ac97de40e 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -45,6 +45,16 @@ class Expression; typedef std::map JacobianMap; +/// Move terms to array, destroys content +void move(JacobianMap& jacobians, std::vector& H) { + assert(H.size()==jacobians.size()); + size_t j = 0; + JacobianMap::iterator it = jacobians.begin(); + for (; it != jacobians.end(); ++it) + it->second.swap(H[j++]); +} + + //----------------------------------------------------------------------------- /** * The CallRecord class stores the Jacobians of applying a function @@ -370,11 +380,7 @@ public: /// Move terms to array, destroys content void move(std::vector& H) { - assert(H.size()==jacobians_.size()); - size_t j = 0; - JacobianMap::iterator it = jacobians_.begin(); - for (; it != jacobians_.end(); ++it) - it->second.swap(H[j++]); + move(jacobians_, H); } }; diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index afd5a60e0..147804fb1 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -108,14 +108,11 @@ public: return root_->keys(); } - /// Return value and optional derivatives - T value(const Values& values) const { - return root_->value(values); - } - /// Return value and derivatives, forward AD version - Augmented forward(const Values& values) const { - return root_->forward(values); + T forward(const Values& values, JacobianMap& jacobians) const { + Augmented augmented = root_->forward(values); + jacobians = augmented.jacobians(); + return augmented.value(); } // Return size needed for memory buffer in traceExecution @@ -130,22 +127,26 @@ public: } /// Return value and derivatives, reverse AD version - Augmented reverse(const Values& values) const { + T reverse(const Values& values, JacobianMap& jacobians) const { size_t size = traceSize(); char raw[size]; ExecutionTrace trace; T value(traceExecution(values, trace, raw)); - Augmented augmented(value); - trace.startReverseAD(augmented.jacobians()); - return augmented; + trace.startReverseAD(jacobians); + return value; + } + + /// Return value + T value(const Values& values) const { + return root_->value(values); } /// Return value and derivatives - Augmented augmented(const Values& values) const { + T value(const Values& values, JacobianMap& jacobians) const { #ifdef EXPRESSION_FORWARD_AD - return forward(values); + return forward(values, jacobians); #else - return reverse(values); + return reverse(values, jacobians); #endif } diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 9ca54924d..5f78df004 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -50,10 +50,11 @@ public: boost::optional&> H = boost::none) const { if (H) { assert(H->size()==size()); - Augmented augmented = expression_.augmented(x); + JacobianMap jacobians; + T value = expression_.value(x, jacobians); // move terms to H, which is pre-allocated to correct size - augmented.move(*H); - return measurement_.localCoordinates(augmented.value()); + move(jacobians, *H); + return measurement_.localCoordinates(value); } else { const T& value = expression_.value(x); return measurement_.localCoordinates(value); @@ -67,15 +68,15 @@ public: return boost::shared_ptr(); // Evaluate error to get Jacobians and RHS vector b - Augmented augmented = expression_.augmented(x); - Vector b = -measurement_.localCoordinates(augmented.value()); + JacobianMap terms; + T value = expression_.value(x, terms); + Vector b = -measurement_.localCoordinates(value); // check(noiseModel_, b); // TODO: use, but defined in NonlinearFactor.cpp // Whiten the corresponding system now // TODO ! this->noiseModel_->WhitenSystem(A, b); // Terms, needed to create JacobianFactor below, are already here! - const JacobianMap& terms = augmented.jacobians(); size_t n = terms.size(); // Get dimensions of matrices diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index e14912a65..38297f156 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -44,10 +44,11 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); TEST(Expression, constant) { Expression R(someR); Values values; - Augmented a = R.augmented(values); - EXPECT(assert_equal(someR, a.value())); + JacobianMap actualMap; + Rot3 actual = R.value(values, actualMap); + EXPECT(assert_equal(someR, actual)); JacobianMap expected; - EXPECT(a.jacobians() == expected); + EXPECT(actualMap == expected); } /* ************************************************************************* */ @@ -56,11 +57,19 @@ TEST(Expression, leaf) { Expression R(100); Values values; values.insert(100, someR); - Augmented a = R.augmented(values); - EXPECT(assert_equal(someR, a.value())); + JacobianMap expected; expected[100] = eye(3); - EXPECT(a.jacobians() == expected); + + JacobianMap actualMap1; + Rot3 actual1 = R.forward(values, actualMap1); + EXPECT(assert_equal(someR, actual1)); + EXPECT(actualMap1 == expected); + + JacobianMap actualMap2; + Rot3 actual2 = R.reverse(values, actualMap2); + EXPECT(assert_equal(someR, actual2)); + EXPECT(actualMap2 == expected); } /* ************************************************************************* */