diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch
index 1c2c689..1096e37 100644
--- a/launch/msckf_vio_tum.launch
+++ b/launch/msckf_vio_tum.launch
@@ -22,10 +22,10 @@
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
-      <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="1"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index 9a31f42..d6f8326 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -554,15 +554,19 @@ void MsckfVio::manageMovingWindow(
 
 
   // undistort Images
+
+  const cv::Matx33d K(cam0.intrinsics[0], 0.0, cam0.intrinsics[2],
+                      0.0, cam0.intrinsics[1], cam0.intrinsics[3],
+                      0.0, 0.0, 1.0);
   
-  //cv::Mat undistortedCam0;
-  //cv::Mat undistortedCam1;
+  cv::Mat undistortedCam0;
+  cv::Mat undistortedCam1;
 
-  //image_handler::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
-  //image_handler::undistortImage(cam1.moving_window[state_server.imu_state.id].image, undistortedCam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
+  image_handler::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
+  image_handler::undistortImage(cam1.moving_window[state_server.imu_state.id].image, undistortedCam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
 
-  //cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone();
-  //cam1.moving_window[state_server.imu_state.id].image = undistortedCam1.clone();
+  cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone();
+  cam1.moving_window[state_server.imu_state.id].image = undistortedCam1.clone();
 
   //TODO handle any massive overflow correctly (should be pruned, before this ever triggers)
   while(cam0.moving_window.size() > 100)
@@ -2061,20 +2065,20 @@ void MsckfVio::removeLostFeatures() {
     MatrixXd pH_xj;
     VectorXd pr_j;
 
-    //PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
+    PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
     featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
     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();
     }  
-    /*
+    
     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();
     }
-    */
+
     // Put an upper bound on the row size of measurement Jacobian,
     // which helps guarantee the executation time.
     //if (stack_cntr > 1500) break;
@@ -2087,7 +2091,7 @@ void MsckfVio::removeLostFeatures() {
   
   // Perform the measurement update step.
   measurementUpdate(H_x, r);
-  //photometricMeasurementUpdate(pH_x, pr);
+  photometricMeasurementUpdate(pH_x, pr);
 
   // Remove all processed features from the map.
   for (const auto& feature_id : processed_feature_ids)
@@ -2217,7 +2221,7 @@ void MsckfVio::pruneLastCamStateBuffer()
     for (const auto& cam_state : state_server.cam_states)
       involved_cam_state_ids.push_back(cam_state.first);
 
-      //PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
+      PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
       featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
 
       if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
@@ -2226,13 +2230,11 @@ void MsckfVio::pruneLastCamStateBuffer()
       stack_cntr += H_xj.rows();
       pruned_cntr++;
       }
-      /*
     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();
     }
-    */
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);
   }
@@ -2245,7 +2247,7 @@ void MsckfVio::pruneLastCamStateBuffer()
   // Perform measurement update.
 
   measurementUpdate(H_x, r);
-  //photometricMeasurementUpdate(pH_x, pr);
+  photometricMeasurementUpdate(pH_x, pr);
 
   //reduction
   int cam_sequence = std::distance(state_server.cam_states.begin(),
@@ -2372,7 +2374,7 @@ void MsckfVio::pruneCamStateBuffer() {
 
     if (involved_cam_state_ids.size() == 0) continue;
     
-    //PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
+    PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
     featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     
     if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
@@ -2380,13 +2382,13 @@ void MsckfVio::pruneCamStateBuffer() {
       r.segment(stack_cntr, r_j.rows()) = r_j;
       stack_cntr += H_xj.rows();
     }
-    /*
+  
     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();
     }
-    */
+
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);
   }
@@ -2399,7 +2401,7 @@ void MsckfVio::pruneCamStateBuffer() {
 
   // Perform measurement update.
   measurementUpdate(H_x, r);
-  //photometricMeasurementUpdate(pH_x, pr);
+  photometricMeasurementUpdate(pH_x, pr);
 
   //reduction
   for (const auto& cam_id : rm_cam_state_ids) {