diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3bc3e9b..985bc6b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -38,6 +38,8 @@ add_executable(Fly_Capture
     CV.c High_GUI2.c
 )
 
+add_executable(undistort undistort.c)
+
 include_directories(/usr/include/flycapture)
 
 target_link_libraries(FlyCapture2Test flycapture-c flycapture)
@@ -49,6 +51,8 @@ target_link_libraries(Map_Test m ${OpenCV_LIBS})
 target_link_libraries(Video_Capture m ${OpenCV_LIBS})
 target_link_libraries(Fly_Capture flycapture-c flycapture m ${OpenCV_LIBS})
 
+target_link_libraries(undistort ${OpenCV_LIBS})
+
 catkin_package(
     CATKIN_DEPENDS geometry_msgs tf
 )
diff --git a/calibration/ceiling3-6-02.jpg b/calibration/ceiling3-6-02.jpg
new file mode 100644
index 0000000..441e142
Binary files /dev/null and b/calibration/ceiling3-6-02.jpg differ
diff --git a/calibration/pg_3_6mm.txt b/calibration/pg_3_6mm.txt
new file mode 100644
index 0000000..5f3fe53
--- /dev/null
+++ b/calibration/pg_3_6mm.txt
@@ -0,0 +1 @@
+fc 422.29324 422.24830 cc 341.78551 225.87651 kc -0.35506 0.18746 0.00056 -0.00047
diff --git a/calibration/pg_6mm.txt b/calibration/pg_6mm.txt
new file mode 100644
index 0000000..bd8f049
--- /dev/null
+++ b/calibration/pg_6mm.txt
@@ -0,0 +1 @@
+fc 1003.53326 1003.56907 cc 325.57087 237.92510 kc -0.37209 0.06406 0.00352 0.00262
diff --git a/undistort.c b/undistort.c
new file mode 100644
index 0000000..ce5ca70
--- /dev/null
+++ b/undistort.c
@@ -0,0 +1,112 @@
+
+#include <stdio.h>
+#include <opencv/cv.h>
+#include <opencv/highgui.h>
+
+
+/* 
+ * dump a matrix to stdout
+ */
+void dumpMat(CvMat* M)
+{
+    for (int i=0; i<M->rows; i++) {
+      for (int j=0; j<M->cols; j++)
+          printf("%8.3lf ", cvGetReal2D(M, i, j));
+      printf("\n");
+    }
+}
+
+
+/* 
+ * Read the calibration file and generate the undistortion maps
+ * in:
+ *   calibFile   -  camera calibration file
+ *   w h         -  width and height of images to undistort
+ * out:
+ *   mapx, mapy, - undistortion maps
+ */
+int setupUndistortion(char *calibFile, int w, int h, IplImage** mapx, IplImage** mapy)
+{
+    double fcx, fcy, ccx, ccy;
+    double kc[4];
+  
+    FILE *fp = fopen(calibFile, "r");
+    if (fp == NULL) {
+        fprintf(stderr, "Could not open \"%s\"\n", calibFile);
+        return -1;
+    }
+
+    /*
+     *  format is fc - focal length, cc, principal point, kc distortion vector
+     */
+    int x = fscanf(fp, "fc %lf %lf cc %lf %lf kc %lf %lf %lf %lf %lf", 
+       &fcx, &fcy, &ccx, &ccy, &kc[0], &kc[1], &kc[2], &kc[3]);
+    if (x != 8) {
+        fprintf(stderr, "Expected 8 parameters got %d\n", x);
+        return -1;
+    }
+    
+    double intvec[9] = {
+        fcx,   0, ccx,
+          0, fcy, ccy,
+          0,   0,   1
+    }; 
+    CvMat intrinsic = cvMat(3, 3, CV_64FC1, intvec);
+    printf("intrinsic matrix\n");
+    dumpMat(&intrinsic);
+
+    CvMat distortion = cvMat(1, 4, CV_64FC1, kc);
+    printf("distortion matrix\n");
+    dumpMat(&distortion);
+
+    *mapx = cvCreateImage(cvSize(w, h), IPL_DEPTH_32F, 1);
+    *mapy = cvCreateImage(cvSize(w, h), IPL_DEPTH_32F, 1);
+
+    cvInitUndistortMap(&intrinsic, &distortion, *mapx, *mapy);
+    return 0;
+}
+
+int main(int argc, char *argv[])
+{
+    IplImage *mapx = NULL;
+    IplImage *mapy = NULL;
+
+    IplImage *image = NULL;
+    IplImage *imageUndis = NULL;
+
+    if (argc < 3) {
+       fprintf(stderr, "Usage %s calibrationFile images ...\n", argv[0]);
+       exit(1);
+    } 
+   
+    cvNamedWindow("Original", CV_WINDOW_AUTOSIZE);
+    cvNamedWindow("Undistorted", CV_WINDOW_AUTOSIZE);
+
+    for (int i=2; i<argc; i++) {
+        printf("image %s\n", argv[i]);
+        image = cvLoadImage(argv[i], CV_LOAD_IMAGE_GRAYSCALE);
+        if (image == NULL) {
+            fprintf(stderr, "Could not load image \"%s\"\n", argv[i]);
+            continue;
+        }
+        int w = image->width;
+        int h = image->height;
+
+        if (mapx == NULL) {
+            int rc = setupUndistortion(argv[1], w, h, &mapx, &mapy);
+            if (rc != 0)
+                exit(1);
+        }
+ 
+        fprintf(stderr, "Maps initialized\n");
+        if (imageUndis == NULL) 
+            imageUndis = cvCreateImage(cvSize(w, h), image->depth, image->nChannels);
+        cvRemap(image, imageUndis, mapx, mapy, CV_INTER_NN|CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
+
+
+        cvShowImage("Original", image);
+        cvShowImage("Undistorted", imageUndis);
+        //cvSaveImage("undisorted.jpg", imageUndis);
+        cvWaitKey(0);
+    }
+}