restructured calcualtion of patches in code

include/msckf_vio/msckf_vio.h

@@ -202,7 +202,8 @@ class MsckfVio {
         Eigen::Vector4d& r);
     // This function computes the Jacobian of all measurements viewed
     // in the given camera states of this feature.
-    void featureJacobian(const FeatureIDType& feature_id,
+    void featureJacobian(
+        const FeatureIDType& feature_id,
         const std::vector<StateIDType>& cam_state_ids,
         Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
 
@@ -212,6 +213,29 @@ class MsckfVio {
         const FeatureIDType& feature_id,
         Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r);
 
+    bool ConstructJacobians(
+        Eigen::MatrixXd& H_rho,
+        Eigen::MatrixXd& H_pl,
+        Eigen::MatrixXd& H_pA,
+        const Feature& feature,
+        const StateIDType&  cam_state_id,
+        Eigen::MatrixXd& H_xl,
+        Eigen::MatrixXd& H_yl);
+
+    bool PhotometricPatchPointResidual(
+        const StateIDType& cam_state_id,
+        const Feature& feature, 
+        Eigen::VectorXd& r);
+
+    bool PhotometricPatchPointJacobian(
+        const CAMState& cam_state,
+        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);
+
     bool PhotometricMeasurementJacobian(
         const StateIDType& cam_state_id,
         const FeatureIDType& feature_id,
@@ -263,6 +287,13 @@ class MsckfVio {
     // Chi squared test table.
     static std::map<int, double> chi_squared_test_table;
 
+    double eval_time;
+
+    IMUState timed_old_imu_state;
+    IMUState timed_old_true_state;
+
+    IMUState old_imu_state;
+    IMUState old_true_state;
 
     // change in position
     Eigen::Vector3d delta_position;

launch/msckf_vio_tinytum.launch

@@ -18,14 +18,14 @@
       output="screen">
 
       <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="FILTER" value="1"/>
+      <param name="FILTER" value="0"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" 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/image_processor.cpp

@@ -223,8 +223,8 @@ void ImageProcessor::stereoCallback(
 
   image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
   image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
-  ROS_INFO("Publishing: %f",
+  //ROS_INFO("Publishing: %f",
-      (ros::Time::now()-start_time).toSec());
+  //    (ros::Time::now()-start_time).toSec());
   // Build the image pyramids once since they're used at multiple places
   createImagePyramids();
 

src/msckf_vio.cpp

@@ -260,6 +260,7 @@ bool MsckfVio::createRosIO() {
   // activating bag playing parameter, for debugging
   nh.setParam("/play_bag", true);
 
+  eval_time = 0;
   odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
   truth_odom_pub = nh.advertise<nav_msgs::Odometry>("true_odom", 10);
   feature_pub = nh.advertise<sensor_msgs::PointCloud2>(
@@ -412,6 +413,10 @@ void MsckfVio::imageCallback(
     return;
   }
 
+  old_imu_state = state_server.imu_state;
+  old_true_state = true_state_server.imu_state;
+
+
   // Start the system if the first image is received.
   // The frame where the first image is received will be
   // the origin.
@@ -419,7 +424,6 @@ void MsckfVio::imageCallback(
     is_first_img = false;
     state_server.imu_state.time = feature_msg->header.stamp.toSec();
   }
-
   static double max_processing_time = 0.0;
   static int critical_time_cntr = 0;
   double processing_start_time = ros::Time::now().toSec();
@@ -428,23 +432,13 @@ void MsckfVio::imageCallback(
   // that are received before the image feature_msg.
   ros::Time start_time = ros::Time::now();
 
+  nh.param<int>("FILTER", FILTER, 0);  
 
   batchImuProcessing(feature_msg->header.stamp.toSec());
 
-  if(GROUNDTRUTH)
-  {
-    state_server.imu_state.position = true_state_server.imu_state.position;
-    state_server.imu_state.orientation = true_state_server.imu_state.orientation;
-    state_server.imu_state.position_null = true_state_server.imu_state.position_null;
-    state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null;
-
-    state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0;
-    state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu;
-  }
   double imu_processing_time = (
       ros::Time::now()-start_time).toSec();
 
-  cout << "1" << endl;
   // Augment the state vector.
   start_time = ros::Time::now();
   //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
@@ -453,7 +447,7 @@ void MsckfVio::imageCallback(
       ros::Time::now()-start_time).toSec();
 
 
-  cout << "2" << endl;
+  // cout << "2" << endl;
   // Add new observations for existing features or new
   // features in the map server.
   start_time = ros::Time::now();
@@ -462,7 +456,7 @@ void MsckfVio::imageCallback(
       ros::Time::now()-start_time).toSec();
 
 
-  cout << "3" << endl;
+  // cout << "3" << endl;
   // Add new images to moving window
   start_time = ros::Time::now();
   manageMovingWindow(cam0_img, cam1_img, feature_msg);
@@ -470,14 +464,14 @@ void MsckfVio::imageCallback(
       ros::Time::now()-start_time).toSec();
 
 
-  cout << "4" << endl;
+  // cout << "4" << endl;
   // Perform measurement update if necessary.
   start_time = ros::Time::now();
   removeLostFeatures();
   double remove_lost_features_time = (
       ros::Time::now()-start_time).toSec();
 
-  cout << "5" << endl;
+  // cout << "5" << endl;
   start_time = ros::Time::now();
   pruneLastCamStateBuffer();
   double prune_cam_states_time = (
@@ -487,7 +481,7 @@ void MsckfVio::imageCallback(
   batchTruthProcessing(feature_msg->header.stamp.toSec());
 
 
-  cout << "6" << endl;
+  // cout << "6" << endl;
   // Publish the odometry.
   start_time = ros::Time::now();
   publish(feature_msg->header.stamp);
@@ -504,18 +498,18 @@ void MsckfVio::imageCallback(
     ++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",
+    //printf("IMU processing time: %f/%f\n",
-        imu_processing_time, imu_processing_time/processing_time);
+    //    imu_processing_time, imu_processing_time/processing_time);
-    printf("State augmentation time: %f/%f\n",
+    //printf("State augmentation time: %f/%f\n",
-        state_augmentation_time, state_augmentation_time/processing_time);
+    //    state_augmentation_time, state_augmentation_time/processing_time);
-    printf("Add observations time: %f/%f\n",
+    //printf("Add observations time: %f/%f\n",
-        add_observations_time, add_observations_time/processing_time);
+    //    add_observations_time, add_observations_time/processing_time);
-    printf("Remove lost features time: %f/%f\n",
+    //printf("Remove lost features time: %f/%f\n",
-        remove_lost_features_time, remove_lost_features_time/processing_time);
+    //    remove_lost_features_time, remove_lost_features_time/processing_time);
-    printf("Remove camera states time: %f/%f\n",
+    //printf("Remove camera states time: %f/%f\n",
-        prune_cam_states_time, prune_cam_states_time/processing_time);
+    //    prune_cam_states_time, prune_cam_states_time/processing_time);
-    printf("Publish time: %f/%f\n",
+    //printf("Publish time: %f/%f\n",
-        publish_time, publish_time/processing_time);
+    //    publish_time, publish_time/processing_time);
   }
 
   if(STREAMPAUSE)
@@ -806,8 +800,6 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
   // Counter how many IMU msgs in the buffer are used.
   int used_truth_msg_cntr = 0;
 
-  const IMUState old_true_state = true_state_server.imu_state;
-
   for (const auto& truth_msg : truth_msg_buffer) {
     double truth_time = truth_msg.header.stamp.toSec();
     if (truth_time < true_state_server.imu_state.time) {
@@ -839,22 +831,76 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
   truth_msg_buffer.erase(truth_msg_buffer.begin(),
       truth_msg_buffer.begin()+used_truth_msg_cntr);
 
+  /*
+  // calculate change
+  delta_position = state_server.imu_state.position - old_imu_state.position;
+  Eigen::Vector4d delta_orientation_quaternion = quaternionMultiplication(quaternionConjugate(state_server.imu_state.orientation), old_imu_state.orientation);
+  delta_orientation = Eigen::Vector3d(delta_orientation_quaternion[0]*2, delta_orientation_quaternion[1]*2, delta_orientation_quaternion[2]*2);
   // calculate error in position
 
   // calculate error in change
 
-  // calculate change
   Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - old_true_state.position;
   Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), old_true_state.orientation);
   Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2);
   Eigen::Vector3d error_delta_position = delta_true_position - delta_position;
   Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation  - delta_orientation;
 
-  double dx  = (error_delta_position[0]/delta_true_position[0]);
+  Eigen::Vector3d error_position = true_state_server.imu_state.position - state_server.imu_state.position;
-  double dy = (error_delta_position[1]/delta_true_position[1]);
+  Eigen::Vector4d error_orientation_q = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), state_server.imu_state.orientation);
-  double dz = (error_delta_position[2]/delta_true_position[2]);
+  Eigen::Vector3d error_orientation = Eigen::Vector3d(error_orientation_q[0]*2, error_orientation_q[1]*2, error_orientation_q[2]*2);
-  cout << "relative pos error: " << sqrt(dx*dx + dy*dy + dz*dz) * 100 << "%" << endl;
 
+  double relerr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/
+               sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2]));
+ 
+  double relOerr = (sqrt(error_delta_orientation[0]*error_delta_orientation[0] + error_delta_orientation[1]*error_delta_orientation[1] + error_delta_orientation[2]*error_delta_orientation[2])/
+               sqrt(delta_smallangle_true_orientation[0]*delta_smallangle_true_orientation[0] + delta_smallangle_true_orientation[1]*delta_smallangle_true_orientation[1] + delta_smallangle_true_orientation[2]*delta_smallangle_true_orientation[2]));
+
+  double abserr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/
+               sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2]));
+
+
+  cout << "relative pos error: " << relerr * 100 << "%" << endl;
+  cout << "relative ori error: " << relOerr * 100 << "%" << endl;
+  //cout << "absolute pos error: " << 
+  */
+  if (eval_time + 1 < ros::Time::now().toSec())
+  {
+
+     // calculate change
+    delta_position = state_server.imu_state.position - timed_old_imu_state.position;
+    Eigen::Vector4d delta_orientation_quaternion = quaternionMultiplication(quaternionConjugate(state_server.imu_state.orientation), timed_old_imu_state.orientation);
+    delta_orientation = Eigen::Vector3d(delta_orientation_quaternion[0]*2, delta_orientation_quaternion[1]*2, delta_orientation_quaternion[2]*2);
+    // calculate error in position
+
+    // calculate error in change
+
+    Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - timed_old_true_state.position;
+    Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), timed_old_true_state.orientation);
+    Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2);
+    Eigen::Vector3d error_delta_position = delta_true_position - delta_position;
+    Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation  - delta_orientation;
+
+    Eigen::Vector3d error_position = true_state_server.imu_state.position - state_server.imu_state.position;
+    Eigen::Vector4d error_orientation_q = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), state_server.imu_state.orientation);
+    Eigen::Vector3d error_orientation = Eigen::Vector3d(error_orientation_q[0]*2, error_orientation_q[1]*2, error_orientation_q[2]*2);
+
+    double relerr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/
+                 sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2]));
+   
+    double relOerr = (sqrt(error_delta_orientation[0]*error_delta_orientation[0] + error_delta_orientation[1]*error_delta_orientation[1] + error_delta_orientation[2]*error_delta_orientation[2])/
+                 sqrt(delta_smallangle_true_orientation[0]*delta_smallangle_true_orientation[0] + delta_smallangle_true_orientation[1]*delta_smallangle_true_orientation[1] + delta_smallangle_true_orientation[2]*delta_smallangle_true_orientation[2]));
+
+    double abserr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/
+                 sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2]));
+
+    // cout << "relative pos error: " << relerr * 100 << "%" << endl;
+    // cout << "relative ori error: " << relOerr * 100 << "%" << endl;
+
+    timed_old_imu_state = state_server.imu_state;
+    timed_old_true_state = true_state_server.imu_state;
+    eval_time = ros::Time::now().toSec();
+  }
 }
 
 void MsckfVio::processTruthtoIMU(const double& time,
@@ -1477,17 +1523,60 @@ void MsckfVio::twodotFeatureJacobian(
   return;
 }
 
-
+bool MsckfVio::PhotometricPatchPointResidual(
-
-bool MsckfVio::PhotometricMeasurementJacobian(
   const StateIDType& cam_state_id,
-    const FeatureIDType& feature_id,
+  const Feature& feature, 
-    MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) 
+  VectorXd& r)
 {
 
-  // Prepare all the required data.
+  VectorXd r_photo = VectorXd::Zero(N*N);
+  int count = 0;
+  auto frame = cam0.moving_window.find(cam_state_id)->second.image;
+
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
-  const Feature& feature = map_server[feature_id];
+
+  //estimate photometric measurement
+  std::vector<double> estimate_irradiance;
+  std::vector<double> estimate_photo_z;
+  std::vector<double> photo_z;
+
+  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);
+
+
+  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
+    photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
+    
+    //calculate photom. residual
+    r_photo(count) = photo_z[count] - estimate_photo_z[count];
+    count++;
+  }
+  r = r_photo;
+  return true;
+}
+
+// generates the jacobian of one patch point regarding rho, anchor and current frame
+bool MsckfVio::PhotometricPatchPointJacobian(
+    const CAMState& cam_state,
+    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) 
+{
 
   const StateIDType anchor_state_id = feature.observations.begin()->first;
   const CAMState anchor_state = state_server.cam_states[anchor_state_id];
@@ -1502,9 +1591,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
   Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
 
   
-  //photometric observation
-  std::vector<double> photo_z;
-  VectorXd r_photo = VectorXd::Zero(N*N);
   // individual Jacobians
   Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
   Matrix<double, 2, 3> dh_dCpij = Matrix<double, 2, 3>::Zero();
@@ -1517,64 +1603,17 @@ bool MsckfVio::PhotometricMeasurementJacobian(
   Matrix<double, 3, 3> dCpij_dCGtheta = Matrix<double, 3, 3>::Zero();
   Matrix<double, 3, 3> dCpij_dGpC = Matrix<double, 3, 3>::Zero();
 
-  // one line of the NxN Jacobians
-  Eigen::Matrix<double, 1, 1> H_rhoj;
-  Eigen::Matrix<double, 1, 6> H_plj;
-  Eigen::Matrix<double, 1, 6> H_pAj;
-
-  // combined Jacobians
-  Eigen::MatrixXd H_rho(N*N, 1);
-  Eigen::MatrixXd H_pl(N*N, 6);
-  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;
-
-  //estimate photometric measurement
-  std::vector<double> estimate_irradiance;
-  std::vector<double> estimate_photo_z;
-  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 +
-                    estimated_illumination.frame_bias + estimated_illumination.feature_bias);
-
-  int count = 0;
   double dx, dy;
 
-  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);
 
