From aa599d409c5e32cd9ab401740090e6713693f2e8 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Mon, 11 Apr 2016 16:04:24 -0400 Subject: [PATCH] Oops...fixed mistypes. Unit tests pass now. --- gtsam_unstable/dynamics/Pendulum.h | 24 +++++++++---------- gtsam_unstable/dynamics/VelocityConstraint3.h | 6 ++--- tests/testGaussianFactorGraphB.cpp | 8 +++---- tests/testGraph.cpp | 2 +- tests/testNonlinearEquality.cpp | 12 +++++----- 5 files changed, 26 insertions(+), 26 deletions(-) diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index 28a26edce..9ec10e39f 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -50,9 +50,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = -Matrix::Zero(p,p); - if (H2) *H2 = Matrix::Zero(p,p); - if (H3) *H3 = Matrix::Zero(p,p)*h_; + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p); + if (H3) *H3 = Matrix::Identity(p,p)*h_; return (Vector(1) << qk+v*h_-qk1).finished(); } @@ -98,9 +98,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = -Matrix::Zero(p,p); - if (H2) *H2 = Matrix::Zero(p,p); - if (H3) *H3 = -Matrix::Zero(p,p)*h_*g_/r_*cos(q); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p); + if (H3) *H3 = -Matrix::Identity(p,p)*h_*g_/r_*cos(q); return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1).finished(); } @@ -154,9 +154,9 @@ public: double mr2_h = 1/h_*m_*r_*r_; double mgrh = m_*g_*r_*h_; - if (H1) *H1 = -Matrix::Zero(p,p); - if (H2) *H2 = Matrix::Zero(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); - if (H3) *H3 = Matrix::Zero(p,p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); + if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk).finished(); } @@ -210,9 +210,9 @@ public: double mr2_h = 1/h_*m_*r_*r_; double mgrh = m_*g_*r_*h_; - if (H1) *H1 = -Matrix::Zero(p,p); - if (H2) *H2 = Matrix::Zero(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); - if (H3) *H3 = Matrix::Zero(p,p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); + if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1).finished(); } diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 63a2c43bf..f98879e41 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -41,9 +41,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = Matrix::Zero(p,p); - if (H2) *H2 = -Matrix::Zero(p,p); - if (H3) *H3 = Matrix::Zero(p,p)*dt_; + if (H1) *H1 = Matrix::Identity(p,p); + if (H2) *H2 = -Matrix::Identity(p,p); + if (H3) *H3 = Matrix::Identity(p,p)*dt_; return (Vector(1) << x1+v*dt_-x2).finished(); } diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index cde79e637..47307d0f8 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -473,13 +473,13 @@ TEST(GaussianFactorGraph, replace) SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0)); GaussianFactorGraph::sharedFactor f1(new JacobianFactor( - ord[X(1)], I_3x3, ord[X(2)], I_3x3, Z_3x3, noise)); + ord[X(1)], I_3x3, ord[X(2)], I_3x3, zero(3), noise)); GaussianFactorGraph::sharedFactor f2(new JacobianFactor( - ord[X(2)], I_3x3, ord[X(3)], I_3x3, Z_3x3, noise)); + ord[X(2)], I_3x3, ord[X(3)], I_3x3, zero(3), noise)); GaussianFactorGraph::sharedFactor f3(new JacobianFactor( - ord[X(3)], I_3x3, ord[X(4)], I_3x3, Z_3x3, noise)); + ord[X(3)], I_3x3, ord[X(4)], I_3x3, zero(3), noise)); GaussianFactorGraph::sharedFactor f4(new JacobianFactor( - ord[X(5)], I_3x3, ord[X(6)], I_3x3, Z_3x3, noise)); + ord[X(5)], I_3x3, ord[X(6)], I_3x3, zero(3), noise)); GaussianFactorGraph actual; actual.push_back(f1); diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index fdb93c29c..b95f16e76 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -112,7 +112,7 @@ TEST( Graph, composePoses ) TEST( GaussianFactorGraph, findMinimumSpanningTree ) { GaussianFactorGraph g; - Matrix I = Z_2x2; + Matrix I = I_2x2; Vector2 b(0, 0); const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); using namespace symbol_shorthand; diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 5e4f832e4..335b68662 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -56,7 +56,7 @@ TEST ( NonlinearEquality, linearization ) { // check linearize SharedDiagonal constraintModel = noiseModel::Constrained::All(3); - JacobianFactor expLF(key, Z_3x3, zero(3), constraintModel); + JacobianFactor expLF(key, I_3x3, zero(3), constraintModel); GaussianFactor::shared_ptr actualLF = nle->linearize(linearize); EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF)); } @@ -180,7 +180,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { // check linearization GaussianFactor::shared_ptr actLinFactor = nle.linearize(config); - Matrix A1 =Z_3x3; + Matrix A1 = I_3x3; Vector b = expVec; SharedDiagonal model = noiseModel::Constrained::All(3); GaussianFactor::shared_ptr expLinFactor( @@ -289,7 +289,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { config1.insert(key, pt); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key, Z_2x2, Matrix::Zero(2,2), hard_model)); + new JacobianFactor(key, I_2x2, zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); Values config2; @@ -297,7 +297,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { config2.insert(key, ptBad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( - new JacobianFactor(key, Z_2x2, Vector2(-1.0, 0.0), hard_model)); + new JacobianFactor(key, I_2x2, Vector2(-1.0, 0.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } @@ -374,7 +374,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config1.insert(key2, x2); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key1, -Z_2x2, key2, Z_2x2, zero(2), + new JacobianFactor(key1, -I_2x2, key2, I_2x2, zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); @@ -385,7 +385,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config2.insert(key2, x2bad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( - new JacobianFactor(key1, -Z_2x2, key2, Z_2x2, Vector2(1.0, 1.0), + new JacobianFactor(key1, -I_2x2, key2, I_2x2, Vector2(1.0, 1.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); }