bandle adjustment works well

GPU_version/vio/depth_filter_log.txt

@@ -0,0 +1,60 @@
+[16:55:41.271] New key frame 
+[16:55:41.271] BA loop run 
+[16:55:41.272] init error: 575.642695 
+[16:55:41.273] end error: 0.013166 
+[16:55:41.405] New key frame 
+[16:55:41.405] BA loop run 
+[16:55:41.405] init error: 189.835833 
+[16:55:41.407] end error: 0.468588 
+[16:55:41.507] New key frame 
+[16:55:41.507] BA loop run 
+[16:55:41.508] init error: 911.856919 
+[16:55:41.510] end error: 1.789062 
+[16:55:41.611] New key frame 
+[16:55:41.611] BA loop run 
+[16:55:41.612] init error: 18717.033656 
+[16:55:41.613] end error: 4.936020 
+[16:55:44.998] New key frame 
+[16:55:44.998] BA loop run 
+[16:55:44.998] init error: 39.791183 
+[16:55:45.001] end error: 10.492954 
+[16:55:45.116] New key frame 
+[16:55:45.116] BA loop run 
+[16:55:45.117] init error: 786.965335 
+[16:55:45.122] end error: 1.421966 
+[16:55:45.268] New key frame 
+[16:55:45.268] BA loop run 
+[16:55:45.270] init error: 1782.595577 
+[16:55:45.277] end error: 2.222177 
+[16:55:45.468] New key frame 
+[16:55:45.469] BA loop run 
+[16:55:45.473] init error: 3538.063991 
+[16:55:45.484] end error: 15.716388 
+[16:55:45.749] New key frame 
+[16:55:45.749] BA loop run 
+[16:55:45.753] init error: 16354.561639 
+[16:55:45.761] end error: 8.216372 
+[16:55:46.186] New key frame 
+[16:55:46.186] BA loop run 
+[16:55:46.206] init error: 414249.527414 
+[16:55:46.289] end error: 138.234439 
+[16:55:48.072] New key frame 
+[16:55:48.072] BA loop run 
+[16:55:48.108] init error: 627388.776280 
+[16:55:48.264] end error: 1448.316701 
+[16:55:48.945] New key frame 
+[16:55:48.945] BA loop run 
+[16:55:49.017] init error: 146955.415274 
+[16:55:49.918] end error: 941.453528 
+[16:56:07.244] New key frame 
+[16:56:07.244] BA loop run 
+[16:56:07.330] init error: 125101697.443424 
+[16:56:08.112] end error: 7023.248058 
+[16:56:29.389] New key frame 
+[16:56:29.389] BA loop run 
+[16:56:29.529] init error: 258236615.290955 
+[16:56:31.054] end error: 24797.607025 
+[16:56:52.584] New key frame 
+[16:56:52.584] BA loop run 
+[16:56:52.866] init error: 8329221.907695 
+[16:57:03.183] end error: 3862.867416 

GPU_version/vio/include/vio/frame_handler_mono.h

@@ -34,7 +34,6 @@ class FrameHandlerMono : public FrameHandlerBase
 {
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  
   FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,3,1>& init);
   virtual ~FrameHandlerMono();
 

GPU_version/vio/include/vio/global_optimizer.h

@@ -61,7 +61,6 @@ public:
       boost::unique_lock< boost::mutex > lk( mtx_);
       new_keyframe_=true;
       cond_.notify_one();
-      std::cerr<<"new_key_frame\n";
 #if VIO_DEBUG
       fprintf(log_,"[%s] New key frame \n",
               vio::time_in_HH_MM_SS_MMM().c_str());
@@ -69,8 +68,6 @@ public:
   }
 protected:
   bool new_keyframe_=false;
-  std::shared_ptr<g2o::OptimizationAlgorithmLevenberg> Levenberg_;
-  std::shared_ptr<g2o::SparseOptimizer> optimizer_;
   boost::condition_variable cond_;
   boost::mutex mtx_;
   boost::thread* thread_;
