changed to simple irradiance calcualation with derivation of image per frame - not working

include/msckf_vio/feature.hpp

@@ -184,6 +184,11 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
                   const CameraCalibration& cam,
                   Eigen::Vector3d& in_p) const;
 
+double CompleteCvKernel(
+                const cv::Point2f pose,
+                const cv::Mat& frame,
+                std::string type) const;
+
   double cvKernel(
             const cv::Point2f pose,
             std::string type) const;
@@ -488,6 +493,45 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const
   else return false;
 }
 
+double Feature::CompleteCvKernel(
+                const cv::Point2f pose,
+                const cv::Mat& frame,
+                std::string type) const
+{
+
+  double delta = 0;
+
+  cv::Mat xder;
+  cv::Mat yder;
+  cv::Mat deeper_frame;
+  frame.convertTo(deeper_frame,CV_16S);
+  //TODO remove this?
+
+
+  cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
+  cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
+
+  xder/=8.;
+  yder/=8.;
+
+  /*
+  cv::Mat norm_abs_xderImage;
+  cv::Mat abs_xderImage2;
+  cv::convertScaleAbs(xder, abs_xderImage2);
+
+  cv::normalize(abs_xderImage2, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
+
+  cv::imshow("xder", norm_abs_xderImage);
+  cvWaitKey(0);
+  */
+
+  if(type == "Sobel_x")
+    delta = ((double)xder.at<short>(pose.y, pose.x))/255.;
+  else if (type == "Sobel_y")
+    delta = ((double)yder.at<short>(pose.y, pose.x))/255.;
+  return delta;
+}
+
 double Feature::cvKernel(
                 const cv::Point2f pose,
                 std::string type) const
@@ -823,6 +867,7 @@ bool Feature::VisualizePatch(
   // residual grid projection, positive - red, negative - blue colored 
   namer.str(std::string());
   namer << "residual";
+  std::cout << "-- photo_r -- \n" << photo_r << " -- " << std::endl;
   cv::putText(irradianceFrame, namer.str() , cvPoint(30+scale*N, scale*N/2-5), 
     cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
 

include/msckf_vio/msckf_vio.h

@@ -229,12 +229,14 @@ class MsckfVio {
 
     bool PhotometricPatchPointJacobian(
         const CAMState& cam_state,
+        const StateIDType& cam_state_id,
         const Feature& feature,
         Eigen::Vector3d point,
         int count,
         Eigen::Matrix<double, 1, 1>& H_rhoj,
         Eigen::Matrix<double, 1, 6>& H_plj,
-        Eigen::Matrix<double, 1, 6>& H_pAj);
+        Eigen::Matrix<double, 1, 6>& H_pAj,
+        Eigen::Matrix<double, 1, 2>& dI_dhj);
 
     bool PhotometricMeasurementJacobian(
         const StateIDType& cam_state_id,

launch/msckf_vio_tinytum.launch

@@ -18,14 +18,14 @@
       output="screen">
 
       <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="FILTER" value="0"/>
+      <param name="FILTER" value="1"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
       <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="3"/>
+      <param name="patch_size_n" value="5"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

launch/msckf_vio_tum.launch

@@ -25,7 +25,7 @@
       <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="1"/>
+      <param name="patch_size_n" value="5"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

src/msckf_vio.cpp

@@ -496,8 +496,8 @@ void MsckfVio::imageCallback(
     processing_end_time - processing_start_time;
   if (processing_time > 1.0/frame_rate) {
     ++critical_time_cntr;
-    ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m",
-        processing_time, critical_time_cntr);
+    //ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m",
+    //    processing_time, critical_time_cntr);
     //printf("IMU processing time: %f/%f\n",
     //    imu_processing_time, imu_processing_time/processing_time);
     //printf("State augmentation time: %f/%f\n",
@@ -1419,7 +1419,6 @@ void MsckfVio::twodotMeasurementJacobian(
     std::stringstream ss;
     ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << "  frame: " << cam_state_cntr;
     feature.MarkerGeneration(marker_pub, state_server.cam_states); 
-    //feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss);
   }
 
   return;
@@ -1540,27 +1539,40 @@ bool MsckfVio::PhotometricPatchPointResidual(
   std::vector<double> estimate_photo_z;
   std::vector<double> photo_z;
 
+  // estimate irradiance based on anchor frame
   IlluminationParameter estimated_illumination;
   feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
   for (auto& estimate_irradiance_j : estimate_irradiance)
-            estimate_photo_z.push_back (estimate_irradiance_j * 
-                    estimated_illumination.frame_gain * estimated_illumination.feature_gain +
-                    estimated_illumination.frame_bias + estimated_illumination.feature_bias);
+            estimate_photo_z.push_back (estimate_irradiance_j);// * 
+                    //estimated_illumination.frame_gain * estimated_illumination.feature_gain +
+                    //estimated_illumination.frame_bias + estimated_illumination.feature_bias);
+
+
+  // irradiance measurement around feature point
+  std::vector<double> true_irradiance;
+  cv::Point2f p_f(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1));
+  p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs);
+  cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
+  for(int i = 0; i<N; i++)
+    for(int j = 0; j<N ; j++)
+      true_irradiance.push_back(feature.PixelIrradiance(cv::Point2f(p_f.x + (i-(N-1)/2), p_f.y + (j-(N-1)/2)), current_image));
 
 
   for(auto point : feature.anchorPatch_3d)
   {
 
-    
     cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
     // test if projection is inside frame 
     if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1)
       return false;
-    //add observation
+    // add observation
     photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
     
-    //calculate photom. residual
-    r_photo(count) = photo_z[count] - estimate_photo_z[count];
+    // calculate photom. residual acc. to paper
+    // r_photo(count) = photo_z[count] - estimate_photo_z[count];
+
+    // calculate alternate photom. residual
+    r_photo(count) = true_irradiance[count] - photo_z[count];
     count++;
   }
   r = r_photo;
@@ -1570,12 +1582,14 @@ bool MsckfVio::PhotometricPatchPointResidual(
 // generates the jacobian of one patch point regarding rho, anchor and current frame
 bool MsckfVio::PhotometricPatchPointJacobian(
     const CAMState& cam_state,
+    const StateIDType& cam_state_id,
     const Feature& feature,
     Eigen::Vector3d point,
     int count,
     Eigen::Matrix<double, 1, 1>& H_rhoj,
     Eigen::Matrix<double, 1, 6>& H_plj,
-    Eigen::Matrix<double, 1, 6>& H_pAj) 
+    Eigen::Matrix<double, 1, 6>& H_pAj,
+    Matrix<double, 1, 2>& dI_dhj)
 {
 
   const StateIDType anchor_state_id = feature.observations.begin()->first;
@@ -1592,7 +1606,7 @@ bool MsckfVio::PhotometricPatchPointJacobian(
 
   
   // individual Jacobians
-  Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
+  /*Matrix<double, 1, 2> */dI_dhj = Matrix<double, 1, 2>::Zero();
   Matrix<double, 2, 3> dh_dCpij = Matrix<double, 2, 3>::Zero();
   Matrix<double, 2, 3> dh_dGpij = Matrix<double, 2, 3>::Zero();
   Matrix<double, 2, 6> dh_dXplj = Matrix<double, 2, 6>::Zero();
@@ -1606,13 +1620,17 @@ bool MsckfVio::PhotometricPatchPointJacobian(
   double dx, dy;
 
   Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
+  cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
   cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point);
 
 
+  auto frame = cam0.moving_window.find(cam_state_id)->second.image;
+
   // calculate derivation for anchor frame, use position for derivation calculation
   // frame derivative calculated convoluting with kernel [-1, 0, 1]
-  dx = feature.cvKernel(p_in_anchor, "Sobel_x");
-  dy = feature.cvKernel(p_in_anchor, "Sobel_y");
+  dx = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_x");
+  dy = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_y");
+  //cout << "dx: " << dx << " : " << feature.cvKernel(p_in_c0, "Sobel_x") << " : " << feature.Kernel(p_in_c0,  frame, "Sobel_x") << endl;
 
   dI_dhj(0, 0) = dx * cam0.intrinsics[0];
   dI_dhj(0, 1) = dy * cam0.intrinsics[1];
@@ -1650,7 +1668,11 @@ bool MsckfVio::PhotometricPatchPointJacobian(
   H_plj =  dI_dhj * dh_dXplj; // 1 x 6
   H_pAj =  dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
 
-  return true;
+  // check if point nullspaceable
+  if (H_rhoj(0, 0) != 0)
+    return true;
+
+  return false;
 }
 
 bool MsckfVio::PhotometricMeasurementJacobian(
@@ -1671,6 +1693,8 @@ bool MsckfVio::PhotometricMeasurementJacobian(
   Eigen::Matrix<double, 1, 6> H_plj;
   Eigen::Matrix<double, 1, 6> H_pAj;
 
+  Eigen::MatrixXd dI_dh(N*N, 2);
+
   // combined Jacobians
   Eigen::MatrixXd H_rho(N*N, 1);
   Eigen::MatrixXd H_pl(N*N, 6);
@@ -1683,26 +1707,33 @@ bool MsckfVio::PhotometricMeasurementJacobian(
 
   // calculate jacobian for patch
   int count = 0;
+  bool valid = false;
+  Matrix<double, 1, 2> dI_dhj;// = Matrix<double, 1, 2>::Zero();
   for (auto point : feature.anchorPatch_3d)
   {
     // get jacobi of single point in patch
-    PhotometricPatchPointJacobian(cam_state, feature, point, count, H_rhoj, H_plj, H_pAj);
+    if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj))
+        valid = true;
 
     // stack point into entire jacobi
     H_rho.block<1, 1>(count, 0) = H_rhoj;
     H_pl.block<1, 6>(count, 0) = H_plj;
     H_pA.block<1, 6>(count, 0) = H_pAj;
 
+    dI_dh.block<1, 2>(count, 0) = dI_dhj;
+
     count++;
   }
 
+  //Eigen::Matrix<double, 2, 1> h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo;
+  //cout << "h photo: \n" << h_photo << endl;
+
   // construct the jacobian structure needed for nullspacing
   MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7);
-  MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1);
+  MatrixXd H_yl = MatrixXd::Zero(N*N, 1);
   
   ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl);
 
-
   // set to return values
   H_x = H_xl;
   H_y = H_yl;
@@ -1726,9 +1757,13 @@ bool MsckfVio::PhotometricMeasurementJacobian(
     //feature.VisualizeKernel(cam_state, cam_state_id, cam0);
   }
 
-  return true;
+  if(valid)
+    return true;
+  else
+    return false;
 }
 
+// uses the calcualted jacobians to construct the final Hx Hy jacobians used for nullspacing
 bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
                                   Eigen::MatrixXd& H_pl,
                                   Eigen::MatrixXd& H_pA,
@@ -1741,7 +1776,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
   auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first);
   int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
   // set anchor Jakobi
