added flag to switch to original, using right null space matrix for calculation now and existing eigen function, gating restult still way to high

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));
 }
 
 cv::Point2f Feature::projectPositionToCamera(
@@ -496,7 +496,7 @@ bool Feature::initializeAnchor(
 
   //initialize patch Size
   //TODO make N size a ros parameter
-  int N = 13;
+  int N = 9;
   int n = (int)(N-1)/2;
 
   auto anchor = observations.begin();

include/msckf_vio/msckf_vio.h

@@ -201,6 +201,9 @@ class MsckfVio {
     // Reset the system online if the uncertainty is too large.
     void onlineReset();
 
+    // Photometry flag
+    bool PHOTOMETRIC;
+
     // Chi squared test table.
     static std::map<int, double> chi_squared_test_table;
 

launch/msckf_vio_debug_tum.launch

@@ -18,6 +18,9 @@
       output="screen"
       launch-prefix="xterm -e gdb --args">
 
+      <!-- Photometry Flag-->
+      <param name="PHOTOMETRIC" value="true"/>
+
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

launch/msckf_vio_tum.launch

@@ -17,6 +17,9 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
+      <!-- Photometry Flag-->
+      <param name="PHOTOMETRIC" value="true"/>
+
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

src/msckf_vio.cpp

@@ -27,6 +27,7 @@
 #include <msckf_vio/math_utils.hpp>
 #include <msckf_vio/utils.h>
 
+
 using namespace std;
 using namespace Eigen;
 
@@ -59,6 +60,10 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh):
 }
 
 bool MsckfVio::loadParameters() {
+  //Photometry Flag
+  nh.param<bool>("PHOTOMETRIC", PHOTOMETRIC, false);
+  
+
   // Frame id
   nh.param<string>("fixed_frame_id", fixed_frame_id, "world");
   nh.param<string>("child_frame_id", child_frame_id, "robot");
@@ -340,7 +345,7 @@ void MsckfVio::imageCallback(
 
   // Augment the state vector.
   start_time = ros::Time::now();
-  PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
+  stateAugmentation(feature_msg->header.stamp.toSec());
   double state_augmentation_time = (
       ros::Time::now()-start_time).toSec();
 
@@ -968,7 +973,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   const Vector3d& t_c0_w = cam_state.position;
 
   //temp N
-  const int N = 13;
+  const int N = 9;
 
   // Cam1 pose.
   Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
@@ -1116,7 +1121,7 @@ void MsckfVio::PhotometricFeatureJacobian(
 
   const auto& feature = map_server[feature_id];
 
-  int N = 13;
+  int N = 9;
   // Check how many camera states in the provided camera
   // id camera has actually seen this feature.
   vector<StateIDType> valid_cam_state_ids(0);
@@ -1179,12 +1184,25 @@ void MsckfVio::PhotometricFeatureJacobian(
   // Project the residual and Jacobians onto the nullspace
   // of H_yj.
 
-  JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
-  MatrixXd A = svd_helper.matrixU().rightCols(
-      jacobian_row_size - 3);
+  // get Nullspace
 
-  H_x = A.transpose() * H_xi;
-  r = A.transpose() * r_i;
+  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 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;
+
+  cout << "r\n" << r << endl;
+  cout << "Hx\n" << H_x << endl;
 
   return;
 }
@@ -1327,7 +1345,6 @@ void MsckfVio::measurementUpdate(
   // complexity as in Equation (28), (29).
   MatrixXd H_thin;
   VectorXd r_thin;
-  cout << " measurement update ..."  << endl;
 
   if (H.rows() > H.cols()) {
     // Convert H to a sparse matrix.
@@ -1431,8 +1448,8 @@ bool MsckfVio::gatingTest(
     MatrixXd::Identity(H.rows(), H.rows());
   double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
 
-  //cout << dof << " " << gamma << " " <<
-  //  chi_squared_test_table[dof] << " ";
+  cout << dof << " " << gamma << " " <<
+    chi_squared_test_table[dof] << " ";
 
   if (gamma < chi_squared_test_table[dof]) {
     //cout << "passed" << endl;
@@ -1517,7 +1534,12 @@ void MsckfVio::removeLostFeatures() {
 
     MatrixXd H_xj;
     VectorXd r_j;
+
+    if(PHOTOMETRIC)
       PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j);
+    else
+      featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
+
     if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
       r.segment(stack_cntr, r_j.rows()) = r_j;
@@ -1529,8 +1551,6 @@ void MsckfVio::removeLostFeatures() {
       cout << "failed gating test" << endl;
     }
 
-    cout << " stacked features up" << endl;
-
     // Put an upper bound on the row size of measurement Jacobian,
     // which helps guarantee the executation time.
     if (stack_cntr > 1500) break;
@@ -1673,9 +1693,10 @@ void MsckfVio::pruneCamStateBuffer() {
     MatrixXd H_xj;
     VectorXd r_j;
 
-    cout << "getting featureJacobian...";
+    if(PHOTOMETRIC)
       PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
-    cout << "done" << endl;
+    else
+      featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
 
     if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
@@ -1691,19 +1712,22 @@ void MsckfVio::pruneCamStateBuffer() {
       feature.observations.erase(cam_id);
   }
 
-  cout << " stacked features up" << endl;
-
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
 
   // Perform measurement update.
   measurementUpdate(H_x, r);
   
+  int augmentationSize = 6;
+  if(PHOTOMETRIC)
+    augmentationSize = 7;
+
   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));
-    int cam_state_start = 21 + 6*cam_sequence;
-    int cam_state_end = cam_state_start + 6;
+    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.
@@ -1723,10 +1747,10 @@ void MsckfVio::pruneCamStateBuffer() {
             state_server.state_cov.cols()-cam_state_end);
 
       state_server.state_cov.conservativeResize(
-          state_server.state_cov.rows()-6, state_server.state_cov.cols()-6);
+          state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize);
     } else {
       state_server.state_cov.conservativeResize(
-          state_server.state_cov.rows()-6, state_server.state_cov.cols()-6);
+          state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize);
     }
 
     // Remove this camera state in the state vector.