diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index 160728e2f..78e4d807d 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -75,8 +75,8 @@ namespace gtsam { Index j1, Index j2) { double e = u - z, e2 = e * e; double c = 2 * logSqrt2PI - log(p) + e2 * p; - Vector g1 = Vector_(1, -e * p); - Vector g2 = Vector_(1, 0.5 / p - 0.5 * e2); + Vector g1 = (Vec(1) << -e * p); + Vector g2 = (Vec(1) << 0.5 / p - 0.5 * e2); Matrix G11 = Matrix_(1, 1, p); Matrix G12 = Matrix_(1, 1, e); Matrix G22 = Matrix_(1, 1, 0.5 / (p * p)); @@ -137,7 +137,7 @@ namespace gtsam { * TODO: Where is this used? should disappear. */ virtual Vector unwhitenedError(const Values& x) const { - return Vector_(1, std::sqrt(2 * error(x))); + return (Vec(1) << std::sqrt(2 * error(x))); } /** diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index ed3207487..c1cd73b85 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -36,7 +36,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) { Matrix A2 = Matrix_(2,2, -0.0455167, -0.0443573, -0.0222154, -0.102489); - Vector b = Vector_(2, 0.0277052, + Vector b = (Vec(2) << 0.0277052, -0.0533393); JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2); @@ -70,7 +70,7 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { Matrix A2 = Matrix_(2,2, -0.0455167, -0.0443573, -0.0222154, -0.102489); - Vector b = Vector_(2, 0.0277052, + Vector b = (Vec(2) << 0.0277052, -0.0533393); JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2); @@ -97,8 +97,8 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { 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); + Vector delta_l1 = (Vec(2) << 1.0, 2.0); + Vector delta_l2 = (Vec(2) << 3.0, 4.0); VectorValues delta = values.zeroVectors(); delta.at(l1) = delta_l1; @@ -130,9 +130,9 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) { 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); - Vector g3 = Vector_(3, 1.0, 2.0, 3.0); + Vector g1 = (Vec(1) << -7.0); + Vector g2 = (Vec(2) << -8.0, -9.0); + Vector g3 = (Vec(3) << 1.0, 2.0, 3.0); double f = 10.0; @@ -166,13 +166,13 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { 1.0, 2.0, 3.0, 5.0, 4.0, 6.0); - Vector g1 = Vector_(3, 1.0, 2.0, 3.0); + Vector g1 = (Vec(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); + Vector g2 = (Vec(2) << -8.0, -9.0); double f = 10.0; @@ -197,16 +197,16 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { 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); + Vector delta_l1 = (Vec(2) << 1.0, 2.0); + Vector delta_x1 = (Vec(3) << 3.0, 4.0, 0.5); + Vector delta_x2 = (Vec(3) << 6.0, 7.0, 0.3); // Check error calculation VectorValues delta = linearizationPoint.zeroVectors(); delta.at(l1) = delta_l1; delta.at(x1) = delta_x1; delta.at(x2) = delta_x2; - EXPECT(assert_equal(Vector_(5, 3.0, 4.0, 0.5, 1.0, 2.0), delta.vector(initFactor.keys()))); + EXPECT(assert_equal((Vec(5) << 3.0, 4.0, 0.5, 1.0, 2.0), delta.vector(initFactor.keys()))); Values noisyValues = linearizationPoint.retract(delta); double expError = initFactor.error(delta); @@ -214,7 +214,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { EXPECT_DOUBLES_EQUAL(initFactor.error(linearizationPoint.zeroVectors()), actFactor.error(linearizationPoint), tol); // Compute updated versions - Vector dv = Vector_(5, 3.0, 4.0, 0.5, 1.0, 2.0); + Vector dv = (Vec(5) << 3.0, 4.0, 0.5, 1.0, 2.0); Vector g(5); g << g1, g2; Vector g_prime = g - G.selfadjointView() * dv; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index d03f78424..3093c5376 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -171,8 +171,8 @@ TEST(Values, expmap_a) config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); VectorValues increment = pair_list_of - (key1, Vector_(3, 1.0, 1.1, 1.2)) - (key2, Vector_(3, 1.3, 1.4, 1.5)); + (key1, (Vec(3) << 1.0, 1.1, 1.2)) + (key2, (Vec(3) << 1.3, 1.4, 1.5)); Values expected; expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); @@ -242,8 +242,8 @@ TEST(Values, localCoordinates) valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); VectorValues expDelta = pair_list_of - (key1, Vector_(3, 0.1, 0.2, 0.3)) - (key2, Vector_(3, 0.4, 0.5, 0.6)); + (key1, (Vec(3) << 0.1, 0.2, 0.3)) + (key2, (Vec(3) << 0.4, 0.5, 0.6)); Values valuesB = valuesA.retract(expDelta); @@ -275,11 +275,11 @@ TEST(Values, extract_keys) TEST(Values, exists_) { Values config0; - config0.insert(key1, LieVector(Vector_(1, 1.))); - config0.insert(key2, LieVector(Vector_(1, 2.))); + config0.insert(key1, LieVector((Vec(1) << 1.))); + config0.insert(key2, LieVector((Vec(1) << 2.))); boost::optional v = config0.exists(key1); - CHECK(assert_equal(Vector_(1, 1.),*v)); + CHECK(assert_equal((Vec(1) << 1.),*v)); } /* ************************************************************************* */