Set default param to use raw images by default

include/vision/sensor/sensor_camera.h

@@ -40,10 +40,14 @@ struct ParamsSensorCamera : public ParamsSensorBase
         Eigen::Vector4d pinhole_model_raw;      ///< k = [u_0, v_0, alpha_u, alpha_v]  vector of pinhole intrinsic parameters
         Eigen::Vector4d pinhole_model_rectified;///< k = [u_0, v_0, alpha_u, alpha_v]  vector of pinhole intrinsic parameters
         Eigen::VectorXd distortion;             ///< d = [d_1, d_2, d_3, ...] radial distortion coefficients
-    ParamsSensorCamera()
-    {
-        //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
-    }
+    ParamsSensorCamera() :
+        width(0),
+        height(0),
+        using_raw(true)
+        {
+            //
+        };
+
     ParamsSensorCamera(std::string _unique_name, const ParamsServer& _server):
         ParamsSensorBase(_unique_name,  _server)
     {
@@ -54,16 +58,34 @@ struct ParamsSensorCamera : public ParamsSensorBase
         VectorXd intrinsic  = _server.getParam<Eigen::VectorXd>(prefix + _unique_name + "/camera_matrix/data");
         VectorXd projection = _server.getParam<Eigen::VectorXd>(prefix + _unique_name + "/projection_matrix/data");
 
-        pinhole_model_raw[0] = intrinsic[2];
-        pinhole_model_raw[1] = intrinsic[5];
-        pinhole_model_raw[2] = intrinsic[0];
-        pinhole_model_raw[3] = intrinsic[4];
-
-        pinhole_model_rectified[0] = projection[2];
-        pinhole_model_rectified[1] = projection[6];
-        pinhole_model_rectified[2] = projection[0];
-        pinhole_model_rectified[3] = projection[5];
-
+        /* Get raw params from K matrix in camera_info:
+         *
+         *     | au  0 u0 |
+         * K = |  0 av v0 |
+         *     |  0  0  1 |
+         */
+        pinhole_model_raw[0] = intrinsic[2]; // u0
+        pinhole_model_raw[1] = intrinsic[5]; // v0
+        pinhole_model_raw[2] = intrinsic[0]; // au
+        pinhole_model_raw[3] = intrinsic[4]; // av
+
+        /* Get rectified params from P matrix in camera_info:
+         *
+         *     | au  0 u0 tx |
+         * P = |  0 av v0 ty |
+         *     |  0  0  1  0 |
+         */
+        pinhole_model_rectified[0] = projection[2]; // u0
+        pinhole_model_rectified[1] = projection[6]; // v0
+        pinhole_model_rectified[2] = projection[0]; // au
+        pinhole_model_rectified[3] = projection[5]; // av
+
+        /* Get distortion params from vector D in camera_info:
+         *
+         * D = [ r1 r2 t1 t2 r3 ], with ri: dadial params; ti: tangential params
+         *
+         * NOTE: Wolf ignores tangential params!!!
+         */
         assert (distort.size() == 5 && "Distortion size must be size 5!");
 
         WOLF_WARN_COND( distort(2) != 0 || distort(3) != 0 , "Wolf does not handle tangential distortion. Please consider re-calibrating without tangential distortion!");

src/sensor/sensor_camera.cpp

@@ -41,7 +41,7 @@ SensorCamera::SensorCamera(const Eigen::VectorXd& _extrinsics, const ParamsSenso
                 correction_(distortion_.size()==0 ? 0 : distortion_.size() + 1), // make correction vector slightly larger in size than the distortion vector
                 pinhole_model_raw_(_intrinsics.pinhole_model_raw), //
                 pinhole_model_rectified_(_intrinsics.pinhole_model_rectified), //
-                using_raw_(not _intrinsics.using_raw)
+                using_raw_(_intrinsics.using_raw)
 {
     assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3d");
     if (using_raw_)