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

No more LieVector

release/4.3a0
dellaert 2014-11-04 15:44:20 +01:00
parent 62cc0344ea
commit c332a44c5e
1 changed files with 22 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
tests/testNonlinearFactor.cpp
View File

@ -234,13 +234,13 @@ TEST( NonlinearFactor, linearize_constraint2 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
class TestFactor4 : public NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> { class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
public: public:
typedef NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> Base; typedef NoiseModelFactor4<double, double, double, double> Base;
TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4)) {} TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4)) {}
virtual Vector virtual Vector
evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4, evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H3 = boost::none,
@ -263,10 +263,10 @@ public:
TEST(NonlinearFactor, NoiseModelFactor4) { TEST(NonlinearFactor, NoiseModelFactor4) {
TestFactor4 tf; TestFactor4 tf;
Values tv; Values tv;
tv.insert(X(1), LieVector((Vector(1) << 1.0))); tv.insert(X(1), double((1.0)));
tv.insert(X(2), LieVector((Vector(1) << 2.0))); tv.insert(X(2), double((2.0)));
tv.insert(X(3), LieVector((Vector(1) << 3.0))); tv.insert(X(3), double((3.0)));
tv.insert(X(4), LieVector((Vector(1) << 4.0))); tv.insert(X(4), double((4.0)));
EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv))); EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
@ -282,9 +282,9 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
class TestFactor5 : public NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> { class TestFactor5 : public NoiseModelFactor5<double, double, double, double, double> {
public: public:
typedef NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base; typedef NoiseModelFactor5<double, double, double, double, double> Base;
TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5)) {} TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5)) {}
virtual Vector virtual Vector
@ -309,11 +309,11 @@ public:
TEST(NonlinearFactor, NoiseModelFactor5) { TEST(NonlinearFactor, NoiseModelFactor5) {
TestFactor5 tf; TestFactor5 tf;
Values tv; Values tv;
tv.insert(X(1), LieVector((Vector(1) << 1.0))); tv.insert(X(1), double((1.0)));
tv.insert(X(2), LieVector((Vector(1) << 2.0))); tv.insert(X(2), double((2.0)));
tv.insert(X(3), LieVector((Vector(1) << 3.0))); tv.insert(X(3), double((3.0)));
tv.insert(X(4), LieVector((Vector(1) << 4.0))); tv.insert(X(4), double((4.0)));
tv.insert(X(5), LieVector((Vector(1) << 5.0))); tv.insert(X(5), double((5.0)));
EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv))); EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
@ -331,9 +331,9 @@ TEST(NonlinearFactor, NoiseModelFactor5) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
class TestFactor6 : public NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> { class TestFactor6 : public NoiseModelFactor6<double, double, double, double, double, double> {
public: public:
typedef NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base; typedef NoiseModelFactor6<double, double, double, double, double, double> Base;
TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {} TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {}
virtual Vector virtual Vector
@ -361,12 +361,12 @@ public:
TEST(NonlinearFactor, NoiseModelFactor6) { TEST(NonlinearFactor, NoiseModelFactor6) {
TestFactor6 tf; TestFactor6 tf;
Values tv; Values tv;
tv.insert(X(1), LieVector((Vector(1) << 1.0))); tv.insert(X(1), double((1.0)));
tv.insert(X(2), LieVector((Vector(1) << 2.0))); tv.insert(X(2), double((2.0)));
tv.insert(X(3), LieVector((Vector(1) << 3.0))); tv.insert(X(3), double((3.0)));
tv.insert(X(4), LieVector((Vector(1) << 4.0))); tv.insert(X(4), double((4.0)));
tv.insert(X(5), LieVector((Vector(1) << 5.0))); tv.insert(X(5), double((5.0)));
tv.insert(X(6), LieVector((Vector(1) << 6.0))); tv.insert(X(6), double((6.0)));
EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv))); EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));