--- a/c/Parameters.cpp	Wed Jun 04 10:56:12 2014 -0400
+++ b/c/Parameters.cpp	Wed Jun 04 10:57:09 2014 -0400
@@ -29,7 +29,12 @@
     ("video-filename", po::value<string>(&videoFilename), "filename of the video to process")
     ("database-filename", po::value<string>(&databaseFilename), "filename of the database where results are saved")
     ("homography-filename", po::value<string>(&homographyFilename), "filename of the homography matrix")
+    ("intrinsic-camera-filename", po::value<string>(&intrinsicCameraFilename), "filename of the homography matrix")
+    ("distortion-coefficients", po::value<std::vector<float> >(&distortionCoefficients)->multitoken(), "")
+    ("undistorted-size-multiplication", po::value<float>(&undistortedImageMultiplication), "undistorted image multiplication")
+    ("interpolation-method", po::value<int>(&interpolationMethod), "Interpolation method for remapping image when correcting for distortion: 0 for INTER_NEAREST - a nearest-neighbor interpolation; 1 for INTER_LINEAR - a bilinear interpolation (used by default); 2 for INTER_CUBIC - a bicubic interpolation over 4x4 pixel neighborhood; 3 for INTER_LANCZOS4")
     ("mask-filename", po::value<string>(&maskFilename), "filename of the mask image (where features are detected)")
+    ("undistort", po::value<bool>(&undistort), "undistort the video for feature tracking")
     ("load-features", po::value<bool>(&loadFeatures), "load features from database")
     ("display", po::value<bool>(&display), "display trajectories on the video")
     ("video-fps", po::value<float>(&videoFPS), "original video frame rate")
@@ -126,7 +131,10 @@
       stream << boost::any_cast<float>(value) << separator;
     else if (value.type() == typeid(string))
       stream << boost::any_cast<string>(value) << separator;
-    else
+    else if (value.type() == typeid(vector<float>)) {
+      for (unsigned int j=0; j<boost::any_cast<vector<float> >(value).size(); j++)
+	     stream << boost::any_cast<vector<float> >(value)[j] << separator;
+    } else
       cerr << "the type of the option " << optionsVec[i]->long_name() << " (" << i << ") is not int, float or string." << endl;
   }
 

--- a/c/feature-based-tracking.cpp	Wed Jun 04 10:56:12 2014 -0400
+++ b/c/feature-based-tracking.cpp	Wed Jun 04 10:57:09 2014 -0400
@@ -23,6 +23,7 @@
 #include <iostream>
 #include <vector>
 #include <ctime>
+#include <cmath>
 
 using namespace std;
 using namespace cv;
@@ -59,7 +60,6 @@
 };
 
 inline void saveFeatures(vector<FeatureTrajectoryPtr>& features, TrajectoryDBAccess<Point2f>& db, const string& positionsTableName, const string& velocitiesTableName) {
-  /// \todo smoothing
   BOOST_FOREACH(FeatureTrajectoryPtr f, features) f->write(db, positionsTableName, velocitiesTableName);
   features.clear();
 }
@@ -73,10 +73,24 @@
   Mat invHomography;
   if (params.display && !homography.empty())
     invHomography = homography.inv();
+  Mat intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " ");
+  //cout << intrinsicCameraMatrix << endl;
 
   float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement;
   Size window = Size(params.windowSize, params.windowSize);
 
+  int interpolationMethod = -1;
+  if (params.interpolationMethod == 0)
+    interpolationMethod = INTER_NEAREST;
+  else if (params.interpolationMethod == 1)
+    interpolationMethod = INTER_LINEAR;
+  else if (params.interpolationMethod == 2)
+    interpolationMethod = INTER_CUBIC;
+  else if (params.interpolationMethod == 3)
+    interpolationMethod = INTER_LANCZOS4;
+  else
+    cout << "Unsupported option " << interpolationMethod << " for interpolation method" << endl;
+
   // BruteForceMatcher<Hamming> descMatcher;
   // vector<DMatch> matches;
 
