structure_only_solver.h
Go to the documentation of this file.
00001 // g2o - General Graph Optimization
00002 // Copyright (C) 2011 H. Strasdat
00003 //
00004 // g2o is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 //
00009 // g2o is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 //
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #ifndef STRUCTURE_ONLY_SOLVER_H
00018 #define STRUCTURE_ONLY_SOLVER_H
00019 
00020 #include "hyper_graph.h"
00021 #include "base_vertex.h"
00022 #include "base_binary_edge.h"
00023 
00024 
00025 namespace g2o
00026 {
00027 
00044 template <int PointDoF>
00045 class StructureOnlySolver
00046 {
00047 public:
00048   StructureOnlySolver()
00049   {
00050     _verbose = true;
00051   }
00052 
00053   void calc(g2o::HyperGraph::VertexIDMap & vertices,
00054             int num_iters,
00055             int num_max_trials=10)
00056   {
00057     double chi2_sum=0;
00058     double old_chi2_sum=0;
00059 
00060 
00061     for (g2o::HyperGraph::VertexIDMap::iterator it_v=vertices.begin();
00062          it_v!=vertices.end(); ++it_v)
00063     {
00064       bool stop = false;
00065 
00066 
00067       g2o::OptimizableGraph::Vertex  * v
00068           = dynamic_cast<g2o::OptimizableGraph::Vertex  *>(it_v->second);
00069 
00070       if(v->marginalized()==0)
00071         continue;
00072 
00073       assert(v->dimension() == PointDoF);
00074 
00075       g2o::HyperGraph::EdgeSet & track = v->edges();
00076 
00077       assert(track.size()>=2);
00078 
00079       double chi2 = 0;
00080 
00081 
00082       double mu = 0.01;
00083       double nu = 2;
00084 
00085 
00086 
00087 
00088 
00089       for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin();
00090            it_t!=track.end(); ++it_t)
00091       {
00092         g2o::OptimizableGraph::Edge * e
00093             = dynamic_cast<g2o::OptimizableGraph::Edge *>(*it_t);
00094 
00095         e->computeError();
00096 
00097         chi2 += e->chi2();
00098       }
00099 
00100 
00101       old_chi2_sum += chi2;
00102       if (v->fixed()==false)
00103       {
00104         for (int i_g=0; i_g<num_iters; ++i_g)
00105         {
00106           double rho = 0; //Assign value, so the compiler is not complaining...
00107 
00108           Matrix<double, PointDoF,PointDoF> H_pp;
00109           H_pp.setZero();
00110 
00111 
00112           v->mapHessianMemory(&(H_pp(0,0)));
00113           v->clearQuadraticForm();
00114 
00115           g2o::HyperGraph::EdgeSet & track = v->edges();
00116 
00117 
00118           assert(track.size()>=2);
00119 
00120           double max_err = 0; (void) max_err;
00121 
00122           for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin();
00123                it_t!=track.end(); ++it_t)
00124           {
00125 
00126             g2o::OptimizableGraph::Edge * e
00127                 = dynamic_cast<g2o::OptimizableGraph::Edge *>(*it_t);
00128 
00129             assert(v==e->vertices()[0]);
00130 
00131             g2o::OptimizableGraph::Vertex * frame_v
00132                 = dynamic_cast<g2o::OptimizableGraph::Vertex *>
00133                 (e->vertices()[1]);
00134 
00135             bool remember_fix_status = frame_v->fixed();
00136             frame_v->setFixed(true);
00137             // Fix frame to prevent g2o to compute its Jacobian
00138 
00139             e->computeError();
00140 
00141             e->linearizeOplus();
00142 
00143 
00144 
00145             e->constructQuadraticForm();
00146 
00147             // Restore frame's initial fixed() values
00148             frame_v->setFixed(remember_fix_status);
00149           }
00150 
00151           Matrix<double,PointDoF,1> b
00152               = Matrix<double,PointDoF,1>(v->bData());
00153 
00154           if (b.norm()<0.001)
00155           {
00156             stop = true;
00157             break;
00158           }
00159 
00160 
00161 
00162           double new_chi2 = 0;
00163 
00164           double new_max_err=0;
00165           int trial=0;
00166 
00167 
00168           do
00169           {
00170             Matrix<double,PointDoF,PointDoF> H_pp_mu
00171                 = mu*Matrix<double,PointDoF,PointDoF>::Identity()+H_pp;
00172 
00173 
00174 
00175 
00176 
00177 
00178 
00179             LDLT<Matrix<double,PointDoF,PointDoF> > Chol_H_pp(H_pp_mu);
00180             Matrix<double,PointDoF,1> delta_p;
00181 
00182             delta_p
00183                 =  Chol_H_pp.solve(b);
00184 
00185 
00186 
00187             v->push();
00188 
00189 
00190             v->oplus(&(delta_p[0]));
00191             for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin();
00192                  it_t!=track.end(); ++it_t)
00193             {
00194               g2o::OptimizableGraph::Edge * e
00195                   = dynamic_cast<g2o::OptimizableGraph::Edge *>(*it_t);
00196               e->computeError();
00197 
00198 
00199 
00200               new_chi2 += e->chi2();
00201 
00202 
00203 
00204             }
00205             assert(isnan(new_chi2)==false);
00206 //            if (isnan(new_chi2))
00207 //            {
00208 //              cerr  << b.transpose() << endl;
00209 //              cerr  <<H_pp_mu << endl;
00210 //              cerr  << mu << endl;
00211 //              cerr  << delta_p.transpose() << endl;
00212 //              cerr  << trial << endl;
00213 //              assert(false);
00214 //            }
00215 
00216             rho = (chi2-new_chi2);
00217             if (rho>0)
00218             {
00219               v->discardTop();
00220               max_err = new_max_err;
00221               chi2 = new_chi2;
00222 
00223               mu *= 1./3.;
00224               nu = 2.;
00225 
00226 
00227 
00228               trial=0;
00229 
00230               break;
00231             }
00232             else
00233             {
00234               v->pop();
00235 
00236               mu *= nu;
00237               nu *= 2.;
00238 
00239               ++trial;
00240               if (trial>=num_max_trials)
00241               {
00242                 stop=true;
00243                 break;
00244               }
00245             }
00246           }while(!stop);
00247 
00248           if (stop)
00249             break;
00250         }
00251       }
00252       chi2_sum += chi2;
00253     }
00254     if (_verbose>0)
00255       std::cerr << " chi vs. new_chi2 "
00256                 << old_chi2_sum
00257                 << " vs. "
00258                 << chi2_sum
00259                 << std::endl;
00260   }
00261 
00262   bool verbose() const
00263   {
00264     return _verbose;
00265   }
00266 
00267   void setVerbose(bool verbose)
00268   {
00269     _verbose = verbose;
00270   }
00271 
00272 protected:
00273   bool _verbose;
00274 
00275 };
00276 
00277 }
00278 
00279 #endif


re_vision
Author(s): Dorian Galvez-Lopez
autogenerated on Sun Jan 5 2014 11:33:00