Add camera calibration file reading for demo3d and demoTracker.

Some minor fixes.

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

src/utils.cpp

@@ -240,4 +240,21 @@ void removePathAndExtension(const std::string &full_string, std::string &name){
         name = name.substr(previous, current);
     
     // std::cout<<"full string: "<<full_string<<" name: "<<name<<std::endl;
-}
+}
+
+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;
+
+}