-  H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA; 
+  H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = H_pA; 
 
   //get position of current frame in cam states
   auto cam_state_iter = state_server.cam_states.find(cam_state_id);
@@ -1749,21 +1784,20 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
   int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter);
   
   // set jakobi of state
-  H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl;
+  H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = H_pl;
 
   // set ones for irradiance bias
   // H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N);
 
   // set irradiance error Block
-  H_yl.block(0, 0,N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N);
+  //H_yl.block(0, 0, N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N);
 
 
   /*for(int i = 0; i< N*N; i++)   
     H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i];
   */
 
-  H_yl.block(0, N*N, N*N, 1) = -H_rho;
-
+  H_yl = H_rho;
 }
 
 
@@ -1772,7 +1806,6 @@ bool MsckfVio::PhotometricFeatureJacobian(
     const std::vector<StateIDType>& cam_state_ids,
     MatrixXd& H_x, VectorXd& r) 
 {
-
   const auto& feature = map_server[feature_id];
 
   // Check how many camera states in the provided camera
@@ -1792,7 +1825,7 @@ bool MsckfVio::PhotometricFeatureJacobian(
 
   MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
       21+state_server.cam_states.size()*7);
-  MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+1);
+  MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); // CHANGED N*N+1 to 1
   VectorXd r_i = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
 
