Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 34 additions & 7 deletions gtsam/nonlinear/IncrementalFixedLagSmoother.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -181,8 +181,10 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::updateImpl(
std::cout << std::endl;
}

// Force iSAM2 to put the marginalizable variables at the beginning
createOrderingConstraints(marginalizableKeys, isActive, constrainedKeys);
// Force iSAM2 to put the marginalizable variables at the beginning, and
// optionally the newest ones at the end
createOrderingConstraints(marginalizableKeys, newFactorKeys,
current_timestamp, isActive, constrainedKeys);

if (debug) {
std::cout << "Constrained Keys: ";
Expand Down Expand Up @@ -298,16 +300,41 @@ void IncrementalFixedLagSmoother::eraseKeysBefore(double timestamp) {

/* ************************************************************************* */
void IncrementalFixedLagSmoother::createOrderingConstraints(
const KeyVector& marginalizableKeys,
const std::function<bool(Key)>& isActive,
const KeyVector& marginalizableKeys, const KeySet& newFactorKeys,
double currentTimestamp, const std::function<bool(Key)>& isActive,
std::optional<FastMap<Key, int> >& constrainedKeys) const {
if (marginalizableKeys.size() > 0) {
// Without constraints, iSAM2 already orders the keys of the new factors last.
// Constraining only the marginalizable keys loses that, so the newest keys
// get their own last group, unless disabled.
if (marginalizableKeys.empty()) {
if (params_.newestKeysLast) {
// Nothing to marginalize: only the newest keys need a group, the others
// stay in the default group 0.
constrainedKeys = FastMap<Key, int>();
for (auto it = timestampKeyMap_.lower_bound(currentTimestamp);
it != timestampKeyMap_.end(); ++it) {
if (isActive(it->second)) {
(*constrainedKeys)[it->second] = 1;
}
}
for (Key key : newFactorKeys) {
(*constrainedKeys)[key] = 1;
}
}
} else {
constrainedKeys = FastMap<Key, int>();
// Generate ordering constraints so that the marginalizable variables will be eliminated first
// Set all variables to Group1
// Set all variables to Group1, or the newest ones to Group2 if enabled
for(const TimestampKeyMap::value_type& timestamp_key: timestampKeyMap_) {
if (isActive(timestamp_key.second)) {
constrainedKeys->operator[](timestamp_key.second) = 1;
const bool newest =
params_.newestKeysLast && timestamp_key.first >= currentTimestamp;
constrainedKeys->operator[](timestamp_key.second) = newest ? 2 : 1;
}
}
if (params_.newestKeysLast) {
for (Key key : newFactorKeys) {
constrainedKeys->operator[](key) = 2;
}
}
// Set marginalizable variables to Group0
Expand Down
69 changes: 58 additions & 11 deletions gtsam/nonlinear/IncrementalFixedLagSmoother.h
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,32 @@

namespace gtsam {

/** Parameters of IncrementalFixedLagSmoother */
struct IncrementalFixedLagSmootherParams {
/** The iSAM2 parameters. By default it reuses the slots of removed factors,
* since the smoother keeps removing factors as they leave the window. */
ISAM2Params isam2;

/**
* In every update, eliminate the keys of the new factors and those at the
* current timestamp last, so the newest states stay at the root of the Bayes
* tree. Then an update or a marginal of the current state only touches a
* short path of the tree. It pays off with several variables per time step
* or several sensors, which otherwise leave the newest states deep in the
* tree when marginalizing. Set it to false for the previous ordering, which
* only constrains the marginalizable keys.
*/
bool newestKeysLast = true;

IncrementalFixedLagSmootherParams() {
isam2.findUnusedFactorSlots = true;
}

/** The default options with the given iSAM2 parameters */
explicit IncrementalFixedLagSmootherParams(const ISAM2Params& isam2Params)
: isam2(isam2Params) {}
};

/**
* This is a base class for the various HMF2 implementations. The HMF2 eliminates the factor graph
* such that the active states are placed in/near the root. This base class implements a function
Expand All @@ -40,11 +66,21 @@ class GTSAM_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother {
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
typedef std::shared_ptr<IncrementalFixedLagSmoother> shared_ptr;

/** default constructor */
IncrementalFixedLagSmoother(double smootherLag = 0.0,
const ISAM2Params& parameters = DefaultISAM2Params()) :
FixedLagSmoother(smootherLag), isam_(parameters) {
}
/**
* Constructor with the given iSAM2 parameters and the default options. For
* the previous ordering, use the constructor with
* IncrementalFixedLagSmootherParams and set newestKeysLast to false.
*/
IncrementalFixedLagSmoother(
double smootherLag = 0.0,
const ISAM2Params& parameters = DefaultISAM2Params())
: IncrementalFixedLagSmoother(
smootherLag, IncrementalFixedLagSmootherParams(parameters)) {}

/** Constructor */
IncrementalFixedLagSmoother(double smootherLag,
const IncrementalFixedLagSmootherParams& params)
: FixedLagSmoother(smootherLag), isam_(params.isam2), params_(params) {}

/** destructor */
~IncrementalFixedLagSmoother() override {
Expand Down Expand Up @@ -154,6 +190,11 @@ class GTSAM_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother {
return isam_.params();
}

/** return the smoother parameters */
const IncrementalFixedLagSmootherParams& smootherParams() const {
return params_;
}

/** Access the current set of factors */
const NonlinearFactorGraph& getFactors() const {
return isam_.getFactorsUnsafe();
Expand Down Expand Up @@ -184,9 +225,7 @@ class GTSAM_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother {

/** Create default parameters */
static ISAM2Params DefaultISAM2Params() {
ISAM2Params params;
params.findUnusedFactorSlots = true;
return params;
return IncrementalFixedLagSmootherParams().isam2;
}

/** An iSAM2 object used to perform inference. The smoother lag is controlled
Expand All @@ -196,6 +235,9 @@ class GTSAM_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother {
/** Store results of latest isam2 update */
ISAM2Result isamResult_;

/** The smoother parameters */
IncrementalFixedLagSmootherParams params_;

/// Shared body of both update() overloads.
Result updateImpl(const NonlinearFactorGraph& newFactors,
const Values& newTheta,
Expand All @@ -207,10 +249,15 @@ class GTSAM_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother {
/** Erase any keys associated with timestamps before the provided time */
void eraseKeysBefore(double timestamp);

/** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */
/**
* Fill in an iSAM2 ConstrainedKeys structure such that the marginalizable
* keys are eliminated before all others and, if newestKeysLast is set, the
* keys of the new factors and those at the current timestamp after all
* others.
*/
void createOrderingConstraints(
const KeyVector& marginalizableKeys,
const std::function<bool(Key)>& isActive,
const KeyVector& marginalizableKeys, const KeySet& newFactorKeys,
double currentTimestamp, const std::function<bool(Key)>& isActive,
std::optional<FastMap<Key, int> >& constrainedKeys) const;

private:
Expand Down
11 changes: 11 additions & 0 deletions gtsam/nonlinear/nonlinear_continuous_time.i
Original file line number Diff line number Diff line change
Expand Up @@ -389,14 +389,25 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
};

#include <gtsam/nonlinear/IncrementalFixedLagSmoother.h>
class IncrementalFixedLagSmootherParams {
IncrementalFixedLagSmootherParams();
IncrementalFixedLagSmootherParams(const gtsam::ISAM2Params& isam2Params);

gtsam::ISAM2Params isam2;
bool newestKeysLast;
};

virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
IncrementalFixedLagSmoother();
IncrementalFixedLagSmoother(double smootherLag);
IncrementalFixedLagSmoother(double smootherLag,
const gtsam::ISAM2Params& parameters);
IncrementalFixedLagSmoother(
double smootherLag, const gtsam::IncrementalFixedLagSmootherParams& params);

gtsam::Matrix marginalCovariance(gtsam::Key key) const;
const gtsam::ISAM2Params& params() const;
const gtsam::IncrementalFixedLagSmootherParams& smootherParams() const;

const gtsam::NonlinearFactorGraph& getFactors() const;
const gtsam::Values& getLinearizationPoint() const;
Expand Down
65 changes: 65 additions & 0 deletions gtsam/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -472,6 +472,71 @@ TEST(IncrementalFixedLagSmoother, ConnectsPendingValueBeforeItAgesOut) {
EXPECT(!smoother.getISAM2().getVariableIndex().empty(X(1)));
}

/* ************************************************************************* */
// Both constructors keep the default of reusing the slots of removed factors,
// unless given other iSAM2 parameters, and use the default options.
TEST(IncrementalFixedLagSmoother, Params) {
EXPECT(IncrementalFixedLagSmoother().params().findUnusedFactorSlots);
EXPECT(IncrementalFixedLagSmoother(1.0, ISAM2Params())
.smootherParams()
.newestKeysLast);
IncrementalFixedLagSmootherParams params;
EXPECT(params.isam2.findUnusedFactorSlots);
EXPECT(params.newestKeysLast);
params.newestKeysLast = false;
EXPECT(!IncrementalFixedLagSmoother(1.0, params)
.smootherParams()
.newestKeysLast);
EXPECT(
IncrementalFixedLagSmoother(1.0, params).params().findUnusedFactorSlots);
EXPECT(!IncrementalFixedLagSmoother(1.0, ISAM2Params())
.params()
.findUnusedFactorSlots);
}

/* ************************************************************************* */
// By default, the newest variables stay at the root of the Bayes tree, also
// in updates that marginalize and in updates without new factors.
TEST(IncrementalFixedLagSmoother, NewestKeysLast) {
using symbol_shorthand::V;
const SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
IncrementalFixedLagSmoother smoother(5.0);
const auto depth = [&](Key key) {
size_t levels = 0;
for (auto clique = smoother.getISAM2()[key]; clique->parent();
clique = clique->parent()) {
++levels;
}
return levels;
};

// Two variables per step, linked to each other and to the previous step.
for (size_t i = 0; i < 20; ++i) {
NonlinearFactorGraph factors;
Values values;
FixedLagSmoother::KeyTimestampMap timestamps;
values.insert(X(i), Point2(0.0, 0.0));
values.insert(V(i), Point2(0.0, 0.0));
timestamps[X(i)] = static_cast<double>(i);
timestamps[V(i)] = static_cast<double>(i);
factors.emplace_shared<BetweenPoint2>(X(i), V(i), Point2(0.0, 0.0), noise);
if (i == 0) {
factors.addPrior(X(0), Point2(0.0, 0.0), noise);
} else {
factors.emplace_shared<BetweenPoint2>(X(i - 1), X(i), Point2(0.0, 0.0),
noise);
factors.emplace_shared<BetweenPoint2>(V(i - 1), V(i), Point2(0.0, 0.0),
noise);
factors.emplace_shared<BetweenPoint2>(X(i - 1), V(i), Point2(0.0, 0.0),
noise);
}
smoother.update(factors, values, timestamps);
EXPECT_LONGS_EQUAL(0, depth(X(i)));
smoother.update();
EXPECT_LONGS_EQUAL(0, depth(X(i)));
}
}

// A pending value with no factor is removed once it leaves the lag window.
TEST(IncrementalFixedLagSmoother, ReapsPendingValueAfterLag) {
const SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
Expand Down
Loading
Loading