WIP add separate intrinsic and projection matrices

src/yaml/sensor_camera_yaml.cpp

@@ -37,10 +37,18 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
         VectorXd projection     = camera_config["projection_matrix"]["data"]        .as<VectorXd>();
         VectorXd distortion     = camera_config["distortion_coefficients"]["data"]  .as<VectorXd>();
 
+        Matrix3d intrinsic_mat  = camera_config["camera_matrix"]["data"]            .as<Matrix3d>();
+        Matrix<double, 3, 4> projection_mat
+                                = camera_config["projection_matrix"]["data"]        .as<Matrix<double, 3, 4>>();
+
         // Eigen:: to wolf::
         std::shared_ptr<IntrinsicsCamera> intrinsics_cam = std::make_shared<IntrinsicsCamera>();
         intrinsics_cam->type = sensor_type;
         intrinsics_cam->name = sensor_name;
+
+        intrinsics_cam->intrinsic_mat  = intrinsic_mat;
+        intrinsics_cam->projection_mat = projection_mat;
+
 //        intrinsics_cam->pinhole_model[0] = intrinsic[2];
 //        intrinsics_cam->pinhole_model[1] = intrinsic[5];
 //        intrinsics_cam->pinhole_model[2] = intrinsic[0];
@@ -49,6 +57,7 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
         intrinsics_cam->pinhole_model[1] = projection[6]; // v0
         intrinsics_cam->pinhole_model[2] = projection[0]; // au
         intrinsics_cam->pinhole_model[3] = projection[5]; // av
+
         assert (distortion.size() == 5 && "Distortion size must be size 5!");
         assert (distortion(2) == 0 && distortion(3) == 0 && "Cannot handle tangential distortion. Please re-calibrate without tangential distortion!");
         if (distortion(4) == 0)
@@ -61,7 +70,7 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
             intrinsics_cam->distortion.head<2>() = distortion.head<2>();
             intrinsics_cam->distortion.tail(dist_tail_size) = distortion.tail(dist_tail_size);
         }
-        intrinsics_cam->width = width;
+        intrinsics_cam->width  = width;
         intrinsics_cam->height = height;