removed incorrect prints

include/msckf_vio/feature.hpp

@@ -329,7 +329,6 @@ void Feature::RhoJacobian(const Eigen::Isometry3d& T_c0_ci,
     double& w) const 
 {
 
-  // Compute hi1, hi2, and hi3 as Equation (37).
   const double& rho = x;
 
   Eigen::Vector3d h = T_c0_ci.linear()*
@@ -1138,10 +1137,6 @@ bool Feature::initializePosition(const CamStateServer& cam_states) {
 
   // Organize camera poses and feature observations properly.
 
-
-initializeRho(cam_states);
-std::cout << "init rho is: " << anchor_rho << std::endl;
-
   std::vector<Eigen::Isometry3d,
     Eigen::aligned_allocator<Eigen::Isometry3d> > cam_poses(0);
   std::vector<Eigen::Vector2d,

include/msckf_vio/msckf_vio.h

@@ -226,6 +226,8 @@ class MsckfVio {
     void removeLostFeatures();
     void findRedundantCamStates(
         std::vector<StateIDType>& rm_cam_state_ids);
+
+    void pruneLastCamStateBuffer();
     void pruneCamStateBuffer();
     // Reset the system online if the uncertainty is too large.
     void onlineReset();

src/msckf_vio.cpp

@@ -475,7 +475,7 @@ void MsckfVio::imageCallback(
       ros::Time::now()-start_time).toSec();
 
   start_time = ros::Time::now();
-  pruneCamStateBuffer();
+  pruneLastCamStateBuffer();
   double prune_cam_states_time = (
       ros::Time::now()-start_time).toSec();
 
@@ -1360,31 +1360,13 @@ void MsckfVio::PhotometricMeasurementJacobian(
     H_plj =  dI_dhj * dh_dXplj; // 1 x 6
     H_pAj =  dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
 
-    /*
-    cout << endl;
-    std::cout << H_plj << endl;
-    cout << r_photo.segment(count, 1) << endl;
-    std::cout << H_plj.colPivHouseholderQr().solve(r_photo.segment(count, 1)) << std::endl;
-    */
     H_rho.block<1, 1>(count, 0) = H_rhoj;
     H_pl.block<1, 6>(count, 0) = H_plj;
     H_pA.block<1, 6>(count, 0) = H_pAj;
 
-    //cout << "H_pl\n" << H_plj << endl;
-   
-    //cout << "H_pA\n" << H_pAj << endl;
-
-    //cout << "H_rho\n" << H_rhoj << endl;
     count++;
   }
 
-  /*
-  std::cout << "\n\n frame change through patch" << std::endl;
-  std::cout << H_pl << std::endl;
-  std::cout << r_photo << std::endl;
-  std::cout << endl;
-  std::cout << H_pl.colPivHouseholderQr().solve(r_photo) << std::endl;
-  */
 
   MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7);
   MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1);
@@ -1426,7 +1408,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   if(PRINTIMAGES)
   {  
     feature.MarkerGeneration(marker_pub, state_server.cam_states);
-    //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector);
+    feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector);
   }
 
   return;
@@ -1531,8 +1513,9 @@ void MsckfVio::PhotometricFeatureJacobian(
     ofstream myfile;
     myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
     myfile << "Hxi\n" << H_xi << "ri\n" << r_i << "Hyi\n" << H_yi << endl;
-    myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
+    myfile << "kernel\n" << A_null_space << endl;
     myfile.close();
+    myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
     cout << "---------- LOGGED -------- " << endl; 
   }
   if(PRINTIMAGES)
