diff --git a/demo/demo/data/pmundist.txt b/demo/demo/data/pmundist.txt
new file mode 100644
index 0000000..8a156ac
--- /dev/null
+++ b/demo/demo/data/pmundist.txt
@@ -0,0 +1,3 @@
+0.1982518156851575	-13.22038303683631	595.3838470607648	
+-0.4806350794284377	-6.567788488139482	598.1732743332492	
+-0.00016595895699412456	-0.007499178419813009	1.0
diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp
index e15f8b2..1042052 100644
--- a/demo/demo/demo.cpp
+++ b/demo/demo/demo.cpp
@@ -5,6 +5,8 @@
 #include <mutex>
 #include <ctime>
 #include <pthread.h>
+
+#include <yaml-cpp/yaml.h>
 #include "utils.h"
 
 #include <opencv2/core/core.hpp>
@@ -67,7 +69,7 @@ int main(int argc, char *argv[])
     char *input = "../demo/demo/data/single_ped_2.mp4";
     if (argc > 2)
         input = argv[2];
-    char *pmatrix = "../demo/demo/data/proj_matrix_map_b.txt";
+    char *pmatrix = "../demo/demo/data/pmundist.txt";
     if (argc > 3)
         pmatrix = argv[3];
     char *tiffile = "../demo/demo/data/map_b.tif";
@@ -114,6 +116,22 @@ int main(int argc, char *argv[])
     int proj_matrix_read = 0;
     cv::Mat H(cv::Size(3, 3), CV_64FC1);
 
+    /*Camera calibration*/
+    float data[9] = {1.6158690952190570e+03, 0., 9.4702812371722337e+02, 0., 1.6123979985757153e+03, 5.1995630055718266e+02, 0., 0., 1.};
+    cv::Mat camera_matrix = cv::Mat(3, 3, CV_32F, data);
+    std::cout << camera_matrix << std::endl;
+    float data_dc[5] = {-4.0971199964304100e-01, 1.8755404192050384e-01, -5.3059322427743867e-03, -1.0380603625304912e-03, 0.};
+    cv::Mat distortion_coefficients = cv::Mat(5, 1, CV_32F, data_dc);
+    std::cout << distortion_coefficients << std::endl;
+
+    YAML::Node config = YAML::LoadFile("/home/classfog1/repos/tkDNN/demo/demo/data/calib36.params");
+    const YAML::Node &node_test1 = config["camera_matrix"];
+    for (std::size_t i = 0; i < node_test1.size(); i++)
+    {
+        const YAML::Node &node_test2 = node_test1[i];
+        //std::cout << "Id: " << node_test2["data"].as<std::string>() << std::endl;
+    }
+
     /*GPS information*/
     double *adfGeoTransform = (double *)malloc(6 * sizeof(double));
     readTiff(tiffile, adfGeoTransform);
@@ -145,8 +163,14 @@ int main(int argc, char *argv[])
     int frame_nbr = 0;
     while (gRun)
     {
+        break;
 
         cap >> frame;
+
+        cv::Mat temp = frame.clone();
+        undistort(temp, frame, camera_matrix, distortion_coefficients);
+        //cv::imwrite("undistorted.jpg", frame);
+
         if (!frame.data)
         {
             usleep(1000000);