@@ -99,6 +113,18 @@
 	  ": width=" << videoSize.width <<
 	  ", height=" << videoSize.height <<
 	  ", nframes=" << nFrames << endl;
+
+  Mat newIntrinsicCameraMatrix = intrinsicCameraMatrix.clone(); 
+  Mat map1, map2;
+  if (params.undistort) {
+    videoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication)));
+    newIntrinsicCameraMatrix.at<float>(0,2) = videoSize.width/2.;
+    newIntrinsicCameraMatrix.at<float>(1,2) = videoSize.height/2.;
+    initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3,3, CV_32FC1), newIntrinsicCameraMatrix, videoSize, CV_32FC1, map1, map2);
+    
+    cout << "Undistorted width=" << videoSize.width <<
+      ", height=" << videoSize.height << endl;
+  }
   
   Mat mask = imread(params.maskFilename, 0);
   if (mask.empty()) {
@@ -122,12 +148,9 @@
   std::vector<FeatureTrajectoryPtr> lostFeatures;
   std::vector<FeaturePointMatch> featurePointMatches;
 
-  //HOGDescriptor hog;
-  //hog.setSVMDetector(HOGDescriptor::getDefaultPeopleDetector());
-
   int key = '?';
   unsigned int savedFeatureId=0;
-  Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW;
+  Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW, undistortedFrame;
 
   unsigned int lastFrameNum = nFrames;
   if (params.nFrames > 0)
@@ -136,9 +159,12 @@
   capture->setFrameNumber(params.frame1);
   for (unsigned int frameNum = params.frame1; (frameNum < lastFrameNum) && !::interruptionKey(key); frameNum++) {
       bool success = capture->getNextFrame(frame);
-
-      if (!success || frame.empty() || frame.size() != videoSize)
-	break;
+      if (params.undistort) {
+	remap(frame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.);
+	frame = undistortedFrame;
+      }
+      //if (!success || frame.empty() || frame.size() != videoSize)
+      //break;
 
       if (frameNum%50 ==0)
 	cout << "frame " << frameNum << endl;

--- a/include/Parameters.hpp	Wed Jun 04 10:56:12 2014 -0400
+++ b/include/Parameters.hpp	Wed Jun 04 10:57:09 2014 -0400
@@ -4,6 +4,7 @@
 /// \todo Class for parameters, with utilities to save and load from configuration files
 
 #include <string>
+#include <vector>
 
 namespace boost{
   namespace program_options {
@@ -19,7 +20,12 @@
   std::string videoFilename;
   std::string databaseFilename;
   std::string homographyFilename;
+  std::string intrinsicCameraFilename;
+  std::vector<float> distortionCoefficients;
+  float undistortedImageMultiplication;
+  int interpolationMethod;
   std::string maskFilename;
+  bool undistort;
   bool loadFeatures;
   bool display;
   float videoFPS;

--- a/python/cvutils.py	Wed Jun 04 10:56:12 2014 -0400
+++ b/python/cvutils.py	Wed Jun 04 10:57:09 2014 -0400
@@ -110,6 +110,15 @@
         else:
             cv2.imshow(windowName, img)
 
+    def computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients):
+        from copy import deepcopy
+        from numpy import identity, array
+        newImgSize = (int(round(width*undistortedImageMultiplication)), int(round(height*undistortedImageMultiplication)))
+        newCameraMatrix = deepcopy(intrinsicCameraMatrix)
+        newCameraMatrix[0,2] = newImgSize[0]/2.
+        newCameraMatrix[1,2] = newImgSize[1]/2.
+        return cv2.initUndistortRectifyMap(intrinsicCameraMatrix, array(distortionCoefficients), identity(3), newCameraMatrix, newImgSize, cv2.CV_32FC1)
+
     def playVideo(filename, firstFrameNum = 0, frameRate = -1, interactive = False, printFrames = True, text = None, rescale = 1.):
         '''Plays the video'''
         wait = 5
@@ -202,13 +211,17 @@
         return imgcrop, yCropMin, yCropMax, xCropMin, xCropMax
 
 
-    def displayTrajectories(videoFilename, objects, boundingBoxes = {}, homography = None, firstFrameNum = 0, lastFrameNumArg = None, printFrames = True, rescale = 1., nFramesStep = 1, saveAllImages = False):
+    def displayTrajectories(videoFilename, objects, boundingBoxes = {}, homography = None, firstFrameNum = 0, lastFrameNumArg = None, printFrames = True, rescale = 1., nFramesStep = 1, saveAllImages = False, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.):
         '''Displays the objects overlaid frame by frame over the video '''
         from moving import userTypeNames
         from math import ceil, log10
+
         capture = cv2.VideoCapture(videoFilename)
         width = int(capture.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH))
         height = int(capture.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT))
+
+        if undistort: # setup undistortion
+            [map1, map2] = computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients)
         if capture.isOpened():
             key = -1
             ret = True
