35 #include <Eigen/Dense> 44 #include "../stuff/macros.h" 45 #include "../stuff/color_macros.h" 46 #include "../stuff/string_tools.h" 47 #include "../stuff/misc.h" 65 _graph(0), _userData(0), _hessianIndex(-1), _fixed(false), _marginalized(false),
66 _colInHessian(-1), _cacheContainer(0)
247 cerr << __FUNCTION__ <<
": FATAL, a vertex with ID " << v->
id() <<
" has already been registered with this graph" << endl;
248 assert(0 &&
"Vertex with this ID already contained in the graph");
252 assert(ov &&
"Vertex does not inherit from OptimizableGraph::Vertex");
254 cerr << __FUNCTION__ <<
": FATAL, vertex with ID " << v->
id() <<
" has already registered with another graph " << ov->
_graph << endl;
255 assert(0 &&
"Vertex already registered with another graph");
267 assert(e &&
"Edge does not inherit from OptimizableGraph::Edge");
275 cerr << __FUNCTION__ <<
": FATAL, cannot resolve parameters for edge " << e << endl;
279 cerr << __FUNCTION__ <<
": FATAL, cannot resolve caches for edge " << e << endl;
292 for (OptimizableGraph::EdgeSet::const_iterator it = this->
edges().begin(); it != this->
edges().end(); ++it) {
301 for (OptimizableGraph::VertexIDMap::iterator it=
_vertices.begin(); it!=
_vertices.end(); ++it) {
309 for (OptimizableGraph::VertexIDMap::iterator it=
_vertices.begin(); it!=
_vertices.end(); ++it) {
317 for (OptimizableGraph::VertexIDMap::iterator it=
_vertices.begin(); it!=
_vertices.end(); ++it) {
325 for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
333 for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
341 for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
349 for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
362 cerr <<
"Loaded " <<
_parameters.size() <<
" parameters" << endl;
365 is.seekg(ios_base::beg);
366 set<string> warnedUnknownTypes;
367 stringstream currentLine;
375 Vertex* previousVertex = 0;
376 Data* previousData = 0;
379 int bytesRead =
readLine(is, currentLine);
382 currentLine >> token;
384 if (bytesRead == 0 || token.size() == 0 || token[0] ==
'#')
388 bool handledCommand =
false;
390 if (token ==
"FIX") {
391 handledCommand =
true;
393 while (currentLine >>
id) {
397 cerr <<
"Fixing vertex " << v->
id() << endl;
401 cerr <<
"Warning: Unable to fix vertex with id " <<
id <<
". Not found in the graph." << endl;
413 token = foundIt->second;
418 if (warnedUnknownTypes.count(token) != 1) {
419 warnedUnknownTypes.insert(token);
426 if (dynamic_cast<Vertex*>(element)) {
432 bool r = v->
read(currentLine);
443 else if (dynamic_cast<Edge*>(element)) {
446 Edge* e =
static_cast<Edge*
>(element);
456 currentLine >> id1 >> id2;
460 if ((!from || !to) ) {
462 cerr <<
__PRETTY_FUNCTION__ <<
": Unable to find vertices for edge " << token <<
" " << id1 <<
" <-> " << id2 << endl;
482 e->
read(currentLine);
484 cerr <<
__PRETTY_FUNCTION__ <<
": Unable to add edge " << token <<
" " << id1 <<
" <-> " << id2 << endl;
491 fromSet.insert(from);
510 for (
int l = 0; l < numV; ++l)
511 currentLine >> ids[l];
512 bool vertsOkay =
true;
513 for (
int l = 0; l < numV; ++l) {
522 for (
int l = 0; l < numV; ++l) {
525 cerr <<
" " << ids[l];
529 bool r = e->
read(currentLine);
532 for (
int l = 0; l < numV; ++l) {
535 cerr <<
" " << ids[l];
541 }
else if (dynamic_cast<Data*>(element)) {
544 bool r = d->
read(currentLine);
546 cerr <<
__PRETTY_FUNCTION__ <<
": Error reading data " << token <<
" for vertex " << previousVertex->
id() << endl;
549 }
else if (previousData){
554 }
else if (previousVertex){
573 ifstream ifs(filename);
578 return load(ifs, createEdges);
583 ofstream ofs(filename);
586 return save(ofs, level);
593 set<Vertex*, VertexIDCompare> verticesToSave;
594 for (HyperGraph::EdgeSet::const_iterator it =
edges().begin(); it !=
edges().end(); ++it) {
597 for (vector<HyperGraph::Vertex*>::const_iterator it = e->
vertices().begin(); it != e->
vertices().end(); ++it) {
598 verticesToSave.insert(static_cast<OptimizableGraph::Vertex*>(*it));
603 for (set<Vertex*, VertexIDCompare>::const_iterator it = verticesToSave.begin(); it != verticesToSave.end(); ++it){
609 for (HyperGraph::EdgeSet::const_iterator it =
edges().begin(); it !=
edges().end(); ++it) {
612 edgesToSave.push_back(const_cast<Edge*>(e));
614 sort(edgesToSave.begin(), edgesToSave.end(),
EdgeIDCompare());
616 for (EdgeContainer::const_iterator it = edgesToSave.begin(); it != edgesToSave.end(); ++it) {
630 for (HyperGraph::VertexSet::const_iterator it=vset.begin(); it!=vset.end(); it++){
634 for (HyperGraph::EdgeSet::const_iterator it =
edges().begin(); it !=
edges().end(); ++it) {
639 bool verticesInEdge =
true;
640 for (vector<HyperGraph::Vertex*>::const_iterator it = e->
vertices().begin(); it != e->
vertices().end(); ++it) {
641 if (vset.find(*it) == vset.end()) {
642 verticesInEdge =
false;
646 if (! verticesInEdge)
659 std::set<OptimizableGraph::Vertex*> vset;
660 for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {
662 for (vector<HyperGraph::Vertex*>::const_iterator it = e->
vertices().begin(); it != e->
vertices().end(); ++it) {
668 for (std::set<OptimizableGraph::Vertex*>::const_iterator it=vset.begin(); it!=vset.end(); ++it){
673 for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {
682 for (HyperGraph::VertexIDMap::iterator it=g->
vertices().begin(); it!=g->
vertices().end(); ++it){
691 for (HyperGraph::EdgeSet::iterator it=g->
edges().begin(); it!=g->
edges().end(); ++it){
696 for (vector<HyperGraph::Vertex*>::const_iterator it = e->
vertices().begin(); it != e->
vertices().end(); ++it) {
707 for (HyperGraph::VertexIDMap::const_iterator it=
vertices().begin(); it!=
vertices().end(); ++it){
709 maxDim = (std::max)(maxDim, v->
dimension());
717 vector<string> typesMap =
strSplit(types,
",");
718 for (
size_t i = 0; i < typesMap.size(); ++i) {
719 vector<string> m =
strSplit(typesMap[i],
"=");
721 cerr <<
__PRETTY_FUNCTION__ <<
": unable to extract type map from " << typesMap[i] << endl;
724 string typeInFile =
trim(m[0]);
725 string loadedType =
trim(m[1]);
726 if (! factory->
knowsTag(loadedType)) {
734 cerr <<
"# load look up table" << endl;
736 cerr <<
"#\t" << it->first <<
" -> " << it->second << endl;
742 std::set<int> auxDims;
743 if (vertDims_.size() == 0) {
746 const set<int>& vertDims = vertDims_.size() == 0 ? auxDims : vertDims_;
747 bool suitableSolver =
true;
748 if (vertDims.size() == 2) {
750 suitableSolver = vertDims.count(solverProperty.
poseDim) == 1 && vertDims.count(solverProperty.
landmarkDim) == 1;
753 suitableSolver = solverProperty.
poseDim == -1;
755 }
else if (vertDims.size() == 1) {
756 suitableSolver = vertDims.count(solverProperty.
poseDim) == 1 || solverProperty.
poseDim == -1;
760 return suitableSolver;
765 std::set<int> auxDims;
766 for (VertexIDMap::const_iterator it =
vertices().begin(); it !=
vertices().end(); ++it) {
776 if (actions.size() > 0) {
778 for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) {
779 (*(*it))(
this, ¶ms);
787 if (actions.size() > 0) {
789 for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) {
790 (*(*it))(
this, ¶ms);
798 return insertResult.second;
804 return insertResult.second;
820 string tag = factory->
tag(v);
821 if (tag.size() > 0) {
822 os << tag <<
" " << v->
id() <<
" ";
827 tag = factory->
tag(d);
828 if (tag.size() > 0) {
836 os <<
"FIX " << v->
id() << endl;
846 string tag = factory->
tag(e);
847 if (tag.size() > 0) {
850 os << e->
id() <<
" ";
851 for (vector<HyperGraph::Vertex*>::const_iterator it = e->
vertices().begin(); it != e->
vertices().end(); ++it) {
853 os << v->
id() <<
" ";
869 bool allEdgeOk =
true;
870 SelfAdjointEigenSolver<MatrixXd> eigenSolver;
871 for (OptimizableGraph::EdgeSet::const_iterator it =
edges().begin(); it !=
edges().end(); ++it) {
875 bool isSymmetric = information.transpose() == information;
876 bool okay = isSymmetric;
879 eigenSolver.compute(information, Eigen::EigenvaluesOnly);
880 bool isSPD = eigenSolver.eigenvalues()(0) >= 0.;
881 okay = okay && isSPD;
883 allEdgeOk = allEdgeOk && okay;
887 cerr <<
"Information Matrix for an edge is not symmetric:";
889 cerr <<
"Information Matrix for an edge is not SPD:";
890 for (
size_t i = 0; i < e->
vertices().size(); ++i)
893 cerr <<
"\teigenvalues: " << eigenSolver.eigenvalues().transpose();
903 # if (defined G2O_OPENMP) && EIGEN_VERSION_AT_LEAST(3,1,0) 904 Eigen::initParallel();
virtual bool save(std::ostream &os, int level=0) const
save the graph to a stream. Again uses the Factory system.
virtual Vertex * createFrom()
virtual bool write(std::ostream &os) const =0
write the vertex to a stream
bool isSolverSuitable(const OptimizationAlgorithmProperty &solverProperty, const std::set< int > &vertDims=std::set< int >()) const
#define __PRETTY_FUNCTION__
void setHessianIndex(int ti)
set the temporary index of the vertex in the parameter blocks
static Factory * instance()
return the instance
int readLine(std::istream &is, std::stringstream ¤tLine)
virtual int optimize(int iterations, bool online=false)
virtual bool getMinimalEstimateData(double *estimate) const
int dimension() const
returns the dimensions of the error function
std::vector< std::string > _parameterTypes
const Vertex * vertex(size_t i) const
const EdgeSet & edges() const
bool fixed() const
true => this node is fixed during the optimization
describe the properties of a solver
const VertexIDMap & vertices() const
virtual void discardTop()=0
pop the last element from the stack, without restoring the current estimate
bool knowsTag(const std::string &tag, int *elementType=0) const
CacheContainer * _cacheContainer
bool removePreIterationAction(HyperGraphAction *action)
remove an action that should no longer be execured before each iteration
std::bitset< HyperGraph::HGET_NUM_ELEMS > GraphElemBitset
void setNext(Data *next_)
virtual void clearParameters()
std::set< Vertex * > VertexSet
double chi2() const
returns the chi2 of the current configuration
const Data * userData() const
the user data associated with this vertex
virtual double chi2() const =0
computes the chi2 based on the cached error value, only valid after computeError has been called...
CacheContainer * cacheContainer()
bool removePostIterationAction(HyperGraphAction *action)
remove an action that should no longer be execured after each iteration
virtual bool read(std::istream &is)=0
read the vertex from a stream, i.e., the internal state of the vertex
virtual void updateCache()
virtual void pop()
pop (restore) the estimate of all variables from the stack
bool saveVertex(std::ostream &os, Vertex *v) const
virtual void push()
push the estimate of all variables onto a stack
int landmarkDim
dimension of the landmar vertices (-1 if variable)
virtual bool read(std::istream &is)=0
read the data from a stream
int level() const
returns the level of the edge
bool addPostIterationAction(HyperGraphAction *action)
add an action to be executed after each iteration
static bool initMultiThreading()
void setRobustKernel(RobustKernel *ptr)
std::vector< std::string > strSplit(const std::string &str, const std::string &delimiters)
virtual bool setMinimalEstimateDataImpl(const double *)
virtual void postIteration(int)
called at the end of an iteration (argument is the number of the iteration)
std::string trim(const std::string &s)
void setVertex(size_t i, Vertex *v)
virtual bool addEdge(Edge *e)
base for all robust cost functions
int dimension() const
dimension of the estimated state belonging to this node
virtual const double * informationData() const =0
returns the memory of the information matrix, usable for example with a Eigen::Map<MatrixXd> ...
void setUpdateNeeded(bool needUpdate=true)
std::vector< Parameter ** > _parameters
virtual Vertex * createTo()
virtual void setId(int id)
sets the id of the node in the graph be sure that the graph keeps consistent after changing the id ...
virtual bool setEstimateDataImpl(const double *)
const VertexContainer & vertices() const
std::vector< OptimizableGraph::Edge * > EdgeContainer
vector container for edges
std::set< Edge * > EdgeSet
virtual ~OptimizableGraph()
bool setParameterId(int argNum, int paramId)
const OptimizableGraph * graph() const
virtual bool resolveCaches()
virtual void initialEstimate(const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to)=0
bool saveSubset(std::ostream &os, HyperGraph::VertexSet &vset, int level=0)
save a subgraph to a stream. Again uses the Factory system.
bool setMinimalEstimateData(const double *estimate)
void updateSize(const HyperGraph::Edge *e)
virtual bool getEstimateData(double *estimate) const
virtual bool setMeasurementData(const double *m)
std::vector< int > _parameterIds
RobustKernel * _robustKernel
create vertices and edges based on TAGs in, for example, a file
virtual void push()=0
backup the position of the vertex to a stack
virtual void clear()
clears the graph and empties all structures.
virtual void pop()=0
restore the position of the vertex by retrieving the position from the stack
OptimizableGraph * graph()
bool verifyInformationMatrices(bool verbose=false) const
std::map< std::string, std::string > _renamedTypesLookup
const EdgeSet & edges() const
returns the set of hyper-edges that are leaving/entering in this vertex
void setUserData(Data *obs)
order edges based on the internal ID, which is assigned to the edge in addEdge()
A general case Vertex for optimization.
abstract Vertex, your types must derive from that one
const std::string & tag(const HyperGraph::HyperGraphElement *v) const
return the TAG given a vertex
OptimizableGraph()
empty constructor
void setRenamedTypesFromString(const std::string &types)
data packet for a vertex. Extend this class to store in the vertices the potential additional informa...
virtual int minimalEstimateDimension() const
void addGraph(OptimizableGraph *g)
adds all edges and vertices of the graph g to this graph.
virtual bool addVertex(Vertex *v)
virtual void resize(size_t size)
virtual bool load(std::istream &is, bool createEdges=true)
load the graph from a stream. Uses the Factory singleton for creating the vertices and edges...
bool setEstimateData(const double *estimate)
Parameter * parameter(int id)
JacobianWorkspace _jacobianWorkspace
int maxDimension() const
return the maximum dimension of all vertices in the graph
virtual void setFixed(HyperGraph::VertexSet &vset, bool fixed)
fixes/releases a set of vertices
HyperGraph::HyperGraphElement * construct(const std::string &tag) const
bool saveEdge(std::ostream &os, Edge *e) const
virtual void preIteration(int)
called at the beginning of an iteration (argument is the number of the iteration) ...
virtual int measurementDimension() const
std::set< HyperGraphAction * > HyperGraphActionSet
virtual bool write(std::ostream &os) const =0
write the vertex to a stream
bool requiresMarginalize
whether the solver requires marginalization of landmarks
virtual void discardTop()
discard the last backup of the estimate for all variables by removing it from the stack ...
int id() const
returns the id
void setFixed(bool fixed)
true => this node should be considered fixed during the optimization
virtual bool setMeasurementFromState()
virtual Vertex * clone() const
returns a deep copy of the current vertex
std::set< int > dimensions() const
virtual bool write(std::ostream &os) const =0
write the data to a stream
virtual bool read(std::istream &is)=0
read the vertex from a stream, i.e., the internal state of the vertex
std::vector< HyperGraphActionSet > _graphActions
virtual bool getMeasurementData(double *m) const
const Data * next() const
virtual bool addEdge(HyperGraph::Edge *e)
virtual bool addVertex(HyperGraph::Vertex *v, Data *userData)
virtual Edge * clone() const
virtual int estimateDimension() const
int poseDim
dimension of the pose vertices (-1 if variable)
bool addPreIterationAction(HyperGraphAction *action)
add an action to be executed before each iteration
Abstract action that operates on an entire graph.
OptimizableGraph * _graph
VertexContainer _vertices