diff --git a/g2o/core/optimizable_graph.h b/g2o/core/optimizable_graph.h index 100e84aab..de4b9034a 100644 --- a/g2o/core/optimizable_graph.h +++ b/g2o/core/optimizable_graph.h @@ -75,701 +75,699 @@ struct G2O_CORE_API OptimizableGraph : public HyperGraph { /** * \brief order vertices based on their ID */ - struct G2O_CORE_API VertexIDCompare { - bool operator()(const Vertex* v1, const Vertex* v2) const { - return v1->id() < v2->id(); - } + struct G2O_CORE_API VertexIDCompare{bool operator()( + const Vertex* v1, const Vertex* v2) const {return v1->id() < v2->id(); +} +}; // namespace g2o + +/** + * \brief order edges based on the internal ID, which is assigned to the edge + * in addEdge() + */ +struct G2O_CORE_API EdgeIDCompare{ + bool operator()(const Edge* e1, const Edge* e2) + const {return e1->internalId() < e2->internalId(); +} +bool operator()(const HyperGraph::Edge* e1, const HyperGraph::Edge* e2) const { + return operator()(static_cast(e1), static_cast(e2)); +} +} +; + +//! vector container for vertices +typedef std::vector VertexContainer; +//! vector container for edges +typedef std::vector EdgeContainer; + +/** + * \brief A general case Vertex for optimization + */ +class G2O_CORE_API Vertex : public HyperGraph::Vertex, + public HyperGraph::DataContainer { + private: + friend struct OptimizableGraph; + + public: + Vertex(); + virtual ~Vertex(); + + //! sets the node to the origin (used in the multilevel stuff) + void setToOrigin() { + setToOriginImpl(); + updateCache(); + } + + //! get the element from the hessian matrix + virtual const double& hessian(int i, int j) const = 0; + virtual double& hessian(int i, int j) = 0; + virtual double hessianDeterminant() const = 0; + virtual double* hessianData() = 0; + + /** maps the internal matrix to some external memory location */ + virtual void mapHessianMemory(double* d) = 0; + + /** + * copies the b vector in the array b_ + * @return the number of elements copied + */ + virtual int copyB(double* b_) const = 0; + + //! get the b vector element + virtual const double& b(int i) const = 0; + virtual double& b(int i) = 0; + //! return a pointer to the b vector associated with this vertex + virtual double* bData() = 0; + + /** + * set the b vector part of this vertex to zero + */ + virtual void clearQuadraticForm() = 0; + + /** + * updates the current vertex with the direct solution x += H_ii\b_ii + * @return the determinant of the inverted hessian + */ + virtual double solveDirect(double lambda = 0) = 0; + + /** + * sets the initial estimate from an array of double + * Implement setEstimateDataImpl() + * @return true on success + */ + bool setEstimateData(const double* estimate); + + /** + * sets the initial estimate from an array of double + * Implement setEstimateDataImpl() + * @return true on success + */ + bool setEstimateData(const std::vector& estimate) { + int dim = estimateDimension(); + if ((dim == -1) || (estimate.size() != std::size_t(dim))) return false; + return setEstimateData(&estimate[0]); }; /** - * \brief order edges based on the internal ID, which is assigned to the edge - * in addEdge() + * sets the initial estimate from an array of double + * Implement setEstimateDataImpl() + * @return true on success */ - struct G2O_CORE_API EdgeIDCompare { - bool operator()(const Edge* e1, const Edge* e2) const { - return e1->internalId() < e2->internalId(); - } - bool operator()(const HyperGraph::Edge* e1, - const HyperGraph::Edge* e2) const { - return operator()(static_cast(e1), - static_cast(e2)); - } + template + bool setEstimateData(const Eigen::MatrixBase& estimate) { + int dim = estimateDimension(); + if ((dim == -1) || (estimate.size() != dim)) return false; + return setEstimateData(estimate.derived().data()); }; - //! vector container for vertices - typedef std::vector VertexContainer; - //! vector container for edges - typedef std::vector EdgeContainer; + /** + * writes the estimater to an array of double + * @returns true on success + */ + virtual bool getEstimateData(double* estimate) const; /** - * \brief A general case Vertex for optimization + * writes the estimater to an array of double + * @returns true on success */ - class G2O_CORE_API Vertex : public HyperGraph::Vertex, - public HyperGraph::DataContainer { - private: - friend struct OptimizableGraph; - - public: - Vertex(); - virtual ~Vertex(); - - //! sets the node to the origin (used in the multilevel stuff) - void setToOrigin() { - setToOriginImpl(); - updateCache(); - } + virtual bool getEstimateData(std::vector& estimate) const { + int dim = estimateDimension(); + if (dim < 0) return false; + estimate.resize(dim); + return getEstimateData(&estimate[0]); + }; - //! get the element from the hessian matrix - virtual const double& hessian(int i, int j) const = 0; - virtual double& hessian(int i, int j) = 0; - virtual double hessianDeterminant() const = 0; - virtual double* hessianData() = 0; - - /** maps the internal matrix to some external memory location */ - virtual void mapHessianMemory(double* d) = 0; - - /** - * copies the b vector in the array b_ - * @return the number of elements copied - */ - virtual int copyB(double* b_) const = 0; - - //! get the b vector element - virtual const double& b(int i) const = 0; - virtual double& b(int i) = 0; - //! return a pointer to the b vector associated with this vertex - virtual double* bData() = 0; - - /** - * set the b vector part of this vertex to zero - */ - virtual void clearQuadraticForm() = 0; - - /** - * updates the current vertex with the direct solution x += H_ii\b_ii - * @return the determinant of the inverted hessian - */ - virtual double solveDirect(double lambda = 0) = 0; - - /** - * sets the initial estimate from an array of double - * Implement setEstimateDataImpl() - * @return true on success - */ - bool setEstimateData(const double* estimate); - - /** - * sets the initial estimate from an array of double - * Implement setEstimateDataImpl() - * @return true on success - */ - bool setEstimateData(const std::vector& estimate) { - int dim = estimateDimension(); - if ((dim == -1) || (estimate.size() != std::size_t(dim))) return false; - return setEstimateData(&estimate[0]); - }; - - /** - * sets the initial estimate from an array of double - * Implement setEstimateDataImpl() - * @return true on success - */ - template - bool setEstimateData(const Eigen::MatrixBase& estimate) { - int dim = estimateDimension(); - if ((dim == -1) || (estimate.size() != dim)) return false; - return setEstimateData(estimate.derived().data()); - }; - - /** - * writes the estimater to an array of double - * @returns true on success - */ - virtual bool getEstimateData(double* estimate) const; - - /** - * writes the estimater to an array of double - * @returns true on success - */ - virtual bool getEstimateData(std::vector& estimate) const { - int dim = estimateDimension(); - if (dim < 0) return false; - estimate.resize(dim); - return getEstimateData(&estimate[0]); - }; - - /** - * writes the estimater to an array of double - * @returns true on success - */ - template - bool getEstimateData(Eigen::MatrixBase& estimate) const { - int dim = estimateDimension(); - // If dim is -ve, getEstimateData is not implemented and fails - if (dim < 0) return false; - - // If the vector isn't the right size to store the estimate, try to resize - // it. This only works if the vector is dynamic. If it is static, fail. - if (estimate.size() != dim) { - if ((estimate.RowsAtCompileTime == Eigen::Dynamic) || - (estimate.ColsAtCompileTime == Eigen::Dynamic)) - estimate.derived().resize(dim); - else - return false; - } - return getEstimateData(estimate.derived().data()); - }; - - /** - * returns the dimension of the extended representation used by - * get/setEstimate(double*) -1 if it is not supported - */ - virtual int estimateDimension() const; - - /** - * sets the initial estimate from an array of double. - * Implement setMinimalEstimateDataImpl() - * @return true on success - */ - bool setMinimalEstimateData(const double* estimate); - - /** - * sets the initial estimate from an array of double. - * Implement setMinimalEstimateDataImpl() - * @return true on success - */ - bool setMinimalEstimateData(const std::vector& estimate) { - int dim = minimalEstimateDimension(); - if ((dim == -1) || (estimate.size() != std::size_t(dim))) return false; - return setMinimalEstimateData(&estimate[0]); - }; - - /** - * sets the initial estimate from an eigen type of double. - * Implement setMinimalEstimateDataImpl() - * @return true on success - */ - template - bool setMinimalEstimateData(const Eigen::MatrixBase& estimate) { - int dim = minimalEstimateDimension(); - if ((dim == -1) || (estimate.size() != dim)) return false; - return setMinimalEstimateData(estimate.derived().data()); - }; - - /** - * writes the estimate to an array of double - * @returns true on success - */ - virtual bool getMinimalEstimateData(double* estimate) const; - - /** - * writes the estimate to an array of double - * @returns true on success - */ - virtual bool getMinimalEstimateData(std::vector& estimate) const { - int dim = minimalEstimateDimension(); - if (dim < 0) return false; - estimate.resize(dim); - return getMinimalEstimateData(&estimate[0]); - }; - - /** - * writes the estimate to an eigen type double - * @returns true on success - */ - template - bool getMinimalEstimateData(Eigen::MatrixBase& estimate) const { - int dim = minimalEstimateDimension(); - // If dim is -ve, getMinimalEstimateData is not implemented and fails - if (dim < 0) return false; - - // If the vector isn't the right size to store the estimate, try to resize - // it. This only works if the vector is dynamic. If it is static, fail. - if (estimate.size() != dim) { - if ((estimate.RowsAtCompileTime == Eigen::Dynamic) || - (estimate.ColsAtCompileTime == Eigen::Dynamic)) - estimate.derived().resize(dim); - else - return false; - } - return getMinimalEstimateData(estimate.derived().data()); - }; - - /** - * returns the dimension of the extended representation used by - * get/setEstimate(double*) -1 if it is not supported - */ - virtual int minimalEstimateDimension() const; - - //! backup the position of the vertex to a stack - virtual void push() = 0; - - //! restore the position of the vertex by retrieving the position from the - //! stack - virtual void pop() = 0; - - //! pop the last element from the stack, without restoring the current - //! estimate - virtual void discardTop() = 0; - - //! return the stack size - virtual int stackSize() const = 0; - - /** - * Update the position of the node from the parameters in v. - * Depends on the implementation of oplusImpl in derived classes to actually - * carry out the update. Will also call updateCache() to update the caches - * of depending on the vertex. - */ - void oplus(const double* v) { - oplusImpl(v); - updateCache(); + /** + * writes the estimater to an array of double + * @returns true on success + */ + template + bool getEstimateData(Eigen::MatrixBase& estimate) const { + int dim = estimateDimension(); + // If dim is -ve, getEstimateData is not implemented and fails + if (dim < 0) return false; + + // If the vector isn't the right size to store the estimate, try to resize + // it. This only works if the vector is dynamic. If it is static, fail. + if (estimate.size() != dim) { + if ((estimate.RowsAtCompileTime == Eigen::Dynamic) || + (estimate.ColsAtCompileTime == Eigen::Dynamic)) + estimate.derived().resize(dim); + else + return false; } + return getEstimateData(estimate.derived().data()); + }; - //! temporary index of this node in the parameter vector obtained from - //! linearization - int hessianIndex() const { return _hessianIndex; } - int G2O_ATTRIBUTE_DEPRECATED(tempIndex() const) { return hessianIndex(); } - //! set the temporary index of the vertex in the parameter blocks - void setHessianIndex(int ti) { _hessianIndex = ti; } - void G2O_ATTRIBUTE_DEPRECATED(setTempIndex(int ti)) { setHessianIndex(ti); } - - //! true => this node is fixed during the optimization - bool fixed() const { return _fixed; } - //! true => this node should be considered fixed during the optimization - void setFixed(bool fixed) { _fixed = fixed; } - - //! true => this node is marginalized out during the optimization - bool marginalized() const { return _marginalized; } - //! true => this node should be marginalized out during the optimization - void setMarginalized(bool marginalized) { _marginalized = marginalized; } - - //! dimension of the estimated state belonging to this node - int dimension() const { return _dimension; } - - //! sets the id of the node in the graph be sure that the graph keeps - //! consistent after changing the id - virtual void setId(int id) { _id = id; } - - //! set the row of this vertex in the Hessian - void setColInHessian(int c) { _colInHessian = c; } - //! get the row of this vertex in the Hessian - int colInHessian() const { return _colInHessian; } - - const OptimizableGraph* graph() const { return _graph; } - OptimizableGraph* graph() { return _graph; } - - /** - * lock for the block of the hessian and the b vector associated with this - * vertex, to avoid race-conditions if multi-threaded. - */ - void lockQuadraticForm() { _quadraticFormMutex.lock(); } - /** - * unlock the block of the hessian and the b vector associated with this - * vertex - */ - void unlockQuadraticForm() { _quadraticFormMutex.unlock(); } - - //! read the vertex from a stream, i.e., the internal state of the vertex - virtual bool read(std::istream& is) = 0; - //! write the vertex to a stream - virtual bool write(std::ostream& os) const = 0; - - virtual void updateCache(); - - CacheContainer* cacheContainer(); - - protected: - OptimizableGraph* _graph; - Data* _userData; - int _hessianIndex; - bool _fixed; - bool _marginalized; - int _dimension; - int _colInHessian; - OpenMPMutex _quadraticFormMutex; - - CacheContainer* _cacheContainer; - - /** - * update the position of the node from the parameters in v. - * Implement in your class! - */ - virtual void oplusImpl(const double* v) = 0; - - //! sets the node to the origin (used in the multilevel stuff) - virtual void setToOriginImpl() = 0; - - /** - * writes the estimater to an array of double - * @returns true on success - */ - virtual bool setEstimateDataImpl(const double*) { return false; } - - /** - * sets the initial estimate from an array of double - * @return true on success - */ - virtual bool setMinimalEstimateDataImpl(const double*) { return false; } + /** + * returns the dimension of the extended representation used by + * get/setEstimate(double*) -1 if it is not supported + */ + virtual int estimateDimension() const; + + /** + * sets the initial estimate from an array of double. + * Implement setMinimalEstimateDataImpl() + * @return true on success + */ + bool setMinimalEstimateData(const double* estimate); + + /** + * sets the initial estimate from an array of double. + * Implement setMinimalEstimateDataImpl() + * @return true on success + */ + bool setMinimalEstimateData(const std::vector& estimate) { + int dim = minimalEstimateDimension(); + if ((dim == -1) || (estimate.size() != std::size_t(dim))) return false; + return setMinimalEstimateData(&estimate[0]); }; - class G2O_CORE_API Edge : public HyperGraph::Edge, - public HyperGraph::DataContainer { - private: - friend struct OptimizableGraph; - - public: - Edge(); - virtual ~Edge(); - - // indicates if all vertices are fixed - virtual bool allVerticesFixed() const = 0; - - // computes the error of the edge and stores it in an internal structure - virtual void computeError() = 0; - - //! sets the measurement from an array of double - //! @returns true on success - virtual bool setMeasurementData(const double* m); - - //! writes the measurement to an array of double - //! @returns true on success - virtual bool getMeasurementData(double* m) const; - - //! returns the dimension of the measurement in the extended representation - //! which is used by get/setMeasurement; - virtual int measurementDimension() const; - - /** - * sets the estimate to have a zero error, based on the current value of the - * state variables returns false if not supported. - */ - virtual bool setMeasurementFromState(); - - //! if NOT NULL, error of this edge will be robustifed with the kernel - RobustKernel* robustKernel() const { return _robustKernel; } - /** - * specify the robust kernel to be used in this edge - */ - void setRobustKernel(RobustKernel* ptr); - - //! returns the error vector cached after calling the computeError; - virtual const double* errorData() const = 0; - virtual double* errorData() = 0; - - //! returns the memory of the information matrix, usable for example with a - //! Eigen::Map - virtual const double* informationData() const = 0; - virtual double* informationData() = 0; - - //! computes the chi2 based on the cached error value, only valid after - //! computeError has been called. - virtual double chi2() const = 0; - - /** - * Linearizes the constraint in the edge. - * Makes side effect on the vertices of the graph by changing - * the parameter vector b and the hessian blocks ii and jj. - * The off diagoinal block is accessed via _hessian. - */ - virtual void constructQuadraticForm() = 0; - - /** - * maps the internal matrix to some external memory location, - * you need to provide the memory before calling constructQuadraticForm - * @param d the memory location to which we map - * @param i index of the vertex i - * @param j index of the vertex j (j > i, upper triangular fashion) - * @param rowMajor if true, will write in rowMajor order to the block. Since - * EIGEN is columnMajor by default, this results in writing the transposed - */ - virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor) = 0; - - /** - * Linearizes the constraint in the edge in the manifold space, and store - * the result in the given workspace - */ - virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace) = 0; - - /** set the estimate of the to vertex, based on the estimate of the from - * vertices in the edge. */ - virtual void initialEstimate(const OptimizableGraph::VertexSet& from, - OptimizableGraph::Vertex* to) = 0; - - /** - * override in your class if it's possible to initialize the vertices in - * certain combinations. The return value may correspond to the cost for - * initiliaizng the vertex but should be positive if the initialization is - * possible and negative if not possible. - */ - virtual double initialEstimatePossible( - const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { - (void)from; - (void)to; - return -1.; + /** + * sets the initial estimate from an eigen type of double. + * Implement setMinimalEstimateDataImpl() + * @return true on success + */ + template + bool setMinimalEstimateData(const Eigen::MatrixBase& estimate) { + int dim = minimalEstimateDimension(); + if ((dim == -1) || (estimate.size() != dim)) return false; + return setMinimalEstimateData(estimate.derived().data()); + }; + + /** + * writes the estimate to an array of double + * @returns true on success + */ + virtual bool getMinimalEstimateData(double* estimate) const; + + /** + * writes the estimate to an array of double + * @returns true on success + */ + virtual bool getMinimalEstimateData(std::vector& estimate) const { + int dim = minimalEstimateDimension(); + if (dim < 0) return false; + estimate.resize(dim); + return getMinimalEstimateData(&estimate[0]); + }; + + /** + * writes the estimate to an eigen type double + * @returns true on success + */ + template + bool getMinimalEstimateData(Eigen::MatrixBase& estimate) const { + int dim = minimalEstimateDimension(); + // If dim is -ve, getMinimalEstimateData is not implemented and fails + if (dim < 0) return false; + + // If the vector isn't the right size to store the estimate, try to resize + // it. This only works if the vector is dynamic. If it is static, fail. + if (estimate.size() != dim) { + if ((estimate.RowsAtCompileTime == Eigen::Dynamic) || + (estimate.ColsAtCompileTime == Eigen::Dynamic)) + estimate.derived().resize(dim); + else + return false; } + return getMinimalEstimateData(estimate.derived().data()); + }; - //! returns the level of the edge - int level() const { return _level; } - //! sets the level of the edge - void setLevel(int l) { _level = l; } + /** + * returns the dimension of the extended representation used by + * get/setEstimate(double*) -1 if it is not supported + */ + virtual int minimalEstimateDimension() const; + + //! backup the position of the vertex to a stack + virtual void push() = 0; - //! returns the dimensions of the error function - int dimension() const { return _dimension; } + //! restore the position of the vertex by retrieving the position from the + //! stack + virtual void pop() = 0; - G2O_ATTRIBUTE_DEPRECATED(virtual Vertex* createFrom()) { return nullptr; } - G2O_ATTRIBUTE_DEPRECATED(virtual Vertex* createTo()) { return nullptr; } - virtual Vertex* createVertex(int) { return nullptr; } + //! pop the last element from the stack, without restoring the current + //! estimate + virtual void discardTop() = 0; - //! read the vertex from a stream, i.e., the internal state of the vertex - virtual bool read(std::istream& is) = 0; - //! write the vertex to a stream - virtual bool write(std::ostream& os) const = 0; + //! return the stack size + virtual int stackSize() const = 0; - //! the internal ID of the edge - long long internalId() const { return _internalId; } + /** + * Update the position of the node from the parameters in v. + * Depends on the implementation of oplusImpl in derived classes to actually + * carry out the update. Will also call updateCache() to update the caches + * of depending on the vertex. + */ + void oplus(const double* v) { + oplusImpl(v); + updateCache(); + } - OptimizableGraph* graph(); - const OptimizableGraph* graph() const; + //! temporary index of this node in the parameter vector obtained from + //! linearization + int hessianIndex() const { return _hessianIndex; } + int G2O_ATTRIBUTE_DEPRECATED(tempIndex() const) { return hessianIndex(); } + //! set the temporary index of the vertex in the parameter blocks + void setHessianIndex(int ti) { _hessianIndex = ti; } + void G2O_ATTRIBUTE_DEPRECATED(setTempIndex(int ti)) { setHessianIndex(ti); } - bool setParameterId(int argNum, int paramId); - inline const Parameter* parameter(int argNo) const { - return *_parameters.at(argNo); - } - inline size_t numParameters() const { return _parameters.size(); } - inline void resizeParameters(size_t newSize) { - _parameters.resize(newSize, 0); - _parameterIds.resize(newSize, -1); - _parameterTypes.resize(newSize, typeid(void*).name()); - } + //! true => this node is fixed during the optimization + bool fixed() const { return _fixed; } + //! true => this node should be considered fixed during the optimization + void setFixed(bool fixed) { _fixed = fixed; } - protected: - int _dimension; - int _level; - RobustKernel* _robustKernel; - long long _internalId; - std::vector _cacheIds; - - template - bool installParameter(ParameterType*& p, size_t argNo, int paramId = -1) { - if (argNo >= _parameters.size()) return false; - _parameterIds[argNo] = paramId; - _parameters[argNo] = (Parameter**)&p; - _parameterTypes[argNo] = typeid(ParameterType).name(); - return true; - } + //! true => this node is marginalized out during the optimization + bool marginalized() const { return _marginalized; } + //! true => this node should be marginalized out during the optimization + void setMarginalized(bool marginalized) { _marginalized = marginalized; } - template - void resolveCache(CacheType*& cache, OptimizableGraph::Vertex*, - const std::string& _type, - const ParameterVector& parameters); + //! dimension of the estimated state belonging to this node + int dimension() const { return _dimension; } - bool resolveParameters(); - virtual bool resolveCaches(); + //! sets the id of the node in the graph be sure that the graph keeps + //! consistent after changing the id + virtual void setId(int id) { _id = id; } - std::vector _parameterTypes; - std::vector _parameters; - std::vector _parameterIds; - }; + //! set the row of this vertex in the Hessian + void setColInHessian(int c) { _colInHessian = c; } + //! get the row of this vertex in the Hessian + int colInHessian() const { return _colInHessian; } - //! returns the vertex number id appropriately casted - inline Vertex* vertex(int id) { - return reinterpret_cast(HyperGraph::vertex(id)); - } + const OptimizableGraph* graph() const { return _graph; } + OptimizableGraph* graph() { return _graph; } - //! returns the vertex number id appropriately casted - inline const Vertex* vertex(int id) const { - return reinterpret_cast(HyperGraph::vertex(id)); - } + /** + * lock for the block of the hessian and the b vector associated with this + * vertex, to avoid race-conditions if multi-threaded. + */ + void lockQuadraticForm() { _quadraticFormMutex.lock(); } + /** + * unlock the block of the hessian and the b vector associated with this + * vertex + */ + void unlockQuadraticForm() { _quadraticFormMutex.unlock(); } + + //! read the vertex from a stream, i.e., the internal state of the vertex + virtual bool read(std::istream& is) = 0; + //! write the vertex to a stream + virtual bool write(std::ostream& os) const = 0; + + virtual void updateCache(); + + CacheContainer* cacheContainer(); - //! empty constructor - OptimizableGraph(); - virtual ~OptimizableGraph(); + protected: + OptimizableGraph* _graph; + Data* _userData; + int _hessianIndex; + bool _fixed; + bool _marginalized; + int _dimension; + int _colInHessian; + OpenMPMutex _quadraticFormMutex; + + CacheContainer* _cacheContainer; /** - * adds a new vertex. The new vertex is then "taken". - * @return false if a vertex with the same id as v is already in the graph, - * true otherwise. + * update the position of the node from the parameters in v. + * Implement in your class! */ - virtual bool addVertex(HyperGraph::Vertex* v, Data* userData); - virtual bool addVertex(HyperGraph::Vertex* v) { return addVertex(v, 0); } - bool addVertex(OptimizableGraph::Vertex* v, Data* userData); - bool addVertex(OptimizableGraph::Vertex* v) { return addVertex(v, 0); } + virtual void oplusImpl(const double* v) = 0; + + //! sets the node to the origin (used in the multilevel stuff) + virtual void setToOriginImpl() = 0; /** - * adds a new edge. - * The edge should point to the vertices that it is connecting - * (setFrom/setTo). - * @return false if the insertion does not work (incompatible types of the - * vertices/missing vertex). true otherwise. + * writes the estimater to an array of double + * @returns true on success */ - virtual bool addEdge(HyperGraph::Edge* e); - bool addEdge(OptimizableGraph::Edge* e); + virtual bool setEstimateDataImpl(const double*) { return false; } /** - * overridden from HyperGraph, to maintain the bookkeeping of the - * caches/parameters and jacobian workspaces consistent upon a change in the - * vertex. - * @return false if something goes wrong. + * sets the initial estimate from an array of double + * @return true on success */ - virtual bool setEdgeVertex(HyperGraph::Edge* e, int pos, - HyperGraph::Vertex* v); + virtual bool setMinimalEstimateDataImpl(const double*) { return false; } +}; - //! returns the chi2 of the current configuration - double chi2() const; +class G2O_CORE_API Edge : public HyperGraph::Edge, + public HyperGraph::DataContainer { + private: + friend struct OptimizableGraph; - //! return the maximum dimension of all vertices in the graph - int maxDimension() const; + public: + Edge(); + virtual ~Edge(); - //! Recompute the size of the Jacobian workspace from all the - //! edges in the graph. - void recomputeJacobianWorkspaceSize() { - _jacobianWorkspace.updateSize(*this, true); - } + // indicates if all vertices are fixed + virtual bool allVerticesFixed() const = 0; + + // computes the error of the edge and stores it in an internal structure + virtual void computeError() = 0; + + //! sets the measurement from an array of double + //! @returns true on success + virtual bool setMeasurementData(const double* m); + + //! writes the measurement to an array of double + //! @returns true on success + virtual bool getMeasurementData(double* m) const; + + //! returns the dimension of the measurement in the extended representation + //! which is used by get/setMeasurement; + virtual int measurementDimension() const; /** - * iterates over all vertices and returns a set of all the vertex dimensions - * in the graph + * sets the estimate to have a zero error, based on the current value of the + * state variables returns false if not supported. */ - std::set dimensions() const; + virtual bool setMeasurementFromState(); + //! if NOT NULL, error of this edge will be robustifed with the kernel + RobustKernel* robustKernel() const { return _robustKernel; } /** - * carry out n iterations - * @return the number of performed iterations + * specify the robust kernel to be used in this edge */ - virtual int optimize(int iterations, bool online = false); - - //! called at the beginning of an iteration (argument is the number of the - //! iteration) - virtual void preIteration(int); - //! called at the end of an iteration (argument is the number of the - //! iteration) - virtual void postIteration(int); - - //! add an action to be executed before each iteration - bool addPreIterationAction(HyperGraphAction* action); - //! add an action to be executed after each iteration - bool addPostIterationAction(HyperGraphAction* action); - - //! remove an action that should no longer be executed before each iteration - bool removePreIterationAction(HyperGraphAction* action); - //! remove an action that should no longer be executed after each iteration - bool removePostIterationAction(HyperGraphAction* action); - - //! push the estimate of all variables onto a stack - virtual void push(); - //! pop (restore) the estimate of all variables from the stack - virtual void pop(); - //! discard the last backup of the estimate for all variables by removing it - //! from the stack - virtual void discardTop(); - - //! load the graph from a stream. Uses the Factory singleton for creating the - //! vertices and edges. - virtual bool load(std::istream& is); - bool load(const char* filename); - //! save the graph to a stream. Again uses the Factory system. - virtual bool save(std::ostream& os, int level = 0) const; - //! function provided for convenience, see save() above - bool save(const char* filename, int level = 0) const; - - //! save a subgraph to a stream. Again uses the Factory system. - bool saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, int level = 0); - - //! save a subgraph to a stream. Again uses the Factory system. - bool saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset); - - //! push the estimate of a subset of the variables onto a stack - virtual void push(HyperGraph::VertexSet& vset); - //! pop (restore) the estimate a subset of the variables from the stack - virtual void pop(HyperGraph::VertexSet& vset); - //! ignore the latest stored element on the stack, remove it from the stack - //! but do not restore the estimate - virtual void discardTop(HyperGraph::VertexSet& vset); - - //! fixes/releases a set of vertices - virtual void setFixed(HyperGraph::VertexSet& vset, bool fixed); + void setRobustKernel(RobustKernel* ptr); + + //! returns the error vector cached after calling the computeError; + virtual const double* errorData() const = 0; + virtual double* errorData() = 0; + + //! returns the memory of the information matrix, usable for example with a + //! Eigen::Map + virtual const double* informationData() const = 0; + virtual double* informationData() = 0; + + //! computes the chi2 based on the cached error value, only valid after + //! computeError has been called. + virtual double chi2() const = 0; /** - * set the renamed types lookup from a string, format is for example: - * VERTEX_CAM=VERTEX_SE3:EXPMAP,EDGE_PROJECT_P2MC=EDGE_PROJECT_XYZ:EXPMAP - * This will change the occurrence of VERTEX_CAM in the file to - * VERTEX_SE3:EXPMAP + * Linearizes the constraint in the edge. + * Makes side effect on the vertices of the graph by changing + * the parameter vector b and the hessian blocks ii and jj. + * The off diagoinal block is accessed via _hessian. */ - void setRenamedTypesFromString(const std::string& types); + virtual void constructQuadraticForm() = 0; /** - * test whether a solver is suitable for optimizing this graph. - * @param solverProperty the solver property to evaluate. - * @param vertDims should equal to the set returned by dimensions() to avoid - * re-evaluating. + * maps the internal matrix to some external memory location, + * you need to provide the memory before calling constructQuadraticForm + * @param d the memory location to which we map + * @param i index of the vertex i + * @param j index of the vertex j (j > i, upper triangular fashion) + * @param rowMajor if true, will write in rowMajor order to the block. Since + * EIGEN is columnMajor by default, this results in writing the transposed */ - bool isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, - const std::set& vertDims = std::set()) const; + virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor) = 0; /** - * remove the parameters of the graph + * Linearizes the constraint in the edge in the manifold space, and store + * the result in the given workspace */ - virtual void clearParameters(); + virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace) = 0; - bool addParameter(Parameter* p) { return _parameters.addParameter(p); } - - Parameter* parameter(int id) { return _parameters.getParameter(id); } + /** set the estimate of the to vertex, based on the estimate of the from + * vertices in the edge. */ + virtual void initialEstimate(const OptimizableGraph::VertexSet& from, + OptimizableGraph::Vertex* to) = 0; /** - * verify that all the information of the edges are semi positive definite, - * i.e., all Eigenvalues are >= 0. - * @param verbose output edges with not PSD information matrix by logging - * @return true if all edges have PSD information matrix + * override in your class if it's possible to initialize the vertices in + * certain combinations. The return value may correspond to the cost for + * initiliaizng the vertex but should be positive if the initialization is + * possible and negative if not possible. */ - bool verifyInformationMatrices(bool verbose = false) const; - - // helper functions to save an individual vertex - bool saveVertex(std::ostream& os, Vertex* v) const; + virtual double initialEstimatePossible( + const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { + (void)from; + (void)to; + return -1.; + } - // helper function to save an individual parameter - bool saveParameter(std::ostream& os, Parameter* v) const; + //! returns the level of the edge + int level() const { return _level; } + //! sets the level of the edge + void setLevel(int l) { _level = l; } - // helper functions to save an individual edge - bool saveEdge(std::ostream& os, Edge* e) const; + //! returns the dimensions of the error function + int dimension() const { return _dimension; } - // helper functions to save the data packets - bool saveUserData(std::ostream& os, HyperGraph::Data* v) const; + G2O_ATTRIBUTE_DEPRECATED(virtual Vertex* createFrom()) { return nullptr; } + G2O_ATTRIBUTE_DEPRECATED(virtual Vertex* createTo()) { return nullptr; } + virtual Vertex* createVertex(int) { return nullptr; } - //! the workspace for storing the Jacobians of the graph - JacobianWorkspace& jacobianWorkspace() { return _jacobianWorkspace; } - const JacobianWorkspace& jacobianWorkspace() const { - return _jacobianWorkspace; - } + //! read the vertex from a stream, i.e., the internal state of the vertex + virtual bool read(std::istream& is) = 0; + //! write the vertex to a stream + virtual bool write(std::ostream& os) const = 0; - /** - * Eigen starting from version 3.1 requires that we call an initialize - * function, if we perform calls to Eigen from several threads. - * Currently, this function calls Eigen::initParallel if g2o is compiled - * with OpenMP support and Eigen's version is at least 3.1 - */ - static bool initMultiThreading(); + //! the internal ID of the edge + long long internalId() const { return _internalId; } - inline ParameterContainer& parameters() { return _parameters; } - inline const ParameterContainer& parameters() const { return _parameters; } + OptimizableGraph* graph(); + const OptimizableGraph* graph() const; - //! apply a unary function to all vertices - void forEachVertex(std::function fn); - //! apply a unary function to the vertices in vset - void forEachVertex(HyperGraph::VertexSet& vset, - std::function fn); + bool setParameterId(int argNum, int paramId); + inline const Parameter* parameter(int argNo) const { + return *_parameters.at(argNo); + } + inline size_t numParameters() const { return _parameters.size(); } + inline void resizeParameters(size_t newSize) { + _parameters.resize(newSize, 0); + _parameterIds.resize(newSize, -1); + _parameterTypes.resize(newSize, typeid(void*).name()); + } protected: - std::map _renamedTypesLookup; - long long _nextEdgeId; - std::vector _graphActions; + int _dimension; + int _level; + RobustKernel* _robustKernel; + long long _internalId; + std::vector _cacheIds; + + template + bool installParameter(ParameterType*& p, size_t argNo, int paramId = -1) { + if (argNo >= _parameters.size()) return false; + _parameterIds[argNo] = paramId; + _parameters[argNo] = (Parameter**)&p; + _parameterTypes[argNo] = typeid(ParameterType).name(); + return true; + } + + template + void resolveCache(CacheType*& cache, OptimizableGraph::Vertex*, + const std::string& _type, + const ParameterVector& parameters); - ParameterContainer _parameters; - JacobianWorkspace _jacobianWorkspace; + bool resolveParameters(); + virtual bool resolveCaches(); - void performActions(int iter, HyperGraphActionSet& actions); + std::vector _parameterTypes; + std::vector _parameters; + std::vector _parameterIds; }; +//! returns the vertex number id appropriately casted +inline Vertex* vertex(int id) { + return reinterpret_cast(HyperGraph::vertex(id)); +} + +//! returns the vertex number id appropriately casted +inline const Vertex* vertex(int id) const { + return reinterpret_cast(HyperGraph::vertex(id)); +} + +//! empty constructor +OptimizableGraph(); +virtual ~OptimizableGraph(); + +/** + * adds a new vertex. The new vertex is then "taken". + * @return false if a vertex with the same id as v is already in the graph, + * true otherwise. + */ +virtual bool addVertex(HyperGraph::Vertex* v, Data* userData); +virtual bool addVertex(HyperGraph::Vertex* v) { return addVertex(v, 0); } +bool addVertex(OptimizableGraph::Vertex* v, Data* userData); +bool addVertex(OptimizableGraph::Vertex* v) { return addVertex(v, 0); } + +/** + * adds a new edge. + * The edge should point to the vertices that it is connecting + * (setFrom/setTo). + * @return false if the insertion does not work (incompatible types of the + * vertices/missing vertex). true otherwise. + */ +virtual bool addEdge(HyperGraph::Edge* e); +bool addEdge(OptimizableGraph::Edge* e); + +/** + * overridden from HyperGraph, to maintain the bookkeeping of the + * caches/parameters and jacobian workspaces consistent upon a change in the + * vertex. + * @return false if something goes wrong. + */ +virtual bool setEdgeVertex(HyperGraph::Edge* e, int pos, HyperGraph::Vertex* v); + +//! returns the chi2 of the current configuration +double chi2() const; + +//! return the maximum dimension of all vertices in the graph +int maxDimension() const; + +//! Recompute the size of the Jacobian workspace from all the +//! edges in the graph. +void recomputeJacobianWorkspaceSize() { + _jacobianWorkspace.updateSize(*this, true); +} + +/** + * iterates over all vertices and returns a set of all the vertex dimensions + * in the graph + */ +std::set dimensions() const; + +/** + * carry out n iterations + * @return the number of performed iterations + */ +virtual int optimize(int iterations, bool online = false); + +//! called at the beginning of an iteration (argument is the number of the +//! iteration) +virtual void preIteration(int); +//! called at the end of an iteration (argument is the number of the +//! iteration) +virtual void postIteration(int); + +//! add an action to be executed before each iteration +bool addPreIterationAction(HyperGraphAction* action); +//! add an action to be executed after each iteration +bool addPostIterationAction(HyperGraphAction* action); + +//! remove an action that should no longer be executed before each iteration +bool removePreIterationAction(HyperGraphAction* action); +//! remove an action that should no longer be executed after each iteration +bool removePostIterationAction(HyperGraphAction* action); + +//! push the estimate of all variables onto a stack +virtual void push(); +//! pop (restore) the estimate of all variables from the stack +virtual void pop(); +//! discard the last backup of the estimate for all variables by removing it +//! from the stack +virtual void discardTop(); + +//! load the graph from a stream. Uses the Factory singleton for creating the +//! vertices and edges. +virtual bool load(std::istream& is); +bool load(const char* filename); +//! save the graph to a stream. Again uses the Factory system. +virtual bool save(std::ostream& os, int level = 0) const; +//! function provided for convenience, see save() above +bool save(const char* filename, int level = 0) const; + +//! save a subgraph to a stream. Again uses the Factory system. +bool saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, int level = 0); + +//! save a subgraph to a stream. Again uses the Factory system. +bool saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset); + +//! push the estimate of a subset of the variables onto a stack +virtual void push(HyperGraph::VertexSet& vset); +//! pop (restore) the estimate a subset of the variables from the stack +virtual void pop(HyperGraph::VertexSet& vset); +//! ignore the latest stored element on the stack, remove it from the stack +//! but do not restore the estimate +virtual void discardTop(HyperGraph::VertexSet& vset); + +//! fixes/releases a set of vertices +virtual void setFixed(HyperGraph::VertexSet& vset, bool fixed); + +/** + * set the renamed types lookup from a string, format is for example: + * VERTEX_CAM=VERTEX_SE3:EXPMAP,EDGE_PROJECT_P2MC=EDGE_PROJECT_XYZ:EXPMAP + * This will change the occurrence of VERTEX_CAM in the file to + * VERTEX_SE3:EXPMAP + */ +void setRenamedTypesFromString(const std::string& types); + +/** + * test whether a solver is suitable for optimizing this graph. + * @param solverProperty the solver property to evaluate. + * @param vertDims should equal to the set returned by dimensions() to avoid + * re-evaluating. + */ +bool isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, + const std::set& vertDims = std::set()) const; + +/** + * remove the parameters of the graph + */ +virtual void clearParameters(); + +bool addParameter(Parameter* p) { return _parameters.addParameter(p); } + +Parameter* parameter(int id) { return _parameters.getParameter(id); } + +/** + * verify that all the information of the edges are semi positive definite, + * i.e., all Eigenvalues are >= 0. + * @param verbose output edges with not PSD information matrix by logging + * @return true if all edges have PSD information matrix + */ +bool verifyInformationMatrices(bool verbose = false) const; + +// helper functions to save an individual vertex +bool saveVertex(std::ostream& os, Vertex* v) const; + +// helper function to save an individual parameter +bool saveParameter(std::ostream& os, Parameter* v) const; + +// helper functions to save an individual edge +bool saveEdge(std::ostream& os, Edge* e) const; + +// helper functions to save the data packets +bool saveUserData(std::ostream& os, HyperGraph::Data* v) const; + +//! the workspace for storing the Jacobians of the graph +JacobianWorkspace& jacobianWorkspace() { return _jacobianWorkspace; } +const JacobianWorkspace& jacobianWorkspace() const { + return _jacobianWorkspace; +} + +/** + * Eigen starting from version 3.1 requires that we call an initialize + * function, if we perform calls to Eigen from several threads. + * Currently, this function calls Eigen::initParallel if g2o is compiled + * with OpenMP support and Eigen's version is at least 3.1 + */ +static bool initMultiThreading(); + +inline ParameterContainer& parameters() { return _parameters; } +inline const ParameterContainer& parameters() const { return _parameters; } + +//! apply a unary function to all vertices +void forEachVertex(std::function fn); +//! apply a unary function to the vertices in vset +void forEachVertex(HyperGraph::VertexSet& vset, + std::function fn); + +protected: +std::map _renamedTypesLookup; +long long _nextEdgeId; +std::vector _graphActions; + +ParameterContainer _parameters; +JacobianWorkspace _jacobianWorkspace; + +void performActions(int iter, HyperGraphActionSet& actions); +} +; + /** @} */ diff --git a/g2o/examples/interactive_slam/slam_parser/parser/flex_scanner.cpp b/g2o/examples/interactive_slam/slam_parser/parser/flex_scanner.cpp index 4cc4e3388..4b8596fb3 100644 --- a/g2o/examples/interactive_slam/slam_parser/parser/flex_scanner.cpp +++ b/g2o/examples/interactive_slam/slam_parser/parser/flex_scanner.cpp @@ -1498,7 +1498,7 @@ void yyFlexLexer::yyensure_buffer_stack(void) { return; } - if ((yy_buffer_stack_top) >= ((yy_buffer_stack_max)) - 1) { + if ((yy_buffer_stack_top) >= ((yy_buffer_stack_max))-1) { /* Increase the buffer to prepare for a possible push. */ yy_size_t grow_size = 8 /* arbitrary grow size */; diff --git a/g2o/stuff/sampler.h b/g2o/stuff/sampler.h index af5661283..b2179ac4d 100644 --- a/g2o/stuff/sampler.h +++ b/g2o/stuff/sampler.h @@ -80,39 +80,37 @@ class GaussianSampler { std::unique_ptr _generator; }; -class G2O_STUFF_API Sampler { - public: - /** - * Gaussian random with a mean and standard deviation. Uses the - * Polar method of Marsaglia. - */ - static double gaussRand(double mean, double sigma) { - double y, r2; - do { - double x = -1.0 + 2.0 * uniformRand(0.0, 1.0); - y = -1.0 + 2.0 * uniformRand(0.0, 1.0); - r2 = x * x + y * y; - } while (r2 > 1.0 || r2 == 0.0); - return mean + sigma * y * std::sqrt(-2.0 * log(r2) / r2); - } +class G2O_STUFF_API +Sampler{public : + /** + * Gaussian random with a mean and standard deviation. Uses the + * Polar method of Marsaglia. + */ + static double gaussRand(double mean, double sigma){double y, r2; +do { + double x = -1.0 + 2.0 * uniformRand(0.0, 1.0); + y = -1.0 + 2.0 * uniformRand(0.0, 1.0); + r2 = x * x + y * y; +} while (r2 > 1.0 || r2 == 0.0); +return mean + sigma * y * std::sqrt(-2.0 * log(r2) / r2); +} // namespace g2o - /** - * sample a number from a uniform distribution - */ - static double uniformRand(double lowerBndr, double upperBndr) { - return lowerBndr + - ((double)std::rand() / (RAND_MAX + 1.0)) * (upperBndr - lowerBndr); - } - /** - * default seed function using the current time in seconds - */ - static void seedRand() { - seedRand(static_cast(std::time(NULL))); - } +/** + * sample a number from a uniform distribution + */ +static double uniformRand(double lowerBndr, double upperBndr) { + return lowerBndr + + ((double)std::rand() / (RAND_MAX + 1.0)) * (upperBndr - lowerBndr); +} +/** + * default seed function using the current time in seconds + */ +static void seedRand() { seedRand(static_cast(std::time(NULL))); } - /** seed the random number generator */ - static void seedRand(unsigned int seed) { std::srand(seed); } -}; +/** seed the random number generator */ +static void seedRand(unsigned int seed) { std::srand(seed); } +} +; } // namespace g2o diff --git a/g2o/types/slam3d/se3quat.h b/g2o/types/slam3d/se3quat.h index 673c13d4b..6818a7e40 100644 --- a/g2o/types/slam3d/se3quat.h +++ b/g2o/types/slam3d/se3quat.h @@ -224,7 +224,7 @@ class G2O_TYPES_SLAM3D_API SE3Quat { V = (Matrix3::Identity() + (1 - std::cos(theta)) / (theta * theta) * Omega + - (theta - std::sin(theta)) / (std::pow(theta, 3)) * Omega2); + (theta - std::sin(theta)) / (std::pow(theta, 3))*Omega2); } return SE3Quat(Quaternion(R), V * upsilon); }