diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index f9a0c91bf..a16ad8a79 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -48,125 +48,7 @@ namespace gtsam { template 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++]); -} - -//----------------------------------------------------------------------------- -/** - * Value and Jacobians - */ -template -class Augmented { - -private: - - T value_; - JacobianMap jacobians_; - - typedef std::pair Pair; - - /// Insert terms into jacobians_, adding if already exists - void add(const JacobianMap& terms) { - BOOST_FOREACH(const Pair& term, terms) { - JacobianMap::iterator it = jacobians_.find(term.first); - if (it != jacobians_.end()) - it->second += term.second; - else - jacobians_[term.first] = term.second; - } - } - - /// Insert terms into jacobians_, premultiplying by H, adding if already exists - void add(const Matrix& H, const JacobianMap& terms) { - BOOST_FOREACH(const Pair& term, terms) { - JacobianMap::iterator it = jacobians_.find(term.first); - if (it != jacobians_.end()) - it->second += H * term.second; - else - jacobians_[term.first] = H * term.second; - } - } - -public: - - /// Construct value that does not depend on anything - Augmented(const T& t) : - value_(t) { - } - - /// Construct value dependent on a single key - Augmented(const T& t, Key key) : - value_(t) { - size_t n = t.dim(); - jacobians_[key] = Eigen::MatrixXd::Identity(n, n); - } - - /// Construct value, pre-multiply jacobians by dTdA - Augmented(const T& t, const Matrix& dTdA, const JacobianMap& jacobians) : - value_(t) { - add(dTdA, jacobians); - } - - /// Construct value, pre-multiply jacobians - Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1, - const Matrix& dTdA2, const JacobianMap& jacobians2) : - value_(t) { - add(dTdA1, jacobians1); - add(dTdA2, jacobians2); - } - - /// Construct value, pre-multiply jacobians - Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1, - const Matrix& dTdA2, const JacobianMap& jacobians2, const Matrix& dTdA3, - const JacobianMap& jacobians3) : - value_(t) { - add(dTdA1, jacobians1); - add(dTdA2, jacobians2); - add(dTdA3, jacobians3); - } - - /// Return value - const T& value() const { - return value_; - } - - /// Return jacobians - const JacobianMap& jacobians() const { - return jacobians_; - } - - /// Return jacobians - JacobianMap& jacobians() { - return jacobians_; - } - - /// Not dependent on any key - bool constant() const { - return jacobians_.empty(); - } - - /// debugging - void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) { - BOOST_FOREACH(const Pair& term, jacobians_) - std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows() - << "x" << term.second.cols() << ") "; - std::cout << std::endl; - } - - /// Move terms to array, destroys content - void move(std::vector& H) { - move(jacobians_, H); - } - -}; +typedef std::map > JacobianMap; //----------------------------------------------------------------------------- /** @@ -244,39 +126,46 @@ public: return p ? boost::optional(p) : boost::none; } } - // *** This is the main entry point for reverseAD, called from Expression::augmented *** - // Called only once, either inserts identity into Jacobians (Leaf) or starts AD (Function) + /// reverseAD in case of Leaf + template + static void handleLeafCase(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + JacobianMap::iterator it = jacobians.find(key); + if (it == jacobians.end()) { + std::cout << "No block for key " << key << std::endl; + throw std::runtime_error("Reverse AD internal error"); + } + // we have pre-loaded them with zeros + Eigen::Block& block = it->second; + block += dTdA; + } + /** + * *** This is the main entry point for reverseAD, called from Expression *** + * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) + */ void startReverseAD(JacobianMap& jacobians) const { if (kind == Leaf) { // This branch will only be called on trivial Leaf expressions, i.e. Priors size_t n = T::Dim(); - jacobians[content.key] = Eigen::MatrixXd::Identity(n, n); + handleLeafCase(Eigen::MatrixXd::Identity(n, n), jacobians, content.key); } else if (kind == Function) // This is the more typical entry point, starting the AD pipeline - // It is inside the startReverseAD that the correctly dimensioned pipeline is chosen. + // Inside the startReverseAD that the correctly dimensioned pipeline is chosen. content.ptr->startReverseAD(jacobians); } // Either add to Jacobians (Leaf) or propagate (Function) void reverseAD(const Matrix& dTdA, JacobianMap& jacobians) const { - if (kind == Leaf) { - JacobianMap::iterator it = jacobians.find(content.key); - if (it != jacobians.end()) - it->second += dTdA; - else - jacobians[content.key] = dTdA; - } else if (kind == Function) + if (kind == Leaf) + handleLeafCase(dTdA, jacobians, content.key); + else if (kind == Function) content.ptr->reverseAD(dTdA, jacobians); } // Either add to Jacobians (Leaf) or propagate (Function) typedef Eigen::Matrix Jacobian2T; void reverseAD2(const Jacobian2T& dTdA, JacobianMap& jacobians) const { - if (kind == Leaf) { - JacobianMap::iterator it = jacobians.find(content.key); - if (it != jacobians.end()) - it->second += dTdA; - else - jacobians[content.key] = dTdA; - } else if (kind == Function) + if (kind == Leaf) + handleLeafCase(dTdA, jacobians, content.key); + else if (kind == Function) content.ptr->reverseAD2(dTdA, jacobians); } @@ -337,8 +226,8 @@ public: } /// Return dimensions for each argument - virtual std::map dimensions() const { - std::map map; + virtual std::map dimensions() const { + std::map map; return map; } @@ -350,9 +239,6 @@ public: /// Return value virtual T value(const Values& values) const = 0; - /// Return value and derivatives - virtual Augmented forward(const Values& values) const = 0; - /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const = 0; @@ -380,11 +266,6 @@ public: return constant_; } - /// Return value and derivatives - virtual Augmented forward(const Values& values) const { - return Augmented(constant_); - } - /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { @@ -417,8 +298,8 @@ public: } /// Return dimensions for each argument - virtual std::map dimensions() const { - std::map map; + virtual std::map dimensions() const { + std::map map; map[key_] = T::dimension; return map; } @@ -428,11 +309,6 @@ public: return values.at(key_); } - /// Return value and derivatives - virtual Augmented forward(const Values& values) const { - return Augmented(values.at(key_), key_); - } - /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { @@ -540,9 +416,9 @@ struct GenerateFunctionalNode: Argument, Base { } /// Return dimensions for each argument - virtual std::map dimensions() const { - std::map map = Base::dimensions(); - std::map myMap = This::expression->dimensions(); + virtual std::map dimensions() const { + std::map map = Base::dimensions(); + std::map myMap = This::expression->dimensions(); map.insert(myMap.begin(), myMap.end()); return map; } @@ -690,19 +566,6 @@ public: return function_(this->template expression()->value(values), boost::none); } - /// Return value and derivatives - virtual Augmented forward(const Values& values) const { - using boost::none; - Augmented a1 = this->template expression()->forward(values); - - // Declare Jacobians - using boost::mpl::at_c; - typename at_c::type H1; - - T t = function_(a1.value(), H1); - return Augmented(t, H1, a1.jacobians()); - } - /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { @@ -756,21 +619,6 @@ public: none, none); } - /// Return value and derivatives - virtual Augmented forward(const Values& values) const { - using boost::none; - Augmented a1 = this->template expression()->forward(values); - Augmented a2 = this->template expression()->forward(values); - - // Declare Jacobians - using boost::mpl::at_c; - typename at_c::type H1; - typename at_c::type H2; - - T t = function_(a1.value(), a2.value(),H1,H2); - return Augmented(t, H1, a1.jacobians(), H2, a2.jacobians()); - } - /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { @@ -826,24 +674,6 @@ public: none, none, none); } - /// Return value and derivatives - virtual Augmented forward(const Values& values) const { - using boost::none; - Augmented a1 = this->template expression()->forward(values); - Augmented a2 = this->template expression()->forward(values); - Augmented a3 = this->template expression()->forward(values); - - // Declare Jacobians - using boost::mpl::at_c; - typename at_c::type H1; - typename at_c::type H2; - typename at_c::type H3; - - T t = function_(a1.value(), a2.value(), a3.value(),H1,H2,H3); - return Augmented(t, H1, a1.jacobians(), H2, a2.jacobians(), - H3, a3.jacobians()); - } - /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { @@ -859,5 +689,5 @@ public: }; //----------------------------------------------------------------------------- - } +} diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 2f6367734..56b418ea3 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -112,11 +112,6 @@ public: return root_->dimensions(); } - /// Return value and derivatives, forward AD version - Augmented forward(const Values& values) const { - return root_->forward(values); - } - // Return size needed for memory buffer in traceExecution size_t traceSize() const { return root_->traceSize(); diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index a26129d2c..3c310b789 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -57,11 +57,26 @@ public: virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if (H) { + // H should be pre-allocated assert(H->size()==size()); - JacobianMap jacobians; - T value = expression_.value(x, jacobians); - // move terms to H, which is pre-allocated to correct size - move(jacobians, *H); + + // Get dimensions of Jacobian matrices + std::map map = expression_.dimensions(); + + // Create and zero out blocks to be passed to expression_ + DenseIndex i = 0; // For block index + typedef std::pair Pair; + std::map blocks; + BOOST_FOREACH(const Pair& pair, map) { + Matrix& Hi = H->at(i++); + size_t mi = pair.second; // width of i'th Jacobian + Hi.resize(T::dimension, mi); + Hi.setZero(); // zero out + Eigen::Block block = Hi.block(0,0,T::dimension, mi); + blocks.insert(std::make_pair(pair.first, block)); + } + + T value = expression_.value(x, blocks); return measurement_.localCoordinates(value); } else { const T& value = expression_.value(x); @@ -77,7 +92,7 @@ public: if (!this->active(x)) return boost::shared_ptr(); - // Get dimensions of matrices + // Get dimensions of Jacobian matrices std::map map = expression_.dimensions(); size_t n = map.size(); @@ -87,17 +102,18 @@ public: // Construct block matrix, is of right size but un-initialized VerticalBlockMatrix Ab(dims, T::dimension, true); + Ab.matrix().setZero(); // zero out - // Create and zero out blocks to be passed to expression_ + // Create blocks to be passed to expression_ DenseIndex i = 0; // For block index typedef std::pair Pair; - BOOST_FOREACH(const Pair& keyValue, map) { - Ab(i++) = zeros(T::dimension, keyValue.second); + std::map blocks; + BOOST_FOREACH(const Pair& pair, map) { + blocks.insert(std::make_pair(pair.first, Ab(i++))); } // Evaluate error to get Jacobians and RHS vector b - // JacobianMap terms; - T value = expression_.value(x); + T value = expression_.value(x, blocks); Vector b = -measurement_.localCoordinates(value); // Whiten the corresponding system now diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index e6fd12ab4..a2aa12868 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -65,13 +65,11 @@ TEST(Expression, leaf) { values.insert(100, someR); JacobianMap expected; - expected[100] = eye(3); - - Augmented actual1 = R.forward(values); - EXPECT(assert_equal(someR, actual1.value())); - EXPECT(actual1.jacobians() == expected); + Matrix H = eye(3); + expected.insert(make_pair(100,H.block(0,0,3,3))); JacobianMap actualMap2; + actualMap2.insert(make_pair(100,H.block(0,0,3,3))); Rot3 actual2 = R.reverse(values, actualMap2); EXPECT(assert_equal(someR, actual2)); EXPECT(actualMap2 == expected); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 5867f9dcf..93c2ecac1 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -62,41 +62,41 @@ TEST(ExpressionFactor, Leaf) { EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); } -/* ************************************************************************* */ -// non-zero noise model -TEST(ExpressionFactor, Model) { - using namespace leaf; - - SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); - - // Create old-style factor to create expected value and derivatives - PriorFactor old(2, Point2(0, 0), model); - - // Concise version - ExpressionFactor f(model, Point2(0, 0), p); - EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); - EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf2 = f.linearize(values); - EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); -} - -/* ************************************************************************* */ -// Constrained noise model -TEST(ExpressionFactor, Constrained) { - using namespace leaf; - - SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0)); - - // Create old-style factor to create expected value and derivatives - PriorFactor old(2, Point2(0, 0), model); - - // Concise version - ExpressionFactor f(model, Point2(0, 0), p); - EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); - EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf2 = f.linearize(values); - EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); -} +///* ************************************************************************* */ +//// non-zero noise model +//TEST(ExpressionFactor, Model) { +// using namespace leaf; +// +// SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); +// +// // Create old-style factor to create expected value and derivatives +// PriorFactor old(2, Point2(0, 0), model); +// +// // Concise version +// ExpressionFactor f(model, Point2(0, 0), p); +// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); +// EXPECT_LONGS_EQUAL(2, f.dim()); +// boost::shared_ptr gf2 = f.linearize(values); +// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); +//} +// +///* ************************************************************************* */ +//// Constrained noise model +//TEST(ExpressionFactor, Constrained) { +// using namespace leaf; +// +// SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0)); +// +// // Create old-style factor to create expected value and derivatives +// PriorFactor old(2, Point2(0, 0), model); +// +// // Concise version +// ExpressionFactor f(model, Point2(0, 0), p); +// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); +// EXPECT_LONGS_EQUAL(2, f.dim()); +// boost::shared_ptr gf2 = f.linearize(values); +// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); +//} /* ************************************************************************* */ // Unary(Leaf)) @@ -256,15 +256,6 @@ TEST(ExpressionFactor, tree) { Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); - // Compare reverse and forward - { - JacobianMap expectedMap; // via reverse - Point2 expectedValue = uv_hat.reverse(values, expectedMap); - Augmented actual = uv_hat.forward(values); - EXPECT(assert_equal(expectedValue, actual.value())); - EXPECT(actual.jacobians() == expectedMap); - } - // Create factor and check value, dimension, linearization ExpressionFactor f(model, measured, uv_hat); EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); @@ -292,7 +283,7 @@ TEST(ExpressionFactor, tree) { /* ************************************************************************* */ -TEST(ExpressionFactor, compose1) { +TEST(ExpressionFactor, Compose1) { // Create expression Rot3_ R1(1), R2(2);