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