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

Lots of code de-duplication and cleanup in testGaussianISAM2

release/4.3a0
Richard Roberts 2012-06-30 22:32:42 +00:00
parent f8b559772f
commit f06c7ad2e9
1 changed files with 71 additions and 677 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

714
tests/testGaussianISAM2.cpp
View File

@ -44,7 +44,6 @@ ISAM2 createSlamlikeISAM2(
// These variables will be reused and accumulate factors and values // These variables will be reused and accumulate factors and values
ISAM2 isam(params); ISAM2 isam(params);
// ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
Values fullinit; Values fullinit;
planarSLAM::Graph fullgraph; planarSLAM::Graph fullgraph;
@ -136,7 +135,7 @@ ISAM2 createSlamlikeISAM2(
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST_UNSAFE(ISAM2, AddVariables) { TEST_UNSAFE(ISAM2, ImplAddVariables) {
// Create initial state // Create initial state
Values theta; Values theta;
@ -207,7 +206,7 @@ TEST_UNSAFE(ISAM2, AddVariables) {
EXPECT(assert_equal(orderingExpected, ordering)); EXPECT(assert_equal(orderingExpected, ordering));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST_UNSAFE(ISAM2, RemoveVariables) { TEST_UNSAFE(ISAM2, ImplRemoveVariables) {
// Create initial state // Create initial state
Values theta; Values theta;
@ -381,7 +380,11 @@ TEST(ISAM2, optimize2) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam) { bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) {
TestResult& result_ = result;
const SimpleString name_ = test.getName();
Values actual = isam.calculateEstimate(); Values actual = isam.calculateEstimate();
Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first; Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first;
GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering);
@ -391,102 +394,12 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, cons
VectorValues delta = optimize(gbn); VectorValues delta = optimize(gbn);
Values expected = fullinit.retract(delta, ordering); Values expected = fullinit.retract(delta, ordering);
return assert_equal(expected, actual); bool isamEqual = assert_equal(expected, actual);
}
/* ************************************************************************* */ // The following two checks make sure that the cached gradients are maintained and used correctly
TEST(ISAM2, slamlike_solution_gaussnewton)
{
// These variables will be reused and accumulate factors and values
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
Values fullinit;
planarSLAM::Graph fullgraph;
// i keeps track of the time step
size_t i = 0;
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((0), Pose2(0.01, 0.01, 0.01));
fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
CHECK(isam_check(fullgraph, fullinit, isam));
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(1.01, 0.01, 0.01));
init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init);
++ i;
}
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init);
++ i;
}
// Compare solutions
CHECK(isam_check(fullgraph, fullinit, isam));
// Check gradient at each node // Check gradient at each node
bool nodeGradientsOk = true;
typedef ISAM2::sharedClique sharedClique; typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient // Compute expected gradient
@ -499,10 +412,14 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
const int dim = clique->conditional()->dim(jit); const int dim = clique->conditional()->dim(jit);
Vector actual = clique->gradientContribution().segment(variablePosition, dim); Vector actual = clique->gradientContribution().segment(variablePosition, dim);
EXPECT(assert_equal(expectedGradient[*jit], actual)); bool gradOk = assert_equal(expectedGradient[*jit], actual);
EXPECT(gradOk);
nodeGradientsOk = nodeGradientsOk && gradOk;
variablePosition += dim; variablePosition += dim;
} }
LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); bool dimOk = clique->gradientContribution().rows() == variablePosition;
EXPECT(dimOk);
nodeGradientsOk = nodeGradientsOk && dimOk;
} }
// Check gradient // Check gradient
@ -511,368 +428,60 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient))); VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
VectorValues actualGradient(*allocateVectorValues(isam)); VectorValues actualGradient(*allocateVectorValues(isam));
gradientAtZero(isam, actualGradient); gradientAtZero(isam, actualGradient);
EXPECT(assert_equal(expectedGradient2, expectedGradient)); bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient);
EXPECT(assert_equal(expectedGradient, actualGradient)); EXPECT(expectedGradOk);
bool totalGradOk = assert_equal(expectedGradient, actualGradient);
EXPECT(totalGradOk);
return nodeGradientsOk && expectedGradOk && totalGradOk;
}
/* ************************************************************************* */
TEST(ISAM2, slamlike_solution_gaussnewton)
{
// These variables will be reused and accumulate factors and values
Values fullinit;
planarSLAM::Graph fullgraph;
ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
// Compare solutions
CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ISAM2, slamlike_solution_dogleg) TEST(ISAM2, slamlike_solution_dogleg)
{ {
// These variables will be reused and accumulate factors and values // These variables will be reused and accumulate factors and values
ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
Values fullinit; Values fullinit;
planarSLAM::Graph fullgraph; planarSLAM::Graph fullgraph;
ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
// i keeps track of the time step
size_t i = 0;
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((0), Pose2(0.01, 0.01, 0.01));
fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
CHECK(isam_check(fullgraph, fullinit, isam));
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(1.01, 0.01, 0.01));
init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init);
++ i;
}
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init);
++ i;
}
// Compare solutions // Compare solutions
CHECK(isam_check(fullgraph, fullinit, isam)); CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
// Check gradient at each node
typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;
jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(jfg, expectedGradient);
// Compare with actual gradients
int variablePosition = 0;
for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
const int dim = clique->conditional()->dim(jit);
Vector actual = clique->gradientContribution().segment(variablePosition, dim);
EXPECT(assert_equal(expectedGradient[*jit], actual));
variablePosition += dim;
}
LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
}
// Check gradient
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
VectorValues actualGradient(*allocateVectorValues(isam));
gradientAtZero(isam, actualGradient);
EXPECT(assert_equal(expectedGradient2, expectedGradient));
EXPECT(assert_equal(expectedGradient, actualGradient));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ISAM2, slamlike_solution_gaussnewton_qr) TEST(ISAM2, slamlike_solution_gaussnewton_qr)
{ {
// These variables will be reused and accumulate factors and values // These variables will be reused and accumulate factors and values
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR));
Values fullinit; Values fullinit;
planarSLAM::Graph fullgraph; planarSLAM::Graph fullgraph;
ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR));
// i keeps track of the time step
size_t i = 0;
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((0), Pose2(0.01, 0.01, 0.01));
fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
CHECK(isam_check(fullgraph, fullinit, isam));
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(1.01, 0.01, 0.01));
init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init);
++ i;
}
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init);
++ i;
}
// Compare solutions // Compare solutions
CHECK(isam_check(fullgraph, fullinit, isam)); CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
// Check gradient at each node
typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;
jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(jfg, expectedGradient);
// Compare with actual gradients
int variablePosition = 0;
for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
const int dim = clique->conditional()->dim(jit);
Vector actual = clique->gradientContribution().segment(variablePosition, dim);
EXPECT(assert_equal(expectedGradient[*jit], actual));
variablePosition += dim;
}
LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
}
// Check gradient
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
VectorValues actualGradient(*allocateVectorValues(isam));
gradientAtZero(isam, actualGradient);
EXPECT(assert_equal(expectedGradient2, expectedGradient));
EXPECT(assert_equal(expectedGradient, actualGradient));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ISAM2, slamlike_solution_dogleg_qr) TEST(ISAM2, slamlike_solution_dogleg_qr)
{ {
// These variables will be reused and accumulate factors and values // These variables will be reused and accumulate factors and values
ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR));
Values fullinit; Values fullinit;
planarSLAM::Graph fullgraph; planarSLAM::Graph fullgraph;
ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR));
// i keeps track of the time step
size_t i = 0;
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((0), Pose2(0.01, 0.01, 0.01));
fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
CHECK(isam_check(fullgraph, fullinit, isam));
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(1.01, 0.01, 0.01));
init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init);
++ i;
}
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init);
++ i;
}
// Compare solutions // Compare solutions
CHECK(isam_check(fullgraph, fullinit, isam)); CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
// Check gradient at each node
typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;
jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(jfg, expectedGradient);
// Compare with actual gradients
int variablePosition = 0;
for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
const int dim = clique->conditional()->dim(jit);
Vector actual = clique->gradientContribution().segment(variablePosition, dim);
EXPECT(assert_equal(expectedGradient[*jit], actual));
variablePosition += dim;
}
LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
}
// Check gradient
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
VectorValues actualGradient(*allocateVectorValues(isam));
gradientAtZero(isam, actualGradient);
EXPECT(assert_equal(expectedGradient2, expectedGradient));
EXPECT(assert_equal(expectedGradient, actualGradient));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -976,127 +585,20 @@ TEST(ISAM2, removeFactors)
// then removes the 2nd-to-last landmark measurement // then removes the 2nd-to-last landmark measurement
// These variables will be reused and accumulate factors and values // These variables will be reused and accumulate factors and values
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
Values fullinit; Values fullinit;
planarSLAM::Graph fullgraph; planarSLAM::Graph fullgraph;
ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
// i keeps track of the time step // Remove the 2nd measurement on landmark 0 (Key 100)
size_t i = 0;
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((0), Pose2(0.01, 0.01, 0.01));
fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
CHECK(isam_check(fullgraph, fullinit, isam));
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(1.01, 0.01, 0.01));
init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init);
++ i;
}
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors[0]);
fullgraph.push_back(newfactors[2]); // Don't add measurement on landmark 0
Values init;
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
ISAM2Result result = isam.update(newfactors, init);
++ i;
// Remove the measurement on landmark 0
FastVector<size_t> toRemove; FastVector<size_t> toRemove;
EXPECT_LONGS_EQUAL(isam.getFactorsUnsafe().size()-2, result.newFactorsIndices[1]); toRemove.push_back(12);
toRemove.push_back(result.newFactorsIndices[1]);
isam.update(planarSLAM::Graph(), Values(), toRemove); isam.update(planarSLAM::Graph(), Values(), toRemove);
}
// Remove the factor from the full system
fullgraph.remove(12);
// Compare solutions // Compare solutions
CHECK(isam_check(fullgraph, fullinit, isam)); CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
// Check gradient at each node
typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;
jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(jfg, expectedGradient);
// Compare with actual gradients
int variablePosition = 0;
for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
const int dim = clique->conditional()->dim(jit);
Vector actual = clique->gradientContribution().segment(variablePosition, dim);
EXPECT(assert_equal(expectedGradient[*jit], actual));
variablePosition += dim;
}
LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
}
// Check gradient
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
VectorValues actualGradient(*allocateVectorValues(isam));
gradientAtZero(isam, actualGradient);
EXPECT(assert_equal(expectedGradient2, expectedGradient));
EXPECT(assert_equal(expectedGradient, actualGradient));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -1253,7 +755,7 @@ TEST_UNSAFE(ISAM2, swapFactors)
// Compare solutions // Compare solutions
EXPECT(assert_equal(fullgraph, planarSLAM::Graph(isam.getFactorsUnsafe()))); EXPECT(assert_equal(fullgraph, planarSLAM::Graph(isam.getFactorsUnsafe())));
EXPECT(isam_check(fullgraph, fullinit, isam)); EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));
// Check gradient at each node // Check gradient at each node
typedef ISAM2::sharedClique sharedClique; typedef ISAM2::sharedClique sharedClique;
@ -1313,7 +815,7 @@ TEST(ISAM2, constrained_ordering)
isam.update(newfactors, init); isam.update(newfactors, init);
} }
CHECK(isam_check(fullgraph, fullinit, isam)); CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
// Add odometry from time 0 to time 5 // Add odometry from time 0 to time 5
for( ; i<5; ++i) { for( ; i<5; ++i) {
@ -1381,7 +883,7 @@ TEST(ISAM2, constrained_ordering)
} }
// Compare solutions // Compare solutions
EXPECT(isam_check(fullgraph, fullinit, isam)); EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));
// Check that x3 and x4 are last, but either can come before the other // Check that x3 and x4 are last, but either can come before the other
EXPECT(isam.getOrdering()[(3)] == 12 && isam.getOrdering()[(4)] == 13); EXPECT(isam.getOrdering()[(3)] == 12 && isam.getOrdering()[(4)] == 13);
@ -1420,122 +922,14 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check)
{ {
// These variables will be reused and accumulate factors and values // These variables will be reused and accumulate factors and values
ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false);
params.enablePartialRelinearizationCheck = true;
ISAM2 isam(params);
Values fullinit; Values fullinit;
planarSLAM::Graph fullgraph; planarSLAM::Graph fullgraph;
ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false);
// i keeps track of the time step params.enablePartialRelinearizationCheck = true;
size_t i = 0; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params);
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((0), Pose2(0.01, 0.01, 0.01));
fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
CHECK(isam_check(fullgraph, fullinit, isam));
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(1.01, 0.01, 0.01));
init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init);
++ i;
}
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init);
++ i;
}
// Compare solutions // Compare solutions
CHECK(isam_check(fullgraph, fullinit, isam)); CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
// Check gradient at each node
typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;
jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(jfg, expectedGradient);
// Compare with actual gradients
int variablePosition = 0;
for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
const int dim = clique->conditional()->dim(jit);
Vector actual = clique->gradientContribution().segment(variablePosition, dim);
EXPECT(assert_equal(expectedGradient[*jit], actual));
variablePosition += dim;
}
LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
}
// Check gradient
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
VectorValues actualGradient(*allocateVectorValues(isam));
gradientAtZero(isam, actualGradient);
EXPECT(assert_equal(expectedGradient2, expectedGradient));
EXPECT(assert_equal(expectedGradient, actualGradient));
} }
/* ************************************************************************* */ /* ************************************************************************* */