added scaling changes to dI/dh for pixel size

include/msckf_vio/feature.hpp

@@ -148,6 +148,14 @@ struct Feature {
   inline bool initializePosition(
       const CamStateServer& cam_states);
 
+  cv::Point2f pixelDistanceAt(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  const CameraCalibration& cam,
+                  Eigen::Vector3d& in_p) const;
+
+
+
 /*
 *  @brief project PositionToCamera Takes a 3d position in a world frame
 *         and projects it into the passed camera frame using pinhole projection
@@ -737,6 +745,37 @@ float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
   return ((float)image.at<uint8_t>(pose.y, pose.x))/255;
 }
 
+cv::Point2f Feature::pixelDistanceAt(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  const CameraCalibration& cam,
+                  Eigen::Vector3d& in_p) const
+{
+
+  cv::Point2f cam_p = projectPositionToCamera(cam_state, cam_state_id, cam, in_p);
+
+  // create vector of patch in pixel plane
+  std::vector<cv::Point2f> surroundingPoints;
+  surroundingPoints.push_back(cv::Point2f(cam_p.x+1, cam_p.y));
+  surroundingPoints.push_back(cv::Point2f(cam_p.x-1, cam_p.y));
+  surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y+1));
+  surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y-1));
+
+  std::vector<cv::Point2f> pure;
+  image_handler::undistortPoints(surroundingPoints, 
+                                cam.intrinsics, 
+                                cam.distortion_model, 
+                                cam.distortion_coeffs, 
+                                pure); 
+
+  // returns the absolute pixel distance at pixels one metres away
+  cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y));
+
+  return distance;
+}
+
+
+
 cv::Point2f Feature::projectPositionToCamera(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,

launch/msckf_vio_tum.launch

@@ -18,13 +18,13 @@
       output="screen">
 
       <!-- Photometry Flag-->
-      <param name="PHOTOMETRIC" value="true"/>
+      <param name="PHOTOMETRIC" value="false"/>
 
       <!-- Debugging Flaggs -->
-      <param name="PrintImages" value="true"/>
+      <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="5"/>
+      <param name="patch_size_n" value="3"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

src/msckf_vio.cpp

@@ -1225,6 +1225,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
   const Feature& feature = map_server[feature_id];
 
+  const StateIDType anchor_state_id = feature.observations.begin()->first;
+  const CAMState anchor_state = state_server.cam_states[anchor_state_id];
+
   // Cam0 pose.
   Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
   const Vector3d& t_c0_w = cam_state.position;
@@ -1263,7 +1266,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   Eigen::MatrixXd H_pA(N*N, 6);
 
   auto frame = cam0.moving_window.find(cam_state_id)->second.image;
-
+  auto anchor_frame = cam0.moving_window.find(anchor_state_id)->second.image;
   //observation
   const Vector4d& z = feature.observations.find(cam_state_id)->second;
 
@@ -1291,8 +1294,10 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
   for (auto point : feature.anchorPatch_3d)
   {
+    //cout << "____feature-measurement_____\n" << endl;
     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);
 
     //add observation
     photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
@@ -1300,13 +1305,17 @@ void MsckfVio::PhotometricMeasurementJacobian(
       //calculate photom. residual
     photo_r.push_back(photo_z[count] - estimate_photo_z[count]);
 
-    // add jacobians
+    //cout << "residual: " << photo_r.back() << endl;
 
+    // add jacobians
+    cv::Point2f pixelDistance = feature.pixelDistanceAt(anchor_state, anchor_state_id, cam0, point);
+
+    // calculate derivation for anchor frame, use position for derivation calculation
     // frame derivative calculated convoluting with kernel [-1, 0, 1]
-    dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame);
+    dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame);
-    dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame);
+    dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame);
-    dI_dhj(0, 0) = dx;
+    dI_dhj(0, 0) = dx/pixelDistance.x;
-    dI_dhj(0, 1) = dy;
+    dI_dhj(0, 1) = dy/pixelDistance.y;
 
     gradientVector.x += dx;
     gradientVector.y += dy; 
@@ -1332,8 +1341,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
     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
+    // 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));
 
@@ -1345,13 +1355,18 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
     // Intermediate Jakobians
     H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1
-    H_plj = dI_dhj * dh_dXplj; // 1 x 6
+    H_plj =  dI_dhj * dh_dXplj; // 1 x 6
-    H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
+    H_pAj =  dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
 
     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;
 
+    //cout << "H_pl\n" << H_plj << endl;
+   
+    //cout << "H_pA\n" << H_pAj << endl;
+
+    //cout << "H_rho\n" << H_rhoj << endl;
     count++;
   }
 
@@ -1491,15 +1506,18 @@ void MsckfVio::PhotometricFeatureJacobian(
 
   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;
+  if(PRINTIMAGES)
-  myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
+  {
-  myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x. * r << endl;
+    ofstream myfile;
-  myfile.close();
+    myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
-  cout << "---------- LOGGED -------- " << endl; 
+    myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
-*/
+    myfile.close();
+    cout << "---------- LOGGED -------- " << endl; 
+  }
   if(PRINTIMAGES)
   {
     std::cout << "resume playback" << std::endl;
@@ -1642,7 +1660,7 @@ void MsckfVio::featureJacobian(
   
   ofstream myfile;
   myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
-  myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.ldlt().solve(r) << endl;
+  myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
   myfile.close();
   cout << "---------- LOGGED -------- " << endl; 
   return;
@@ -1866,7 +1884,7 @@ void MsckfVio::removeLostFeatures() {
     else
       featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
 
-    if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) {
+    if (gatingTest(H_xj, r_j, r_j.size())) { //, 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;
       stack_cntr += H_xj.rows();
@@ -2028,7 +2046,7 @@ void MsckfVio::pruneCamStateBuffer() {
     else
       featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
 
-    if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {
+    if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
       r.segment(stack_cntr, r_j.rows()) = r_j;
       stack_cntr += H_xj.rows();