40 lines
1.5 KiB
C++
40 lines
1.5 KiB
C++
/**
|
|
* @file GaussianISAM
|
|
* @brief Full non-linear ISAM.
|
|
* @author Michael Kaess
|
|
*/
|
|
|
|
// \callgraph
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/inference/ISAM2.h>
|
|
#include <gtsam/linear/GaussianConditional.h>
|
|
#include <gtsam/linear/GaussianFactor.h>
|
|
#include <gtsam/slam/simulated2D.h>
|
|
#include <gtsam/slam/planarSLAM.h>
|
|
|
|
namespace gtsam {
|
|
|
|
typedef ISAM2<GaussianConditional, simulated2D::Config> GaussianISAM2;
|
|
typedef ISAM2<GaussianConditional, planarSLAM::Config> GaussianISAM2_P;
|
|
|
|
// optimize the BayesTree, starting from the root
|
|
boost::shared_ptr<VectorConfig> optimize2(const GaussianISAM2::sharedClique& root);
|
|
|
|
// optimize the BayesTree, starting from the root; "replaced" needs to contain
|
|
// all variables that are contained in the top of the Bayes tree that has been
|
|
// redone; "delta" is the current solution, an offset from the linearization
|
|
// point; "threshold" is the maximum change against the PREVIOUS delta for
|
|
// non-replaced variables that can be ignored, ie. the old delta entry is kept
|
|
// and recursive backsubstitution might eventually stop if none of the changed
|
|
// variables are contained in the subtree.
|
|
// returns the number of variables that were solved for
|
|
int optimize2(const GaussianISAM2::sharedClique& root,
|
|
double threshold, const std::set<Symbol>& replaced, VectorConfig& delta);
|
|
|
|
// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
|
|
int calculate_nnz(const GaussianISAM2::sharedClique& clique);
|
|
|
|
}/// namespace gtsam
|