diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index fa6bd203c..803bb7c3f 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -145,7 +145,7 @@ TEST(Pose2, expmap0d) { /* ************************************************************************* */ // test case for screw motion in the plane -namespace screw { +namespace screwPose2 { double w=0.3; Vector xi = (Vector(3) << 0.0, w, w).finished(); Rot2 expectedR = Rot2::fromAngle(w); @@ -155,9 +155,9 @@ namespace screw { TEST(Pose2, expmap_c) { - EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); - EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6)); - EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6)); + EXPECT(assert_equal(screwPose2::expected, expm(screwPose2::xi),1e-6)); + EXPECT(assert_equal(screwPose2::expected, Pose2::Expmap(screwPose2::xi),1e-6)); + EXPECT(assert_equal(screwPose2::xi, Pose2::Logmap(screwPose2::expected),1e-6)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index d40344d35..0c131a028 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -109,7 +109,7 @@ TEST(Pose3, expmap_b) /* ************************************************************************* */ // test case for screw motion in the plane -namespace screw { +namespace screwPose3 { double a=0.3, c=cos(a), s=sin(a), w=0.3; Vector xi = (Vector(6) << 0.0, 0.0, w, w, 0.0, 1.0).finished(); Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); @@ -121,24 +121,24 @@ namespace screw { // Checks correct exponential map (Expmap) with brute force matrix exponential TEST(Pose3, expmap_c_full) { - EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); - EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6)); + EXPECT(assert_equal(screwPose3::expected, expm(screwPose3::xi),1e-6)); + EXPECT(assert_equal(screwPose3::expected, Pose3::Expmap(screwPose3::xi),1e-6)); } /* ************************************************************************* */ // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) TEST(Pose3, Adjoint_full) { - Pose3 expected = T * Pose3::Expmap(screw::xi) * T.inverse(); - Vector xiprime = T.Adjoint(screw::xi); + Pose3 expected = T * Pose3::Expmap(screwPose3::xi) * T.inverse(); + Vector xiprime = T.Adjoint(screwPose3::xi); EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); - Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * T2.inverse(); - Vector xiprime2 = T2.Adjoint(screw::xi); + Pose3 expected2 = T2 * Pose3::Expmap(screwPose3::xi) * T2.inverse(); + Vector xiprime2 = T2.Adjoint(screwPose3::xi); EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); - Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * T3.inverse(); - Vector xiprime3 = T3.Adjoint(screw::xi); + Pose3 expected3 = T3 * Pose3::Expmap(screwPose3::xi) * T3.inverse(); + Vector xiprime3 = T3.Adjoint(screwPose3::xi); EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); } @@ -634,9 +634,9 @@ TEST( Pose3, unicycle ) /* ************************************************************************* */ TEST( Pose3, adjointMap) { - Matrix res = Pose3::adjointMap(screw::xi); - Matrix wh = skewSymmetric(screw::xi(0), screw::xi(1), screw::xi(2)); - Matrix vh = skewSymmetric(screw::xi(3), screw::xi(4), screw::xi(5)); + Matrix res = Pose3::adjointMap(screwPose3::xi); + Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2)); + Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5)); Matrix Z3 = zeros(3,3); Matrix6 expected; expected << wh, Z3, vh, wh; @@ -704,13 +704,13 @@ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) { } TEST( Pose3, adjoint) { - Vector expected = testDerivAdjoint(screw::xi, screw::xi); + Vector expected = testDerivAdjoint(screwPose3::xi, screwPose3::xi); Matrix actualH; - Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH); + Vector actual = Pose3::adjoint(screwPose3::xi, screwPose3::xi, actualH); Matrix numericalH = numericalDerivative21( - testDerivAdjoint, screw::xi, screw::xi, 1e-5); + testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5); EXPECT(assert_equal(expected,actual,1e-5)); EXPECT(assert_equal(numericalH,actualH,1e-5)); diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index fb3884542..26f975296 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -148,7 +148,6 @@ TEST( GaussianBayesNet, DeterminantTest ) } /* ************************************************************************* */ -typedef Eigen::Matrix Vector10; namespace { double computeError(const GaussianBayesNet& gbn, const Vector10& values) { diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 78fe5187a..05f8c3d2a 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -175,7 +175,6 @@ TEST(GaussianBayesTree, complicatedMarginal) { } /* ************************************************************************* */ -typedef Eigen::Matrix Vector10; namespace { double computeError(const GaussianBayesTree& gbt, const Vector10& values) { pair Rd = GaussianFactorGraph(gbt).jacobian();