Go to the documentation of this file.
48 const std::optional<FactorIndices>& removeFactorIndices) {
57 if(removeFactorIndices){
59 std::cout <<
"ConcurrentIncrementalSmoother::update removeFactorIndices - not implemented yet" << std::endl;
70 constrainedKeys[
key] = 1;
71 noRelinKeys.push_back(
key);
142 gttic(get_summarized_factors);
150 gttoc(get_summarized_factors);
190 std::set<ISAM2Clique::shared_ptr> separatorCliques;
193 separatorCliques.insert( clique );
199 for(
Key key: clique->conditional()->frontals()) {
200 cliqueKeys.push_back(
key);
203 std::sort(cliqueKeys.begin(), cliqueKeys.end());
206 std::set<size_t> cliqueFactorSlots;
207 for(
Key key: cliqueKeys) {
211 std::set<Key> factorKeys(factor->begin(), factor->end());
212 if(std::includes(cliqueKeys.begin(), cliqueKeys.end(), factorKeys.begin(), factorKeys.end())) {
213 cliqueFactorSlots.insert(slot);
221 cliqueFactorSlots.erase(slot);
226 for(
size_t slot: cliqueFactorSlots) {
231 std::set<ISAM2Clique::shared_ptr> childCliques;
234 childCliques.insert(clique->children.begin(), clique->children.end());
238 childCliques.erase(clique);
244 graph.push_back( factor );
Values smootherValues_
New variables to be added to the smoother during the next update.
std::shared_ptr< This > shared_ptr
const VariableIndex & getVariableIndex() const
bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
NonlinearFactorGraph filterSummarizationFactors_
New filter summarization factors to replace the existing filter summarization during the next update.
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< FactorIndices > &removeFactorIndices={})
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
const NonlinearFactorGraph & getFactorsUnsafe() const
FactorIndices newFactorsIndices
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
GaussianFactorGraph::Eliminate getEliminationFunction() const
virtual bool equals(const ISAM2 &other, double tol=1e-9) const
void updateSmootherSummarization()
const sharedFactor at(size_t i) const
bool synchronizationUpdatesAvailable_
Flag indicating the currently stored synchronization updates have not been applied yet.
const Values & getLinearizationPoint() const
Access the current linearization point.
double error(const Values &values) const
const ValueType at(Key j) const
virtual void resize(size_t size)
Values separatorValues_
The linearization points of the separator variables. These should not be changed during optimization.
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph &graph, const Values &theta, const KeySet &remainingKeys, const GaussianFactorGraph::Eliminate &eliminateFunction)
NonlinearFactorGraph smootherSummarization_
A temporary holding place for calculated smoother summarization.
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
ISAM2 isam2_
iSAM2 inference engine
bool equals(const Values &other, double tol=1e-9) const
bool equals(const ConcurrentSmoother &rhs, double tol=1e-9) const override
void getSummarizedFactors(NonlinearFactorGraph &summarizedFactors, Values &separatorValues) override
const gtsam::Symbol key('X', 0)
Values calculateEstimate() const
std::shared_ptr< This > shared_ptr
std::shared_ptr< This > shared_ptr
FactorIndices filterSummarizationSlots_
The slots in factor graph that correspond to the current filter summarization factors.
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)
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
void insert(Key j, const Value &val)
NonlinearFactorGraph smootherFactors_
New factors to be added to the smoother during the next update.
void synchronize(const NonlinearFactorGraph &smootherFactors, const Values &smootherValues, const NonlinearFactorGraph &summarizedFactors, const Values &separatorValues) override
An iSAM2-based Smoother that implements the Concurrent Filtering and Smoothing interface.
NonlinearFactorGraph graph
std::uint64_t Key
Integer nonlinear key type.
bool equal(const T &obj1, const T &obj2, double tol)
void print(const std::string &s="Concurrent Incremental Smoother:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
FastVector< FactorIndex > FactorIndices
Define collection types:
const ISAM2Params & params() const
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:09