diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 75af5f634..1be3dabed 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -24,6 +24,9 @@ #include +#include +#include + #include using boost::assign::list_of; using boost::assign::map_list_of; @@ -276,6 +279,125 @@ TEST(Expression, ternary) { EXPECT(expected == ABC.keys()); } +/* ************************************************************************* */ +// Test with multiple compositions on duplicate keys + +bool checkMatricesNear(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, + double tolerance) { + bool near = true; + for (int i = 0; i < lhs.rows(); ++i) { + for (int j = 0; j < lhs.cols(); ++j) { + const double& lij = lhs(i, j); + const double& rij = rhs(i, j); + const double& diff = std::abs(lij - rij); + if (!std::isfinite(lij) || + !std::isfinite(rij) || + diff > tolerance) { + near = false; + + std::cout << "\nposition " << i << "," << j << " evaluates to " + << lij << " and " << rij << std::endl; + } + } + } + return near; +} + + +// Compute finite difference Jacobians for an expression factor. +template +JacobianFactor computeFiniteDifferenceJacobians(ExpressionFactor expression_factor, + const Values& values, + double fd_step) { + Eigen::VectorXd e = expression_factor.unwhitenedError(values); + const size_t rows = e.size(); + + std::map jacobians; + typename ExpressionFactor::const_iterator key_it = expression_factor.begin(); + VectorValues dX = values.zeroVectors(); + for(; key_it != expression_factor.end(); ++key_it) { + size_t key = *key_it; + // Compute central differences using the values struct. + const size_t cols = dX.dim(key); + Eigen::MatrixXd J = Eigen::MatrixXd::Zero(rows, cols); + for(size_t col = 0; col < cols; ++col) { + Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); + dx[col] = fd_step; + dX[key] = dx; + Values eval_values = values.retract(dX); + Eigen::VectorXd left = expression_factor.unwhitenedError(eval_values); + dx[col] = -fd_step; + dX[key] = dx; + eval_values = values.retract(dX); + Eigen::VectorXd right = expression_factor.unwhitenedError(eval_values); + J.col(col) = (left - right) * (1.0/(2.0 * fd_step)); + } + jacobians[key] = J; + } + + // Next step...return JacobianFactor + return JacobianFactor(jacobians, -e); +} + +template +bool testExpressionJacobians(Expression expression, + const Values& values, + double fd_step, + double tolerance) { + // Create factor + size_t size = traits::dimension; + ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); + + // Check linearization + JacobianFactor expected = computeFiniteDifferenceJacobians(f, values, fd_step); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + + typedef std::pair Jacobian; + Jacobian evalJ = jf->jacobianUnweighted(); + Jacobian estJ = expected.jacobianUnweighted(); + + return checkMatricesNear(evalJ.first, estJ.first, tolerance); +} + +double doubleSumImplementation(const double& v1, const double& v2, + OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2) { + if (H1) { + H1->setIdentity(); + } + if (H2) { + H2->setIdentity(); + } + return v1+v2; +} + +TEST(Expression, testMultipleCompositions) { + const double tolerance = 1e-5; + const double fd_step = 1e-9; + + double v1 = 0; + double v2 = 1; + + Values values; + values.insert(1, v1); + values.insert(2, v2); + + Expression ev1(Key(1)); + Expression ev2(Key(2)); + + Expression sum(doubleSumImplementation, ev1, ev2); + Expression sum2(doubleSumImplementation, sum, ev1); + Expression sum3(doubleSumImplementation, sum2, sum); + + std::cout << "Testing sum " << std::endl; + EXPECT(testExpressionJacobians(sum, values, fd_step, tolerance)); + std::cout << "Testing sum2 " << std::endl; + EXPECT(testExpressionJacobians(sum2, values, fd_step, tolerance)); + std::cout << "Testing sum3 " << std::endl; + EXPECT(testExpressionJacobians(sum3, values, fd_step, tolerance)); +} + /* ************************************************************************* */ int main() { TestResult tr;