diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index c46c65901..2f8bf378f 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -412,38 +412,40 @@ TEST (Unit3, FromPoint3) { //******************************************************************************* TEST(Unit3, ErrorBetweenFactor) { - std::vector data = {Unit3(1.0, 0.0, 0.0), Unit3(0.0, 0.0, 1.0)}; + std::vector data; + data.push_back(Unit3(1.0, 0.0, 0.0)); + data.push_back(Unit3(0.0, 0.0, 1.0)); NonlinearFactorGraph graph; Values initial_values; // Add prior factors. SharedNoiseModel R_prior = noiseModel::Unit::Create(2); - for (int i = 0; i < data.size(); i++) { + for (size_t i = 0; i < data.size(); i++) { graph.add(PriorFactor(U(i), data[i], R_prior)); } // Add process factors using the dot product error function. SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01); - for (int i = 0; i < data.size() - 1; i++) { + for (size_t i = 0; i < data.size() - 1; i++) { Expression exp(Expression(U(i)), &Unit3::errorVector, Expression(U(i + 1))); graph.addExpressionFactor(R_process, Vector2::Zero(), exp); } // Add initial values. Since there is no identity, just pick something. - for (int i = 0; i < data.size(); i++) { + for (size_t i = 0; i < data.size(); i++) { initial_values.insert(U(i), Unit3(0.0, 1.0, 0.0)); } Values values = GaussNewtonOptimizer(graph, initial_values).optimize(); // Check that the y-value is very small for each. - for (int i = 0; i < data.size(); i++) { + for (size_t i = 0; i < data.size(); i++) { EXPECT(assert_equal(0.0, values.at(U(i)).unitVector().y(), 1e-3)); } // Check that the dot product between variables is close to 1. - for (int i = 0; i < data.size() - 1; i++) { + for (size_t i = 0; i < data.size() - 1; i++) { EXPECT(assert_equal(1.0, values.at(U(i)).dot(values.at(U(i + 1))), 1e-2)); } }