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

Added unit tests for higher-ary NonlinearFactors

release/4.3a0
Richard Roberts 2011-10-26 02:07:35 +00:00
parent 475977ad83
commit c120ee93f6
1 changed files with 157 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

157
tests/testNonlinearFactor.cpp
View File

@ -30,10 +30,12 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/slam/smallExample.h>
#include <gtsam/slam/simulated2D.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h>
using namespace std;
using namespace gtsam;
@ -232,6 +234,161 @@ TEST( NonlinearFactor, linearize_constraint2 )
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
}
/* ************************************************************************* */
typedef TypedSymbol<LieVector, 'x'> TestKey;
typedef LieValues<TestKey> TestValues;
/* ************************************************************************* */
class TestFactor4 : public NonlinearFactor4<TestValues, TestKey, TestKey, TestKey, TestKey> {
public:
typedef NonlinearFactor4<TestValues, TestKey, TestKey, TestKey, TestKey> Base;
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4) {}
virtual Vector
evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none) const {
if(H1) {
*H1 = Matrix_(1,1, 1.0);
*H2 = Matrix_(1,1, 2.0);
*H3 = Matrix_(1,1, 3.0);
*H4 = Matrix_(1,1, 4.0);
}
return (Vector(1) << x1 + x2 + x3 + x4).finished();
}
};
/* ************************************ */
TEST(NonlinearFactor, NonlinearFactor4) {
TestFactor4 tf;
TestValues tv;
tv.insert(1, LieVector(1, 1.0));
tv.insert(2, LieVector(1, 2.0));
tv.insert(3, LieVector(1, 3.0));
tv.insert(4, LieVector(1, 4.0));
EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
LONGS_EQUAL(jf.keys()[2], 2);
LONGS_EQUAL(jf.keys()[3], 3);
EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
EXPECT(assert_equal(Vector_(1, -5.0), jf.getb()));
}
/* ************************************************************************* */
class TestFactor5 : public NonlinearFactor5<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey> {
public:
typedef NonlinearFactor5<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {}
virtual Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const {
if(H1) {
*H1 = Matrix_(1,1, 1.0);
*H2 = Matrix_(1,1, 2.0);
*H3 = Matrix_(1,1, 3.0);
*H4 = Matrix_(1,1, 4.0);
*H5 = Matrix_(1,1, 5.0);
}
return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished();
}
};
/* ************************************ */
TEST(NonlinearFactor, NonlinearFactor5) {
TestFactor5 tf;
TestValues tv;
tv.insert(1, LieVector(1, 1.0));
tv.insert(2, LieVector(1, 2.0));
tv.insert(3, LieVector(1, 3.0));
tv.insert(4, LieVector(1, 4.0));
tv.insert(5, LieVector(1, 5.0));
EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
LONGS_EQUAL(jf.keys()[2], 2);
LONGS_EQUAL(jf.keys()[3], 3);
LONGS_EQUAL(jf.keys()[4], 4);
EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4)));
EXPECT(assert_equal(Vector_(1, -7.5), jf.getb()));
}
/* ************************************************************************* */
class TestFactor6 : public NonlinearFactor6<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> {
public:
typedef NonlinearFactor6<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {}
virtual Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none,
boost::optional<Matrix&> H6 = boost::none) const {
if(H1) {
*H1 = Matrix_(1,1, 1.0);
*H2 = Matrix_(1,1, 2.0);
*H3 = Matrix_(1,1, 3.0);
*H4 = Matrix_(1,1, 4.0);
*H5 = Matrix_(1,1, 5.0);
*H6 = Matrix_(1,1, 6.0);
}
return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished();
}
};
/* ************************************ */
TEST(NonlinearFactor, NonlinearFactor6) {
TestFactor6 tf;
TestValues tv;
tv.insert(1, LieVector(1, 1.0));
tv.insert(2, LieVector(1, 2.0));
tv.insert(3, LieVector(1, 3.0));
tv.insert(4, LieVector(1, 4.0));
tv.insert(5, LieVector(1, 5.0));
tv.insert(6, LieVector(1, 6.0));
EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5), TestKey(6);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
LONGS_EQUAL(jf.keys()[2], 2);
LONGS_EQUAL(jf.keys()[3], 3);
LONGS_EQUAL(jf.keys()[4], 4);
LONGS_EQUAL(jf.keys()[5], 5);
EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4)));
EXPECT(assert_equal(Matrix_(1,1, 3.0), jf.getA(jf.begin()+5)));
EXPECT(assert_equal(Vector_(1, -10.5), jf.getb()));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */