added full switch

include/msckf_vio/msckf_vio.h

@@ -202,7 +202,7 @@ class MsckfVio {
         Eigen::Vector4d& r);
     // This function computes the Jacobian of all measurements viewed
     // in the given camera states of this feature.
-    void featureJacobian(
+    bool featureJacobian(
         const FeatureIDType& feature_id,
         const std::vector<StateIDType>& cam_state_ids,
         Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
@@ -246,7 +246,7 @@ class MsckfVio {
         Eigen::VectorXd& r);
 
 
-    void twodotFeatureJacobian(
+    bool twodotFeatureJacobian(
         const FeatureIDType& feature_id,
         const std::vector<StateIDType>& cam_state_ids,
         Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
@@ -283,6 +283,7 @@ class MsckfVio {
     bool nan_flag;
     bool play;
     double last_time_bound;
+    double time_offset;
 
     // Patch size for Photometry
     int N;

launch/image_processor_tinytum.launch

@@ -18,7 +18,7 @@
       <param name="grid_row" value="4"/>
       <param name="grid_col" value="4"/>
       <param name="grid_min_feature_num" value="3"/>
-      <param name="grid_max_feature_num" value="4"/>
+      <param name="grid_max_feature_num" value="5"/>
       <param name="pyramid_levels" value="3"/>
       <param name="patch_size" value="15"/>
       <param name="fast_threshold" value="10"/>
@@ -27,9 +27,9 @@
       <param name="ransac_threshold" value="3"/>
       <param name="stereo_threshold" value="5"/>
 
-      <remap from="~imu" to="/imu02"/>
+      <remap from="~imu" to="/imu0"/>
-      <remap from="~cam0_image" to="/cam0/image_raw2"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
-      <remap from="~cam1_image" to="/cam1/image_raw2"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
 
 
     </node>

launch/msckf_vio_tinytum.launch

@@ -17,8 +17,7 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
-      <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
+      <param name="FILTER" value="0"/>
-      <param name="FILTER" value="1"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
@@ -33,7 +32,7 @@
       <param name="frame_rate" value="20"/>
       <param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
       <param name="child_frame_id" value="odom"/>
-      <param name="max_cam_state_size" value="20"/>
+      <param name="max_cam_state_size" value="12"/>
       <param name="position_std_threshold" value="8.0"/>
 
       <param name="rotation_threshold" value="0.2618"/>

src/msckf_vio.cpp

@@ -50,7 +50,7 @@ Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity();
 
 // Static member variables in Feature class.
 FeatureIDType Feature::next_id = 0;
-double Feature::observation_noise = 0.05;
+double Feature::observation_noise = 0.01;
 Feature::OptimizationConfig Feature::optimization_config;
 
 map<int, double> MsckfVio::chi_squared_test_table;
@@ -425,6 +425,7 @@ void MsckfVio::imageCallback(
   // the origin.
   if (is_first_img) {
     is_first_img = false;
+    time_offset = feature_msg->header.stamp.toSec();
     state_server.imu_state.time = feature_msg->header.stamp.toSec();
   }
   static double max_processing_time = 0.0;
@@ -786,7 +787,7 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) {
 void MsckfVio::calcErrorState()
 {
 
-  // imu error
+  // imu "error"
   err_state_server.imu_state.id = state_server.imu_state.id;
   err_state_server.imu_state.time = state_server.imu_state.time;
 
@@ -832,9 +833,9 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
 
   // Counter how many IMU msgs in the buffer are used.
   int used_truth_msg_cntr = 0;
-
+  double truth_time;
   for (const auto& truth_msg : truth_msg_buffer) {
-    double truth_time = truth_msg.header.stamp.toSec();
+    truth_time = truth_msg.header.stamp.toSec();
     if (truth_time < true_state_server.imu_state.time) {
       ++used_truth_msg_cntr;
       continue;
@@ -854,6 +855,7 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
     // Execute process model.
     processTruthtoIMU(truth_time, m_rotation, m_translation);
     ++used_truth_msg_cntr;
+
   }
   last_time_bound = time_bound;
 
@@ -864,6 +866,31 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
   truth_msg_buffer.erase(truth_msg_buffer.begin(),
       truth_msg_buffer.begin()+used_truth_msg_cntr);
 
+  std::ofstream logfile;
+
+  int FileHandler;
+  char FileBuffer[1024];
+  float load = 0;
+
+  FileHandler = open("/proc/loadavg", O_RDONLY);
+  if( FileHandler >= 0) {
+    auto file  = read(FileHandler, FileBuffer, sizeof(FileBuffer) - 1);
+    sscanf(FileBuffer, "%f", &load);
+    close(FileHandler);
+  }
+
+  auto gt = true_state_server.imu_state.position;
+  auto gr = true_state_server.imu_state.orientation;
+  logfile.open("/home/raphael/dev/MSCKF_ws/bag/log.txt", std::ios_base::app);
+  //ros time, cpu load , ground truth, estimation, ros time
+  logfile << true_state_server.imu_state.time - time_offset << "; " << load << "; ";
+  logfile << gt[0] << "; " << gt[1] << "; " << gt[2] << "; " << gr[0] << "; " << gr[1] << "; " << gr[2] << "; " << gr[3] << ";";
+
+  // estimation
+  auto et = state_server.imu_state.position;
+  auto er = state_server.imu_state.orientation;
+  logfile << et[0] << "; "  << et[1] << "; "  << et[2] << "; " << er[0] << "; " << er[1] << "; " << er[2] << "; " << er[3] << "; \n" << endl;;  
+  logfile.close();
   /*
   // calculate change
   delta_position = state_server.imu_state.position - old_imu_state.position;
@@ -1459,12 +1486,14 @@ void MsckfVio::twodotMeasurementJacobian(
   return;
 }
 
-void MsckfVio::twodotFeatureJacobian(
+bool MsckfVio::twodotFeatureJacobian(
     const FeatureIDType& feature_id,
     const std::vector<StateIDType>& cam_state_ids,
     MatrixXd& H_x, VectorXd& r) 
 {
-
+  if(FILTER != 2)
+    return false;
+ 
   const auto& feature = map_server[feature_id];
   
 
@@ -1561,7 +1590,7 @@ void MsckfVio::twodotFeatureJacobian(
     std::cout << "resume playback" << std::endl;
     nh.setParam("/play_bag", true);
   }
-  return;
+  return true;
 }
 
 
@@ -1891,6 +1920,8 @@ bool MsckfVio::PhotometricFeatureJacobian(
     const std::vector<StateIDType>& cam_state_ids,
     MatrixXd& H_x, VectorXd& r) 
 {
+  if(FILTER != 1)
+    return false;
 
   const auto& feature = map_server[feature_id];
 
@@ -2101,12 +2132,15 @@ void MsckfVio::measurementJacobian(
   return;
 }
 
-void MsckfVio::featureJacobian(
+bool MsckfVio::featureJacobian(
     const FeatureIDType& feature_id,
     const std::vector<StateIDType>& cam_state_ids,
     MatrixXd& H_x, VectorXd& r) 
 {
-    // stop playing bagfile if printing images
+  
+
+  if(FILTER != 0)
+    return false;
 
   const auto& feature = map_server[feature_id];
 
@@ -2239,7 +2273,7 @@ void MsckfVio::featureJacobian(
     myfile.close();
   }
 
-  return;
+  return true;
 
 }
 
@@ -2625,37 +2659,10 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof,
 
   double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
 
-  if(gamma > 1000000)
-  {
-    cout << " logging " << endl;
-    ofstream myfile;
-    myfile.open("/home/raphael/dev/octave/log2octave");
-    myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
-           << "# name: H\n"
-           << "# type: matrix\n"
-           << "# rows: " << H.rows() << "\n"
-           << "# columns: " << H.cols() << "\n"
-           << H << endl;
-
-    myfile << "# name: r\n"
-           << "# type: matrix\n"
-           << "# rows: " << r.rows() << "\n"  
-           << "# columns: " << 1 << "\n"
-           << r << endl;
-
-    myfile << "# name: C\n"
-           << "# type: matrix\n"
-           << "# rows: " << state_server.state_cov.rows() << "\n"  
-           << "# columns: " << state_server.state_cov.cols() << "\n"
-           << state_server.state_cov << endl;
-    myfile.close();
-  }
-
   if (chi_squared_test_table[dof] == 0)
       return false;
   if (gamma < chi_squared_test_table[dof]) {
     // cout << "passed" << endl;
-    if(filter == 1)
     cout << "gate: " << dof << " " << gamma << " " <<
       chi_squared_test_table[dof] << endl;
     return true;
@@ -2769,20 +2776,25 @@ void MsckfVio::removeLostFeatures() {
       }
     }
     
-    featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
+    if(featureJacobian(feature.id, cam_state_ids, H_xj, r_j))
-    twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
+    {
-
+      if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) {
-    if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) {
+        H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
-      H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
+        r.segment(stack_cntr, r_j.rows()) = r_j;
-      r.segment(stack_cntr, r_j.rows()) = r_j;
+        stack_cntr += H_xj.rows();
-      stack_cntr += H_xj.rows();
+      }  
-    }  
-
-    if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) {
-      twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
-      twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
-      twostack_cntr += twoH_xj.rows();
     }
+
+    if(twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j))
+    {
+      if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) {
+        twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
+        twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
+        twostack_cntr += twoH_xj.rows();
+      }  
+    }
+
+    
     
     // Put an upper bound on the row size of measurement Jacobian,
     // which helps guarantee the executation time.
@@ -2795,16 +2807,22 @@ void MsckfVio::removeLostFeatures() {
     pr.conservativeResize(pstack_cntr);
     photometricMeasurementUpdate(pH_x, pr);
   }
-  H_x.conservativeResize(stack_cntr, H_x.cols());
+
-  r.conservativeResize(stack_cntr);
+  if(stack_cntr)
+  {
+    H_x.conservativeResize(stack_cntr, H_x.cols());
+    r.conservativeResize(stack_cntr);
+    measurementUpdate(H_x, r);
  
-  twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
+  }
-  twor.conservativeResize(twostack_cntr);
+  if(twostack_cntr)
-  
+  {  
-  // Perform the measurement update step.
+    twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
-  measurementUpdate(H_x, r);
+    twor.conservativeResize(twostack_cntr);
-  twoMeasurementUpdate(twoH_x, twor);
+    twoMeasurementUpdate(twoH_x, twor);
-  
+  }
+ 
+
   // Remove all processed features from the map.
   for (const auto& feature_id : processed_feature_ids)
     map_server.erase(feature_id);
@@ -2951,20 +2969,23 @@ void MsckfVio::pruneLastCamStateBuffer()
       }
     }
     
-    featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
+    if(featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j))
-    twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
+    {
-
       if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
-      H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
+        H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
-      r.segment(stack_cntr, r_j.rows()) = r_j;
+        r.segment(stack_cntr, r_j.rows()) = r_j;
-      stack_cntr += H_xj.rows();
+        stack_cntr += H_xj.rows();
-      pruned_cntr++;
+        pruned_cntr++;
       }
-    
+    }
-    if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
+
-      twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
+    if(twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j))
-      twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
+    {
-      twostack_cntr += twoH_xj.rows();
+      if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
+        twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
+        twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
+        twostack_cntr += twoH_xj.rows();
+      } 
     }
 
     for (const auto& cam_id : involved_cam_state_ids)
@@ -2978,16 +2999,20 @@ void MsckfVio::pruneLastCamStateBuffer()
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-  H_x.conservativeResize(stack_cntr, H_x.cols());
+  if(stack_cntr)
-  r.conservativeResize(stack_cntr);
+  {  
- 
+    H_x.conservativeResize(stack_cntr, H_x.cols());
-  twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
+    r.conservativeResize(stack_cntr);
-  twor.conservativeResize(twostack_cntr);
+    measurementUpdate(H_x, r);
- 
+  }
-  // Perform the measurement update step.
-  measurementUpdate(H_x, r);
-  twoMeasurementUpdate(twoH_x, twor);
   
+  if(twostack_cntr)
+  {  
+    twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
+    twor.conservativeResize(twostack_cntr);
+    twoMeasurementUpdate(twoH_x, twor);
+  }
+
   //reduction
   int cam_sequence = std::distance(state_server.cam_states.begin(),
       state_server.cam_states.find(rm_cam_state_id));
@@ -3128,21 +3153,22 @@ void MsckfVio::pruneCamStateBuffer() {
         pstack_cntr += pH_xj.rows();
       }
     }
-    featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
+    if(featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j))
-    twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
+    {
-
+      if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
-    if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
+        H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
-      H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
+        r.segment(stack_cntr, r_j.rows()) = r_j;
-      r.segment(stack_cntr, r_j.rows()) = r_j;
+        stack_cntr += H_xj.rows();
-      stack_cntr += H_xj.rows();
+      }
     }
-
+    if(twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j))
-    if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
+    {
-      twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
+      if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
-      twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
+        twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
-      twostack_cntr += twoH_xj.rows();
+        twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
+        twostack_cntr += twoH_xj.rows();
+      }
     }
-    
 
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);
@@ -3156,16 +3182,20 @@ void MsckfVio::pruneCamStateBuffer() {
     photometricMeasurementUpdate(pH_x, pr);
   }
 
-  H_x.conservativeResize(stack_cntr, H_x.cols());
+  if(stack_cntr)
-  r.conservativeResize(stack_cntr);
+  {  
- 
+    H_x.conservativeResize(stack_cntr, H_x.cols());
-  twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
+    r.conservativeResize(stack_cntr);
-  twor.conservativeResize(twostack_cntr);
+    measurementUpdate(H_x, r);
-  
+  }
-  // Perform the measurement update step.
+
-  measurementUpdate(H_x, r);
+  if(stack_cntr)
-  twoMeasurementUpdate(twoH_x, twor);
+  {  
-  
+    twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
+    twor.conservativeResize(twostack_cntr);
+    twoMeasurementUpdate(twoH_x, twor);
+  }
+
   //reduction
   for (const auto& cam_id : rm_cam_state_ids) {
     int cam_sequence = std::distance(state_server.cam_states.begin(),