29 using namespace gtsam;
30 using std::shared_ptr;
43 Values* init_values =
nullptr,
47 ISAM2Params::CHOLESKY,
true,
49 size_t maxPoses = 10) {
68 isam.
update(newfactors, init);
81 init.
insert((i+1),
Pose2(
double(i+1)+0.1, -0.1, 0.01));
82 fullinit.
insert((i+1),
Pose2(
double(i+1)+0.1, -0.1, 0.01));
84 isam.
update(newfactors, init);
109 isam.
update(newfactors, init);
123 init.
insert((i+1),
Pose2(
double(i+1)+0.1, -0.1, 0.01));
124 fullinit.
insert((i+1),
Pose2(
double(i+1)+0.1, -0.1, 0.01));
126 isam.
update(newfactors, init);
147 isam.
update(newfactors, init);
154 *full_graph = fullgraph;
157 *init_values = fullinit;
201 consistent(true), isam(isam) {}
204 if(find(isam.
roots().begin(), isam.
roots().end(), node) == isam.
roots().end())
206 if(node->parent_.expired())
208 if(find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end())
211 for(
Key j: node->conditional()->frontals())
213 if(isam.
nodes().at(
j).get() != node.get())
224 const string name_ = test.getName();
236 expectedHessian.bottomRightCorner(1,1) = actualHessian.bottomRightCorner(1,1);
237 bool isamTreeEqual =
assert_equal(expectedHessian, actualHessian);
248 bool nodeGradientsOk =
true;
249 for (
const auto& [
key, clique] : isam.
nodes()) {
256 for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
257 const DenseIndex dim = clique->conditional()->getDim(jit);
258 Vector actual = clique->gradientContribution().segment(variablePosition, dim);
259 bool gradOk =
assert_equal(expectedGradient[*jit], actual);
261 nodeGradientsOk = nodeGradientsOk && gradOk;
262 variablePosition += dim;
264 bool dimOk = clique->gradientContribution().rows() == variablePosition;
266 nodeGradientsOk = nodeGradientsOk && dimOk;
273 bool expectedGradOk =
assert_equal(expectedGradient2, expectedGradient);
275 bool totalGradOk =
assert_equal(expectedGradient, actualGradient);
278 return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent;
284 for(
size_t i = 0;
i < 10; ++
i) {
390 toRemove.push_back(12);
410 toRemove.push_back(7);
411 toRemove.push_back(14);
437 toRemove.push_back(swap_idx);
438 fullgraph.
remove(swap_idx);
452 for (
const auto& [
key, clique]: isam.
nodes()) {
459 for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
460 const DenseIndex dim = clique->conditional()->getDim(jit);
461 Vector actual = clique->gradientContribution().segment(variablePosition, dim);
463 variablePosition += dim;
465 EXPECT_LONGS_EQUAL((
long)clique->gradientContribution().rows(), (long)variablePosition);
486 constrained.insert(make_pair((3), 1));
487 constrained.insert(make_pair((4), 2));
502 isam.
update(newfactors, init);
514 init.
insert((i+1),
Pose2(
double(i+1)+0.1, -0.1, 0.01));
515 fullinit.
insert((i+1),
Pose2(
double(i+1)+0.1, -0.1, 0.01));
520 isam.
update(newfactors, init);
550 init.
insert((i+1),
Pose2(
double(i+1)+0.1, -0.1, 0.01));
551 fullinit.
insert((i+1),
Pose2(
double(i+1)+0.1, -0.1, 0.01));
576 for (
const auto& [
key, clique]: isam.
nodes()) {
583 for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
584 const DenseIndex dim = clique->conditional()->getDim(jit);
585 Vector actual = clique->gradientContribution().segment(variablePosition, dim);
587 variablePosition += dim;
589 LONGS_EQUAL((
long)clique->gradientContribution().rows(), (long)variablePosition);
601 TEST(
ISAM2, slamlike_solution_partial_relinearization_check)
616 Matrix expectedAugmentedHessian, expected3AugmentedHessian;
619 if(find(leafKeys.begin(), leafKeys.end(),
key) == leafKeys.end()) {
620 toKeep.push_back(
key);
633 ->marginal(toKeep,
EliminateQR)->augmentedHessian();
644 assert(actualAugmentedHessian.allFinite());
648 bool treeEqual =
assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1
e-6);
651 actualAugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1);
bool nonlinEqual =
assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1
e-6);
655 bool afterNonlinCorrect =
assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1
e-6);
657 bool ok = treeEqual && nonlinEqual && afterNonlinCorrect;
673 values.insert(0, 0.0);
674 values.insert(1, 0.0);
675 values.insert(2, 0.0);
678 constrainedKeys.insert(make_pair(0, 0));
679 constrainedKeys.insert(make_pair(1, 1));
680 constrainedKeys.insert(make_pair(2, 2));
685 EXPECT(checkMarginalizeLeaves(isam, leafKeys));
701 values.insert(0, 0.0);
702 values.insert(1, 0.0);
703 values.insert(2, 0.0);
704 values.insert(3, 0.0);
707 constrainedKeys.insert(make_pair(0, 0));
708 constrainedKeys.insert(make_pair(1, 1));
709 constrainedKeys.insert(make_pair(2, 2));
710 constrainedKeys.insert(make_pair(3, 3));
715 EXPECT(checkMarginalizeLeaves(isam, leafKeys));
736 values.insert(0, 0.0);
737 values.insert(1, 0.0);
738 values.insert(2, 0.0);
739 values.insert(3, 0.0);
740 values.insert(4, 0.0);
741 values.insert(5, 0.0);
744 constrainedKeys.insert(make_pair(0, 0));
745 constrainedKeys.insert(make_pair(1, 1));
746 constrainedKeys.insert(make_pair(2, 2));
747 constrainedKeys.insert(make_pair(3, 3));
748 constrainedKeys.insert(make_pair(4, 4));
749 constrainedKeys.insert(make_pair(5, 5));
754 EXPECT(checkMarginalizeLeaves(isam, leafKeys));
767 values.insert(0, 0.0);
768 values.insert(1, 0.0);
769 values.insert(2, 0.0);
772 constrainedKeys.insert(make_pair(0, 0));
773 constrainedKeys.insert(make_pair(1, 1));
774 constrainedKeys.insert(make_pair(2, 2));
779 EXPECT(checkMarginalizeLeaves(isam, leafKeys));
790 EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys));
810 int actual = isam.
roots().front()->calculate_nnz();
const gtsam::Symbol key('X', 0)
Provides additional testing facilities for common data structures.
Matrix marginalCovariance(Key key) const
ConsistencyVisitor(const ISAM2 &isam)
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
static const SharedNoiseModel model
static int runAllTests(TestResult &result)
T between(const T &t1, const T &t2)
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
const Nodes & nodes() const
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
const GaussianFactorGraph factors
Matrix augmentedHessian(const Ordering &ordering) const
virtual ISAM2Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const FactorIndices &removeFactorIndices=FactorIndices(), const std::optional< FastMap< Key, int > > &constrainedKeys={}, const std::optional< FastList< Key > > &noRelinKeys={}, const std::optional< FastList< Key > > &extraReelimKeys={}, bool force_relinearize=false)
static const KeyFormatter DefaultKeyFormatter
ptrdiff_t DenseIndex
The index type for Eigen objects.
const Values & getLinearizationPoint() const
Access the current linearization point.
static const SmartProjectionParams params
Values retract(const VectorValues &delta) const
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
FastVector< FactorIndex > FactorIndices
Define collection types:
const NonlinearFactorGraph & getFactorsUnsafe() const
ISAM2 createSlamlikeISAM2(Values *init_values=nullptr, NonlinearFactorGraph *full_graph=nullptr, const ISAM2Params ¶ms=ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true, ISAM2Params::CHOLESKY, true, DefaultKeyFormatter, true), size_t maxPoses=10)
std::shared_ptr< FactorGraphType > marginal(const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
int operator()(const ISAM2::sharedClique &node, int &parentData)
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
#define EXPECT(condition)
VectorValues gradientAtZero() const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
NonlinearISAM isam(relinearizeInterval)
bool isam_check(const NonlinearFactorGraph &fullgraph, const Values &fullinit, const ISAM2 &isam, Test &test, TestResult &result)
Linear Factor Graph where all factors are Gaussians.
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Base::sharedClique sharedClique
Shared pointer to a clique.
void marginalizeLeaves(const FastList< Key > &leafKeys, FactorIndices *marginalFactorsIndices=nullptr, FactorIndices *deletedFactorsIndices=nullptr)
#define LONGS_EQUAL(expected, actual)
noiseModel::Diagonal::shared_ptr SharedDiagonal
std::vector< float > Values
#define EXPECT_LONGS_EQUAL(expected, actual)
const Roots & roots() const
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
bool enablePartialRelinearizationCheck
const VectorValues & getDelta() const
Create small example with two poses and one landmark.
Chordal Bayes Net, the result of eliminating a factor graph.
void insert(Key j, const Value &val)
A class for computing marginals in a NonlinearFactorGraph.
Jet< T, N > sqrt(const Jet< T, N > &f)
VectorValues gradient(const VectorValues &x0) const
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
virtual VectorValues gradientAtZero() const
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
std::uint64_t Key
Integer nonlinear key type.
Values calculateEstimate() const
noiseModel::Base::shared_ptr SharedNoiseModel