remove logs, trying to add more key frames by setting some situation for depth mean

GPU_version/svo/include/gpu_svo/global.h

@@ -62,6 +62,13 @@ namespace svo
                -sin_theta_,0.0,cos_theta_;
             T=new SE3(R,Vector3d(X_,0.0,Z_));
         };
+        void set(){
+            X_=T->translation().x();
+            Z_=T->translation().z();
+            tetha_=atan(T->matrix()(0,2)/T->matrix()(0,0));
+            cos_theta_=cos(tetha_);
+            sin_theta_=sin(tetha_);
+        }
         ~SE2_5(){}
   };
 

GPU_version/svo/src/frame_handler_mono.cpp

@@ -137,24 +137,32 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
 FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 {
   SE3 tem=imu_integPtr_->preintegrate_predict();
-  new_frame_->T_f_w_ = SE2_5(tem.translation().x(),tem.translation().z(),atan(tem.rotation_matrix()(0,2)/tem.rotation_matrix()(0,0)));
+  if((new_frame_->T_f_w_.T->translation()-tem.translation()).norm()>0.5){
+      new_frame_->T_f_w_ = SE2_5(tem.translation().x(),tem.translation().z(),atan(tem.rotation_matrix()(0,2)/tem.rotation_matrix()(0,0)));
+  }else{
+      new_frame_->T_f_w_ = last_frame_->T_f_w_;
+  }
+  new_frame_->T_f_w_.set();
   //new_frame_->T_f_w_ = last_frame_->T_f_w_;
   // sparse image align
   SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
                            30, SparseImgAlign::LevenbergMarquardt, false, false);
   img_align.run(last_frame_, new_frame_);
+  new_frame_->T_f_w_.set();
   // map reprojection & feature alignment
   reprojector_.reprojectMap(new_frame_, overlap_kfs_);
   size_t sfba_n_edges_final=0;
-
+  new_frame_->T_f_w_.set();
   double sfba_thresh, sfba_error_init, sfba_error_final;
   if(!imu_integPtr_->predict(new_frame_,last_frame_,sfba_n_edges_final,reprojector_.n_matches_)){
         return RESULT_FAILURE;
   }
+  new_frame_->T_f_w_.set();
   pose_optimizer::optimizeGaussNewton(
-            Config::poseOptimThresh(), Config::poseOptimNumIter(), true,
+            Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
             new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
 
+  new_frame_->T_f_w_.set();
   std::cerr<<"Reprojection Map: "<<"\t nPoint:"<<overlap_kfs_.back().second
   <<"\t nCell = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_
   <<" \t Reprojected points after opmization: "<<sfba_n_edges_final<<'\n';
@@ -168,6 +176,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
   //SVO_STOP_TIMER("point_optimizer");
 
+  new_frame_->T_f_w_.set();
   // select keyframe
   core_kfs_.insert(new_frame_);
   setTrackingQuality(sfba_n_edges_final);
@@ -176,20 +185,26 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
     return RESULT_FAILURE;
   }
   double depth_mean, depth_min;
+
+  new_frame_->T_f_w_.set();
   frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
-  if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)
+
+  new_frame_->T_f_w_.set();
+  if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)//edited
   {
     depth_filter_->addFrame(new_frame_);
     return RESULT_NO_KEYFRAME;
   }
   new_frame_->setKeyframe();
 
+    new_frame_->T_f_w_.set();
   // new keyframe selected
   for(auto&& it:new_frame_->fts_){
       if(it->point != NULL)it->point->addFrameRef(it);
   }
   map_.point_candidates_.addCandidatePointToFrame(new_frame_);
 
+    new_frame_->T_f_w_.set();
   // init new depth-filters
   depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
 
@@ -201,6 +216,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
     map_.safeDeleteFrame(furthest_frame);
   }
 
+    new_frame_->T_f_w_.set();
   // add keyframe to map
   map_.addKeyframe(new_frame_);
   std::cerr<<"Svo pipline was finished\n";
