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

(in branch) incremental dogleg bug fix and unit test (wasn't computing Gauss-Newton point)

release/4.3a0
Richard Roberts 2011-12-15 15:37:52 +00:00
parent f3de9e425f
commit 21140ea0d5
2 changed files with 138 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/nonlinear/ISAM2-inl.h
View File

@ -600,10 +600,7 @@ VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateBestEstimate() const {
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class VALUES, class GRAPH>
VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam) { VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam) {
VectorValues delta = *allocateVectorValues(isam); VectorValues delta = *allocateVectorValues(isam);
for(Index j=0; j<isam.getDelta().size(); ++j) optimize2(isam.root(), delta);
delta[j] = isam.getDelta()[j];
delta.print("delta: ");
assert_equal(isam.getDelta().container(), delta);
return delta; return delta;
} }

141
tests/testGaussianISAM2.cpp
View File

@ -194,7 +194,7 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const planarSLAM::Values& fu
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST_UNSAFE(ISAM2, slamlike_solution) TEST(ISAM2, slamlike_solution_gaussnewton)
{ {
// SETDEBUG("ISAM2 update", true); // SETDEBUG("ISAM2 update", true);
@ -230,7 +230,7 @@ TEST_UNSAFE(ISAM2, slamlike_solution)
isam.update(newfactors, init); isam.update(newfactors, init);
} }
EXPECT(isam_check(fullgraph, fullinit, isam)); CHECK(isam_check(fullgraph, fullinit, isam));
// Add odometry from time 0 to time 5 // Add odometry from time 0 to time 5
for( ; i<5; ++i) { for( ; i<5; ++i) {
@ -295,7 +295,7 @@ TEST_UNSAFE(ISAM2, slamlike_solution)
} }
// Compare solutions // Compare solutions
EXPECT(isam_check(fullgraph, fullinit, isam)); CHECK(isam_check(fullgraph, fullinit, isam));
// Check gradient at each node // Check gradient at each node
typedef GaussianISAM2<planarSLAM::Values>::sharedClique sharedClique; typedef GaussianISAM2<planarSLAM::Values>::sharedClique sharedClique;
@ -327,7 +327,140 @@ TEST_UNSAFE(ISAM2, slamlike_solution)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST_UNSAFE(ISAM2, clone) { TEST(ISAM2, slamlike_solution_dogleg)
{
// SETDEBUG("ISAM2 update", true);
// SETDEBUG("ISAM2 update verbose", true);
// SETDEBUG("ISAM2 recalculate", true);
// Pose and landmark key types from planarSLAM
typedef planarSLAM::PoseKey PoseKey;
typedef planarSLAM::PointKey PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
planarSLAM::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.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(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.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(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.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(PointKey(1), 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.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(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.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(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
typedef GaussianISAM2<planarSLAM::Values>::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, clone) {
// Pose and landmark key types from planarSLAM // Pose and landmark key types from planarSLAM
typedef planarSLAM::PoseKey PoseKey; typedef planarSLAM::PoseKey PoseKey;