corrent situation

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::VertexCam*    v_kf_=nullptr;                  //!< Temporary pointer to the g2o node object of the keyframe.
+        g2o::VertexSE3Expmap*    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

@@ -84,7 +84,7 @@ protected:
         };
 
 /// Create a g2o vertice from a keyframe object.
-        g2o::VertexCam* createG2oFrameSE3(
+        g2o::VertexSE3Expmap* createG2oFrameSE3(
                 FramePtr kf,
                 size_t id,
                 bool fixed);
@@ -95,7 +95,7 @@ protected:
                 bool fixed);
   /// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
         g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3(
-                g2o::VertexCam* v_kf,
+                g2o::VertexSE3Expmap* v_kf,
                 g2o::VertexSBAPointXYZ* v_mp,
                 const Vector2d& f_up,
                 bool robust_kernel,

GPU_version/vio/param/vo_fast.yaml

@@ -13,6 +13,6 @@ vio:
   ACC_ekf: 0.2          #acc white noise in continuous in EKF
   GYO_ekf: 0.5         #gyro white noise in continuous in EKF
   VO_ekf_translation: 1.0              #VO noise  in EKF
-  VO_ekf_orientation: 0.05              #VO noise  in EKF
+  VO_ekf_orientation: 1.5              #VO noise  in EKF
   CMD_ekf_translation: 0.1  #control vector noise in EKF
   CMD_ekf_orientation: 0.01  #control vector noise in EKF

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;

GPU_version/vio/src/global_optimizer.cpp

@@ -104,7 +104,7 @@ namespace vio {
                 it_kf != map_.keyframes_.end(); ++it_kf)
             {
                 // New Keyframe Vertex
-               g2o::VertexCam* v_kf = createG2oFrameSE3(*it_kf, v_id++, false);
+               g2o::VertexSE3Expmap* 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_)
@@ -200,17 +200,17 @@ namespace vio {
         }
     }
 
-   g2o::VertexCam*
+   g2o::VertexSE3Expmap*
    BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed)
    {
-        if(frame->id_==0 || frame->id_==1){
-            g2o::VertexCam* v= new g2o::VertexCam();
+/*        if(frame->id_==0 || frame->id_==1){
+            g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
             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();
+        }*/
+       g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
        v->setId(id);
        v->setFixed(false);
        v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
@@ -224,14 +224,14 @@ 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::VertexCam* v_frame,
+    g2o::EdgeProjectXYZ2UV* BA_Glob::createG2oEdgeSE3( g2o::VertexSE3Expmap* v_frame,
                      g2o::VertexSBAPointXYZ* v_point,
                      const Vector2d& f_up,
                      bool robust_kernel,