@@ -223,6 +236,8 @@
             while ret and not quitKey(key) and frameNum < lastFrameNum:
                 ret, img = capture.read()
                 if ret:
+                    if undistort:
+                        img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR)
                     if printFrames:
                         print('frame {0}'.format(frameNum))
                     for obj in objects:

--- a/python/storage.py	Wed Jun 04 10:56:12 2014 -0400
+++ b/python/storage.py	Wed Jun 04 10:57:09 2014 -0400
@@ -9,6 +9,10 @@
 __metaclass__ = type
 
 
+commentChar = '#'
+
+delimiterChar = '%';
+
 ngsimUserTypes = {'twowheels':1,
                   'car':2,
                   'truck':3}
@@ -352,6 +356,69 @@
 # txt files
 #########################
 
+def openCheck(filename, option = 'r', quit = False):
+    '''Open file filename in read mode by default
+    and checks it is open'''
+    try:
+        return open(filename, option)
+    except IOError:
+        print 'File %s could not be opened.' % filename
+        if quit:
+            from sys import exit
+            exit()
+        return None
+
+def readline(f, commentChar = commentChar):
+    '''Modified readline function to skip comments.'''
+    s = f.readline()
+    while (len(s) > 0) and s.startswith(commentChar):
+        s = f.readline()
+    return s.strip()
+
+def getLines(f, commentChar = commentChar):
+    '''Gets a complete entry (all the lines) in between delimiterChar.'''
+    dataStrings = []
+    s = readline(f, commentChar)
+    while len(s) > 0:
+        dataStrings += [s.strip()]
+        s = readline(f, commentChar)
+    return dataStrings
+
+def writeList(filename, l):
+    f = utils.openCheck(filename, 'w')
+    for x in l:
+        f.write('{}\n'.format(x))
+    f.close()
+
+def loadListStrings(filename, commentChar = commentChar):
+    f = utils.openCheck(filename, 'r')
+    result = getLines(f, commentChar)
+    f.close()
+    return result
+
+def getValuesFromINIFile(filename, option, delimiterChar = '=', commentChar = commentChar):
+    values = []
+    for l in loadListStrings(filename, commentChar):
+        if l.startswith(option):
+            values.append(l.split(delimiterChar)[1].strip())
+    return values
+
+class FakeSecHead(object):
+    '''Add fake section header [asection]
+
+    from http://stackoverflow.com/questions/2819696/parsing-properties-file-in-python/2819788#2819788
+    use read_file in Python 3.2+
+    '''
+    def __init__(self, fp):
+        self.fp = fp
+        self.sechead = '[main]\n'
+
+    def readline(self):
+        if self.sechead:
+            try: return self.sechead
+            finally: self.sechead = None
+        else: return self.fp.readline()
+
 def loadTrajectoriesFromNgsimFile(filename, nObjects = -1, sequenceNum = -1):
     '''Reads data from the trajectory data provided by NGSIM project 
     and returns the list of Feature objects'''
