alterations at nullspaceing, jakobi changes

launch/msckf_vio_tum.launch

@@ -21,8 +21,8 @@
       <param name="PHOTOMETRIC" value="true"/>
 
       <!-- Debugging Flaggs -->
-      <param name="PrintImages" value="true"/>
+      <param name="PrintImages" value="false"/>
-      <param name="GroundTruth" value="true"/>
+      <param name="GroundTruth" value="false"/>
 
       <param name="patch_size_n" value="7"/>
       <!-- Calibration parameters -->

src/msckf_vio.cpp

@@ -1158,8 +1158,6 @@ 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.conservativeResizeLike(Eigen::MatrixXd::Zero(old_rows+7, old_cols+7));
 
@@ -1287,7 +1285,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
     dI_dhj(0, 1) = dy;
     
     //dh / d{}^Cp_{ij}
-    dh_dCpij.block<2, 2>(0, 0) = Eigen::Matrix<double, 2, 2>::Identity();
+    dh_dCpij(0, 0) = 1 / p_c0(2);
+    dh_dCpij(1, 1) = 1 / p_c0(2);
     dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
     dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
 
@@ -1305,16 +1304,16 @@ void MsckfVio::PhotometricMeasurementJacobian(
     //d{}^Gp_P{ij} / \rho_i
     double rho = feature.anchor_rho;
     // Isometry T_anchor_w takes a vector in anchor frame to world frame
-    dGpj_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), -1/(rho*rho));
+    dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho));
 
-    dGpj_XpAj.block<3, 3>(0, 0) = - skewSymmetric(feature.T_anchor_w.linear()
+    dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
-                                 * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), 
+                                 * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), 
                                                    feature.anchorPatch_ideal[count].y/(rho), 
                                                    1/(rho)));
     dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
 
     // Intermediate Jakobians
-    H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 3
+    H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1
     H_plj = dI_dhj * dh_dXplj; // 1 x 6
     H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
 
@@ -1335,6 +1334,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   IlluminationParameter estimated_illumination;
   feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
   
+  // calculated here, because we need true 'estimate_irradiance' later for jacobi
   for (auto& estimate_irradiance_j : estimate_irradiance)
             estimate_photo_z.push_back (estimate_irradiance_j * 
                     estimated_illumination.frame_gain * estimated_illumination.feature_gain +
@@ -1382,7 +1382,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
   count = 0; 
   for(auto data : photo_r)
     r[count++] = data;
-
   std::stringstream ss;
   ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << "  frame: " << cam_state_cntr;
   if(PRINTIMAGES)
@@ -1469,25 +1468,21 @@ void MsckfVio::PhotometricFeatureJacobian(
   // of H_yj.
 
   // get Nullspace
+  FullPivLU<MatrixXd> lu(H_yi.transpose());
+  MatrixXd A_null_space = lu.kernel();
+  /*
   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-9)
+    if(singularValues[i] > 1e-12)
       sv_size++;
 
-  int null_space_size = svd_helper.matrixU().cols() - sv_size; //TEST used instead of svd_helper.singularValues().size();
+  MatrixXd A = svd_helper.matrixU().rightCols(jacobian_row_size - singularValues.size());
-  MatrixXd A = svd_helper.matrixU().rightCols(null_space_size);
+  */
-  
+  H_x = A_null_space.transpose() * H_xi;
-  H_x = A.transpose() * H_xi;
+  r = A_null_space.transpose() * r_i;
-  r = A.transpose() * r_i;
-  
-  ofstream myfile;
-  myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
-  myfile << "nulls:\n" << A.transpose() * H_yi <<endl;
-  myfile.close();
-  cout << "---------- LOGGED -------- " << endl;
 
   if(PRINTIMAGES)
   {
@@ -1544,7 +1539,7 @@ void MsckfVio::measurementJacobian(
   // original jacobi
   //dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
   // my version of calculation
-  dpc0_dxc.leftCols(3) = skewSymmetric(R_w_c0 * p_w) -  skewSymmetric(R_w_c0 * t_c0_w);
+  dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
   //dpc0_dxc.leftCols(3) = - skewSymmetric(R_w_c0.transpose() * (t_c0_w - p_w)) * R_w_c0;
   dpc0_dxc.rightCols(3) = -R_w_c0;
 
@@ -1612,26 +1607,32 @@ 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-5)
       sv_size++;
-  
+  cout << "sv size: " << sv_size << endl;
-  int null_space_size = svd_helper.matrixU().cols() - sv_size;
  
   MatrixXd A = svd_helper.matrixU().rightCols(
-      jacobian_row_size - sv_size);
+      jacobian_row_size -  3);
+  */
+  FullPivLU<MatrixXd> lu(H_fj.transpose());
+  MatrixXd A = lu.kernel();
 
    H_x = A.transpose() * H_xj;
    r = A.transpose() * r_j;
 
+  /*
   ofstream myfile;
   myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
   myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <<endl;
   myfile.close();
   cout << "---------- LOGGED -------- " << endl; 
+  */
   nh.setParam("/play_bag", false);
 
   return;