compiles again with g2o.

svo/include/svo/bundle_adjustment.h

@@ -22,16 +22,22 @@
 namespace g2o {
 class EdgeProjectXYZ2UV;
 class SparseOptimizer;
+class VertexSE3Expmap;
+class VertexSBAPointXYZ;
 }
-typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
 
 namespace svo {
 
+typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
+typedef g2o::VertexSE3Expmap g2oFrameSE3;
+typedef g2o::VertexSBAPointXYZ g2oPoint;
+
+class Frame;
 class Point;
 class Feature;
 class Map;
 
-/// Local, global and 2-view bundle adjustment with g2o.
+/// Local, global and 2-view bundle adjustment with g2o
 namespace ba {
 
 /// Temporary container to hold the g2o edge with reference to frame and point.

svo/src/bundle_adjustment.cpp

@@ -28,6 +28,7 @@
 #include <g2o/solvers/structure_only/structure_only_solver.h>
 #include <svo/bundle_adjustment.h>
 #include <svo/frame.h>
+#include <svo/feature.h>
 #include <svo/point.h>
 #include <svo/config.h>
 #include <svo/map.h>
@@ -37,10 +38,11 @@
 namespace svo {
 namespace ba {
 
-void
-twoViewBA(Frame* frame1, Frame* frame2,
-          double reproj_thresh,
-          Map* map)
+void twoViewBA(
+    Frame* frame1,
+    Frame* frame2,
+    double reproj_thresh,
+    Map* map)
 {
   // scale reprojection threshold in pixels to unit plane
   reproj_thresh /= frame1->cam_->errorMultiplier2();
@@ -104,12 +106,12 @@ twoViewBA(Frame* frame1, Frame* frame2,
   const double reproj_thresh_squared = reproj_thresh*reproj_thresh;
   size_t n_incorrect_edges = 0;
   for(list<EdgeContainerSE3>::iterator it_e = edges.begin(); it_e != edges.end(); ++it_e)
-    if(it_e->edge_->chi2() > reproj_thresh_squared)
+    if(it_e->edge->chi2() > reproj_thresh_squared)
     {
-      if(it_e->feature_->point != NULL)
+      if(it_e->feature->point != NULL)
       {
-        map->safeDeletePoint(it_e->feature_->point);
-        it_e->feature_->point = NULL;
+        map->safeDeletePoint(it_e->feature->point);
+        it_e->feature->point = NULL;
       }
       ++n_incorrect_edges;
     }
@@ -117,14 +119,14 @@ twoViewBA(Frame* frame1, Frame* frame2,
   printf("2-View BA: Wrong edges =  %zu\n", n_incorrect_edges);
 }
 
-void
-localBA(Frame* center_kf,
-        set<FramePtr>* core_kfs,
-        Map* map,
-        size_t& n_incorrect_edges_1,
-        size_t& n_incorrect_edges_2,
-        double& init_error,
-        double& final_error)
+void localBA(
+    Frame* center_kf,
+    set<FramePtr>* core_kfs,
+    Map* map,
+    size_t& n_incorrect_edges_1,
+    size_t& n_incorrect_edges_2,
+    double& init_error,
+    double& final_error)
 {
 
   // init g2o
@@ -170,7 +172,7 @@ localBA(Frame* center_kf,
     ++n_mps;
 
     // Add edges
-    Observations::iterator it_obs=(*it_pt)->obs_.begin();
+    list<Feature*>::iterator it_obs=(*it_pt)->obs_.begin();
     while(it_obs!=(*it_pt)->obs_.end())
     {
       Vector2d error = vk::project2d((*it_obs)->f) - vk::project2d((*it_obs)->frame->w2f((*it_pt)->pos_));
@@ -236,9 +238,9 @@ localBA(Frame* center_kf,
   double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2;
   for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
   {
-    if(it->edge_->chi2() > reproj_thresh_2_squared) //*(1<<it->feature_->level))
+    if(it->edge->chi2() > reproj_thresh_2_squared) //*(1<<it->feature_->level))
     {
-      map->removePtFrameRef(it->frame_, it->feature_);
+      map->removePtFrameRef(it->frame, it->feature);
       ++n_incorrect_edges_2;
     }
   }
@@ -248,8 +250,7 @@ localBA(Frame* center_kf,
   final_error = sqrt(final_error)*center_kf->cam_->errorMultiplier2();
 }
 
-void
-globalBA(Map* map)
+void globalBA(Map* map)
 {
   // init g2o
   g2o::SparseOptimizer optimizer;
@@ -334,15 +335,14 @@ globalBA(Map* map)
   double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2;
   for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
   {
-    if(it->edge_->chi2() > reproj_thresh_2_squared)
+    if(it->edge->chi2() > reproj_thresh_2_squared)
     {
-      map->removePtFrameRef(it->frame_, it->feature_);
+      map->removePtFrameRef(it->frame, it->feature);
     }
   }
 }
 
-void
-setupG2o(g2o::SparseOptimizer * optimizer)
+void setupG2o(g2o::SparseOptimizer * optimizer)
 {
   // TODO: What's happening with all this HEAP stuff? Memory Leak?
   optimizer->setVerbose(false);

svo/src/frame_handler_mono.cpp

@@ -111,7 +111,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
 
   // two-frame bundle adjustment
 #ifdef USE_BUNDLE_ADJUSTMENT
-  ba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), map_);
+  ba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), &map_);
 #endif
 
   new_frame_->setKeyframe();
@@ -207,10 +207,10 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
     SVO_START_TIMER("local_ba");
     setCoreKfs(Config::coreNKfs());
     size_t loba_n_erredges_init, loba_n_erredges_fin;
-    double loba_err_init, ba_final_error;
+    double loba_err_init, loba_err_fin;
     ba::localBA(new_frame_.get(), &core_kfs_, &map_,
-                loba_n_erredges_init, ba_incorrect_edges_2,
-                ba_init_error, loba_err_fin);
+                loba_n_erredges_init, loba_n_erredges_fin,
+                loba_err_init, loba_err_fin);
     SVO_STOP_TIMER("local_ba");
     SVO_LOG4(loba_n_erredges_init, loba_n_erredges_fin, loba_err_init, loba_err_fin);
     SVO_DEBUG_STREAM("Local BA:\t RemovedEdges {"<<loba_n_erredges_init<<", "<<loba_n_erredges_fin<<"} \t "