@@ -86,14 +83,6 @@ protected:
             std::shared_ptr<Feature>        feature;
         };
 
-/// Initialize g2o with solver type, optimization strategy and camera model.
-        void setupG2o();
-        /// Run the optimization on the provided graph.
-        void runSparseBAOptimizer(
-                unsigned int num_iter,
-                double& init_error,
-                double& final_error);
-
 /// Create a g2o vertice from a keyframe object.
         g2o::VertexSE3Expmap* createG2oFrameSE3(
                 FramePtr kf,

GPU_version/vio/launch/Px30.launch

@@ -1,6 +1,6 @@
 <launch>
 
-  <!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" / -->
+  <node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
 
     <!-- Camera topic to subscribe to -->
     <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />

GPU_version/vio/param/vo_fast.yaml

@@ -1,7 +1,7 @@
 svo:
-  grid_size: 30            #Feature grid size of a cell in [px].
+  grid_size: 8            #Feature grid size of a cell in [px].
   max_n_kfs: 20            #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed.  Minimum number of keyframes is 3.
-  loba_num_iter: 0         #Number of iterations in the local bundle adjustment.
+  loba_num_iter: 10         #Number of iterations in the local bundle adjustment.
   quality_min_fts: 10      #If the number of tracked features drops below this threshold. Tracking quality is bad.
   quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad.
   init_min_tracked: 100

GPU_version/vio/src/config.cpp

@@ -26,7 +26,7 @@ Config::Config() :
     use_imu(vk::getParam<bool>("svo/use_imu", false)),
     core_n_kfs(vk::getParam<int>("svo/core_n_kfs", 3)),
     map_scale(vk::getParam<double>("svo/map_scale", 1.0)),
-    grid_size(vk::getParam<int>("svo/grid_size", 30)),
+    grid_size(vk::getParam<int>("svo/grid_size", 10)),
     init_min_disparity(vk::getParam<double>("svo/init_min_disparity", 50.0)),
     init_min_tracked(vk::getParam<int>("svo/init_min_tracked", 50)),
     init_min_inliers(vk::getParam<int>("svo/init_min_inliers", 40)),

GPU_version/vio/src/frame.cpp

@@ -178,7 +178,7 @@ bool getSceneDepth(const FramePtr frame, double& depth_mean, double& depth_min)
   {
     if((*it)->point != NULL)
     {
-        Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
+        Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3().inverse()*(*it)->point->pos_));
         if((e /= (1<<(*it)->level)).norm()>0.5)continue;
         double z = frame->w2f((*it)->point->pos_).z();
         depth_vec.push_back(z);

GPU_version/vio/src/frame_handler_mono.cpp

@@ -166,7 +166,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
     fprintf(log_,"[%s] Init: Selected Second frame. \t The number of features: %d depth mean:%f min:%f\n",
                        vio::time_in_HH_MM_SS_MMM().c_str(),new_frame_->fts_.size(),depth_mean,depth_min);
 #endif
-  //std::cerr<<"run BA\n";
   ba_glob_->new_key_frame();
   ROS_INFO("VIO initialized :)");
   ROS_INFO("Running ...");
@@ -257,7 +256,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   }
   // add keyframe to map
   map_.addKeyframe(new_frame_);
-  std::cerr<<"BA run\n";
   ba_glob_->new_key_frame();
   return RESULT_IS_KEYFRAME;
 }

GPU_version/vio/src/global_optimizer.cpp

