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:
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)) {}
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&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
@ -263,10 +263,10 @@ public:
TEST(NonlinearFactor, NoiseModelFactor4) {
TestFactor4 tf;
Values tv;
tv.insert(X(1), LieVector((Vector(1) << 1.0)));
tv.insert(X(2), LieVector((Vector(1) << 2.0)));
tv.insert(X(3), LieVector((Vector(1) << 3.0)));
tv.insert(X(4), LieVector((Vector(1) << 4.0)));
tv.insert(X(1), double((1.0)));
tv.insert(X(2), double((2.0)));
tv.insert(X(3), double((3.0)));
tv.insert(X(4), double((4.0)));
EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
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:
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)) {}
virtual Vector
@ -309,11 +309,11 @@ public:
TEST(NonlinearFactor, NoiseModelFactor5) {
TestFactor5 tf;
Values tv;
tv.insert(X(1), LieVector((Vector(1) << 1.0)));
tv.insert(X(2), LieVector((Vector(1) << 2.0)));
tv.insert(X(3), LieVector((Vector(1) << 3.0)));
tv.insert(X(4), LieVector((Vector(1) << 4.0)));
tv.insert(X(5), LieVector((Vector(1) << 5.0)));
tv.insert(X(1), double((1.0)));
tv.insert(X(2), double((2.0)));
tv.insert(X(3), double((3.0)));
tv.insert(X(4), double((4.0)));
tv.insert(X(5), double((5.0)));
EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
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:
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)) {}
virtual Vector
@ -361,12 +361,12 @@ public:
TEST(NonlinearFactor, NoiseModelFactor6) {
TestFactor6 tf;
Values tv;
tv.insert(X(1), LieVector((Vector(1) << 1.0)));
tv.insert(X(2), LieVector((Vector(1) << 2.0)));
tv.insert(X(3), LieVector((Vector(1) << 3.0)));
tv.insert(X(4), LieVector((Vector(1) << 4.0)));
tv.insert(X(5), LieVector((Vector(1) << 5.0)));
tv.insert(X(6), LieVector((Vector(1) << 6.0)));
tv.insert(X(1), double((1.0)));
tv.insert(X(2), double((2.0)));
tv.insert(X(3), double((3.0)));
tv.insert(X(4), double((4.0)));
tv.insert(X(5), double((5.0)));
tv.insert(X(6), double((6.0)));
EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));