61 lines
1.4 KiB
C++
61 lines
1.4 KiB
C++
/*
|
|
* ConstrainedNonlinearFactorGraph.h
|
|
*
|
|
* Created on: Aug 10, 2009
|
|
* Author: alexgc
|
|
*/
|
|
|
|
#ifndef CONSTRAINEDNONLINEARFACTORGRAPH_H_
|
|
#define CONSTRAINEDNONLINEARFACTORGRAPH_H_
|
|
|
|
#include "NonlinearFactorGraph.h"
|
|
#include "EqualityFactor.h"
|
|
#include "ConstrainedLinearFactorGraph.h"
|
|
|
|
namespace gtsam {
|
|
|
|
class ConstrainedNonlinearFactorGraph: public NonlinearFactorGraph {
|
|
protected:
|
|
/** collection of equality factors */
|
|
std::vector<EqualityFactor::shared_ptr> eq_factors;
|
|
|
|
public:
|
|
// iterators over equality factors
|
|
typedef std::vector<EqualityFactor::shared_ptr>::const_iterator eq_const_iterator;
|
|
typedef std::vector<EqualityFactor::shared_ptr>::iterator eq_iterator;
|
|
|
|
/**
|
|
* Default constructor
|
|
*/
|
|
ConstrainedNonlinearFactorGraph();
|
|
|
|
/**
|
|
* Copy constructor from regular NLFGs
|
|
*/
|
|
ConstrainedNonlinearFactorGraph(const NonlinearFactorGraph& nfg);
|
|
|
|
virtual ~ConstrainedNonlinearFactorGraph();
|
|
|
|
/**
|
|
* Linearize a nonlinear graph to produce a linear graph
|
|
* Note that equality constraints will just pass through
|
|
*/
|
|
ConstrainedLinearFactorGraph linearize(const FGConfig& initial) const;
|
|
|
|
/**
|
|
* Insert a equality factor into the graph
|
|
*/
|
|
void push_back_eq(const EqualityFactor::shared_ptr& eq) {
|
|
eq_factors.push_back(eq);
|
|
}
|
|
|
|
/**
|
|
* converts the graph to a regular nonlinear graph - removes equality constraints
|
|
*/
|
|
NonlinearFactorGraph convert() const;
|
|
};
|
|
|
|
}
|
|
|
|
#endif /* CONSTRAINEDNONLINEARFACTORGRAPH_H_ */
|