@@ -73,7 +73,7 @@ namespace vio {
             while(map_.keyframes_.empty() || new_keyframe_ == false)
                 cond_.wait(lk);
 #if VIO_DEBUG
-            fprintf(log_,"[%s] BA loop \n",
+            fprintf(log_,"[%s] BA loop run \n",
                     vio::time_in_HH_MM_SS_MMM().c_str());
 #endif
             new_keyframe_=false;
@@ -115,7 +115,7 @@ namespace vio {
                     if(v_mp == NULL)
                     {
                         // mappoint-vertex doesn't exist yet. create a new one:
-                        v_mp = createG2oPoint(mp->pos_, v_id++, false);
+                        v_mp = createG2oPoint(mp->pos_, v_id++, true);
                         mp->v_pt_ = v_mp;
                         optimizer.addVertex(v_mp);
                     }
@@ -141,10 +141,19 @@ namespace vio {
             }
 
             // Optimization
-            double init_error=0.0, final_error=0.0;
-            if(Config::lobaNumIter() > 0)
-                runSparseBAOptimizer( Config::lobaNumIter(), init_error, final_error);
 
+            point_mut_.lock();
+            optimizer.initializeOptimization();
+            optimizer.computeActiveErrors();
+#if VIO_DEBUG
+            fprintf(log_,"[%s] init error: %f \n",
+                    vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
+#endif
+            optimizer.optimize(vio::Config::lobaNumIter());
+#if VIO_DEBUG
+            fprintf(log_,"[%s] end error: %f \n",
+                    vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
+#endif
             // Update Keyframe and MapPoint Positions
             for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
                 it_kf != map_.keyframes_.end(); ++it_kf)
@@ -163,6 +172,7 @@ namespace vio {
                     mp->v_pt_ = NULL;
                 }
             }
+            point_mut_.unlock();
 
             // Remove Measurements with too large reprojection error
             for(list< pair<FramePtr,std::shared_ptr<Feature>> >::iterator it=incorrect_edges.begin();
@@ -179,36 +189,6 @@ namespace vio {
         }
     }
 
-   void BA_Glob::setupG2o()
-   {
-       // TODO: What's happening with all this HEAP stuff? Memory Leak?
-/*       optimizer_=std::make_shared<g2o::SparseOptimizer>();
-       optimizer_->setVerbose(false);
-       auto linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();
-       g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
-               g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));
-
-       optimizer_->setAlgorithm(solver);
-
-       // setup camera
-       std::shared_ptr<g2o::CameraParameters> cam_params = std::make_shared<g2o::CameraParameters>(1.0, Vector2d(0.,0.), 0.);
-       cam_params->setId(0);
-       if (!optimizer_->addParameter(cam_params.get())) {
-           assert(false);*/
-      // }
-   }
-
-   void
-   BA_Glob::runSparseBAOptimizer(unsigned int num_iter,
-                        double& init_error, double& final_error)
-   {
-       optimizer_->initializeOptimization();
-       optimizer_->computeActiveErrors();
-       init_error = optimizer_->activeChi2();
-       optimizer_->optimize(num_iter);
-       final_error = optimizer_->activeChi2();
-   }
-
    g2o::VertexSE3Expmap*
    BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed)
    {
@@ -226,7 +206,7 @@ namespace vio {
    {
        g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
        v->setId(id);
-       v->setFixed(fixed);
+       v->setMarginalized(fixed);
        v->setEstimate(pos);
        return v;
 
@@ -242,8 +222,7 @@ namespace vio {
        g2o::EdgeProjectXYZ2UV* e= new g2o::EdgeProjectXYZ2UV;
        e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
        e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
-       e->setMeasurement(Vector2d(static_cast<int>(g2o::Sampler::uniformRand(0, 640)),
+       e->setMeasurement(f_up);
-                                  static_cast<int>(g2o::Sampler::uniformRand(0, 480))));
        e->information() = Eigen::Matrix2d::Identity();
        g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
        rk->setDelta(huber_width);

GPU_version/vio/src/math_utils.cpp

@@ -12,7 +12,7 @@ namespace vk {
 
 using namespace Eigen;
 /*
- * R and T are the rotation and translation of the current frames with respect to the refrence frame
+ * R and T are the rotation and translation of the current frames with respect to the refrence frame, T from ref to current
  * */
 
 Vector3d

GPU_version/vio/src/point.cpp

@@ -184,7 +184,8 @@ void Point::optimize(const size_t n_iter)
   }
   for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
             Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * pos_));
-            if (e.norm() > 0.3)
+            e /= (1<<(*it)->level);
+            if (e.norm() > 3.0/ (*it)->frame->cam_->errorMultiplier2())
                 n_failed_reproj_++;
   }
 

GPU_version/vio/src/pose_optimizer.cpp

@@ -124,30 +124,19 @@ void optimizeGaussNewton(
   }
 
   size_t n_deleted_refs = 0;
-  //for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
   for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
   {
-    //std::cerr<<"123 - pose optimizer - frame->fts_.size() : "<<frame->fts_.size()<<std::endl;
-    //std::cerr<<"124 - pose optimizer - (*it)->point : "<<(*it)->point<<std::endl;
     if((*it)->point == NULL) {
         it++;
-        //std::cerr<<"126 - pose optimizer - (*it)->point == NULL : "<<std::endl;
         continue;
     }
-    //std::cerr<<"129 - pose optimizer "<<(*it)->f.x()<<" "<<(*it)->f.y()<<" "<<(*it)->f.z()<<"\t"<<(*it)->point->pos_.x()<<" "<<(*it)->point->pos_.y()<<" "<<(*it)->point->pos_.z()<<std::endl;
     Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
-    //std::cerr<<"131 - pose optimizer"<<std::endl;
     e /= (1<<(*it)->level);
-    //std::cerr<<"133 - pose optimizer"<<std::endl;
     chi2_vec_final.push_back(e.norm());
-    //std::cerr<<"135 - pose optimizer"<<std::endl;
+    if(e.norm() >  1.0 / frame->cam_->errorMultiplier2())
-    if(e.norm() >  2.0 / frame->cam_->errorMultiplier2())
     {
-      // we don't need to delete a reference in the point since it was not created yet
       (*it)->point.reset();
-      //std::cerr<<"befor erase 140 pose optimizer"<<std::endl;
       it = frame->fts_.erase(it);
-      //std::cerr<<"after erase 142 pose optimizer"<<std::endl;
       ++n_deleted_refs;
     }
     else

GPU_version/vio/src/reprojector.cpp

@@ -75,9 +75,9 @@ namespace vio {
         resetGrid();
         Features keypoints;
         cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING2,false);
-        feature_detection::FastDetector detector(
+        std::unique_ptr<feature_detection::FastDetector> detector=std::make_unique<feature_detection::FastDetector>(
                 frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast_,Config::nPyrLevels());
-        detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), keypoints);
+        detector->detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), keypoints);
         std::vector<cv::KeyPoint> keypoints_cur;
         list<std::shared_ptr<Feature>>::iterator it_cur=keypoints.begin();
         cv::Mat cur_des=cv::Mat(keypoints.size(),64,CV_8UC1);
@@ -125,7 +125,6 @@ namespace vio {
                 else{
                     matcher->knnMatch(ref_des, cur_des, matches, 1, cv::InputArray(maskdown));
                 }
-                //matcher->knnMatch(ref_des, cur_des, matches, 1);
 
                 for(auto&& match:matches.back()){
                     it_cur=keypoints.begin();
@@ -169,14 +168,9 @@ namespace vio {
                         overlap_kfs.back().second++;
                     }
                 }
-/*                cv::Mat imgMatch,key_point_ref,key_point_cur;
-                cv::drawMatches( it_frame.item.first->img_pyr_[0], keypoints_kfs, frame->img_pyr_[0], keypoints_cur,matches, imgMatch);
-                cv::imshow("img",imgMatch);
-                cv::waitKey();*/
                 ++it_ref;
             }
         }
-        //exit(0);
         {
             for (auto&& cell : grid_.cells) {
                 if (reprojectCell(*cell, frame, log_))
@@ -216,7 +210,7 @@ namespace vio {
             if(!res)
             {
                 it->pt->n_failed_reproj_++;
-                if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)it->pt.reset();
+                if(it->pt->n_failed_reproj_ > 15)it->pt.reset();
                 it = cell.erase(it);
                 continue;
             }