@@ -1803,7 +1836,7 @@ bool MsckfVio::PhotometricFeatureJacobian(
     Eigen::VectorXd r_l = VectorXd::Zero(N*N);
 
     if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l))
-      continue;
+          continue;
 
     auto cam_state_iter = state_server.cam_states.find(cam_id);
     int cam_state_cntr = std::distance(
@@ -1815,19 +1848,25 @@ bool MsckfVio::PhotometricFeatureJacobian(
     r_i.segment(stack_cntr, N*N) = r_l;
     stack_cntr += N*N;
   }
-  if(stack_cntr < 2*N*N)
-    return false;
+  // if not enough to propper nullspace (in paper implementation)
+  if(stack_cntr < N*N)
+     return false;
 
   // Project the residual and Jacobians onto the nullspace
   // of H_yj.
 
   // get Nullspace
+
+  bool valid = false;
+  for(int i = 0; i < H_yi.rows(); i++)
+    if(H_yi(i,0) != 0)
+      valid = true;
+
   FullPivLU<MatrixXd> lu(H_yi.transpose());
   MatrixXd A_null_space = lu.kernel();
 
   H_x = A_null_space.transpose() * H_xi;
   r = A_null_space.transpose() * r_i;
-
   //cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r)  << endl;
 
   if(PRINTIMAGES)
@@ -1956,6 +1995,7 @@ void MsckfVio::measurementJacobian(
   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));
 
+  // cout << "h reg: \n" << r(0) << "\n" << r(1) << endl;
   return;
 }
 
@@ -2333,9 +2373,8 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
     SPQR<SparseMatrix<double> > spqr_helper;
     spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);
 
-    cout << "comp" << endl;
     spqr_helper.compute(H_sparse);
-    cout << "done" << endl;
+    
     MatrixXd H_temp;
     VectorXd r_temp;
 
@@ -2364,6 +2403,9 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
 
   VectorXd delta_x = K * r;
   // Update the IMU state.
+  cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
+
+
   if (FILTER != 1) return;
 
   if(PRINTIMAGES)
@@ -2387,9 +2429,6 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
 
     myfile.close();
   }
-
-  cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
-
   const VectorXd& delta_x_imu = delta_x.head<21>();
 
   if (//delta_x_imu.segment<3>(0).norm() > 0.15 ||
@@ -2445,7 +2484,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
 
 
 bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
-
+  return true;
   MatrixXd P1 = H * state_server.state_cov * H.transpose();
 
   MatrixXd P2 = Feature::observation_noise *
@@ -2453,8 +2492,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
 
   double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
 
-    //cout << "gate: " << dof << " " << gamma << " " <<
-    //chi_squared_test_table[dof] << endl;
+    cout << "gate: " << dof << " " << gamma << " " <<
+    chi_squared_test_table[dof] << endl;
 
     if (chi_squared_test_table[dof] == 0)
       return false;
@@ -2562,7 +2601,7 @@ void MsckfVio::removeLostFeatures() {
     MatrixXd twoH_xj;
     VectorXd twor_j;
     
-    if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true)
+    if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j))
     {
         if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
         pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
@@ -2747,7 +2786,7 @@ void MsckfVio::pruneLastCamStateBuffer()
 
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
     {
-      if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
+        if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
         pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
         pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
         pstack_cntr += pH_xj.rows();
@@ -2769,6 +2808,7 @@ void MsckfVio::pruneLastCamStateBuffer()
       twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
       twostack_cntr += twoH_xj.rows();
     }
+
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);
   }
@@ -2928,7 +2968,7 @@ void MsckfVio::pruneCamStateBuffer() {
     
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
     { 
-      if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
+        if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
         pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
         pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
         pstack_cntr += pH_xj.rows();