From cba120c96d68f956082e82de0288d917d01b53d6 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 21 Nov 2012 19:02:13 +0000 Subject: [PATCH] LinearContainerFactor now includes ability to "relinearize" when supplied with an original linearization point --- .../nonlinear/LinearContainerFactor.cpp | 41 +++- .../nonlinear/LinearContainerFactor.h | 5 +- .../tests/testLinearContainerFactor.cpp | 217 ++++++++++++++---- 3 files changed, 213 insertions(+), 50 deletions(-) diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp index 8d3346a8e..fa7813bb9 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp @@ -140,7 +140,7 @@ double LinearContainerFactor::error(const Values& c) const { if (!linearizationPoint_) return 0; - // Extract subset of values for comparision + // Extract subset of values for comparison Values csub; BOOST_FOREACH(const gtsam::Key& key, keys()) csub.insert(key, c.at(key)); @@ -185,7 +185,39 @@ GaussianFactor::shared_ptr LinearContainerFactor::order(const Ordering& ordering /* ************************************************************************* */ GaussianFactor::shared_ptr LinearContainerFactor::linearize( const Values& c, const Ordering& ordering) const { - return order(ordering); + if (!hasLinearizationPoint()) + return order(ordering); + + // Extract subset of values + Values subsetC; + BOOST_FOREACH(const gtsam::Key& key, this->keys()) + subsetC.insert(key, c.at(key)); + + // Create a temp ordering for this factor + Ordering localOrdering = *subsetC.orderingArbitrary(); + + // Determine delta between linearization points using new ordering + VectorValues delta = linearizationPoint_->localCoordinates(subsetC, localOrdering); + + // clone and reorder linear factor to final ordering + GaussianFactor::shared_ptr linFactor = order(localOrdering); + if (isJacobian()) { + JacobianFactor::shared_ptr jacFactor = boost::shared_dynamic_cast(linFactor); + jacFactor->getb() += jacFactor->unweighted_error(delta) + jacFactor->getb(); + } else { + HessianFactor::shared_ptr hesFactor = boost::shared_dynamic_cast(linFactor); + size_t dim = hesFactor->linearTerm().size(); + Eigen::Block Gview = hesFactor->info().block(0, 0, dim, dim); + Vector G_delta = Gview.selfadjointView() * delta.vector(); + hesFactor->constantTerm() += delta.vector().dot(G_delta) + delta.vector().dot(hesFactor->linearTerm()); + hesFactor->linearTerm() += G_delta; + } + + // reset ordering + Ordering::InvertedMap invLocalOrdering = localOrdering.invert(); + BOOST_FOREACH(Index& idx, linFactor->keys()) + idx = ordering[invLocalOrdering[idx] ]; + return linFactor; } /* ************************************************************************* */ @@ -193,6 +225,11 @@ bool LinearContainerFactor::isJacobian() const { return boost::shared_dynamic_cast(factor_); } +/* ************************************************************************* */ +bool LinearContainerFactor::isHessian() const { + return boost::shared_dynamic_cast(factor_); +} + /* ************************************************************************* */ JacobianFactor::shared_ptr LinearContainerFactor::toJacobian() const { return boost::shared_dynamic_cast(factor_); diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.h b/gtsam_unstable/nonlinear/LinearContainerFactor.h index 70c3af735..b2b3f9645 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.h +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.h @@ -114,10 +114,13 @@ public: // casting syntactic sugar + inline bool hasLinearizationPoint() const { return linearizationPoint_; } + /** - * Simple check whether this is a Jacobian or Hessian factor + * Simple checks whether this is a Jacobian or Hessian factor */ bool isJacobian() const; + bool isHessian() const; /** Casts to JacobianFactor */ JacobianFactor::shared_ptr toJacobian() const; diff --git a/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp index ed2ec59c6..9ff2cec12 100644 --- a/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp @@ -7,11 +7,15 @@ #include +#include + #include #include #include +using namespace std; +using namespace boost::assign; using namespace gtsam; const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)); @@ -27,20 +31,26 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) { Ordering initOrdering; initOrdering += x1, x2, l1, l2; - JacobianFactor expLinFactor1( - initOrdering[l1], - Matrix_(2,2, - 2.74222, -0.0067457, - 0.0, 2.63624), - initOrdering[l2], - Matrix_(2,2, - -0.0455167, -0.0443573, - -0.0222154, -0.102489), - Vector_(2, 0.0277052, - -0.0533393), - diag_model2); + Matrix A1 = Matrix_(2,2, + 2.74222, -0.0067457, + 0.0, 2.63624); + Matrix A2 = Matrix_(2,2, + -0.0455167, -0.0443573, + -0.0222154, -0.102489); + Vector b = Vector_(2, 0.0277052, + -0.0533393); + + JacobianFactor expLinFactor(initOrdering[l1], A1, initOrdering[l2], A2, b, diag_model2); + + LinearContainerFactor actFactor(expLinFactor, initOrdering); + EXPECT_LONGS_EQUAL(2, actFactor.size()); + EXPECT(actFactor.isJacobian()); + EXPECT(!actFactor.isHessian()); + + // check keys + std::vector expKeys; expKeys += l1, l2; + EXPECT(assert_container_equality(expKeys, actFactor.keys())); - LinearContainerFactor actFactor1(expLinFactor1, initOrdering); Values values; values.insert(l1, landmark1); values.insert(l2, landmark2); @@ -48,26 +58,13 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) { values.insert(x2, poseA2); // Check reconstruction from same ordering - GaussianFactor::shared_ptr actLinearizationA = actFactor1.linearize(values, initOrdering); - EXPECT(assert_equal(*expLinFactor1.clone(), *actLinearizationA, tol)); + GaussianFactor::shared_ptr actLinearizationA = actFactor.linearize(values, initOrdering); + EXPECT(assert_equal(*expLinFactor.clone(), *actLinearizationA, tol)); // Check reconstruction from new ordering Ordering newOrdering; newOrdering += x1, l1, x2, l2; - GaussianFactor::shared_ptr actLinearizationB = actFactor1.linearize(values, newOrdering); - - JacobianFactor expLinFactor2( - newOrdering[l1], - Matrix_(2,2, - 2.74222, -0.0067457, - 0.0, 2.63624), - newOrdering[l2], - Matrix_(2,2, - -0.0455167, -0.0443573, - -0.0222154, -0.102489), - Vector_(2, 0.0277052, - -0.0533393), - diag_model2); - + GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(values, newOrdering); + JacobianFactor expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, b, diag_model2); EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol)); } @@ -76,18 +73,16 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { Ordering ordering; ordering += x1, x2, l1, l2; - JacobianFactor expLinFactor( - ordering[l1], - Matrix_(2,2, - 2.74222, -0.0067457, - 0.0, 2.63624), - ordering[l2], - Matrix_(2,2, - -0.0455167, -0.0443573, - -0.0222154, -0.102489), - Vector_(2, 0.0277052, - -0.0533393), - diag_model2); + Matrix A1 = Matrix_(2,2, + 2.74222, -0.0067457, + 0.0, 2.63624); + Matrix A2 = Matrix_(2,2, + -0.0455167, -0.0443573, + -0.0222154, -0.102489); + Vector b = Vector_(2, 0.0277052, + -0.0533393); + + JacobianFactor expLinFactor(ordering[l1], A1, ordering[l2], A2, b, diag_model2); Values values; values.insert(l1, landmark1); @@ -107,16 +102,27 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { expLinPoint.insert(l1, landmark1); expLinPoint.insert(l2, landmark2); CHECK(actFactor.linearizationPoint()); + EXPECT(actFactor.hasLinearizationPoint()); EXPECT(assert_equal(expLinPoint, *actFactor.linearizationPoint())); // Check error evaluation + Vector delta_l1 = Vector_(2, 1.0, 2.0); + Vector delta_l2 = Vector_(2, 3.0, 4.0); + VectorValues delta = values.zeroVectors(ordering); - delta.at(ordering[l1]) = Vector_(2, 1.0, 2.0); - delta.at(ordering[l2]) = Vector_(2, 3.0, 4.0); + delta.at(ordering[l1]) = delta_l1; + delta.at(ordering[l2]) = delta_l2; Values noisyValues = values.retract(delta, ordering); double expError = expLinFactor.error(delta); EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol); EXPECT_DOUBLES_EQUAL(expLinFactor.error(values.zeroVectors(ordering)), actFactor.error(values), tol); + + // Check linearization with corrections for updated linearization point + Ordering newOrdering; newOrdering += x1, l1, x2, l2; + GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(noisyValues, newOrdering); + Vector bprime = b + A1 * delta_l1 + A2 * delta_l2; + JacobianFactor expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, bprime, diag_model2); + EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol)); } /* ************************************************************************* */ @@ -125,10 +131,14 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) { Matrix G12 = Matrix_(1,2, 2.0, 4.0); Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0); - Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); + Matrix G22 = Matrix_(2,2, 3.0, 5.0, + 0.0, 6.0); + Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, + 1.0, 2.0, 4.0); - Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); + Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, + 0.0, 5.0, 6.0, + 0.0, 0.0, 9.0); Vector g1 = Vector_(1, -7.0); Vector g2 = Vector_(2, -8.0, -9.0); @@ -147,6 +157,9 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) { values.insert(x2, poseA2); LinearContainerFactor actFactor(initFactor, initOrdering); + EXPECT(!actFactor.isJacobian()); + EXPECT(actFactor.isHessian()); + GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values, initOrdering); EXPECT(assert_equal(*initFactor.clone(), *actLinearization1, tol)); @@ -157,6 +170,116 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) { EXPECT(assert_equal(*expLinFactor.clone(), *actLinearization2, tol)); } +/* ************************************************************************* */ +TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { + // 2 variable example, one pose, one landmark (planar) + // Initial ordering: x1, l1 + + Matrix G11 = Matrix_(3,3, + 1.0, 2.0, 3.0, + 0.0, 5.0, 6.0, + 0.0, 0.0, 9.0); + Matrix G12 = Matrix_(3,2, + 1.0, 2.0, + 3.0, 5.0, + 4.0, 6.0); + Vector g1 = Vector_(3, 1.0, 2.0, 3.0); + + Matrix G22 = Matrix_(2,2, + 0.5, 0.2, + 0.0, 0.6); + + Vector g2 = Vector_(2, -8.0, -9.0); + + double f = 10.0; + + // Construct full matrices + Matrix G(5,5); + G << G11, G12, Matrix::Zero(2,3), G22; + + Ordering ordering; ordering += x1, x2, l1; + HessianFactor initFactor(ordering[x1], ordering[l1], G11, G12, g1, G22, g2, f); + + Values linearizationPoint, expLinPoints; + linearizationPoint.insert(l1, landmark1); + linearizationPoint.insert(x1, poseA1); + expLinPoints = linearizationPoint; + linearizationPoint.insert(x2, poseA2); + + LinearContainerFactor actFactor(initFactor, ordering, linearizationPoint); + EXPECT(!actFactor.isJacobian()); + EXPECT(actFactor.isHessian()); + + EXPECT(actFactor.hasLinearizationPoint()); + Values actLinPoint = *actFactor.linearizationPoint(); + EXPECT(assert_equal(expLinPoints, actLinPoint)); + + // Create delta + Vector delta_l1 = Vector_(2, 1.0, 2.0); + Vector delta_x1 = Vector_(3, 3.0, 4.0, 0.5); + Vector delta_x2 = Vector_(3, 6.0, 7.0, 0.3); + + // Check error calculation + VectorValues delta = linearizationPoint.zeroVectors(ordering); + delta.at(ordering[l1]) = delta_l1; + delta.at(ordering[x1]) = delta_x1; + delta.at(ordering[x2]) = delta_x2; + EXPECT(assert_equal(Vector_(5, 3.0, 4.0, 0.5, 1.0, 2.0), delta.vector(initFactor.keys()))); + Values noisyValues = linearizationPoint.retract(delta, ordering); + + double expError = initFactor.error(delta); + EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol); + EXPECT_DOUBLES_EQUAL(initFactor.error(linearizationPoint.zeroVectors(ordering)), actFactor.error(linearizationPoint), tol); + + // Compute updated versions + Vector dv = Vector_(5, 3.0, 4.0, 0.5, 1.0, 2.0); + Vector g(5); g << g1, g2; + Vector g_prime = G.selfadjointView() * dv + g; + + // Check linearization with corrections for updated linearization point + Vector g1_prime = g_prime.head(3); + Vector g2_prime = g_prime.tail(2); + double f_prime = f + dv.transpose() * G.selfadjointView() * dv + dv.transpose() * g; + HessianFactor expNewFactor(ordering[x1], ordering[l1], G11, G12, g1_prime, G22, g2_prime, f_prime); + EXPECT(assert_equal(*expNewFactor.clone(), *actFactor.linearize(noisyValues, ordering), tol)); +} + +/* ************************************************************************* */ +TEST( testLinearContainerFactor, creation ) { + // Create a set of local keys (No robot label) + Key l1 = 11, l2 = 12, + l3 = 13, l4 = 14, + l5 = 15, l6 = 16, + l7 = 17, l8 = 18; + + // creating an ordering to decode the linearized factor + Ordering ordering; + ordering += l1,l2,l3,l4,l5,l6,l7,l8; + + // create a linear factor + SharedDiagonal model = noiseModel::Unit::Create(2); + JacobianFactor::shared_ptr linear_factor(new JacobianFactor( + ordering[l3], eye(2,2), ordering[l5], 2.0 * eye(2,2), zero(2), model)); + + // create a set of values - build with full set of values + gtsam::Values full_values, exp_values; + full_values.insert(l3, Point2(1.0, 2.0)); + full_values.insert(l5, Point2(4.0, 3.0)); + exp_values = full_values; + full_values.insert(l1, Point2(3.0, 7.0)); + + LinearContainerFactor actual(linear_factor, ordering, full_values); + + // Verify the keys + std::vector expKeys; + expKeys += l3, l5; + EXPECT(assert_container_equality(expKeys, actual.keys())); + + // Verify subset of linearization points + EXPECT(assert_equal(exp_values, actual.linearizationPoint(), tol)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ +