Add demo_utils.

This commit splits the utils file into two different files:
utils and demo_utils, to overcome some opencv problems caused
by demo_utils includes.

Signed-off-by: Davide Sapienza <sapienza.dav@gmail.com>

src/demo_utils.cpp

@@ -0,0 +1,19 @@
+
+#include "demo_utils.h"
+
+void readCalibrationMatrix(const std::string& path, cv::Mat& calib_mat){
+    YAML::Node config   = YAML::LoadFile(path);
+
+    //read camera matrix
+    int rows = config["camera_matrix"]["rows"].as<int>();
+    int cols = config["camera_matrix"]["cols"].as<int>();
+
+    cv::Mat calib = cv::Mat(cv::Size(rows, cols), CV_32F);
+    float *vals = (float *)calib.data;
+    
+    for(int i=0; i < config["camera_matrix"]["data"].size(); ++i )
+        vals[i] = config["camera_matrix"]["data"][i].as<float>();
+
+    calib_mat = calib;
+
+}