1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 H. Strasdat
3 // Copyright (C) 2012 R. Kuemmerle
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // * Redistributions of source code must retain the above copyright notice,
11 //   this list of conditions and the following disclaimer.
12 // * Redistributions in binary form must reproduce the above copyright
13 //   notice, this list of conditions and the following disclaimer in the
14 //   documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
17 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
18 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
19 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
23 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
24 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
25 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 
28 #ifndef G2O_STRUCTURE_ONLY_SOLVER_H
29 #define G2O_STRUCTURE_ONLY_SOLVER_H
30 
31 #include "g2o/core/base_vertex.h"
32 #include "g2o/core/base_binary_edge.h"
33 #include "g2o/core/optimization_algorithm.h"
34 #include "g2o/core/sparse_optimizer.h"
35 
36 namespace g2o
37 {
38 
39 /**
40  * \brief This is a solver for "structure-only" optimization"
41  *
42  * Given the problem of landmark-based SLAM or bundle adjustment, this class
43  * performs optimization on the landmarks while the poses are kept fixed. This
44  * can be done very efficiently, since the position on the landmarks are
45  * indepdented given the poses are known.
46  *
47  * This class slightly misuses the API of g2o. It is designed in a way, it can
48  * work on the very same graph which reflects the problem of landmark-based
49  * SLAM, bundle adjustment and which is meant to be solved using the Schur
50  * complement. Thus, it can be called just before or after joint-optimization
51  * without the need of additional setup. Call calc() with the point features you
52  * want to optimize.
53  *
54  * This class is still considered as being experimentally!
55  */
56 template <int PointDoF>
57 class StructureOnlySolver : public OptimizationAlgorithm
58 {
59   public:
StructureOnlySolver()60     StructureOnlySolver()
61     {
62       _verbose = true;
63     }
64 
65     virtual OptimizationAlgorithm::SolverResult solve(int iteration, bool online = false)
66     {
67       (void) iteration;
68       (void) online;
69       return calc(_points, 1);
70     }
71 
72     OptimizationAlgorithm::SolverResult calc(OptimizableGraph::VertexContainer& vertices, int num_iters, int num_max_trials=10)
73     {
74       JacobianWorkspace auxWorkspace;
75       auxWorkspace.updateSize(2, 50);
76       auxWorkspace.allocate();
77 
78       for (OptimizableGraph::VertexContainer::iterator it_v=vertices.begin(); it_v!=vertices.end(); ++it_v) {
79         bool stop = false;
80         g2o::OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it_v);
81         assert(v->dimension() == PointDoF);
82         g2o::HyperGraph::EdgeSet& track = v->edges();
83         assert(track.size()>=2);
84         number_t chi2 = 0;
85         // TODO make these parameters
86         number_t mu = cst(0.01);
87         number_t nu = 2;
88 
89         for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin(); it_t!=track.end(); ++it_t) {
90           g2o::OptimizableGraph::Edge* e = dynamic_cast<g2o::OptimizableGraph::Edge *>(*it_t);
91           e->computeError();
92           if (e->robustKernel())
93           {
94             Vector3 rho;
95             e->robustKernel()->robustify(e->chi2(), rho);
96             chi2 += rho[0];
97           }
98           else
99           {
100             chi2 += e->chi2();
101           }
102         }
103 
104         if (v->fixed() == false) {
105           Eigen::Matrix<number_t, PointDoF, PointDoF, Eigen::ColMajor> H_pp;
106           H_pp.resize(v->dimension(), v->dimension());
107           v->mapHessianMemory(H_pp.data());
108           for (int i_g = 0; i_g < num_iters; ++i_g) {
109             H_pp.setZero();
110             v->clearQuadraticForm();
111 
112             g2o::HyperGraph::EdgeSet& track = v->edges();
113             assert(track.size()>=1);
114 
115             for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin(); it_t!=track.end(); ++it_t) {
116               g2o::OptimizableGraph::Edge* e = dynamic_cast<g2o::OptimizableGraph::Edge *>(*it_t);
117 
118               // fix all the other vertices and remember their fix value
119 #ifdef WINDOWS
120               std::vector<bool> remember_fix_status(e->vertices().size());
121 #else
122               bool remember_fix_status[e->vertices().size()];
123 #endif
124               for (size_t k = 0; k < e->vertices().size(); ++k) {
125                 OptimizableGraph::Vertex* otherV = static_cast<OptimizableGraph::Vertex*>(e->vertex(k));
126                 if (otherV != v) {
127                   remember_fix_status[k] = otherV->fixed();
128                   otherV->setFixed(true);
129                 }
130               }
131 
132               // build the matrix
133               e->computeError();
134               e->linearizeOplus(auxWorkspace);
135               e->constructQuadraticForm();
136 
137               // Restore frame's initial fixed() values
138               for (size_t k = 0; k < e->vertices().size(); ++k) {
139                 OptimizableGraph::Vertex* otherV = static_cast<g2o::OptimizableGraph::Vertex*>(e->vertex(k));
140                 if (otherV != v) {
141                   otherV->setFixed(remember_fix_status[k]);
142                 }
143               }
144             }
145 
146             Eigen::Map<Eigen::Matrix<number_t,PointDoF,1,Eigen::ColMajor> > b(v->bData(), v->dimension());
147 
148             if (b.norm()<0.001) {
149               stop = true;
150               break;
151             }
152 
153             int trial=0;
154             do {
155               Eigen::Matrix<number_t,PointDoF,PointDoF,Eigen::ColMajor> H_pp_mu = H_pp;
156               H_pp_mu.diagonal().array() += mu;
157               Eigen::LDLT<Eigen::Matrix<number_t,PointDoF,PointDoF,Eigen::ColMajor> > chol_H_pp(H_pp_mu);
158               bool goodStep = false;
159               if (chol_H_pp.isPositive()) {
160                 Eigen::Matrix<number_t,PointDoF,1,Eigen::ColMajor> delta_p = chol_H_pp.solve(b);
161                 v->push();
162                 v->oplus(delta_p.data());
163                 number_t new_chi2 = 0;
164                 for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin(); it_t!=track.end(); ++it_t) {
165                   g2o::OptimizableGraph::Edge* e = dynamic_cast<g2o::OptimizableGraph::Edge *>(*it_t);
166                   e->computeError();
167                   if (e->robustKernel())
168                   {
169                     Vector3 rho;
170                     e->robustKernel()->robustify(e->chi2(), rho);
171                     new_chi2 += rho[0];
172                   }
173                   else
174                   {
175                     new_chi2 += e->chi2();
176                   }
177                 }
178                 assert(g2o_isnan(new_chi2)==false && "Chi is NaN");
179                 number_t rho = (chi2 - new_chi2);
180                 if (rho > 0 && g2o_isfinite(new_chi2)) {
181                   goodStep = true;
182                   chi2 = new_chi2;
183                   v->discardTop();
184                 } else {
185                   goodStep = false;
186                   v->pop();
187                 }
188               }
189 
190               // update the damping factor based on the result of the last increment
191               if (goodStep) {
192                 mu *= cst(1./3.);
193                 nu = 2.;
194                 trial=0;
195                 break;
196               } else {
197                 mu *= nu;
198                 nu *= 2.;
199                 ++trial;
200                 if (trial >= num_max_trials) {
201                   stop=true;
202                   break;
203                 }
204               }
205             } while(!stop);
206             if (stop)
207               break;
208           }
209         }
210       }
211       return OK;
212     }
213 
init(bool)214     virtual bool init(bool )
215     {
216       // collect the vertices
217       _points.clear();
218       for (OptimizableGraph::VertexContainer::const_iterator it =  optimizer()->activeVertices().begin(); it != optimizer()->activeVertices().end(); ++it) {
219         OptimizableGraph::Vertex* v = *it;
220         if (v->marginalized()) {
221           _points.push_back(v);
222         }
223       }
224       return true;
225     }
226 
computeMarginals(SparseBlockMatrix<MatrixX> &,const std::vector<std::pair<int,int>> &)227     virtual bool computeMarginals(SparseBlockMatrix<MatrixX>&, const std::vector<std::pair<int, int> >&) { return false;}
228 
updateStructure(const std::vector<HyperGraph::Vertex * > &,const HyperGraph::EdgeSet &)229     virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& , const HyperGraph::EdgeSet& ) { return true;}
230 
231     //! return the points of the optimization problem
points()232     OptimizableGraph::VertexContainer& points() { return _points;}
points()233     const OptimizableGraph::VertexContainer& points() const { return _points;}
234 
235   protected:
236     bool _verbose;
237     OptimizableGraph::VertexContainer _points;
238 };
239 
240 }
241 
242 #endif
243