Added yet another configuration parameter to iSAM2. 'enablePartialRelinearizationCheck' checks variables for relinearization by descending the Bayes tree. Once a variable does not need to be relinearized, we stop checking that branch. A full check is still the default.
Stephen Williams
parent
b2e15eea4e
commit
c7ff913f64
|
|
@ -82,7 +82,7 @@ FastSet<Index> ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const N
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FastSet<Index> ISAM2::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
|
||||
FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const Permuted<VectorValues>& delta, const Ordering& ordering,
|
||||
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
|
||||
FastSet<Index> relinKeys;
|
||||
|
||||
|
|
@ -109,6 +109,76 @@ FastSet<Index> ISAM2::Impl::CheckRelinearization(const Permuted<VectorValues>& d
|
|||
return relinKeys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double threshold, const Permuted<VectorValues>& delta, const ISAM2Clique::shared_ptr& clique) {
|
||||
|
||||
// Check the current clique for relinearization
|
||||
bool relinearize = false;
|
||||
BOOST_FOREACH(Index var, clique->conditional()->keys()) {
|
||||
double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
|
||||
if(maxDelta >= threshold) {
|
||||
relinKeys.insert(var);
|
||||
relinearize = true;
|
||||
}
|
||||
}
|
||||
|
||||
// If this node was relinearized, also check its children
|
||||
if(relinearize) {
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) {
|
||||
CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const Permuted<VectorValues>& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) {
|
||||
|
||||
// Check the current clique for relinearization
|
||||
bool relinearize = false;
|
||||
BOOST_FOREACH(Index var, clique->conditional()->keys()) {
|
||||
|
||||
// Lookup the key associated with this index
|
||||
Key key = decoder.at(var);
|
||||
|
||||
// Find the threshold for this variable type
|
||||
const Vector& threshold = thresholds.find(Symbol(key).chr())->second;
|
||||
|
||||
// Verify the threshold vector matches the actual variable size
|
||||
if(threshold.rows() != delta[var].rows())
|
||||
throw std::invalid_argument("Relinearization threshold vector dimensionality passed into iSAM2 parameters does not match actual variable dimensionality");
|
||||
|
||||
// Check for relinearization
|
||||
if((delta[var].array().abs() > threshold.array()).any()) {
|
||||
relinKeys.insert(var);
|
||||
relinearize = true;
|
||||
}
|
||||
}
|
||||
|
||||
// If this node was relinearized, also check its children
|
||||
if(relinearize) {
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) {
|
||||
CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, decoder, child);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted<VectorValues>& delta, const Ordering& ordering,
|
||||
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
|
||||
|
||||
FastSet<Index> relinKeys;
|
||||
|
||||
if(relinearizeThreshold.type() == typeid(double)) {
|
||||
CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
|
||||
|
||||
} else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>)) {
|
||||
Ordering::InvertedMap decoder = ordering.invert();
|
||||
CheckRelinearizationRecursiveMap(relinKeys, boost::get<FastMap<char,Vector> >(relinearizeThreshold), delta, decoder, root);
|
||||
}
|
||||
|
||||
return relinKeys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
|
||||
static const bool debug = false;
|
||||
|
|
|
|||
|
|
@ -68,7 +68,21 @@ struct ISAM2::Impl {
|
|||
* @return The set of variable indices in delta whose magnitude is greater than or
|
||||
* equal to relinearizeThreshold
|
||||
*/
|
||||
static FastSet<Index> CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
|
||||
static FastSet<Index> CheckRelinearizationFull(const Permuted<VectorValues>& delta, const Ordering& ordering,
|
||||
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
|
||||
/**
|
||||
* Find the set of variables to be relinearized according to relinearizeThreshold.
|
||||
* This check is performed recursively, starting at the top of the tree. Once a
|
||||
* variable in the tree does not need to be relinearized, no further checks in
|
||||
* that branch are performed. This is an approximation of the Full version, designed
|
||||
* to save time at the expense of accuracy.
|
||||
* @param delta The linear delta to check against the threshold
|
||||
* @param keyFormatter Formatter for printing nonlinear keys during debugging
|
||||
* @return The set of variable indices in delta whose magnitude is greater than or
|
||||
* equal to relinearizeThreshold
|
||||
*/
|
||||
static FastSet<Index> CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted<VectorValues>& delta, const Ordering& ordering,
|
||||
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -588,8 +588,11 @@ ISAM2Result ISAM2::update(
|
|||
tic(5,"gather relinearize keys");
|
||||
vector<bool> markedRelinMask(ordering_.nVars(), false);
|
||||
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
|
||||
relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold);
|
||||
if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging
|
||||
if(params_.enablePartialRelinearizationCheck)
|
||||
relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold);
|
||||
else
|
||||
relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, params_.relinearizeThreshold);
|
||||
if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, 0.0); // This is used for debugging
|
||||
|
||||
// Above relin threshold keys for detailed results
|
||||
if(params_.enableDetailedResults) {
|
||||
|
|
|
|||
|
|
@ -125,6 +125,13 @@ struct ISAM2Params {
|
|||
|
||||
bool enableDetailedResults; ///< Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: false)
|
||||
|
||||
/** Check variables for relinearization in tree-order, stopping the check once a variable does not need to be relinearized (default: false).
|
||||
* This can improve speed by only checking a small part of the top of the tree. However, variables below the check cut-off can accumulate
|
||||
* significant deltas without triggering relinearization. This is particularly useful in exploration scenarios where real-time performance
|
||||
* is desired over correctness. Use with caution.
|
||||
*/
|
||||
bool enablePartialRelinearizationCheck;
|
||||
|
||||
/** Specify parameters as constructor arguments */
|
||||
ISAM2Params(
|
||||
OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams
|
||||
|
|
@ -139,7 +146,7 @@ struct ISAM2Params {
|
|||
relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
|
||||
evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization),
|
||||
cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter),
|
||||
enableDetailedResults(false) {}
|
||||
enableDetailedResults(false), enablePartialRelinearizationCheck(false) {}
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -1242,6 +1242,129 @@ TEST(ISAM2, constrained_ordering)
|
|||
EXPECT(assert_equal(expectedGradient, actualGradient));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ISAM2, slamlike_solution_partial_relinearization_check)
|
||||
{
|
||||
|
||||
// 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;
|
||||
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
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue