diff --git a/config/camchain-imucam-euroc.yaml b/config/camchain-imucam-euroc.yaml
index de4c321..ad2572c 100644
--- a/config/camchain-imucam-euroc.yaml
+++ b/config/camchain-imucam-euroc.yaml
@@ -9,7 +9,7 @@ cam0:
                      0,                   0,                   0,   1.000000000000000]
   camera_model: pinhole
   distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
-  distortion_model: radtan
+  distortion_model: pre-radtan
   intrinsics: [458.654, 457.296, 367.215, 248.375]
   resolution: [752, 480]
   timeshift_cam_imu: 0.0
@@ -26,7 +26,7 @@ cam1:
                      0,                   0,                   0,   1.000000000000000]
   camera_model: pinhole
   distortion_coeffs: [-0.28368365,  0.07451284, -0.00010473, -3.55590700e-05]
-  distortion_model: radtan
+  distortion_model: pre-radtan
   intrinsics: [457.587, 456.134, 379.999, 255.238]
   resolution: [752, 480]
   timeshift_cam_imu: 0.0
diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp
index 5cc997f..44f2a9a 100644
--- a/include/msckf_vio/feature.hpp
+++ b/include/msckf_vio/feature.hpp
@@ -823,11 +823,10 @@ bool Feature::VisualizePatch(
   cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N), 
     cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
 
-  cv::Point2f p_f(observations.find(cam_state_id)->second(0)*cam.intrinsics[0] + cam.intrinsics[2],observations.find(cam_state_id)->second(1)*cam.intrinsics[1] + cam.intrinsics[3]);
+  cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
   // move to real pixels
 
   p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs);
-
   for(int i = 0; i<N; i++)
   {
     for(int j = 0; j<N ; j++)
diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch
index ec463d9..26ceacd 100644
--- a/launch/image_processor_tinytum.launch
+++ b/launch/image_processor_tinytum.launch
@@ -27,9 +27,9 @@
       <param name="ransac_threshold" value="3"/>
       <param name="stereo_threshold" value="5"/>
 
-      <remap from="~imu" to="/imu02"/>
-      <remap from="~cam0_image" to="/cam0/image_raw2"/>
-      <remap from="~cam1_image" to="/cam1/image_raw2"/>
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
 
 
     </node>
diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch
index 864f15e..8f59c97 100644
--- a/launch/msckf_vio_euroc.launch
+++ b/launch/msckf_vio_euroc.launch
@@ -19,11 +19,11 @@
 
 
       <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="FILTER" value="0"/>
+      <param name="FILTER" value="1"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
-      <param name="PrintImages" value="false"/>
+      <param name="PrintImages" value="true"/>
       <param name="GroundTruth" value="false"/>
 
       <param name="patch_size_n" value="3"/>
diff --git a/src/image_processor.cpp b/src/image_processor.cpp
index 65acd43..9242b15 100644
--- a/src/image_processor.cpp
+++ b/src/image_processor.cpp
@@ -226,8 +226,16 @@ void ImageProcessor::stereoCallback(
 
   ros::Time start_time = ros::Time::now();
 
-  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);
+
+  cv::Mat new_cam0;
+  cv::Mat new_cam1;
+
+  image_handler::undistortImage(cam0_curr_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
+  image_handler::undistortImage(cam1_curr_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
+  
+  new_cam0.copyTo(cam0_curr_img_ptr->image);
+  new_cam1.copyTo(cam1_curr_img_ptr->image);
+
   //ROS_INFO("Publishing: %f",
   //    (ros::Time::now()-start_time).toSec());
 
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index f6fa93f..c9cb9e9 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -544,13 +544,14 @@ void MsckfVio::manageMovingWindow(
   cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img,
       sensor_msgs::image_encodings::MONO8);
 
-  image_handler::undistortImage(cam0_img_ptr->image, cam0_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
-  image_handler::undistortImage(cam1_img_ptr->image, cam1_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
-
+  cv::Mat new_cam0;
+  cv::Mat new_cam1;
+  image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
+  image_handler::undistortImage(cam1_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
 
   // save image information into moving window
-  cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone();
-  cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone();
+  cam0.moving_window[state_server.imu_state.id].image = new_cam0.clone();
+  cam1.moving_window[state_server.imu_state.id].image = new_cam1.clone();
 
   cv::Mat xder;
   cv::Mat yder;
@@ -1704,7 +1705,8 @@ bool MsckfVio::PhotometricPatchPointJacobian(
   dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0];
   dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1];
 
-  //cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl;
+  cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl;
+  cout << dI_dhj(1, 2) << ", " << dI_dhj(1, 3) << endl;
 
   // add jacobian
   
@@ -1782,6 +1784,8 @@ bool MsckfVio::PhotometricMeasurementJacobian(
   if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo))
     return false;
 
+  cout << "-\n" << r_photo << "-" << endl;
+
   //cout << "r\n" << r_photo  << endl;
   // calculate jacobian for patch
   int count = 0;