cache

include/msckf_vio/feature.hpp

@@ -206,12 +206,6 @@ struct Feature {
                                const StateIDType& cam_state_id,
                                std::vector<float>& return_i) const;
 
-  /*
-  * @brief AnchorPixelToPosition uses the calcualted pixels
-  *     of the anchor patch to generate 3D positions of all of em
-  */
-inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p,
-          const CameraCalibration& cam);
 
   /*
   * @brief Irradiance returns irradiance value of a pixel
@@ -821,20 +815,6 @@ Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCal
   //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
 }
 
-
-Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam)
-{
-  // use undistorted position of point of interest
-  // project it back into 3D space using pinhole model
-  // save resulting NxN positions for this feature
-
-  Eigen::Vector3d PositionInCamera(in_p.x/anchor_rho, in_p.y/anchor_rho, 1/anchor_rho);
-  Eigen::Vector3d PositionInWorld = T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
-  return PositionInWorld;
-  //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
-}
-
-
 //@test center projection must always be initial feature projection
 bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
 {

src/msckf_vio.cpp

@@ -1231,9 +1231,11 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
   // one line of the NxN Jacobians
   Matrix<double, 1, 6> H_xi;
+  Matrix<double, 1, 6> H_xA;
   Matrix<double, 1, 3> H_fi;
 
   MatrixXd H_xl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 6);
+  MatrixXd H_xAl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 6);
   MatrixXd H_yl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 3);
 
   auto frame = cam0.moving_window.find(cam_state_id)->second.image;
@@ -1301,34 +1303,12 @@ void MsckfVio::PhotometricMeasurementJacobian(
     H_xi = dI_dhj*dz_dpc0*dpc0_dxc;
     H_fi = dI_dhj*dz_dpc0*dpc0_dpg;
 
-    gradientVector.x += dx;
-    gradientVector.y += dy; 
-    
-    residualVector.x += dx * photo_r[count];
-    residualVector.y += dy * photo_r[count];
-    res_sum += photo_r[count];
-
-    //dh / d{}^Cp_{ij}
-    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));
-
     dCpij_dGpij = quaternionToRotation(cam_state.orientation);
 
     //orientation takes camera frame to world frame, we wa
-    dh_dGpij = dh_dCpij * dCpij_dGpij;
+    dh_dGpij = dz_dpc0 * dCpij_dGpij;
 
-    //dh / d X_{pl}
+    double rho = feature.rh
-    dCpij_dCGtheta = skewSymmetric(p_c0);
-    dCpij_dGpC = -quaternionToRotation(cam_state.orientation);
-    dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta;
-    dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC;
-
-    //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_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
                                  * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), 
@@ -1336,8 +1316,11 @@ void MsckfVio::PhotometricMeasurementJacobian(
                                                    1/(rho)));
     dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
 
+    H_xA = dI_dhj*dh_dGpij*dGpj_XpAj
+
     H_xl.block<1, 6>(count, 0) = H_xi;
     H_yl.block<1, 3>(count, 0) = H_fi;
+    H_xAl.block<1, 6>(count, 0) = H_xA;
 
     count++;
   }
@@ -1350,12 +1333,12 @@ void MsckfVio::PhotometricMeasurementJacobian(
   Eigen::VectorXd r_l = Eigen::VectorXd::Zero(count-1);
 
   for(int i = 0; i < r_l.size(); i++)
-    r_l(i) = z_irr[i]- z_irr_est[i];
+    r_l(i) = z_irr[i]- feature.anchorPatch[i];
 
 
   r = r_l;
   H_x = H_xl;
-  H_y = H_yl;
+  H_y = H_xAl;
 
 
   //TODO make this more fluent as well
@@ -1438,32 +1421,21 @@ void MsckfVio::PhotometricFeatureJacobian(
     int cam_state_cntr = std::distance(
         state_server.cam_states.begin(), cam_state_iter);
 
+    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);
+
+
     // Stack the Jacobians.
+    H_xi.block(stack_cntr, 21+cam_state_cntr_anchor*7, H_yl.rows(), H_yl.cols()) = -H_yi;
     H_xi.block(stack_cntr, 21+cam_state_cntr*7, H_xl.rows(), H_xl.cols()) = H_xl;
-    H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
+
+
     r_i.segment(stack_cntr, N*N) = r_l;
     stack_cntr += N*N;
   }
 
-  // Project the residual and Jacobians onto the nullspace
+  H_x = H_xi;
-  // of H_yj.
+  r = r_i;
-
-  // 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-12)
-      sv_size++;
-
-  MatrixXd A = svd_helper.matrixU().rightCols(jacobian_row_size - singularValues.size());
-  */
-  H_x = A_null_space.transpose() * H_xi;
-  r = A_null_space.transpose() * r_i;
 
   ofstream myfile;
   myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");