made residual change more direct. starts diverging but then sometimes appriximates correctly after some camera movement

src/msckf_vio.cpp

@@ -1258,8 +1258,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
   auto point = feature.anchorPatch_3d[0];
 
   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);
-
   // add jacobian
   
   //dh / d{}^Cp_{ij}
@@ -1309,16 +1307,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
   
   //calculate residual
 
-  //project pixel point to 'pure' position
-  cv::Point2f und_p_in_c0;
-  image_handler::undistortPoint(p_in_c0,
-                                cam0.intrinsics,
-                                cam0.distortion_model,
-                                cam0.distortion_coeffs,
-                                und_p_in_c0);
-
-  r_i[0] = z[0] - und_p_in_c0.x;
-  r_i[1] = z[1] - und_p_in_c0.y;
+  r_i[0] = z[0] - p_c0(0)/p_c0(2);
+  r_i[1] = z[1] - p_c0(1)/p_c0(2);
 
   MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7);
  
@@ -1376,6 +1366,8 @@ void MsckfVio::PhotometricFeatureJacobian(
     if (feature.observations.find(cam_id) ==
         feature.observations.end()) continue;
 
+    if (feature.observations.find(cam_id) ==
+        feature.observations.begin()) continue;
     valid_cam_state_ids.push_back(cam_id);
   }