12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Oops...fixed mistypes. Unit tests pass now.

release/4.3a0
Alex Hagiopol 2016-04-11 16:04:24 -04:00
parent 379ad8b3d2
commit aa599d409c
5 changed files with 26 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam_unstable/dynamics/Pendulum.h
View File

@ -50,9 +50,9 @@ public:
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = 1; const size_t p = 1;
if (H1) *H1 = -Matrix::Zero(p,p); if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Zero(p,p); if (H2) *H2 = Matrix::Identity(p,p);
if (H3) *H3 = Matrix::Zero(p,p)*h_; if (H3) *H3 = Matrix::Identity(p,p)*h_;
return (Vector(1) << qk+v*h_-qk1).finished(); return (Vector(1) << qk+v*h_-qk1).finished();
} }
@ -98,9 +98,9 @@ public:
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = 1; const size_t p = 1;
if (H1) *H1 = -Matrix::Zero(p,p); if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Zero(p,p); if (H2) *H2 = Matrix::Identity(p,p);
if (H3) *H3 = -Matrix::Zero(p,p)*h_*g_/r_*cos(q); if (H3) *H3 = -Matrix::Identity(p,p)*h_*g_/r_*cos(q);
return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1).finished(); 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 mr2_h = 1/h_*m_*r_*r_;
double mgrh = m_*g_*r_*h_; double mgrh = m_*g_*r_*h_;
if (H1) *H1 = -Matrix::Zero(p,p); if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Zero(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); if (H2) *H2 = Matrix::Identity(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 (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(); 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 mr2_h = 1/h_*m_*r_*r_;
double mgrh = m_*g_*r_*h_; double mgrh = m_*g_*r_*h_;
if (H1) *H1 = -Matrix::Zero(p,p); if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Zero(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); if (H2) *H2 = Matrix::Identity(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 (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(); return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1).finished();
} }

6
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

@ -41,9 +41,9 @@ public:
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = 1; const size_t p = 1;
if (H1) *H1 = Matrix::Zero(p,p); if (H1) *H1 = Matrix::Identity(p,p);
if (H2) *H2 = -Matrix::Zero(p,p); if (H2) *H2 = -Matrix::Identity(p,p);
if (H3) *H3 = Matrix::Zero(p,p)*dt_; if (H3) *H3 = Matrix::Identity(p,p)*dt_;
return (Vector(1) << x1+v*dt_-x2).finished(); return (Vector(1) << x1+v*dt_-x2).finished();
} }

8
tests/testGaussianFactorGraphB.cpp
View File

@ -473,13 +473,13 @@ TEST(GaussianFactorGraph, replace)
SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0)); SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0));
GaussianFactorGraph::sharedFactor f1(new JacobianFactor( 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( 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( 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( 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; GaussianFactorGraph actual;
actual.push_back(f1); actual.push_back(f1);

2
tests/testGraph.cpp
View File

@ -112,7 +112,7 @@ TEST( Graph, composePoses )
TEST( GaussianFactorGraph, findMinimumSpanningTree ) TEST( GaussianFactorGraph, findMinimumSpanningTree )
{ {
GaussianFactorGraph g; GaussianFactorGraph g;
Matrix I = Z_2x2; Matrix I = I_2x2;
Vector2 b(0, 0); Vector2 b(0, 0);
const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
using namespace symbol_shorthand; using namespace symbol_shorthand;

12
tests/testNonlinearEquality.cpp
View File

@ -56,7 +56,7 @@ TEST ( NonlinearEquality, linearization ) {
// check linearize // check linearize
SharedDiagonal constraintModel = noiseModel::Constrained::All(3); 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); GaussianFactor::shared_ptr actualLF = nle->linearize(linearize);
EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF)); EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF));
} }
@ -180,7 +180,7 @@ TEST ( NonlinearEquality, allow_error_pose ) {
// check linearization // check linearization
GaussianFactor::shared_ptr actLinFactor = nle.linearize(config); GaussianFactor::shared_ptr actLinFactor = nle.linearize(config);
Matrix A1 =Z_3x3; Matrix A1 = I_3x3;
Vector b = expVec; Vector b = expVec;
SharedDiagonal model = noiseModel::Constrained::All(3); SharedDiagonal model = noiseModel::Constrained::All(3);
GaussianFactor::shared_ptr expLinFactor( GaussianFactor::shared_ptr expLinFactor(
@ -289,7 +289,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) {
config1.insert(key, pt); config1.insert(key, pt);
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
GaussianFactor::shared_ptr expected1( 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)); EXPECT(assert_equal(*expected1, *actual1, tol));
Values config2; Values config2;
@ -297,7 +297,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) {
config2.insert(key, ptBad); config2.insert(key, ptBad);
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2);
GaussianFactor::shared_ptr expected2( 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)); EXPECT(assert_equal(*expected2, *actual2, tol));
} }
@ -374,7 +374,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
config1.insert(key2, x2); config1.insert(key2, x2);
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
GaussianFactor::shared_ptr expected1( 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)); hard_model));
EXPECT(assert_equal(*expected1, *actual1, tol)); EXPECT(assert_equal(*expected1, *actual1, tol));
@ -385,7 +385,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
config2.insert(key2, x2bad); config2.insert(key2, x2bad);
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2);
GaussianFactor::shared_ptr expected2( 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)); hard_model));
EXPECT(assert_equal(*expected2, *actual2, tol)); EXPECT(assert_equal(*expected2, *actual2, tol));
} }