-    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)
-    {
-      cout << "skipped" << endl;
-      return false;
-    }
-    //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];
-    //cout << "residual: " << photo_r.back() << endl;
 
   // 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.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_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 * cam0.intrinsics[0];
   dI_dhj(0, 1) = dy * cam0.intrinsics[1];
 
@@ -1611,6 +1650,45 @@ bool MsckfVio::PhotometricMeasurementJacobian(
   H_plj =  dI_dhj * dh_dXplj; // 1 x 6
   H_pAj =  dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
 
+  return true;
+}
+
+bool MsckfVio::PhotometricMeasurementJacobian(
+    const StateIDType& cam_state_id,
+    const FeatureIDType& feature_id,
+    MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) 
+{
+
+  // Prepare all the required data.
+  const CAMState& cam_state = state_server.cam_states[cam_state_id];
+  const Feature& feature = map_server[feature_id];
+
+  //photometric observation
+  VectorXd r_photo = VectorXd::Zero(N*N);
+
+  // one line of the NxN Jacobians
+  Eigen::Matrix<double, 1, 1> H_rhoj;
+  Eigen::Matrix<double, 1, 6> H_plj;
+  Eigen::Matrix<double, 1, 6> H_pAj;
+
+  // combined Jacobians
+  Eigen::MatrixXd H_rho(N*N, 1);
+  Eigen::MatrixXd H_pl(N*N, 6);
+  Eigen::MatrixXd H_pA(N*N, 6);
+
+  auto frame = cam0.moving_window.find(cam_state_id)->second.image;
+
+  // calcualte residual of patch
+  PhotometricPatchPointResidual(cam_state_id, feature, r_photo);
+
+  // calculate jacobian for patch
+  int count = 0;
+  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);
+  
+    // 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;
@@ -1618,22 +1696,58 @@ bool MsckfVio::PhotometricMeasurementJacobian(
     count++;
   }
 