@@ -274,6 +290,7 @@ void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame)
 
 bool FrameHandlerMono::needNewKf(double scene_depth_mean)
 {
+  if(scene_depth_mean >10.0)return true;
   for(auto&& it:overlap_kfs_)
   {
     Vector3d relpos = new_frame_->w2f(it.first->pos());

GPU_version/svo/src/imu_integration.cpp

@@ -15,7 +15,7 @@
 #include <vikit/params_helper.h>
 #include <gpu_svo/config.h>
 #include <gpu_svo/for_it.hpp>
-#define debug 1
+#define debug 0
 Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
     valuesPtr=std::make_shared<gtsam::Values>();
@@ -52,9 +52,9 @@ bool Imu_Integration::update(double* imu){
     auto in=std::chrono::steady_clock::now();
     if(std::chrono::duration<double>(in-t_1).count()>20.0){
         t_1=in;
-        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0]-svo::Config::X_off(),0.0,-1.0*imu[1]-svo::Config::Y_off()),Eigen::Vector3d(0.0,imu[5]-svo::Config::Teta_off(),0.0),0.005);
+        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(-1.0*(imu[0]-svo::Config::X_off()),0.0,-1.0*(imu[1]-svo::Config::Y_off())),Eigen::Vector3d(0.0,imu[5]-svo::Config::Teta_off(),0.0),0.005);
     }else{
-        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0]-svo::Config::X_off(),0.0,-1.0*imu[1]-svo::Config::Y_off()),Eigen::Vector3d(0.0,imu[5]-svo::Config::Teta_off(),0.0),std::chrono::duration<double>(in-t_1).count());
+        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(-1.0*(imu[0]-svo::Config::X_off()),0.0,-1.0*(imu[1]-svo::Config::Y_off())),Eigen::Vector3d(0.0,imu[5]-svo::Config::Teta_off(),0.0),std::chrono::duration<double>(in-t_1).count());
         t_1=in;
         ++imu_n;
     }
@@ -98,13 +98,16 @@ bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost:
     ++imu_factor_id;
     syn=false;
     gtsam::Marginals marginals(*graphPtr,result);
-    if(debug)fprintf(logtime, "%f,%f,%f,%f,%f,%f,\n",
+    if(debug)fprintf(logtime, "%f,%f,%f,%f,%f,%f,%f,%f,\n",
                      new_frame->T_f_w_.T->translation().x(),
                      new_frame->T_f_w_.T->translation().y(),
                      new_frame->T_f_w_.T->translation().z(),
+                     atan(new_frame->T_f_w_.T->rotation_matrix()(0,2)/new_frame->T_f_w_.T->rotation_matrix()(0,0)),
                      statePtr->pose().x(),
                      statePtr->pose().y(),
-                     statePtr->pose().z());
+                     statePtr->pose().z(),
+                     atan(statePtr->R()(0,2)/statePtr->R()(0,0)));
+    if((statePtr->t()-new_frame->T_f_w_.T->translation()).norm()>2.0)return false;
     new_frame->T_f_w_=svo::SE2_5(statePtr->t().x(),statePtr->t().z(),atan(statePtr->R()(0,2)/statePtr->R()(0,0)));
     new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1));
     graphPtr->resize(0);
@@ -147,7 +150,7 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,
                                                 P(imu_factor_id));
     gtsam::LevenbergMarquardtParams params0;
     //params0.verbosity=gtsam::LevenbergMarquardtParams::LINEAR;
-    params0.verbosityLM = gtsam::LevenbergMarquardtParams::SUMMARY;
+    params0.verbosityLM = gtsam::LevenbergMarquardtParams::SILENT;
     params0.setlambdaFactor(10.0);
     params0.setlambdaInitial(100);
     params0.setlambdaUpperBound(1e10);

GPU_version/svo/src/pose_optimizer.cpp

