added some debug output, added qr decomp

include/msckf_vio/msckf_vio.h

@@ -236,6 +236,7 @@ class MsckfVio {
     bool PHOTOMETRIC;
 
     // debug flag
+    bool STREAMPAUSE;
     bool PRINTIMAGES;
     bool GROUNDTRUTH;
 

launch/msckf_vio_tum.launch

@@ -18,13 +18,14 @@
       output="screen">
 
       <!-- Photometry Flag-->
-      <param name="PHOTOMETRIC" value="true"/>
+      <param name="PHOTOMETRIC" value="false"/>
 
       <!-- Debugging Flaggs -->
+      <param name="StreamPause" value="true"/>
       <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="5"/>
+      <param name="patch_size_n" value="3"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 
@@ -71,4 +72,6 @@
     </node>
   </group>
 
+  <node name="player" pkg="bagcontrol" type="control.py" />
+
 </launch>

src/msckf_vio.cpp

@@ -69,6 +69,7 @@ bool MsckfVio::loadParameters() {
   //Photometry Flag
   nh.param<bool>("PHOTOMETRIC", PHOTOMETRIC, false);
   nh.param<bool>("PrintImages", PRINTIMAGES, false);
+  nh.param<bool>("StreamPause", STREAMPAUSE, false);
   nh.param<bool>("GroundTruth", GROUNDTRUTH, false);
 
   // Frame id
@@ -304,10 +305,10 @@ bool MsckfVio::initialize() {
 
   // Initialize the chi squared test table with confidence
   // level 0.95.
-  for (int i = 1; i < 100; ++i) {
+  for (int i = 1; i < 1000; ++i) {
     boost::math::chi_squared chi_squared_dist(i);
     chi_squared_test_table[i] =
-      boost::math::quantile(chi_squared_dist, 0.2);
+      boost::math::quantile(chi_squared_dist, 0.05);
   }
 
   if (!createRosIO()) return false;
@@ -407,6 +408,10 @@ void MsckfVio::imageCallback(
   // Return if the gravity vector has not been set.
   if (!is_gravity_set) return;
 
+    // stop playing bagfile if printing images
+  if(STREAMPAUSE)
+    nh.setParam("/play_bag", false);
+  
   // Start the system if the first image is received.
   // The frame where the first image is received will be
   // the origin.
@@ -443,6 +448,7 @@ void MsckfVio::imageCallback(
   double imu_processing_time = (
       ros::Time::now()-start_time).toSec();
 
+  //cout << "1" << endl;
   // Augment the state vector.
   start_time = ros::Time::now();
   if(PHOTOMETRIC)
@@ -455,6 +461,8 @@ void MsckfVio::imageCallback(
   double state_augmentation_time = (
       ros::Time::now()-start_time).toSec();
 
+
+  //cout << "2" << endl;
   // Add new observations for existing features or new
   // features in the map server.
   start_time = ros::Time::now();
@@ -462,23 +470,29 @@ void MsckfVio::imageCallback(
   double add_observations_time = (
       ros::Time::now()-start_time).toSec();
 
+
+  //cout << "3" << endl;
   // Add new images to moving window
   start_time = ros::Time::now();
   manageMovingWindow(cam0_img, cam1_img, feature_msg);
   double manage_moving_window_time = (
       ros::Time::now()-start_time).toSec();
 
+
+  //cout << "4" << endl;
   // Perform measurement update if necessary.
   start_time = ros::Time::now();
   removeLostFeatures();
   double remove_lost_features_time = (
       ros::Time::now()-start_time).toSec();
 
+  //cout << "5" << endl;
   start_time = ros::Time::now();
-  pruneLastCamStateBuffer();
+  pruneCamStateBuffer();
   double prune_cam_states_time = (
       ros::Time::now()-start_time).toSec();
 
+  //cout << "6" << endl;
   // Publish the odometry.
   start_time = ros::Time::now();
   publish(feature_msg->header.stamp);
@@ -509,6 +523,8 @@ void MsckfVio::imageCallback(
         publish_time, publish_time/processing_time);
   }
 
+  if(STREAMPAUSE)
+    nh.setParam("/play_bag", true);
   return;
 }
 
@@ -1428,16 +1444,8 @@ void MsckfVio::PhotometricFeatureJacobian(
     MatrixXd& H_x, VectorXd& r) 
 {
 
-  // stop playing bagfile if printing images
-  if(PRINTIMAGES)
-  {
-    std::cout << "stopped playpack" << std::endl;
-    nh.setParam("/play_bag", false);
-  }
-
   const auto& feature = map_server[feature_id];
 
-
   // Check how many camera states in the provided camera
   // id camera has actually seen this feature.
   vector<StateIDType> valid_cam_state_ids(0);
@@ -1538,13 +1546,8 @@ void MsckfVio::PhotometricFeatureJacobian(
     myfile.close();
     cout << "---------- LOGGED -------- " << endl; 
     cvWaitKey(0);
+  }
 
-  }
-  if(PRINTIMAGES)
-  {
-    std::cout << "resume playback" << std::endl;
-    nh.setParam("/play_bag", true);
-  }
   return;
 }
 
@@ -1619,12 +1622,6 @@ void MsckfVio::featureJacobian(
     MatrixXd& H_x, VectorXd& r) 
 {
     // stop playing bagfile if printing images
-  if(PRINTIMAGES)
-  {
-    std::cout << "stopped playpack" << std::endl;
-    nh.setParam("/play_bag", false);
-  }
-
 
   const auto& feature = map_server[feature_id];
 
@@ -1686,9 +1683,6 @@ void MsckfVio::featureJacobian(
    H_x = A.transpose() * H_xj;
    r = A.transpose() * r_j;
 
-
-  cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl;
-
       // stop playing bagfile if printing images
   if(PRINTIMAGES)
   {
@@ -1698,8 +1692,6 @@ void MsckfVio::featureJacobian(
     myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
     myfile.close();
     cout << "---------- LOGGED -------- " << endl; 
-    std::cout << "stopped playpack" << std::endl;
-    nh.setParam("/play_bag", true);
   }
 
   return;
@@ -1708,8 +1700,8 @@ void MsckfVio::featureJacobian(
 
 void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 
-  if (H.rows() == 0 || r.rows() == 0) return;
+   if (H.rows() == 0 || r.rows() == 0)
-
+    return;
   // Decompose the final Jacobian matrix to reduce computational
   // complexity as in Equation (28), (29).
   MatrixXd H_thin;
@@ -1718,7 +1710,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
   if(PHOTOMETRIC)
     augmentationSize = 7;
 
-  /*
+  // QR decomposition to make stuff faster
   if (H.rows() > H.cols()) {
     // Convert H to a sparse matrix.
     SparseMatrix<double> H_sparse = H.sparseView();
@@ -1736,29 +1728,24 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
     H_thin = H_temp.topRows(21+state_server.cam_states.size()*augmentationSize);
     r_thin = r_temp.head(21+state_server.cam_states.size()*augmentationSize);
 
-    //HouseholderQR<MatrixXd> qr_helper(H);
-    //MatrixXd Q = qr_helper.householderQ();
-    //MatrixXd Q1 = Q.leftCols(21+state_server.cam_states.size()*6);
-
-    //H_thin = Q1.transpose() * H;
-    //r_thin = Q1.transpose() * r;
   } else {
     H_thin = H;
     r_thin = r;
   }
-  */
+
 
   // Compute the Kalman gain.
   const MatrixXd& P = state_server.state_cov;
-  MatrixXd S = H*P*H.transpose() +
+  MatrixXd S = H_thin*P*H_thin.transpose() +
       Feature::observation_noise*MatrixXd::Identity(
-        H.rows(), H.rows());
+        H_thin.rows(), H_thin.rows());
   //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P);
-  MatrixXd K_transpose = S.ldlt().solve(H*P);
+  cout << "inverting: " << S.rows() << ":" << S.cols() << endl;
+  MatrixXd K_transpose = S.ldlt().solve(H_thin*P);
   MatrixXd K = K_transpose.transpose();
-
   // Compute the error of the state.
   VectorXd delta_x = K * r;
+  cout << "update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
   // Update the IMU state.
   const VectorXd& delta_x_imu = delta_x.head<21>();
 
@@ -1800,7 +1787,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
   }
 
   // Update state covariance.
-  MatrixXd I_KH = MatrixXd::Identity(K.rows(), H.cols()) - K*H;
+  MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin;
   //state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() +
   //  K*K.transpose()*Feature::observation_noise;
   state_server.state_cov = I_KH*state_server.state_cov;
@@ -1814,7 +1801,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 }
 
 bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
-  return 1;
   MatrixXd P1 = H * state_server.state_cov * H.transpose();
 
 
@@ -1827,7 +1813,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
     chi_squared_test_table[dof] << endl;
 
     if (chi_squared_test_table[dof] == 0)
-      return true;
+      return false;
   if (gamma < chi_squared_test_table[dof]) {
     //cout << "passed" << endl;
     return true;
@@ -1929,11 +1915,6 @@ void MsckfVio::removeLostFeatures() {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
       r.segment(stack_cntr, r_j.rows()) = r_j;
       stack_cntr += H_xj.rows();
-      cout << "passed" << endl;
-    }
-    else
-    {
-      cout << "rejected" << endl;
     }
 
     // Put an upper bound on the row size of measurement Jacobian,
@@ -1941,6 +1922,8 @@ void MsckfVio::removeLostFeatures() {
     if (stack_cntr > 1500) break;
   }
 
+  cout << "processed features: " << processed_feature_ids.size() << endl;
+
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
 
@@ -2000,6 +1983,7 @@ void MsckfVio::findRedundantCamStates(vector<StateIDType>& rm_cam_state_ids) {
 
 void MsckfVio::pruneLastCamStateBuffer()
 {
+
   if (state_server.cam_states.size() < max_cam_state_size)
     return;
 
@@ -2058,7 +2042,7 @@ void MsckfVio::pruneLastCamStateBuffer()
   MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size());
   VectorXd r = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
-
+  int pruned_cntr = 0;
   for (auto& item : map_server) {
     auto& feature = item.second;
     
@@ -2077,24 +2061,26 @@ void MsckfVio::pruneLastCamStateBuffer()
     else
       featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
 
-    if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
+    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;
       r.segment(stack_cntr, r_j.rows()) = r_j;
       stack_cntr += H_xj.rows();
-      cout << "passed" << endl;
+      pruned_cntr++;
-    }
-    else
-    {
-      cout << "rejected" << endl;
     }
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);
   }
+
+  if(pruned_cntr != 0)
+  {
+    cout << "pruned features: " << pruned_cntr << endl;
+
     H_x.conservativeResize(stack_cntr, H_x.cols());
     r.conservativeResize(stack_cntr);
 
     // Perform measurement update.
     measurementUpdate(H_x, r);
+  } 
   
   //reduction
   int cam_sequence = std::distance(state_server.cam_states.begin(),
@@ -2137,6 +2123,7 @@ void MsckfVio::pruneLastCamStateBuffer()
 
 void MsckfVio::pruneCamStateBuffer() {
 
+
   if (state_server.cam_states.size() < max_cam_state_size)
     return;
 
@@ -2228,11 +2215,6 @@ void MsckfVio::pruneCamStateBuffer() {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
       r.segment(stack_cntr, r_j.rows()) = r_j;
       stack_cntr += H_xj.rows();
-      cout << "passed" << endl;
-    }
-    else
-    {
-      cout << "rejected" << endl;
     }
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);