@@ -454,18 +521,89 @@
         writePositionsToCsv(f, obj)
     f.close()
 
-def writeList(filename, l):
-    f = utils.openCheck(filename, 'w')
-    for x in l:
-        f.write('{}\n'.format(x))
-    f.close()
+
+#########################
+# Utils to read .ini type text files for configuration, meta data...
+#########################
+
+class TrackingParameters:
+    '''Class for tracking and safety parameters
+
+    Note: framerate is already taken into account'''
+
+    def loadConfigFile(self, filename):
+        from ConfigParser import ConfigParser
+        from numpy import loadtxt
+        from os import path
+
+        config = ConfigParser()
+        config.readfp(FakeSecHead(openCheck(filename)))
+        self.sectionHeader = config.sections()[0]
+        self.videoFilename = config.get(self.sectionHeader, 'video-filename')
+        self.databaseFilename = config.get(self.sectionHeader, 'database-filename')
+        self.homographyFilename = config.get(self.sectionHeader, 'homography-filename')
+        if (path.exists(self.homographyFilename)):
+            self.homography = loadtxt(self.homographyFilename)
+        else:
+            self.homography = None
+        self.intrinsicCameraFilename = config.get(self.sectionHeader, 'intrinsic-camera-filename')
+        if (path.exists(self.intrinsicCameraFilename)):
+            self.intrinsicCameraMatrix = loadtxt(self.intrinsicCameraFilename)
+        else:
+            self.intrinsicCameraMatrix = None
+        distortionCoefficients = getValuesFromINIFile(filename, 'distortion-coefficients', '=')        
+        self.distortionCoefficients = [float(x) for x in distortionCoefficients]
+        self.undistortedImageMultiplication  = config.getfloat(self.sectionHeader, 'undistorted-size-multiplication')
+        self.undistort = config.getboolean(self.sectionHeader, 'undistort')
+        self.firstFrameNum = config.getint(self.sectionHeader, 'frame1')
+        self.videoFrameRate = config.getfloat(self.sectionHeader, 'video-fps')
 
-def loadListStrings(filename):
-    f = utils.openCheck(filename, 'r')
-    result = [l.strip() for l in f]
-    f.close()
-    return result
-        
+        self.maxPredictedSpeed = config.getfloat(self.sectionHeader, 'max-predicted-speed')/3.6/self.videoFrameRate
+        self.predictionTimeHorizon = config.getfloat(self.sectionHeader, 'prediction-time-horizon')*self.videoFrameRate
+        self.collisionDistance = config.getfloat(self.sectionHeader, 'collision-distance')
+        self.crossingZones = config.getboolean(self.sectionHeader, 'crossing-zones')
+        self.predictionMethod = config.get(self.sectionHeader, 'prediction-method')
+        self.nPredictedTrajectories = config.getint(self.sectionHeader, 'npredicted-trajectories')
+        self.maxNormalAcceleration = config.getfloat(self.sectionHeader, 'max-normal-acceleration')/self.videoFrameRate**2
+        self.maxNormalSteering = config.getfloat(self.sectionHeader, 'max-normal-steering')/self.videoFrameRate
+        self.minExtremeAcceleration = config.getfloat(self.sectionHeader, 'min-extreme-acceleration')/self.videoFrameRate**2
+        self.maxExtremeAcceleration = config.getfloat(self.sectionHeader, 'max-extreme-acceleration')/self.videoFrameRate**2
+        self.maxExtremeSteering = config.getfloat(self.sectionHeader, 'max-extreme-steering')/self.videoFrameRate
+        self.useFeaturesForPrediction = config.getboolean(self.sectionHeader, 'use-features-prediction')
+
+    def __init__(self, filename = None):
+        if filename != None:
+            self.loadConfigFile(filename)
+
+class SceneParameters:
+    def __init__(self, config, sectionName):
+        from ConfigParser import NoOptionError
+        from ast import literal_eval
+        try:
+            self.sitename = config.get(sectionName, 'sitename')
+            self.databaseFilename = config.get(sectionName, 'data-filename')
+            self.homographyFilename = config.get(sectionName, 'homography-filename')
+            self.calibrationFilename = config.get(sectionName, 'calibration-filename') 
+            self.videoFilename = config.get(sectionName, 'video-filename')
+            self.frameRate = config.getfloat(sectionName, 'framerate')
+            self.date = datetime.strptime(config.get(sectionName, 'date'), datetimeFormat) # 2011-06-22 11:00:39
+            self.translation = literal_eval(config.get(sectionName, 'translation')) #         = [0.0, 0.0]
+            self.rotation = config.getfloat(sectionName, 'rotation')
+            self.duration = config.getint(sectionName, 'duration')
+        except NoOptionError as e:
+            print(e)
+            print('Not a section for scene meta-data')
+
+    @staticmethod
+    def loadConfigFile(filename):
+        from ConfigParser import ConfigParser
+        config = ConfigParser()
+        config.readfp(openCheck(filename))
+        configDict = dict()
+        for sectionName in config.sections():
+            configDict[sectionName] = SceneParameters(config, sectionName) 
+        return configDict
+
 
 if __name__ == "__main__":
     import doctest

--- a/python/utils.py	Wed Jun 04 10:56:12 2014 -0400
+++ b/python/utils.py	Wed Jun 04 10:57:09 2014 -0400
@@ -7,10 +7,6 @@
 
 __metaclass__ = type
 
-commentChar = '#'
-
-delimiterChar = '%';
-
 datetimeFormat = "%Y-%m-%d %H:%M:%S"
 
 #########################
@@ -467,50 +463,6 @@
 # file I/O section
 #########################
 
-def openCheck(filename, option = 'r', quit = False):
-    '''Open file filename in read mode by default
-    and checks it is open'''
-    try:
-        return open(filename, option)
-    except IOError:
-        print 'File %s could not be opened.' % filename
-        if quit:
-            from sys import exit
-            exit()
-        return None
-
-def readline(f, commentCharacter = commentChar):
-    '''Modified readline function to skip comments.'''
-    s = f.readline()
-    while (len(s) > 0) and s.startswith(commentCharacter):
-        s = f.readline()
-    return s.strip()
-
-def getLines(f, delimiterCharacter = delimiterChar):
-    '''Gets a complete entry (all the lines) in between delimiterChar.'''
-    dataStrings = []
-    s = readline(f)
-    while (len(s) > 0) and (not s.startswith(delimiterCharacter)):
-        dataStrings += [s.strip()]
-        s = readline(f)
-    return dataStrings
-
-class FakeSecHead(object):
-    '''Add fake section header [asection]
-
-    from http://stackoverflow.com/questions/2819696/parsing-properties-file-in-python/2819788#2819788
-    use read_file in Python 3.2+
-    '''
-    def __init__(self, fp):
-        self.fp = fp
-        self.sechead = '[main]\n'
-
-    def readline(self):
-        if self.sechead:
-            try: return self.sechead
-            finally: self.sechead = None
-        else: return self.fp.readline()
-
 def removeExtension(filename, delimiter = '.'):
     '''Returns the filename minus the extension (all characters after last .)'''
     i = filename.rfind(delimiter)
@@ -594,74 +546,6 @@
 
 
 #########################
-# Utils to read .ini type text files for configuration, meta data...
-#########################
-
-class TrackingParameters:
-    '''Class for tracking and safety parameters
-
-    Note: framerate is already taken into account'''
-    def loadConfigFile(self, filename):
-        from ConfigParser import ConfigParser
-        from numpy import loadtxt
-        from os import path
-
-        config = ConfigParser()
-        config.readfp(FakeSecHead(openCheck(filename)))
-        self.sectionHeader = config.sections()[0]
-        self.videoFilename = config.get(self.sectionHeader, 'video-filename')
-        self.databaseFilename = config.get(self.sectionHeader, 'database-filename')
-        self.homographyFilename = config.get(self.sectionHeader, 'homography-filename')
-        if (path.exists(self.homographyFilename)):
-            self.homography = loadtxt(self.homographyFilename)
-        else:
-            self.homography = None
-        self.firstFrameNum = config.getint(self.sectionHeader, 'frame1')
-        self.videoFrameRate = config.getfloat(self.sectionHeader, 'video-fps')
-
-        self.maxPredictedSpeed = config.getfloat(self.sectionHeader, 'max-predicted-speed')/3.6/self.videoFrameRate
-        self.predictionTimeHorizon = config.getfloat(self.sectionHeader, 'prediction-time-horizon')*self.videoFrameRate
-        self.collisionDistance = config.getfloat(self.sectionHeader, 'collision-distance')
-        self.crossingZones = config.getboolean(self.sectionHeader, 'crossing-zones')
-        self.predictionMethod = config.get(self.sectionHeader, 'prediction-method')
-        self.nPredictedTrajectories = config.getint(self.sectionHeader, 'npredicted-trajectories')
-        self.maxNormalAcceleration = config.getfloat(self.sectionHeader, 'max-normal-acceleration')/self.videoFrameRate**2
-        self.maxNormalSteering = config.getfloat(self.sectionHeader, 'max-normal-steering')/self.videoFrameRate
-        self.minExtremeAcceleration = config.getfloat(self.sectionHeader, 'min-extreme-acceleration')/self.videoFrameRate**2
-        self.maxExtremeAcceleration = config.getfloat(self.sectionHeader, 'max-extreme-acceleration')/self.videoFrameRate**2
-        self.maxExtremeSteering = config.getfloat(self.sectionHeader, 'max-extreme-steering')/self.videoFrameRate
-        self.useFeaturesForPrediction = config.getboolean(self.sectionHeader, 'use-features-prediction')
-
-class SceneParameters:
-    def __init__(self, config, sectionName):
-        from ConfigParser import NoOptionError
-        from ast import literal_eval
-        try:
-            self.sitename = config.get(sectionName, 'sitename')
-            self.databaseFilename = config.get(sectionName, 'data-filename')
-            self.homographyFilename = config.get(sectionName, 'homography-filename')
-            self.calibrationFilename = config.get(sectionName, 'calibration-filename') 
-            self.videoFilename = config.get(sectionName, 'video-filename')
-            self.frameRate = config.getfloat(sectionName, 'framerate')
-            self.date = datetime.strptime(config.get(sectionName, 'date'), datetimeFormat) # 2011-06-22 11:00:39
-            self.translation = literal_eval(config.get(sectionName, 'translation')) #         = [0.0, 0.0]
-            self.rotation = config.getfloat(sectionName, 'rotation')
-            self.duration = config.getint(sectionName, 'duration')
-        except NoOptionError as e:
-            print(e)
-            print('Not a section for scene meta-data')
-
-    @staticmethod
-    def loadConfigFile(filename):
-        from ConfigParser import ConfigParser
-        config = ConfigParser()
-        config.readfp(openCheck(filename))
-        configDict = dict()
-        for sectionName in config.sections():
-            configDict[sectionName] = SceneParameters(config, sectionName) 
-        return configDict
-
-#########################
 # running tests
 #########################
 

--- a/scripts/compute-homography.py	Wed Jun 04 10:56:12 2014 -0400
+++ b/scripts/compute-homography.py	Wed Jun 04 10:57:09 2014 -0400
@@ -25,6 +25,10 @@
 parser.add_argument('-n', dest = 'nPoints', help = 'number of corresponding points to input', default = 4, type = int)
 parser.add_argument('-u', dest = 'unitsPerPixel', help = 'number of units per pixel', default = 1., type = float)
 parser.add_argument('--display', dest = 'displayPoints', help = 'display original and projected points on both images', action = 'store_true')
+parser.add_argument('--intrinsic', dest = 'intrinsicCameraMatrixFilename', help = 'name of the intrinsic camera file')
+parser.add_argument('--distortion-coefficients', dest = 'distortionCoefficients', help = 'distortion coefficients', nargs = '*', type = float)
+parser.add_argument('--undistorted-multiplication', dest = 'undistortedImageMultiplication', help = 'undistorted image multiplication', type = float)
+parser.add_argument('--undistort', dest = 'undistort', help = 'undistort the video (because features have been extracted that way)', action = 'store_true')
 
 args = parser.parse_args()
 
@@ -76,6 +80,9 @@
 elif args.videoFrameFilename != None and args.worldFilename != None:
     worldImg = plt.imread(args.worldFilename)
     videoImg = plt.imread(args.videoFrameFilename)
+    if args.undistort:        
+        [map1, map2] = cvutils.computeUndistortMaps(videoImg.shape[1], videoImg.shape[0], args.undistortedImageMultiplication, np.loadtxt(args.intrinsicCameraMatrixFilename), args.distortionCoefficients)
+        videoImg = cv2.remap(videoImg, map1, map2, interpolation=cv2.INTER_LINEAR)
     print('Click on {0} points in the video frame'.format(args.nPoints))
     plt.figure()
     plt.imshow(videoImg)
@@ -98,6 +105,9 @@
 if args.displayPoints and args.videoFrameFilename != None and args.worldFilename != None and homography.size>0:
     worldImg = cv2.imread(args.worldFilename)
     videoImg = cv2.imread(args.videoFrameFilename)
+    if args.undistort:        
+        [map1, map2] = cvutils.computeUndistortMaps(videoImg.shape[1], videoImg.shape[0], args.undistortedImageMultiplication, np.loadtxt(args.intrinsicCameraMatrixFilename), args.distortionCoefficients)
+        videoImg = cv2.remap(videoImg, map1, map2, interpolation=cv2.INTER_LINEAR)
     invHomography = np.linalg.inv(homography)
     projectedWorldPts = cvutils.projectArray(invHomography, worldPts.T).T
     projectedVideoPts = cvutils.projectArray(homography, videoPts.T).T

--- a/scripts/display-trajectories.py	Wed Jun 04 10:56:12 2014 -0400
+++ b/scripts/display-trajectories.py	Wed Jun 04 10:57:09 2014 -0400
@@ -13,32 +13,52 @@
 parser.add_argument('-i', dest = 'videoFilename', help = 'name of the video file')
 parser.add_argument('-t', dest = 'trajectoryType', help = 'type of trajectories to display', choices = ['feature', 'object'], default = 'feature')
 parser.add_argument('-o', dest = 'homographyFilename', help = 'name of the image to world homography file')
-parser.add_argument('-f', dest = 'firstFrameNum', help = 'number of first frame number to display', default = 0, type = int)
+parser.add_argument('--intrinsic', dest = 'intrinsicCameraMatrixFilename', help = 'name of the intrinsic camera file')
+parser.add_argument('--distortion-coefficients', dest = 'distortionCoefficients', help = 'distortion coefficients', nargs = '*', type = float)
+parser.add_argument('--undistorted-multiplication', dest = 'undistortedImageMultiplication', help = 'undistorted image multiplication', type = float)
+parser.add_argument('-u', dest = 'undistort', help = 'undistort the video (because features have been extracted that way)', action = 'store_true')
+parser.add_argument('-f', dest = 'firstFrameNum', help = 'number of first frame number to display', type = int)
 parser.add_argument('-r', dest = 'rescale', help = 'rescaling factor for the displayed image', default = 1., type = float)
 parser.add_argument('-s', dest = 'nFramesStep', help = 'number of frames between each display', default = 1, type = int)
 parser.add_argument('--save-images', dest = 'saveAllImages', help = 'save all images', action = 'store_true')
-parser.add_argument('--last-frame', dest = 'lastFrameNum', help = 'number of last frame number to save (for image saving, no display is made)', default = None, type = int)
+parser.add_argument('--last-frame', dest = 'lastFrameNum', help = 'number of last frame number to save (for image saving, no display is made)', type = int)
 
 args = parser.parse_args()
 
-homography = None
 if args.configFilename: # consider there is a configuration file
-    params = utils.TrackingParameters()
-    params.loadConfigFile(args.configFilename)
+    params = storage.TrackingParameters(args.configFilename)
     videoFilename = params.videoFilename
     databaseFilename = params.databaseFilename
-    homography = inv(loadtxt(params.homographyFilename))
+    homography = inv(params.homography)
+    intrinsicCameraMatrix = params.intrinsicCameraMatrix
+    distortionCoefficients = params.distortionCoefficients
+    undistortedImageMultiplication = params.undistortedImageMultiplication
+    undistort = params.undistort
     firstFrameNum = params.firstFrameNum
+else:
+    homography = None
+    undistort = False
+    intrinsicCameraMatrix = None
+    distortionCoefficients = []
+    undistortedImageMultiplication = None
+    firstFrameNum = 0
 
-if args.videoFilename != None:
+if not args.configFilename and args.videoFilename != None:
     videoFilename = args.videoFilename
-if args.databaseFilename != None:
+if not args.configFilename and args.databaseFilename != None:
     databaseFilename = args.databaseFilename
-if args.homographyFilename != None:
+if not args.configFilename and args.homographyFilename != None:
     homography = inv(loadtxt(args.homographyFilename))            
-if args.firstFrameNum != None:
+if not args.configFilename and args.intrinsicCameraMatrixFilename != None:
+    intrinsicCameraMatrix = loadtxt(args.intrinsicCameraMatrixFilename)
+if not args.configFilename and args.distortionCoefficients != None:
+    distortionCoefficients = args.distortionCoefficients
+if not args.configFilename and args.undistortedImageMultiplication != None:
+    undistortedImageMultiplication = args.undistortedImageMultiplication
+if not args.configFilename and args.firstFrameNum != None:
     firstFrameNum = args.firstFrameNum
 
+
 objects = storage.loadTrajectoriesFromSqlite(databaseFilename, args.trajectoryType)
 boundingBoxes = storage.loadBoundingBoxTable(databaseFilename)
-cvutils.displayTrajectories(videoFilename, objects, boundingBoxes, homography, firstFrameNum, args.lastFrameNum, rescale = args.rescale, nFramesStep = args.nFramesStep, saveAllImages = args.saveAllImages)
+cvutils.displayTrajectories(videoFilename, objects, boundingBoxes, homography, firstFrameNum, args.lastFrameNum, rescale = args.rescale, nFramesStep = args.nFramesStep, saveAllImages = args.saveAllImages, undistort = (undistort or args.undistort), intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication)

--- a/tracking.cfg	Wed Jun 04 10:56:12 2014 -0400
+++ b/tracking.cfg	Wed Jun 04 10:57:09 2014 -0400
@@ -4,8 +4,22 @@
 database-filename = laurier.sqlite
 # filename of the homography matrix
 homography-filename = laurier-homography.txt
+# filename of the homography matrix
+intrinsic-camera-filename = intrinsic-camera.txt
+# -0.11759321 0.0148536 0.00030756 -0.00020578 -0.00091816
+distortion-coefficients = -0.11759321
+distortion-coefficients = 0.0148536
+distortion-coefficients = 0.00030756 
+distortion-coefficients = -0.00020578 
+distortion-coefficients = -0.00091816
+# undistorted image multiplication
+undistorted-size-multiplication = 1.31
+# Interpolation method for remapping image when correcting for distortion: 0 for INTER_NEAREST - a nearest-neighbor interpolation; 1 for INTER_LINEAR - a bilinear interpolation (used by default); 2 for INTER_CUBIC - a bicubic interpolation over 4x4 pixel neighborhood; 3 for INTER_LANCZOS4
+interpolation-method = 1
 # filename of the mask image (where features are detected)
 mask-filename = none
+# undistort the video for feature tracking
+undistort = true
 # load features from database
 load-features = false
 # display trajectories on the video