@@ -98,7 +98,6 @@ void optimizeGaussNewton(
       b.noalias() -= J.transpose()*e*weight;
       new_chi2 += e.squaredNorm()*weight;
     }
-
     // solve linear system
     const Vector6d dT(A.ldlt().solve(b));
 

GPU_version/svo/src/reprojector.cpp

@@ -72,7 +72,7 @@ void Reprojector::reprojectMap(
   list< pair<FramePtr,double> > close_kfs;
   map_.getCloseKeyframes(frame, close_kfs);
 
-  std::cout<<"The number of closed frames: "<<std::to_string(close_kfs.size())<<'\n';
+  //std::cout<<"The number of closed frames: "<<std::to_string(close_kfs.size())<<'\n';
   // Sort KFs with overlap according to their closeness
   close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
                  boost::bind(&std::pair<FramePtr, double>::second, _2));

GPU_version/vio/param/vo_fast.yaml

@@ -17,6 +17,6 @@ svo:
   EBP: 0.1                #error in the bias used for preintegration
   NRP: 0.001                #noise used for projection in preintegration
   gravity: -9.65
-  Acceleration_offset_x: -0.05
+  Acceleration_offset_x: 0.0 #-0.05
-  Acceleration_offset_y: -0.04
+  Acceleration_offset_y: 0.0 #-0.04
-  Gyroscope_offset_z: 0.001
+  Gyroscope_offset_z: 0.0 #0.001

GPU_version/vio/src/visualizer.cpp

@@ -65,10 +65,10 @@ void Visualizer::publishMinimal(
     msg_pose->pose.pose.position.x = p[0];
     msg_pose->pose.pose.position.y = p[1];
     msg_pose->pose.pose.position.z = p[2];
-    msg_pose->pose.pose.orientation.x = q.x();
+    msg_pose->pose.pose.orientation.x = q.x()/q.norm();
-    msg_pose->pose.pose.orientation.y = q.y();
+    msg_pose->pose.pose.orientation.y = q.y()/q.norm();
-    msg_pose->pose.pose.orientation.z = q.z();
+    msg_pose->pose.pose.orientation.z = q.z()/q.norm();
-    msg_pose->pose.pose.orientation.w = q.w();
+    msg_pose->pose.pose.orientation.w = q.w()/q.norm();
     for(size_t i=0; i<36; ++i)
       msg_pose->pose.covariance[i] = Cov(i%6, i/6);
     pub_pose_.publish(msg_pose);

GPU_version/vio/src/vo_node.cpp

@@ -105,12 +105,12 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
 }
 void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
     double imu_in[6];
-    imu_in[0] = 0.1*imu_[0]+0.9*imu->linear_acceleration.x;
+    imu_in[0] = 0.4*imu_[0]+0.6*imu->linear_acceleration.x;
-    imu_in[1] = 0.1*imu_[1]+0.9*imu->linear_acceleration.y;
+    imu_in[1] = 0.4*imu_[1]+0.6*imu->linear_acceleration.y;
-    imu_in[2] = 0.1*imu_[2]+0.9*imu->linear_acceleration.z;
+    imu_in[2] = 0.4*imu_[2]+0.6*imu->linear_acceleration.z;
-    imu_in[3] = 0.1*imu_[3]+0.9*imu->angular_velocity.x;
+    imu_in[3] = 0.4*imu_[3]+0.6*imu->angular_velocity.x;
-    imu_in[4] = 0.1*imu_[4]+0.9*imu->angular_velocity.y;
+    imu_in[4] = 0.4*imu_[4]+0.6*imu->angular_velocity.y;
-    imu_in[5] = 0.1*imu_[5]+0.9*imu->angular_velocity.z;
+    imu_in[5] = 0.4*imu_[5]+0.6*imu->angular_velocity.z;
     memcpy(imu_, imu_in, static_cast<std::size_t>(6*sizeof(double)));
     vo_->imu_integPtr_->update(imu_in);
 }