Improve Manifold testing 1. Increase number of trials. 2. Make all the matchers per point. 3. Add matchers for delta = 0. 4. Add a macro which invokes all the matchers, reducing boilerplate Change-Id: Ia1bf110323c5877a1b92aef34c12c39008256f05
diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc index 40c36be..23c59af 100644 --- a/internal/ceres/manifold_test.cc +++ b/internal/ceres/manifold_test.cc
@@ -50,13 +50,51 @@ // helpful to expose them as testing utilities which can be used by the user // when implementing their own manifold objects. -constexpr int kNumTrials = 10; -constexpr double kEpsilon = 1e-10; +constexpr int kNumTrials = 100; +constexpr double kEpsilon = 1e-9; + +// Checks that the invariant Plus(x, 0) == x holds. +MATCHER_P(XPlusZeroIsXAt, x, "") { + const int ambient_size = arg.AmbientSize(); + const int tangent_size = arg.TangentSize(); + + Vector actual = Vector::Zero(ambient_size); + Vector zero = Vector::Zero(tangent_size); + arg.Plus(x.data(), zero.data(), actual.data()); + const double n = (actual - x).norm(); + const double d = x.norm(); + const double diffnorm = (d == 0.0) ? n : (n / d); + if (diffnorm > kEpsilon) { + *result_listener << "\nexpected (x): " << x.transpose() + << "\nactual: " << actual.transpose() + << "\ndiffnorm: " << diffnorm; + return false; + } + return true; +} + +// Checks that the invariant Minus(x, x) == 0 holds. +MATCHER_P(XMinusXIsZeroAt, x, "") { + const int ambient_size = arg.AmbientSize(); + const int tangent_size = arg.TangentSize(); + Vector actual = Vector::Zero(tangent_size); + arg.Minus(x.data(), x.data(), actual.data()); + const double diffnorm = actual.norm(); + if (diffnorm > kEpsilon) { + *result_listener << "\nx: " << x.transpose() // + << "\nexpected: 0 0 0" + << "\nactual: " << actual.transpose() + << "\ndiffnorm: " << diffnorm; + return false; + } + return true; +} // Helper struct to curry Plus(x, .) so that it can be numerically // differentiated. struct PlusFunctor { - PlusFunctor(const Manifold& manifold, double* x) : manifold(manifold), x(x) {} + PlusFunctor(const Manifold& manifold, const double* x) + : manifold(manifold), x(x) {} bool operator()(double const* const* parameters, double* x_plus_delta) const { return manifold.Plus(x, parameters[0], x_plus_delta); } @@ -67,70 +105,62 @@ // Checks that the output of PlusJacobian matches the one obtained by // numerically evaluating D_2 Plus(x,0). -MATCHER(HasCorrectPlusJacobian, "") { +MATCHER_P(HasCorrectPlusJacobianAt, x, "") { const int ambient_size = arg.AmbientSize(); const int tangent_size = arg.TangentSize(); - for (int trial = 0; trial < kNumTrials; ++trial) { - Vector x = Vector::Random(ambient_size); + NumericDiffOptions options; + options.ridders_relative_initial_step_size = 1e-4; - NumericDiffOptions options; - DynamicNumericDiffCostFunction<PlusFunctor, RIDDERS> cost_function( - new PlusFunctor(arg, x.data())); - cost_function.AddParameterBlock(tangent_size); - cost_function.SetNumResiduals(ambient_size); + DynamicNumericDiffCostFunction<PlusFunctor, RIDDERS> cost_function( + new PlusFunctor(arg, x.data()), TAKE_OWNERSHIP, options); + cost_function.AddParameterBlock(tangent_size); + cost_function.SetNumResiduals(ambient_size); - Vector zero = Vector::Zero(tangent_size); - double* parameters[1] = {zero.data()}; + Vector zero = Vector::Zero(tangent_size); + double* parameters[1] = {zero.data()}; - Vector x_plus_zero = Vector::Zero(ambient_size); - Matrix expected = Matrix::Zero(ambient_size, tangent_size); - double* jacobians[1] = {expected.data()}; + Vector x_plus_zero = Vector::Zero(ambient_size); + Matrix expected = Matrix::Zero(ambient_size, tangent_size); + double* jacobians[1] = {expected.data()}; - CHECK(cost_function.Evaluate(parameters, x_plus_zero.data(), jacobians)); + CHECK(cost_function.Evaluate(parameters, x_plus_zero.data(), jacobians)); - Matrix actual = Matrix::Random(ambient_size, tangent_size); - arg.PlusJacobian(x.data(), actual.data()); + Matrix actual = Matrix::Random(ambient_size, tangent_size); + arg.PlusJacobian(x.data(), actual.data()); - const double n = (actual - expected).norm(); - const double d = expected.norm(); - const bool result = (d == 0.0) ? (n == 0.0) : (n <= kEpsilon * d); - if (!result) { - *result_listener << "\nx: " << x.transpose() << "\nexpected: \n" - << expected << "\nactual:\n" - << actual << "\ndiff:\n" - << expected - actual; - - return false; - } + const double n = (actual - expected).norm(); + const double d = expected.norm(); + const double diffnorm = (d == 0.0) ? n : n / d; + if (diffnorm > kEpsilon) { + *result_listener << "\nx: " << x.transpose() << "\nexpected: \n" + << expected << "\nactual:\n" + << actual << "\ndiff:\n" + << expected - actual << "\ndiffnorm : " << diffnorm; + return false; } return true; } // Checks that the invariant Minus(Plus(x, delta), x) == delta holds. -MATCHER_P(HasCorrectMinusAt, x, "") { +MATCHER_P2(HasCorrectMinusAt, x, delta, "") { const int ambient_size = arg.AmbientSize(); const int tangent_size = arg.TangentSize(); - for (int trial = 0; trial < kNumTrials; ++trial) { - Vector expected = Vector::Random(tangent_size); + Vector x_plus_delta = Vector::Zero(ambient_size); + arg.Plus(x.data(), delta.data(), x_plus_delta.data()); + Vector actual = Vector::Zero(tangent_size); + arg.Minus(x_plus_delta.data(), x.data(), actual.data()); - Vector x_plus_expected = Vector::Zero(ambient_size); - arg.Plus(x.data(), expected.data(), x_plus_expected.data()); - - Vector actual = Vector::Zero(tangent_size); - arg.Minus(x_plus_expected.data(), x.data(), actual.data()); - - const double n = (actual - expected).norm(); - const double d = expected.norm(); - const bool result = (d == 0.0) ? (n == 0.0) : (n <= kEpsilon * d); - if (!result) { - *result_listener << "\nx: " << x.transpose() - << "\nexpected: " << expected.transpose() - << "\nactual:" << actual.transpose() - << "\ndiff:" << (expected - actual).transpose(); - - return false; - } + const double n = (actual - delta).norm(); + const double d = delta.norm(); + const double diffnorm = (d == 0.0) ? n : (n / d); + if (diffnorm > kEpsilon) { + *result_listener << "\nx: " << x.transpose() + << "\nexpected: " << delta.transpose() + << "\nactual:" << actual.transpose() + << "\ndiff:" << (delta - actual).transpose() + << "\ndiffnorm: " << diffnorm; + return false; } return true; } @@ -138,7 +168,7 @@ // Helper struct to curry Minus(., x) so that it can be numerically // differentiated. struct MinusFunctor { - MinusFunctor(const Manifold& manifold, double* x) + MinusFunctor(const Manifold& manifold, const double* x) : manifold(manifold), x(x) {} bool operator()(double const* const* parameters, double* y_minus_x) const { return manifold.Minus(parameters[0], x, y_minus_x); @@ -150,56 +180,52 @@ // Checks that the output of MinusJacobian matches the one obtained by // numerically evaluating D_1 Minus(x,x). -MATCHER(HasCorrectMinusJacobian, "") { +MATCHER_P(HasCorrectMinusJacobianAt, x, "") { const int ambient_size = arg.AmbientSize(); const int tangent_size = arg.TangentSize(); - for (int trial = 0; trial < kNumTrials; ++trial) { - Vector y = Vector::Random(ambient_size); - Vector x = y; - Vector y_minus_x = Vector::Zero(tangent_size); + Vector y = x; + Vector y_minus_x = Vector::Zero(tangent_size); - NumericDiffOptions options; - DynamicNumericDiffCostFunction<MinusFunctor, RIDDERS> cost_function( - new MinusFunctor(arg, x.data())); - cost_function.AddParameterBlock(ambient_size); - cost_function.SetNumResiduals(tangent_size); + NumericDiffOptions options; + options.ridders_relative_initial_step_size = 1e-4; + DynamicNumericDiffCostFunction<MinusFunctor, RIDDERS> cost_function( + new MinusFunctor(arg, x.data()), TAKE_OWNERSHIP, options); + cost_function.AddParameterBlock(ambient_size); + cost_function.SetNumResiduals(tangent_size); - double* parameters[1] = {y.data()}; + double* parameters[1] = {y.data()}; - Matrix expected = Matrix::Zero(tangent_size, ambient_size); - double* jacobians[1] = {expected.data()}; + Matrix expected = Matrix::Zero(tangent_size, ambient_size); + double* jacobians[1] = {expected.data()}; - CHECK(cost_function.Evaluate(parameters, y_minus_x.data(), jacobians)); + CHECK(cost_function.Evaluate(parameters, y_minus_x.data(), jacobians)); - Matrix actual = Matrix::Random(tangent_size, ambient_size); - arg.MinusJacobian(x.data(), actual.data()); + Matrix actual = Matrix::Random(tangent_size, ambient_size); + arg.MinusJacobian(x.data(), actual.data()); - const double n = (actual - expected).norm(); - const double d = expected.norm(); - const bool result = (d == 0.0) ? (n == 0.0) : (n <= kEpsilon * d); - if (!result) { - *result_listener << "\nx: " << x.transpose() << "\nexpected: \n" - << expected << "\nactual:\n" - << actual << "\ndiff:\n" - << expected - actual; - - return false; - } + const double n = (actual - expected).norm(); + const double d = expected.norm(); + const double diffnorm = (d == 0.0) ? n : (n / d); + if (diffnorm > kEpsilon) { + *result_listener << "\nx: " << x.transpose() << "\nexpected: \n" + << expected << "\nactual:\n" + << actual << "\ndiff:\n" + << expected - actual << "\ndiffnorm: " << diffnorm; + return false; } return true; } // Verify that the output of RightMultiplyByPlusJacobian is ambient_matrix * // plus_jacobian. -MATCHER(HasCorrectRightMultiplyByPlusJacobian, "") { +MATCHER_P(HasCorrectRightMultiplyByPlusJacobianAt, x, "") { const int ambient_size = arg.AmbientSize(); const int tangent_size = arg.TangentSize(); constexpr int kMinNumRows = 0; constexpr int kMaxNumRows = 3; for (int num_rows = kMinNumRows; num_rows <= kMaxNumRows; ++num_rows) { - Vector x = Vector::Random(ambient_size); Matrix plus_jacobian = Matrix::Random(ambient_size, tangent_size); arg.PlusJacobian(x.data(), plus_jacobian.data()); @@ -211,53 +237,61 @@ x.data(), num_rows, ambient_matrix.data(), actual.data()); const double n = (actual - expected).norm(); const double d = expected.norm(); - const bool result = (d == 0.0) ? (n == 0.0) : (n <= kEpsilon * d); - if (!result) { + const double diffnorm = (d == 0.0) ? n : (n / d); + if (diffnorm > kEpsilon) { *result_listener << "\nx: " << x.transpose() << "\nambient_matrix : \n" << ambient_matrix << "\nplus_jacobian : \n" << plus_jacobian << "\nexpected: \n" << expected << "\nactual:\n" << actual << "\ndiff:\n" - << expected - actual; - + << expected - actual << "\ndiffnorm : " << diffnorm; return false; } } return true; } +#define EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta) \ + Vector zero_tangent = Vector::Zero(manifold.TangentSize()); \ + EXPECT_THAT(manifold, XPlusZeroIsXAt(x)); \ + EXPECT_THAT(manifold, XMinusXIsZeroAt(x)); \ + EXPECT_THAT(manifold, HasCorrectMinusAt(x, delta)); \ + EXPECT_THAT(manifold, HasCorrectMinusAt(x, zero_tangent)); \ + EXPECT_THAT(manifold, HasCorrectPlusJacobianAt(x)); \ + EXPECT_THAT(manifold, HasCorrectMinusJacobianAt(x)); \ + EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobianAt(x)); + TEST(EuclideanManifold, NormalFunctionTest) { EuclideanManifold manifold(3); EXPECT_EQ(manifold.AmbientSize(), 3); EXPECT_EQ(manifold.TangentSize(), 3); - Vector x = Vector::Random(3); - Vector delta = Vector::Random(3); - Vector x_plus_delta = Vector::Zero(3); + Vector zero_tangent = Vector::Zero(manifold.TangentSize()); + for (int trial = 0; trial < kNumTrials; ++trial) { + const Vector x = Vector::Random(manifold.AmbientSize()); + Vector delta = Vector::Random(manifold.TangentSize()); + Vector x_plus_delta = Vector::Zero(manifold.AmbientSize()); - manifold.Plus(x.data(), delta.data(), x_plus_delta.data()); - EXPECT_NEAR( - (x_plus_delta - x - delta).norm() / (x + delta).norm(), 0.0, kEpsilon); - EXPECT_THAT(manifold, HasCorrectMinusAt(x)); - EXPECT_THAT(manifold, HasCorrectPlusJacobian()); - EXPECT_THAT(manifold, HasCorrectMinusJacobian()); - EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobian()); + manifold.Plus(x.data(), delta.data(), x_plus_delta.data()); + EXPECT_NEAR( + (x_plus_delta - x - delta).norm() / (x + delta).norm(), 0.0, kEpsilon); + + EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta); + } } TEST(SubsetManifold, EmptyConstantParameters) { SubsetManifold manifold(3, {}); + for (int trial = 0; trial < kNumTrials; ++trial) { + Vector x = Vector::Random(3); + Vector delta = Vector::Random(3); + Vector x_plus_delta = Vector::Zero(3); - Vector x = Vector::Random(3); - Vector delta = Vector::Random(3); - Vector x_plus_delta = Vector::Zero(3); - - manifold.Plus(x.data(), delta.data(), x_plus_delta.data()); - EXPECT_NEAR( - (x_plus_delta - x - delta).norm() / (x + delta).norm(), 0.0, kEpsilon); - EXPECT_THAT(manifold, HasCorrectMinusAt(x)); - EXPECT_THAT(manifold, HasCorrectPlusJacobian()); - EXPECT_THAT(manifold, HasCorrectMinusJacobian()); - EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobian()); + manifold.Plus(x.data(), delta.data(), x_plus_delta.data()); + EXPECT_NEAR( + (x_plus_delta - x - delta).norm() / (x + delta).norm(), 0.0, kEpsilon); + EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta); + } } TEST(SubsetManifold, NegativeParameterIndexDeathTest) { @@ -277,26 +311,30 @@ TEST(SubsetManifold, NormalFunctionTest) { const int kAmbientSize = 4; const int kTangentSize = 3; - Vector x = Vector::Random(kAmbientSize); - Vector delta = Vector::Random(kTangentSize); - Vector x_plus_delta = Vector::Zero(kAmbientSize); for (int i = 0; i < kAmbientSize; ++i) { - SubsetManifold manifold(kAmbientSize, {i}); - x_plus_delta.setZero(); - manifold.Plus(x.data(), delta.data(), x_plus_delta.data()); - int k = 0; - for (int j = 0; j < kAmbientSize; ++j) { - if (j == i) { - EXPECT_EQ(x_plus_delta[j], x[j]); - } else { - EXPECT_EQ(x_plus_delta[j], x[j] + delta[k++]); + SubsetManifold manifold_with_ith_parameter_constant(kAmbientSize, {i}); + + for (int trial = 0; trial < kNumTrials; ++trial) { + const Vector x = Vector::Random(kAmbientSize); + Vector delta = Vector::Random(kTangentSize); + Vector x_plus_delta = Vector::Zero(kAmbientSize); + + x_plus_delta.setZero(); + manifold_with_ith_parameter_constant.Plus( + x.data(), delta.data(), x_plus_delta.data()); + int k = 0; + for (int j = 0; j < kAmbientSize; ++j) { + if (j == i) { + EXPECT_EQ(x_plus_delta[j], x[j]); + } else { + EXPECT_EQ(x_plus_delta[j], x[j] + delta[k++]); + } } + + EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD( + manifold_with_ith_parameter_constant, x, delta); } - EXPECT_THAT(manifold, HasCorrectMinusAt(x)); - EXPECT_THAT(manifold, HasCorrectPlusJacobian()); - EXPECT_THAT(manifold, HasCorrectMinusJacobian()); - EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobian()); } } @@ -350,48 +388,47 @@ ProductManifold manifold(manifold1, manifold2, manifold3, manifold4); - Vector x = Vector::Random(manifold.AmbientSize()); - Vector delta = Vector::Random(manifold.TangentSize()); - Vector x_plus_delta = Vector::Zero(manifold.AmbientSize()); - Vector x_plus_delta_expected = Vector::Zero(manifold.AmbientSize()); + for (int trial = 0; trial < kNumTrials; ++trial) { + Vector x = Vector::Random(manifold.AmbientSize()); + Vector delta = Vector::Random(manifold.TangentSize()); + Vector x_plus_delta = Vector::Zero(manifold.AmbientSize()); + Vector x_plus_delta_expected = Vector::Zero(manifold.AmbientSize()); - EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), x_plus_delta.data())); + EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), x_plus_delta.data())); - int ambient_cursor = 0; - int tangent_cursor = 0; + int ambient_cursor = 0; + int tangent_cursor = 0; - EXPECT_TRUE(manifold1->Plus(&x[ambient_cursor], - &delta[tangent_cursor], - &x_plus_delta_expected[ambient_cursor])); - ambient_cursor += manifold1->AmbientSize(); - tangent_cursor += manifold1->TangentSize(); + EXPECT_TRUE(manifold1->Plus(&x[ambient_cursor], + &delta[tangent_cursor], + &x_plus_delta_expected[ambient_cursor])); + ambient_cursor += manifold1->AmbientSize(); + tangent_cursor += manifold1->TangentSize(); - EXPECT_TRUE(manifold2->Plus(&x[ambient_cursor], - &delta[tangent_cursor], - &x_plus_delta_expected[ambient_cursor])); - ambient_cursor += manifold2->AmbientSize(); - tangent_cursor += manifold2->TangentSize(); + EXPECT_TRUE(manifold2->Plus(&x[ambient_cursor], + &delta[tangent_cursor], + &x_plus_delta_expected[ambient_cursor])); + ambient_cursor += manifold2->AmbientSize(); + tangent_cursor += manifold2->TangentSize(); - EXPECT_TRUE(manifold3->Plus(&x[ambient_cursor], - &delta[tangent_cursor], - &x_plus_delta_expected[ambient_cursor])); - ambient_cursor += manifold3->AmbientSize(); - tangent_cursor += manifold3->TangentSize(); + EXPECT_TRUE(manifold3->Plus(&x[ambient_cursor], + &delta[tangent_cursor], + &x_plus_delta_expected[ambient_cursor])); + ambient_cursor += manifold3->AmbientSize(); + tangent_cursor += manifold3->TangentSize(); - EXPECT_TRUE(manifold4->Plus(&x[ambient_cursor], - &delta[tangent_cursor], - &x_plus_delta_expected[ambient_cursor])); - ambient_cursor += manifold4->AmbientSize(); - tangent_cursor += manifold4->TangentSize(); + EXPECT_TRUE(manifold4->Plus(&x[ambient_cursor], + &delta[tangent_cursor], + &x_plus_delta_expected[ambient_cursor])); + ambient_cursor += manifold4->AmbientSize(); + tangent_cursor += manifold4->TangentSize(); - for (int i = 0; i < x.size(); ++i) { - EXPECT_EQ(x_plus_delta[i], x_plus_delta_expected[i]); + for (int i = 0; i < x.size(); ++i) { + EXPECT_EQ(x_plus_delta[i], x_plus_delta_expected[i]); + } + + EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta); } - - EXPECT_THAT(manifold, HasCorrectMinusAt(x)); - EXPECT_THAT(manifold, HasCorrectPlusJacobian()); - EXPECT_THAT(manifold, HasCorrectMinusJacobian()); - EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobian()); } TEST(ProductManifold, ZeroTangentSizeAndEuclidean) { @@ -401,20 +438,19 @@ EXPECT_EQ(manifold.AmbientSize(), 3); EXPECT_EQ(manifold.TangentSize(), 2); - Vector x = Vector::Random(3); - Vector delta = Vector::Random(2); - Vector x_plus_delta = Vector::Zero(3); + for (int trial = 0; trial < kNumTrials; ++trial) { + Vector x = Vector::Random(3); + Vector delta = Vector::Random(2); + Vector x_plus_delta = Vector::Zero(3); - EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), x_plus_delta.data())); + EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), x_plus_delta.data())); - EXPECT_EQ(x_plus_delta[0], x[0]); - EXPECT_EQ(x_plus_delta[1], x[1] + delta[0]); - EXPECT_EQ(x_plus_delta[2], x[2] + delta[1]); + EXPECT_EQ(x_plus_delta[0], x[0]); + EXPECT_EQ(x_plus_delta[1], x[1] + delta[0]); + EXPECT_EQ(x_plus_delta[2], x[2] + delta[1]); - EXPECT_THAT(manifold, HasCorrectMinusAt(x)); - EXPECT_THAT(manifold, HasCorrectPlusJacobian()); - EXPECT_THAT(manifold, HasCorrectMinusJacobian()); - EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobian()); + EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta); + } } TEST(ProductManifold, EuclideanAndZeroTangentSize) { @@ -424,20 +460,19 @@ EXPECT_EQ(manifold.AmbientSize(), 3); EXPECT_EQ(manifold.TangentSize(), 2); - Vector x = Vector::Random(3); - Vector delta = Vector::Random(2); - Vector x_plus_delta = Vector::Zero(3); + for (int trial = 0; trial < kNumTrials; ++trial) { + Vector x = Vector::Random(3); + Vector delta = Vector::Random(2); + Vector x_plus_delta = Vector::Zero(3); - EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), x_plus_delta.data())); + EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), x_plus_delta.data())); - EXPECT_EQ(x_plus_delta[0], x[0] + delta[0]); - EXPECT_EQ(x_plus_delta[1], x[1] + delta[1]); - EXPECT_EQ(x_plus_delta[2], x[2]); + EXPECT_EQ(x_plus_delta[0], x[0] + delta[0]); + EXPECT_EQ(x_plus_delta[1], x[1] + delta[1]); + EXPECT_EQ(x_plus_delta[2], x[2]); - EXPECT_THAT(manifold, HasCorrectMinusAt(x)); - EXPECT_THAT(manifold, HasCorrectPlusJacobian()); - EXPECT_THAT(manifold, HasCorrectMinusJacobian()); - EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobian()); + EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta); + } } } // namespace internal