--- a/c/Motion.cpp	Fri Jul 14 00:12:03 2017 -0400
+++ b/c/Motion.cpp	Fri Jul 14 02:11:21 2017 -0400
@@ -125,17 +125,21 @@
 
 #ifdef USE_OPENCV
 /// \todo add option for anti-aliased drawing, thickness
-void FeatureTrajectory::draw(Mat& img, const Mat& homography, const Scalar& color) const {
+void FeatureTrajectory::draw(Mat& img, const Mat& homography, const Mat& intrinsicCameraMatrix, const Scalar& color) const {
   Point2f p1, p2;
   if (!homography.empty())
     p1 = project((*positions)[0], homography);
   else
     p1 = (*positions)[0];
+  if (!intrinsicCameraMatrix.empty())
+    p1 = cameraProject(p1, intrinsicCameraMatrix);
   for (unsigned int i=1; i<positions->size(); i++) {
     if (!homography.empty())
       p2 = project((*positions)[i], homography);
     else
       p2 = (*positions)[i];
+    if (!intrinsicCameraMatrix.empty())
+      p2 = cameraProject(p2, intrinsicCameraMatrix);
     line(img, p1, p2, color, 1);
     p1 = p2;
   }