From de07296d31839b19d48370bd4e31964947775efd Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Thu, 25 Apr 2019 13:44:21 +0200
Subject: [PATCH] added minor changes to nullspace

---
 src/msckf_vio.cpp | 52 +++++++++++++++++++++++++----------------------
 1 file changed, 28 insertions(+), 24 deletions(-)

diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index 5ed2eca..a4a6874 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -345,7 +345,10 @@ void MsckfVio::imageCallback(
 
   // Augment the state vector.
   start_time = ros::Time::now();
-  stateAugmentation(feature_msg->header.stamp.toSec());
+  if(PHOTOMETRIC)
+    PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
+  else
+    stateAugmentation(feature_msg->header.stamp.toSec());
   double state_augmentation_time = (
       ros::Time::now()-start_time).toSec();
 
@@ -1186,20 +1189,20 @@ void MsckfVio::PhotometricFeatureJacobian(
 
   // get Nullspace
 
-  FullPivLU<MatrixXd> lu(H_yi);
-  MatrixXd A_right = lu.kernel();
-  //FullPivLU<MatrixXd> lu2(A_right.transpose());
-  //MatrixXd A = lu2.kernel().transpose();
-  /*
+  
   JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
+  int sv_size = 0;
+  Eigen::VectorXd singularValues = svd_helper.singularValues();
+  for(int i = 0; i < singularValues.size(); i++)
+    if(singularValues[i] < 1e-3)
+      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);
-  */
-  //H_x = A.transpose() * H_xi;
-  //r = A.transpose() * r_i;
-  H_x = H_xi * A_right;
-  r = r_i * A_right;
+  
+  H_x = A.transpose() * H_xi;
+  r = A.transpose() * r_i;
 
   cout << "r\n" << r << endl;
   cout << "Hx\n" << H_x << endl;
@@ -1262,17 +1265,6 @@ void MsckfVio::measurementJacobian(
   H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc;
   H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg;
 
-  // Modifty the measurement Jacobian to ensure
-  // observability constrain.
-  Matrix<double, 4, 6> A = H_x;
-  Matrix<double, 6, 1> u = Matrix<double, 6, 1>::Zero();
-  u.block<3, 1>(0, 0) = quaternionToRotation(
-      cam_state.orientation_null) * IMUState::gravity;
-  u.block<3, 1>(3, 0) = skewSymmetric(
-      p_w-cam_state.position_null) * IMUState::gravity;
-  H_x = A - A*u*(u.transpose()*u).inverse()*u.transpose();
-  H_f = -H_x.block<4, 3>(0, 3);
-
   // Compute the residual.
   r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2),
       p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
@@ -1327,11 +1319,23 @@ void MsckfVio::featureJacobian(
   // Project the residual and Jacobians onto the nullspace
   // of H_fj.
   JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV);
+  int sv_size = 0;
+  Eigen::VectorXd singularValues = svd_helper.singularValues();
+  for(int i = 0; i < singularValues.size(); i++)
+    if(singularValues[i] < 1e-3)
+      sv_size++;
+  
+  int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().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);
+  
+   H_x = A.transpose() * H_xj;
+   r = A.transpose() * r_j;
 
-  H_x = A.transpose() * H_xj;
-  r = A.transpose() * r_j;
 
   return;
 }