diff --git a/src/image_processor.cpp b/src/image_processor.cpp
index 6867743..b083aaf 100644
--- a/src/image_processor.cpp
+++ b/src/image_processor.cpp
@@ -221,9 +221,13 @@ 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);
-  //ROS_INFO("Publishing: %f",
+  cv::Mat newImage;
+  image_handler::undistortImage(cam0_curr_img_ptr->image, newImage, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
+  newImage.copyTo(cam0_curr_img_ptr->image);
+  image_handler::undistortImage(cam1_curr_img_ptr->image, newImage, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
+  newImage.copyTo( cam1_curr_img_ptr->image);
+
+//ROS_INFO("Publishing: %f",
   //    (ros::Time::now()-start_time).toSec());
   // Build the image pyramids once since they're used at multiple places
   createImagePyramids();
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index 590398e..fe071b1 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -541,14 +541,15 @@ void MsckfVio::manageMovingWindow(
       sensor_msgs::image_encodings::MONO8);
   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 newImage0;
+  cv::Mat newImage1;
+  image_handler::undistortImage(cam0_img_ptr->image, newImage0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
+  image_handler::undistortImage(cam1_img_ptr->image, newImage1, 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 = newImage0.clone();
+  cam1.moving_window[state_server.imu_state.id].image = newImage1.clone();
 
   cv::Mat xder;
   cv::Mat yder;