removed resizing not correcting for photometric info, added N as global variable

src/msckf_vio.cpp

@@ -127,6 +127,10 @@ bool MsckfVio::loadParameters() {
   nh.param<double>("initial_covariance/irradiance_frame_bias",
       irradiance_frame_bias, 0.1);
 
+  // Get the photometric patch size
+  nh.param<int>("patch_size_n",
+      N, 3);
+
   // get camera information (used for back projection)
   nh.param<string>("cam0/distortion_model",
       cam0.distortion_model, string("radtan"));
@@ -288,7 +292,7 @@ bool MsckfVio::initialize() {
   for (int i = 1; i < 100; ++i) {
     boost::math::chi_squared chi_squared_dist(i);
     chi_squared_test_table[i] =
-      boost::math::quantile(chi_squared_dist, 0.05);
+      boost::math::quantile(chi_squared_dist, 0.2);
   }
 
   if (!createRosIO()) return false;
@@ -979,9 +983,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
   Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
   const Vector3d& t_c0_w = cam_state.position;
 
-  //temp N
-  const int N = 3;
-
   // Cam1 pose.
   Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
   Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0;
@@ -1128,7 +1129,6 @@ void MsckfVio::PhotometricFeatureJacobian(
 
   const auto& feature = map_server[feature_id];
 
-  int N = 3;
   // Check how many camera states in the provided camera
   // id camera has actually seen this feature.
   vector<StateIDType> valid_cam_state_ids(0);
@@ -1346,7 +1346,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
   if (H.rows() == 0 || r.rows() == 0) return;
 
 
-  cout << "decomposition...";
+  cout << "Updating...";
   // Decompose the final Jacobian matrix to reduce computational
   // complexity as in Equation (28), (29).
   MatrixXd H_thin;
@@ -1379,8 +1379,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
     H_thin = H;
     r_thin = r;
   }
-  cout << "done" << endl;
-  cout << "computing K...";
+
   // Compute the Kalman gain.
   const MatrixXd& P = state_server.state_cov;
   MatrixXd S = H_thin*P*H_thin.transpose() +
@@ -1390,8 +1389,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
   MatrixXd K_transpose = S.ldlt().solve(H_thin*P);
   MatrixXd K = K_transpose.transpose();
 
-  cout << "done" << endl;
-
   // Compute the error of the state.
   VectorXd delta_x = K * r_thin;
 
@@ -1457,15 +1454,10 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
   MatrixXd P2 = Feature::observation_noise *
     MatrixXd::Identity(H.rows(), H.rows());
 
-  //cout << "H: \n" << H << endl;
-  //cout << "cov: \n" << state_server.state_cov << endl;
-  //cout << "P1: \n" << P1 << endl;
-  //cout << "solv: \n" << (P1+P2).ldlt().solve(r) << endl;
-
   double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
 
   cout << dof << " " << gamma << " " <<
-    chi_squared_test_table[dof] << " ";
+    chi_squared_test_table[dof] << endl;
 
   if (gamma < chi_squared_test_table[dof]) {
     //cout << "passed" << endl;
@@ -1480,11 +1472,14 @@ void MsckfVio::removeLostFeatures() {
   // Remove the features that lost track.
   // BTW, find the size the final Jacobian matrix and residual vector.
   int jacobian_row_size = 0;
+  int augmentationSize = 6;
+  if(PHOTOMETRIC)
+    augmentationSize = 7;
+
+
   vector<FeatureIDType> invalid_feature_ids(0);
   vector<FeatureIDType> processed_feature_ids(0);
 
-  int N = 3;
-
   for (auto iter = map_server.begin();
       iter != map_server.end(); ++iter) {
     // Rename the feature to be checked.
@@ -1513,7 +1508,7 @@ void MsckfVio::removeLostFeatures() {
     }
     if(!feature.is_anchored)
     {
-      if(!feature.initializeAnchor(cam0))
+      if(!feature.initializeAnchor(cam0, N))
       {
         invalid_feature_ids.push_back(feature.id);
         continue;
@@ -1540,10 +1535,6 @@ void MsckfVio::removeLostFeatures() {
   // Return if there is no lost feature to be processed.
   if (processed_feature_ids.size() == 0) return;
 
-  int augmentationSize = 6;
-    if(PHOTOMETRIC)
-      augmentationSize = 7;
-
   MatrixXd H_x = MatrixXd::Zero(jacobian_row_size,
       21+augmentationSize*state_server.cam_states.size());
   VectorXd r = VectorXd::Zero(jacobian_row_size);
@@ -1569,11 +1560,11 @@ 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 << "approved chi" << endl;
+      cout << "passed" << endl;
     }
     else
     {
-      cout << "rejected chi" << endl;
+      cout << "rejected" << endl;
     }
 
     // Put an upper bound on the row size of measurement Jacobian,
@@ -1647,10 +1638,12 @@ void MsckfVio::pruneCamStateBuffer() {
   vector<StateIDType> rm_cam_state_ids(0);
   findRedundantCamStates(rm_cam_state_ids);
 
-  int N = 3;
-
   // Find the size of the Jacobian matrix.
   int jacobian_row_size = 0;
+    int augmentationSize = 6;
+    if(PHOTOMETRIC)
+      augmentationSize = 7;
+
   for (auto& item : map_server) {
     auto& feature = item.second;
     // Check how many camera states to be removed are associated
@@ -1686,7 +1679,7 @@ void MsckfVio::pruneCamStateBuffer() {
     }
     if(!feature.is_anchored)
     {
-      if(!feature.initializeAnchor(cam0))
+      if(!feature.initializeAnchor(cam0, N))
       {
         for (const auto& cam_id : involved_cam_state_ids)
               feature.observations.erase(cam_id);
@@ -1702,8 +1695,9 @@ void MsckfVio::pruneCamStateBuffer() {
   //cout << "jacobian row #: " << jacobian_row_size << endl;
 
   // Compute the Jacobian and residual.
-  MatrixXd H_x = MatrixXd::Zero(jacobian_row_size,
-      21+6*state_server.cam_states.size());
+  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) {
@@ -1719,9 +1713,6 @@ void MsckfVio::pruneCamStateBuffer() {
 
     if (involved_cam_state_ids.size() == 0) continue;
 
-    MatrixXd H_xj;
-    VectorXd r_j;
-
     if(PHOTOMETRIC)
       PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     else
@@ -1731,29 +1722,23 @@ 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 << "made gating test" << endl;
+      cout << "passed" << endl;
     }
     else
     {
-      cout << "failed gating test" << endl;
+      cout << "rejected" << endl;
     }
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);
   }
 
-  cout << "resize" << endl;
-
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
 
-  cout << "done" << endl;
   // Perform measurement update.
   measurementUpdate(H_x, r);
-  
-  int augmentationSize = 6;
-  if(PHOTOMETRIC)
-    augmentationSize = 7;
 
+  //reduction
   for (const auto& cam_id : rm_cam_state_ids) {
     int cam_sequence = std::distance(state_server.cam_states.begin(),
         state_server.cam_states.find(cam_id));