fixed non 0 filling of new state covariance

include/msckf_vio/feature.hpp

@@ -441,7 +441,7 @@ bool Feature::VisualizePatch(
 
 float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
 {
-  return (float)(image.at<uint8_t>(pose.x, pose.y));
+  return ((float)image.at<uint8_t>(pose.x, pose.y))/256;
 }
 
 cv::Point2f Feature::projectPositionToCamera(
@@ -496,7 +496,7 @@ bool Feature::initializeAnchor(
 
   //initialize patch Size
   //TODO make N size a ros parameter
-  int N = 9;
+  int N = 3;
   int n = (int)(N-1)/2;
 
   auto anchor = observations.begin();
@@ -551,7 +551,7 @@ bool Feature::initializeAnchor(
       return false;
   }
   for(auto point : vec)
-    anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)point.x,(int)point.y));
+    anchorPatch.push_back(PixelIrradiance(point, anchorImage));
 
   // project patch pixel to 3D space
   for(auto point : und_v)

include/msckf_vio/msckf_vio.h

@@ -204,6 +204,8 @@ class MsckfVio {
     // Photometry flag
     bool PHOTOMETRIC;
 
+    bool nan_flag;
+
     // Chi squared test table.
     static std::map<int, double> chi_squared_test_table;
 

src/msckf_vio.cpp

@@ -55,6 +55,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh):
   is_gravity_set(false),
   is_first_img(true),
   image_sub(10),
+  nan_flag(false),
   nh(pnh) {
   return;
 }
@@ -488,6 +489,7 @@ bool MsckfVio::resetCallback(
     std_srvs::Trigger::Request& req,
     std_srvs::Trigger::Response& res) {
 
+  cout << "reset" << endl;
   ROS_WARN("Start resetting msckf vio...");
   // Temporarily shutdown the subscribers to prevent the
   // state from updating.
@@ -901,8 +903,10 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) {
   size_t old_rows = state_server.state_cov.rows();
   size_t old_cols = state_server.state_cov.cols();
 
+  MatrixXd temp_cov = state_server.state_cov;
+
   // add 7 for camera state + irradiance bias eta = b_l
-  state_server.state_cov.conservativeResize(old_rows+7, old_cols+7);
+  state_server.state_cov.conservativeResizeLike(Eigen::MatrixXd::Zero(old_rows+7, old_cols+7));
 
   // Rename some matrix blocks for convenience.
   const Matrix<double, 21, 21>& P11 =
@@ -918,13 +922,13 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) {
     J * P11 * J.transpose();
 
   // Add photometry P_eta and surrounding Zeros
-  state_server.state_cov.block<1, 12>(old_rows+6, 0) = Matrix<double, 1, 12>::Zero();
-  state_server.state_cov.block<12, 1>(0, old_cols+6) = Matrix<double, 12, 1>::Zero();
   state_server.state_cov(old_rows+6, old_cols+6) = irradiance_frame_bias;
+  
   // Fix the covariance to be symmetric
   MatrixXd state_cov_fixed = (state_server.state_cov +
       state_server.state_cov.transpose()) / 2.0;
   state_server.state_cov = state_cov_fixed;
+
   return;
 }
 
@@ -976,7 +980,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   const Vector3d& t_c0_w = cam_state.position;
 
   //temp N
-  const int N = 9;
+  const int N = 3;
 
   // Cam1 pose.
   Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
@@ -1124,7 +1128,7 @@ void MsckfVio::PhotometricFeatureJacobian(
 
   const auto& feature = map_server[feature_id];
 
-  int N = 9;
+  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);
@@ -1194,26 +1198,24 @@ void MsckfVio::PhotometricFeatureJacobian(
   int sv_size = 0;
   Eigen::VectorXd singularValues = svd_helper.singularValues();
   for(int i = 0; i < singularValues.size(); i++)
-    if(singularValues[i] < 1e-3)
+    if(singularValues[i] > 1e-5)
       sv_size++;
 
   int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size();
   MatrixXd A = svd_helper.matrixU().rightCols(
-      jacobian_row_size-null_space_size);
+      jacobian_row_size-sv_size);
   
   H_x = A.transpose() * H_xi;
   r = A.transpose() * r_i;
 