@@ -1860,7 +1843,7 @@ void MsckfVio::removeLostFeatures() {
         invalid_feature_ids.push_back(feature.id);
         continue;
       } else {
-        if(!feature.initializePosition(state_server.cam_states)) {
+        if(!feature.initializeRho(state_server.cam_states)) {
           invalid_feature_ids.push_back(feature.id);
           continue;
         }
@@ -1989,6 +1972,143 @@ void MsckfVio::findRedundantCamStates(vector<StateIDType>& rm_cam_state_ids) {
   return;
 }
 
+void MsckfVio::pruneLastCamStateBuffer()
+{
+  if (state_server.cam_states.size() < max_cam_state_size)
+    return;
+
+  auto rm_cam_state_id = state_server.cam_states.begin()->first;
+
+  // Set the size of the Jacobian matrix.
+  int jacobian_row_size = 0;
+  int augmentationSize = 6;
+  if(PHOTOMETRIC)
+    augmentationSize = 7;
+  
+  //initialize all the features which are going to be removed
+  for (auto& item : map_server) {
+    auto& feature = item.second;
+    
+    // check if feature is involved, if not continue
+    if (feature.observations.find(rm_cam_state_id) ==
+        feature.observations.end())
+      continue;
+
+
+    if (!feature.is_initialized) {
+
+      // Check if the feature can be initialized
+      if (!feature.checkMotion(state_server.cam_states)) {
+
+        // remove any features that can't be initialized
+        feature.observations.erase(rm_cam_state_id);
+        continue;
+      } else {
+        if(!feature.initializeRho(state_server.cam_states)) {
+          feature.observations.erase(rm_cam_state_id);
+          continue;
+        }
+      }
+    }
+    if(!feature.is_anchored)
+    {
+      if(!feature.initializeAnchor(cam0, N))
+      {
+        feature.observations.erase(rm_cam_state_id);
+        continue;
+      }
+    }
+
+    if(PHOTOMETRIC)
+      //just use max. size, as gets shrunken down after anyway
+      jacobian_row_size += N*N*feature.observations.size();
+    else
+      jacobian_row_size += 4*feature.observations.size() - 3;
+    
+  }
+
+  MatrixXd H_xj;
+  VectorXd r_j;
+  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;
+
+  for (auto& item : map_server) {
+    auto& feature = item.second;
+    
+    // check if feature is involved, if not continue
+    if (feature.observations.find(rm_cam_state_id) ==
+        feature.observations.end())
+      continue;
+
+
+    vector<StateIDType> involved_cam_state_ids(0);
+    for (const auto& cam_state : state_server.cam_states)
+      involved_cam_state_ids.push_back(cam_state.first);
+
+    if(PHOTOMETRIC)
+      PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
+    else
+      featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_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;
+      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);
+  }
+  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(),
+      state_server.cam_states.find(rm_cam_state_id));
+  int cam_state_start = 21 + augmentationSize*cam_sequence;
+  int cam_state_end = cam_state_start + augmentationSize;
+
+
+  // Remove the corresponding rows and columns in the state
+  // covariance matrix.
+  if (cam_state_end < state_server.state_cov.rows()) {
+    state_server.state_cov.block(cam_state_start, 0,
+        state_server.state_cov.rows()-cam_state_end,
+        state_server.state_cov.cols()) =
+      state_server.state_cov.block(cam_state_end, 0,
+          state_server.state_cov.rows()-cam_state_end,
+          state_server.state_cov.cols());
+
+    state_server.state_cov.block(0, cam_state_start,
+        state_server.state_cov.rows(),
+        state_server.state_cov.cols()-cam_state_end) =
+      state_server.state_cov.block(0, cam_state_end,
+          state_server.state_cov.rows(),
+          state_server.state_cov.cols()-cam_state_end);
+
+    state_server.state_cov.conservativeResize(
+        state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize);
+  } else {
+    state_server.state_cov.conservativeResize(
+        state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize);
+  }
+
+  // Remove this camera state in the state vector.
+  state_server.cam_states.erase(rm_cam_state_id);
+  cam0.moving_window.erase(rm_cam_state_id);
+  cam1.moving_window.erase(rm_cam_state_id);
+
+  return;
+}
+
 void MsckfVio::pruneCamStateBuffer() {
 
   if (state_server.cam_states.size() < max_cam_state_size)
@@ -2030,7 +2150,7 @@ void MsckfVio::pruneCamStateBuffer() {
           feature.observations.erase(cam_id);
         continue;
       } else {
-        if(!feature.initializePosition(state_server.cam_states)) {
+        if(!feature.initializeRho(state_server.cam_states)) {
           for (const auto& cam_id : involved_cam_state_ids)
             feature.observations.erase(cam_id);
           continue;