debugging

GPU_version/vio/CMakeLists.txt

@@ -68,6 +68,7 @@ LIST(APPEND LINK_LIBS
   ${G2O_SOLVER_PCG}
   ${G2O_TYPES_SBA}
   ${G2O_TYPES_SLAM3D}
+  ${G2O_TYPES_ICP}
   cholmod cxsparse
 )
 

GPU_version/vio/depth_filter_log.txt

@@ -0,0 +1,135 @@
+[08:29:34.448] New key frame 
+[08:29:34.448] BA loop run 
+[08:29:34.448] init error: -nan 
+[08:29:34.450] end error: nan 
+[08:29:35.070] New key frame 
+[08:29:35.070] BA loop run 
+[08:29:35.071] init error: -nan 
+[08:29:35.073] end error: nan 
+[08:29:35.178] New key frame 
+[08:29:35.178] BA loop run 
+[08:29:35.178] init error: -nan 
+[08:29:35.181] end error: nan 
+[08:29:35.307] New key frame 
+[08:29:35.307] BA loop run 
+[08:29:35.307] init error: -nan 
+[08:29:35.311] end error: nan 
+[08:29:35.575] New key frame 
+[08:29:35.575] BA loop run 
+[08:29:35.575] init error: -nan 
+[08:29:35.577] end error: nan 
+[08:29:35.688] New key frame 
+[08:29:35.688] BA loop run 
+[08:29:35.689] init error: -nan 
+[08:29:35.691] end error: nan 
+[08:29:36.088] New key frame 
+[08:29:36.088] BA loop run 
+[08:29:36.088] init error: -nan 
+[08:29:36.090] end error: nan 
+[08:29:36.302] New key frame 
+[08:29:36.302] BA loop run 
+[08:29:36.303] init error: -nan 
+[08:29:36.305] end error: nan 
+[08:29:36.531] New key frame 
+[08:29:36.531] BA loop run 
+[08:29:36.531] init error: -nan 
+[08:29:36.534] end error: nan 
+[08:29:37.070] New key frame 
+[08:29:37.070] BA loop run 
+[08:29:37.070] init error: -nan 
+[08:29:37.073] end error: nan 
+[08:29:38.421] New key frame 
+[08:29:38.421] BA loop run 
+[08:29:38.422] init error: -nan 
+[08:29:38.423] end error: nan 
+[08:29:38.532] New key frame 
+[08:29:38.532] BA loop run 
+[08:29:38.532] init error: -nan 
+[08:29:38.533] end error: nan 
+[08:29:38.745] New key frame 
+[08:29:38.745] BA loop run 
+[08:29:38.746] init error: -nan 
+[08:29:38.747] end error: nan 
+[08:29:39.045] New key frame 
+[08:29:39.045] BA loop run 
+[08:29:39.046] init error: -nan 
+[08:29:39.047] end error: nan 
+[08:29:39.259] New key frame 
+[08:29:39.259] BA loop run 
+[08:29:39.259] init error: -nan 
+[08:29:39.261] end error: nan 
+[08:29:39.498] New key frame 
+[08:29:39.498] BA loop run 
+[08:29:39.498] init error: -nan 
+[08:29:39.501] end error: nan 
+[08:29:39.619] New key frame 
+[08:29:39.619] BA loop run 
+[08:29:39.620] init error: -nan 
+[08:29:39.623] end error: nan 
+[08:30:06.987] New key frame 
+[08:30:06.988] BA loop run 
+[08:30:06.988] init error: -nan 
+[08:30:06.989] end error: nan 
+[08:30:07.098] New key frame 
+[08:30:07.098] BA loop run 
+[08:30:07.099] init error: -nan 
+[08:30:07.100] end error: nan 
+[08:30:07.237] New key frame 
+[08:30:07.237] BA loop run 
+[08:30:07.237] init error: -nan 
+[08:30:07.239] end error: nan 
+[08:30:07.357] New key frame 
+[08:30:07.357] BA loop run 
+[08:30:07.357] init error: -nan 
+[08:30:07.359] end error: nan 
+[08:30:25.067] New key frame 
+[08:30:25.067] BA loop run 
+[08:30:25.068] init error: -nan 
+[08:30:25.068] end error: nan 
+[08:30:25.169] New key frame 
+[08:30:25.169] BA loop run 
+[08:30:25.169] init error: -nan 
+[08:30:25.170] end error: nan 
+[08:30:25.993] New key frame 
+[08:30:25.993] BA loop run 
+[08:30:25.993] init error: -nan 
+[08:30:25.994] end error: nan 
+[08:30:26.116] New key frame 
+[08:30:26.116] BA loop run 
+[08:30:26.117] init error: -nan 
+[08:30:26.117] end error: nan 
+[08:30:26.451] New key frame 
+[08:30:26.451] BA loop run 
+[08:30:26.451] init error: -nan 
+[08:30:26.452] end error: nan 
+[08:30:27.334] New key frame 
+[08:30:27.334] BA loop run 
+[08:30:27.335] init error: -nan 
+[08:30:27.335] end error: nan 
+[08:30:27.584] New key frame 
+[08:30:27.584] BA loop run 
+[08:30:27.584] init error: -nan 
+[08:30:27.585] end error: nan 
+[08:30:27.719] New key frame 
+[08:30:27.719] BA loop run 
+[08:30:27.719] init error: -nan 
+[08:30:27.720] end error: nan 
+[08:30:27.987] New key frame 
+[08:30:27.987] BA loop run 
+[08:30:27.987] init error: -nan 
+[08:30:27.987] end error: nan 
+[08:30:28.565] New key frame 
+[08:30:28.565] BA loop run 
+[08:30:28.565] init error: -nan 
+[08:30:28.566] end error: nan 
+[08:30:28.682] New key frame 
+[08:30:28.682] BA loop run 
+[08:30:28.683] init error: -nan 
+[08:30:28.683] end error: nan 
+[08:30:28.807] New key frame 
+[08:30:28.807] BA loop run 
+[08:30:28.808] init error: -nan 
+[08:30:28.808] end error: nan 
+[08:30:28.912] New key frame 
+[08:30:28.913] BA loop run 
+[08:30:28.91

GPU_version/vio/include/vio/bundle_adjustment.h

@@ -15,7 +15,7 @@ namespace g2o {
 
 namespace vio {
 
-    typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
+    typedef EdgeProjectP2MC g2oEdgeSE3;
     typedef g2o::VertexSE2Expmap g2oFrameSE2;
     typedef g2o::VertexSBAPointXYZ g2oPoint;
 

GPU_version/vio/include/vio/frame.h

@@ -50,7 +50,7 @@ namespace vio {
         Features                      fts_;                   //!< List of features in the image.
         vector<std::shared_ptr<Feature>>  key_pts_;               //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view.
         bool                          is_keyframe_;           //!< Was this frames selected as keyframe?
-        g2o::VertexSE3Expmap*    v_kf_=nullptr;                  //!< Temporary pointer to the g2o node object of the keyframe.
+        g2o::VertexCam*    v_kf_=nullptr;                  //!< Temporary pointer to the g2o node object of the keyframe.
         int                           last_published_ts_;     //!< Timestamp of last publishing.
 
         Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp);

GPU_version/vio/include/vio/global_optimizer.h

@@ -31,10 +31,9 @@
 #include <g2o/core/robust_kernel_impl.h>
 #include <g2o/core/optimization_algorithm_levenberg.h>
 #include <g2o/solvers/cholmod/linear_solver_cholmod.h>
-#include <g2o/solvers/csparse/linear_solver_csparse.h>
-#include <g2o/solvers/dense/linear_solver_dense.h>
 #include <g2o/solvers/structure_only/structure_only_solver.h>
 #include <g2o/core/optimization_algorithm_factory.h>
+#include <g2o/types/icp/types_icp.h>
 #include <g2o/stuff/sampler.h>
 #include <memory>
 
@@ -79,13 +78,13 @@ protected:
 #endif
         /// Temporary container to hold the g2o edge with reference to frame and point.
         struct EdgeContainerSE3{
-            g2o::EdgeProjectXYZ2UV*     edge;
+            EdgeProjectP2MC*     edge;
             std::shared_ptr<Frame>          frame;
             std::shared_ptr<Feature>        feature;
         };
 
 /// Create a g2o vertice from a keyframe object.
-        g2o::VertexSE3Expmap* createG2oFrameSE3(
+        g2o::VertexCam* createG2oFrameSE3(
                 FramePtr kf,
                 size_t id,
                 bool fixed);
@@ -95,8 +94,8 @@ protected:
                 size_t id,
                 bool fixed);
   /// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
-        g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3(
+        g2o::EdgeProjectP2MC* createG2oEdgeSE3(
-                g2o::VertexSE3Expmap* v_kf,
+                g2o::VertexCam* v_kf,
                 g2o::VertexSBAPointXYZ* v_mp,
                 const Vector2d& f_up,
                 bool robust_kernel,

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/src/frame_handler_mono.cpp

@@ -132,7 +132,7 @@ 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
-  ba_glob_->new_key_frame();
+  //ba_glob_->new_key_frame();
   ROS_INFO("VIO initialized :)");
   ROS_INFO("Running ...");
   return RESULT_IS_KEYFRAME;
@@ -239,7 +239,7 @@ bool FrameHandlerMono::needNewKf()
     fprintf(log_,"[%s] need key frame pitch dis: %f translation dif:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
             fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()),(closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm());
 #endif
-  if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.1 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.1)return true;
+  if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.05 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.05)return true;
   return false;
 }
 void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){

GPU_version/vio/src/global_optimizer.cpp

@@ -24,6 +24,7 @@
 #include <vio/point.h>
 #include <vio/feature.h>
 #include <vio/config.h>
+#include <g2o/solvers/structure_only/structure_only_solver.h>
 #if VIO_DEBUG
 #include <sys/types.h>
 #include <sys/stat.h>
@@ -103,7 +104,7 @@ namespace vio {
                 it_kf != map_.keyframes_.end(); ++it_kf)
             {
                 // New Keyframe Vertex
-               g2o::VertexSE3Expmap* v_kf = createG2oFrameSE3(*it_kf, v_id++, false);
+               g2o::VertexCam* v_kf = createG2oFrameSE3(*it_kf, v_id++, false);
                 (*it_kf)->v_kf_ = v_kf;
                 optimizer.addVertex(v_kf);
                 for(auto&& it_ftr:(*it_kf)->fts_)
@@ -111,6 +112,7 @@ namespace vio {
                     if(it_ftr->point==NULL)continue;
                     // for each keyframe add edges to all observed mapoints
                     std::shared_ptr<Point> mp = it_ftr->point;
+                    if(mp->pos_.hasNaN())continue;
                     g2o::VertexSBAPointXYZ* v_mp = mp->v_pt_;
                     if(v_mp == NULL)
                     {
@@ -127,7 +129,8 @@ namespace vio {
                         incorrect_edges.push_back(pair<FramePtr,std::shared_ptr<Feature>>(*it_kf, it_ftr));
                     else
                     {
-                        g2o::EdgeProjectXYZ2UV* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f),
+                        g2o::Edge_XYZ_VSC e1;
+                        g2o::EdgeProjectP2MC* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f),
                                                                          true,
                                                                          Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth());
                         EdgeContainerSE3 edge;
@@ -143,6 +146,17 @@ namespace vio {
             // Optimization
             optimizer.initializeOptimization();
             optimizer.computeActiveErrors();
+
+            g2o::StructureOnlySolver<3> structure_only_ba;
+            g2o::OptimizableGraph::VertexContainer points;
+            for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
+                g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second);
+                if (v->dimension() == 3)
+                    points.push_back(v);
+            }
+
+            structure_only_ba.calc(points, 10);
+/*
 #if VIO_DEBUG
             fprintf(log_,"[%s] init error: %f \n",
                     vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
@@ -151,7 +165,7 @@ namespace vio {
 #if VIO_DEBUG
             fprintf(log_,"[%s] end error: %f \n",
                     vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
-#endif
+#endif*/
             // Update Keyframe and MapPoint Positions
             for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
                 it_kf != map_.keyframes_.end();++it_kf)
@@ -161,9 +175,12 @@ namespace vio {
                 (*it_kf)->v_kf_ = NULL;
                 for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
                 {
-                    if((*it_ftr)->point == NULL)
-                        continue;
                     std::shared_ptr<Point> mp = (*it_ftr)->point;
+                    if(mp == NULL)
+                        continue;
+                    if(mp->v_pt_ == NULL)
+                        continue;       // mp was updated before
+                    mp->pos_ = mp->v_pt_->estimate();
                     mp->v_pt_ = NULL;
                 }
             }
@@ -184,13 +201,20 @@ namespace vio {
         }
     }
 
-   g2o::VertexSE3Expmap*
+   g2o::VertexCam*
    BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed)
    {
-       g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
+        if(frame->id_==0 || frame->id_==1){
+            g2o::VertexCam* v= new g2o::VertexCam();
+            v->setId(id);
+            v->setToOrigin();
+            v->setFixed(true);
+            v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
+        }
+       g2o::VertexCam* v= new g2o::VertexCam();
        v->setId(id);
        v->setFixed(false);
-       v->setEstimate(g2o::SE3Quat(frame->T_f_w_.se3().unit_quaternion(), frame->T_f_w_.se3().translation()));
+       v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
        return v;
    }
 
@@ -201,25 +225,25 @@ namespace vio {
    {
        g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
        v->setId(id);
-       v->setFixed(fixed);
+       //v->setFixed(fixed);
        v->setMarginalized(true);
        v->setEstimate(pos);
        return v;
 
    }
 
-   g2o::EdgeProjectXYZ2UV* BA_Glob::createG2oEdgeSE3( g2o::VertexSE3Expmap* v_frame,
+    g2o::EdgeProjectP2MC* BA_Glob::createG2oEdgeSE3( g2o::VertexCam* v_frame,
                      g2o::VertexSBAPointXYZ* v_point,
                      const Vector2d& f_up,
                      bool robust_kernel,
                      double huber_width,
                      double weight)
    {
-       g2o::EdgeProjectXYZ2UV* e= new g2o::EdgeProjectXYZ2UV;
+       g2o::EdgeProjectP2MC* e= new g2o::EdgeProjectP2MC;
        e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
        e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
        e->setMeasurement(f_up);
-       e->information() = weight*Eigen::Matrix2d::Identity();
+       e->information() = Eigen::Matrix2d::Identity();
        g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
        rk->setDelta(huber_width);
        e->setRobustKernel(rk);