From b49d2f85731c959268fb587358ce4785d5292391 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Tue, 27 May 2014 20:38:23 +0200 Subject: [PATCH] compiles again with g2o. --- svo/include/svo/bundle_adjustment.h | 10 ++++-- svo/src/bundle_adjustment.cpp | 50 ++++++++++++++--------------- svo/src/frame_handler_mono.cpp | 8 ++--- 3 files changed, 37 insertions(+), 31 deletions(-) diff --git a/svo/include/svo/bundle_adjustment.h b/svo/include/svo/bundle_adjustment.h index 215d563..f3c3501 100644 --- a/svo/include/svo/bundle_adjustment.h +++ b/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. diff --git a/svo/src/bundle_adjustment.cpp b/svo/src/bundle_adjustment.cpp index a2075f4..eaba2b0 100644 --- a/svo/src/bundle_adjustment.cpp +++ b/svo/src/bundle_adjustment.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -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::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* 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* 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::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::iterator it = edges.begin(); it != edges.end(); ++it) { - if(it->edge_->chi2() > reproj_thresh_2_squared) //*(1<feature_->level)) + if(it->edge->chi2() > reproj_thresh_2_squared) //*(1<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::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); diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp index 46bcd45..3de6f7f 100644 --- a/svo/src/frame_handler_mono.cpp +++ b/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 {"<