-  cout << "r\n" << r << endl;
-  cout << "Hx\n" << H_x << endl;
-
   return;
 }
 
 void MsckfVio::measurementJacobian(
     const StateIDType& cam_state_id,
     const FeatureIDType& feature_id,
-    Matrix<double, 4, 6>& H_x, Matrix<double, 4, 3>& H_f, Vector4d& r) {
+    Matrix<double, 4, 6>& H_x, Matrix<double, 4, 3>& H_f, Vector4d& r)
+{
 
   // Prepare all the required data.
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
@@ -1275,7 +1277,8 @@ void MsckfVio::measurementJacobian(
 void MsckfVio::featureJacobian(
     const FeatureIDType& feature_id,
     const std::vector<StateIDType>& cam_state_ids,
-    MatrixXd& H_x, VectorXd& r) {
+    MatrixXd& H_x, VectorXd& r) 
+{
 
   const auto& feature = map_server[feature_id];
 
@@ -1322,29 +1325,28 @@ void MsckfVio::featureJacobian(
   int sv_size = 0;
   Eigen::VectorXd singularValues = svd_helper.singularValues();
   for(int i = 0; i < singularValues.size(); i++)
-    if(singularValues[i] < 1e-3)
+    if(singularValues[i] > 1e-5)
       sv_size++;
   
-  int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size();
+  int null_space_size = svd_helper.matrixU().cols() - sv_size;
-    
-  //cout << "singular values: \n" << svd_helper.singularValues(); 
-  cout << "null_space: " << null_space_size << endl; 
 
   MatrixXd A = svd_helper.matrixU().rightCols(
-      jacobian_row_size - 3);
+      jacobian_row_size - sv_size);
   
    H_x = A.transpose() * H_xj;
    r = A.transpose() * r_j;
 
+   cout << "A: \n" << A.transpose() << endl;
 
   return;
 }
 
-void MsckfVio::measurementUpdate(
+void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
-    const MatrixXd& H, const VectorXd& r) {
 
   if (H.rows() == 0 || r.rows() == 0) return;
 
+
+  cout << "decomposition...";
   // Decompose the final Jacobian matrix to reduce computational
   // complexity as in Equation (28), (29).
   MatrixXd H_thin;
@@ -1377,7 +1379,8 @@ void MsckfVio::measurementUpdate(
     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() +
@@ -1387,6 +1390,8 @@ void MsckfVio::measurementUpdate(
   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;
 
@@ -1444,12 +1449,19 @@ void MsckfVio::measurementUpdate(
   return;
 }
 
-bool MsckfVio::gatingTest(
+bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
-    const MatrixXd& H, const VectorXd& r, const int& dof) {
 
   MatrixXd P1 = H * state_server.state_cov * H.transpose();
+
+
   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 << " " <<
@@ -1465,13 +1477,14 @@ bool MsckfVio::gatingTest(
 }
 
 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;
   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.
@@ -1507,6 +1520,10 @@ void MsckfVio::removeLostFeatures() {
       }
     }
 
+    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;
     processed_feature_ids.push_back(feature.id);
   }
@@ -1523,8 +1540,12 @@ 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+6*state_server.cam_states.size());
+      21+augmentationSize*state_server.cam_states.size());
   VectorXd r = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
 
@@ -1548,11 +1569,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 << "made gating test" << endl;
+      cout << "approved chi" << endl;
     }
     else
     {
-      cout << "failed gating test" << endl;
+      cout << "rejected chi" << endl;
     }
 
     // Put an upper bound on the row size of measurement Jacobian,
@@ -1573,8 +1594,7 @@ void MsckfVio::removeLostFeatures() {
   return;
 }
 
-void MsckfVio::findRedundantCamStates(
+void MsckfVio::findRedundantCamStates(vector<StateIDType>& rm_cam_state_ids) {
-    vector<StateIDType>& rm_cam_state_ids) {
 
   // Move the iterator to the key position.
   auto key_cam_state_iter = state_server.cam_states.end();
@@ -1627,6 +1647,8 @@ 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;
   for (auto& item : map_server) {
@@ -1671,6 +1693,9 @@ void MsckfVio::pruneCamStateBuffer() {
         continue;
       }
     }
+     if(PHOTOMETRIC)
+      jacobian_row_size += N*N*involved_cam_state_ids.size();
+    else
       jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
   }
 
@@ -1716,9 +1741,12 @@ void MsckfVio::pruneCamStateBuffer() {
       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);