-  cout << "done" << 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);
 
-  // set anchor Jakobi
+  ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl);
-    // get position of anchor in cam states
 
+
+  // set to return values
+  H_x = H_xl;
+  H_y = H_yl;
+  r = r_photo;
+
+  if(PRINTIMAGES)
+  { 
+    // pregenerating information for visualization
+    std::stringstream ss;
     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);
-  H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA; 
-
-  // set frame Jakobi
     //get position of current frame in cam states
     auto cam_state_iter = state_server.cam_states.find(cam_state_id);
     int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter);
 
+    ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << "  frame: " << cam_state_cntr;
+
+    // visualizing functions
+    feature.MarkerGeneration(marker_pub, state_server.cam_states);
+    feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss);
+    //feature.VisualizeKernel(cam_state, cam_state_id, cam0);
+  }
+
+  return true;
+}
+
+bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
+                                  Eigen::MatrixXd& H_pl,
+                                  Eigen::MatrixXd& H_pA,
+                                  const Feature& feature,
+                                  const StateIDType&  cam_state_id,
+                                  Eigen::MatrixXd& H_xl,
+                                  Eigen::MatrixXd& H_yl)
+{
+  // get position of anchor in cam states
+  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; 
+
+  //get position of current frame in cam states
+  auto cam_state_iter = state_server.cam_states.find(cam_state_id);
+  // set frame Jakobi
+  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;
 
@@ -1643,7 +1757,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
   // 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);
 
-  // TODO make this calculation more fluent
 
   /*for(int i = 0; i< N*N; i++)   
     H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i];
@@ -1651,21 +1764,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
 
   H_yl.block(0, N*N, N*N, 1) = -H_rho;
 
-  H_x = H_xl;
-  H_y = H_yl;
-
-  r = r_photo;
-
-  std::stringstream ss;
-  ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << "  frame: " << cam_state_cntr;
-  if(PRINTIMAGES)
-  {  
-    feature.MarkerGeneration(marker_pub, state_server.cam_states);
-    feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss);
-    //feature.VisualizeKernel(cam_state, cam_state_id, cam0);
-  }
-
-  return true;
 }
 
 
@@ -1717,11 +1815,9 @@ bool MsckfVio::PhotometricFeatureJacobian(
     r_i.segment(stack_cntr, N*N) = r_l;
     stack_cntr += N*N;
   }
-  if(stack_cntr == 0)
+  if(stack_cntr < 2*N*N)
-  {
-    cout << "skip feature" << endl;
     return false;
-  }
+
   // Project the residual and Jacobians onto the nullspace
   // of H_yj.
 
@@ -2027,8 +2123,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
   VectorXd delta_x = K * r;
 
 
-  cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
+  // cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
-  cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
+  cout << "reg: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
 
   if(FILTER != 0) return;
 
@@ -2053,9 +2149,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 
     myfile.close();
   }
-  delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]);
-  delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]);
-
   // Update the IMU state.
   
   const VectorXd& delta_x_imu = delta_x.head<21>();
@@ -2239,10 +2332,13 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
     // Perform QR decompostion on H_sparse.
     SPQR<SparseMatrix<double> > spqr_helper;
     spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);
-    spqr_helper.compute(H_sparse);
 
+    cout << "comp" << endl;
+    spqr_helper.compute(H_sparse);
+    cout << "done" << endl;
     MatrixXd H_temp;
     VectorXd r_temp;
+
     (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp);
     (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp);
 
@@ -2261,6 +2357,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
       Feature::observation_noise*MatrixXd::Identity(
         H_thin.rows(), H_thin.rows());
   //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P);
+
  MatrixXd K_transpose = S.ldlt().solve(H_thin*P);
   MatrixXd K = K_transpose.transpose();
   // Compute the error of the state.
@@ -2291,8 +2388,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
     myfile.close();
   }
 
-  delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]);
+  cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
-  delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]);
 
   const VectorXd& delta_x_imu = delta_x.head<21>();
 
@@ -2349,8 +2445,8 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
 
 
 bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
-  MatrixXd P1 = H * state_server.state_cov * H.transpose();
 
+  MatrixXd P1 = H * state_server.state_cov * H.transpose();
 
   MatrixXd P2 = Feature::observation_noise *
     MatrixXd::Identity(H.rows(), H.rows());
@@ -2363,10 +2459,10 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
     if (chi_squared_test_table[dof] == 0)
       return false;
   if (gamma < chi_squared_test_table[dof]) {
-    //cout << "passed" << endl;
+    // cout << "passed" << endl;
     return true;
   } else {
-    //cout << "failed" << endl;
+    // cout << "failed" << endl;
     return false;
   }
 }
@@ -2466,15 +2562,14 @@ 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) == true);
     {
         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();
       }
-    }*/
+    }
 
     featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
@@ -2503,6 +2598,7 @@ void MsckfVio::removeLostFeatures() {
     photometricMeasurementUpdate(pH_x, pr);
   }
 
+
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
  
@@ -2648,17 +2744,15 @@ void MsckfVio::pruneLastCamStateBuffer()
     for (const auto& cam_state : state_server.cam_states)
       involved_cam_state_ids.push_back(cam_state.first);
 
-    /*
-    if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true);
-    {
 
+    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)) {
         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();
       }
-    }*/
+    }
-
 
     featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
@@ -2679,7 +2773,8 @@ void MsckfVio::pruneLastCamStateBuffer()
       feature.observations.erase(cam_id);
   }
 
-  if (pstack_cntr)
+
+  if(pstack_cntr)
   {
     pH_x.conservativeResize(pstack_cntr, pH_x.cols());
     pr.conservativeResize(pstack_cntr);
@@ -2690,15 +2785,14 @@ void MsckfVio::pruneLastCamStateBuffer()
 
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
-  // Perform measurement update.  
  
   twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
   twor.conservativeResize(twostack_cntr);
-  // Perform measurement update.
-
   
+  // Perform the measurement update step.
   measurementUpdate(H_x, r);
   twoMeasurementUpdate(twoH_x, twor);
+
   //reduction
   int cam_sequence = std::distance(state_server.cam_states.begin(),
       state_server.cam_states.find(rm_cam_state_id));
@@ -2831,7 +2925,7 @@ void MsckfVio::pruneCamStateBuffer() {
 
     if (involved_cam_state_ids.size() == 0) continue;
 
-    /*
+    
     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())) {
@@ -2839,7 +2933,8 @@ void MsckfVio::pruneCamStateBuffer() {
         pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
         pstack_cntr += pH_xj.rows();
       }
-    }*/
+    }
+
     featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
     
@@ -2860,11 +2955,11 @@ void MsckfVio::pruneCamStateBuffer() {
   }
 
 
-  if(pstack_cntr > 0)
+  
+  if(pstack_cntr)
   {
     pH_x.conservativeResize(pstack_cntr, pH_x.cols());
     pr.conservativeResize(pstack_cntr);
-  
     photometricMeasurementUpdate(pH_x, pr);
   }
 
@@ -2874,9 +2969,10 @@ void MsckfVio::pruneCamStateBuffer() {
   twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
   twor.conservativeResize(twostack_cntr);
   
-  // Perform measurement update.
+  // Perform the measurement update step.
   measurementUpdate(H_x, r);
   twoMeasurementUpdate(twoH_x, twor);
+
   //reduction
   for (const auto& cam_id : rm_cam_state_ids) {
     int cam_sequence = std::distance(state_server.cam_states.begin(),