28 #ifndef G2O_STRUCTURE_ONLY_SOLVER_H 29 #define G2O_STRUCTURE_ONLY_SOLVER_H 56 template <
int Po
intDoF>
65 virtual OptimizationAlgorithm::SolverResult
solve(
int iteration,
bool online =
false)
78 for (OptimizableGraph::VertexContainer::iterator it_v=vertices.begin(); it_v!=vertices.end(); ++it_v) {
83 assert(track.size()>=2);
89 for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin(); it_t!=track.end(); ++it_t) {
95 if (v->
fixed() ==
false) {
96 Eigen::Matrix<double, PointDoF, PointDoF, Eigen::ColMajor> H_pp;
99 for (
int i_g = 0; i_g < num_iters; ++i_g) {
104 assert(track.size()>=1);
106 for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin(); it_t!=track.end(); ++it_t) {
111 std::vector<bool> remember_fix_status(e->
vertices().size());
113 bool remember_fix_status[e->
vertices().size()];
115 for (
size_t k = 0; k < e->
vertices().size(); ++k) {
118 remember_fix_status[k] = otherV->
fixed();
129 for (
size_t k = 0; k < e->
vertices().size(); ++k) {
132 otherV->
setFixed(remember_fix_status[k]);
137 Eigen::Map<Eigen::Matrix<double,PointDoF,1,Eigen::ColMajor> > b(v->
bData(), v->
dimension());
139 if (b.norm()<0.001) {
146 Eigen::Matrix<double,PointDoF,PointDoF,Eigen::ColMajor> H_pp_mu = H_pp;
147 H_pp_mu.diagonal().array() += mu;
148 Eigen::LDLT<Eigen::Matrix<double,PointDoF,PointDoF,Eigen::ColMajor> > chol_H_pp(H_pp_mu);
149 bool goodStep =
false;
150 if (chol_H_pp.isPositive()) {
151 Eigen::Matrix<double,PointDoF,1,Eigen::ColMajor> delta_p = chol_H_pp.solve(b);
153 v->
oplus(delta_p.data());
154 double new_chi2 = 0.;
155 for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin(); it_t!=track.end(); ++it_t) {
158 new_chi2 += e->
chi2();
160 assert(
g2o_isnan(new_chi2)==
false &&
"Chi is NaN");
161 double rho = (chi2 - new_chi2);
182 if (trial >= num_max_trials) {
std::vector< OptimizableGraph::Vertex * > VertexContainer
vector container for vertices
const Vertex * vertex(size_t i) const
const OptimizableGraph::VertexContainer & points() const
virtual bool updateStructure(const std::vector< HyperGraph::Vertex * > &, const HyperGraph::EdgeSet &)
virtual void discardTop()=0
pop the last element from the stack, without restoring the current estimate
virtual double chi2() const =0
computes the chi2 based on the cached error value, only valid after computeError has been called...
This is a solver for "structure-only" optimization".
const SparseOptimizer * optimizer() const
return the optimizer operating on
virtual OptimizationAlgorithm::SolverResult solve(int iteration, bool online=false)
virtual void linearizeOplus(JacobianWorkspace &jacobianWorkspace)=0
OptimizableGraph::VertexContainer & points()
return the points of the optimization problem
void oplus(const double *v)
std::set< Edge * > EdgeSet
int dimension() const
dimension of the estimated state belonging to this node
Protocol The SLAM executable accepts such as solving the and retrieving or vertices in the explicitly state the reprensentation poses are represented by poses by VERTEX_XYZRPY In the Quaternions and other representations could be but note that it is up to the SLAM algorithm to choose the internal representation of the angles The keyword is followed by a unique vertex ID and an optional initialization of the or edges in the explicitly state the type of the constraint pose constraints are given by pose constraints by EDGE_XYZRPY The keyword is followed by a unique edge the IDs of the referenced vertices
const VertexContainer & vertices() const
const EdgeSet & edges() const
returns the set of hyper-edges that are leaving/entering in this vertex
virtual bool computeMarginals(SparseBlockMatrix< MatrixXD > &, const std::vector< std::pair< int, int > > &)
virtual void constructQuadraticForm()=0
virtual double * bData()=0
return a pointer to the b vector associated with this vertex
void updateSize(const HyperGraph::Edge *e)
virtual void computeError()=0
virtual void push()=0
backup the position of the vertex to a stack
virtual void pop()=0
restore the position of the vertex by retrieving the position from the stack
virtual void mapHessianMemory(double *d)=0
virtual void clearQuadraticForm()=0
bool marginalized() const
true => this node is marginalized out during the optimization
A general case Vertex for optimization.
const VertexContainer & activeVertices() const
the vertices active in the current optimization
bool fixed() const
true => this node is fixed during the optimization
void setFixed(bool fixed)
true => this node should be considered fixed during the optimization
Generic interface for a non-linear solver operating on a graph.
provide memory workspace for computing the Jacobians
OptimizableGraph::VertexContainer _points
Sparse matrix which uses blocks.
OptimizationAlgorithm::SolverResult calc(OptimizableGraph::VertexContainer &vertices, int num_iters, int num_max_trials=10)