diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
index 3b2b68c18cf8b8c347ce78037e38d00d4f990406..cde8790298ad7e3709c1e2dc2a0ecd16f1c398a1 100644
--- a/src/examples/test_wolf_factories.cpp
+++ b/src/examples/test_wolf_factories.cpp
@@ -65,7 +65,7 @@ int main(void)
     IntrinsicsBasePtr intr_cam_ptr = IntrinsicsFactory::get().create("CAMERA", wolf_config + "/camera_params_ueye_sim.yaml");
     ProcessorParamsBasePtr params_ptr = ProcessorParamsFactory::get().create("IMAGE FEATURE", wolf_config + "/processor_image_feature.yaml");
 
-    cout << "CAMERA with intrinsics      : " << (static_pointer_cast<IntrinsicsCamera>(intr_cam_ptr))->pinhole_model.transpose() << endl;
+    cout << "CAMERA with intrinsics      : " << (static_pointer_cast<IntrinsicsCamera>(intr_cam_ptr))->pinhole_model_raw.transpose() << endl;
 //    cout << "Processor IMAGE image width : " << (static_pointer_cast<ProcessorParamsImage>(params_ptr))->image.width << endl;
 
     cout << "\n==================== Install Sensors ====================" << endl;
diff --git a/src/problem.cpp b/src/problem.cpp
index 658bd033dad8e177330dfededc49fb4fbe259d90..2b7ac27841a195836a492cd83462aed4e37313a1 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -4,17 +4,14 @@
 #include "trajectory_base.h"
 #include "map_base.h"
 #include "sensor_base.h"
-#include "factory.h"
+#include "processor_motion.h"
+#include "processor_tracker.h"
+#include "capture_pose.h"
+#include "constraint_base.h"
 #include "sensor_factory.h"
 #include "processor_factory.h"
-#include "constraint_base.h"
 #include "state_block.h"
-#include "processor_motion.h"
-#include "sensor_GPS.h"
 
-#include "processor_tracker.h"
-//#include "processors/processor_tracker_feature_trifocal.h"
-#include "capture_pose.h"
 
 // IRI libs includes
 
diff --git a/src/processor_base.cpp b/src/processor_base.cpp
index ea4bdc4e82e1edbcb83c9500656f0b0d05de6ab8..90943540e929a22bf24c12d55ca89329b5f3c935 100644
--- a/src/processor_base.cpp
+++ b/src/processor_base.cpp
@@ -8,7 +8,7 @@ namespace wolf {
 unsigned int ProcessorBase::processor_id_count_ = 0;
 
 ProcessorBase::ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _params) :
-        NodeBase("PROCESSOR", _type, _params->name),
+        NodeBase("PROCESSOR", _type),
         processor_id_(++processor_id_count_),
         params_(_params),
         sensor_ptr_()
diff --git a/src/processor_base.h b/src/processor_base.h
index bcf5c46b62730d593165f99fdb8d021296964812..2b996e17f3e58bacf694f8485f71444bb96cafd5 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -112,13 +112,9 @@ struct ProcessorParamsBase
     ProcessorParamsBase() = default;
 
     ProcessorParamsBase(bool _voting_active,
-                        Scalar _time_tolerance,
-                        const std::string& _type,
-                        const std::string& _name)
+                        Scalar _time_tolerance)
       : voting_active(_voting_active)
       , time_tolerance(_time_tolerance)
-      , type(_type)
-      , name(_name)
     {
       //
     }
@@ -131,9 +127,6 @@ struct ProcessorParamsBase
     /// a particular Capture of this processor to allow assigning
     /// this Capture to the Keyframe.
     Scalar time_tolerance = Scalar(0);
-
-    std::string type;
-    std::string name;
 };
 
 //class ProcessorBase
diff --git a/src/processors/processor_tracker_feature_trifocal.cpp b/src/processors/processor_tracker_feature_trifocal.cpp
index 340f4ec153ff972be60b44731e7f6feddb23c348..162e42b23da84ed9ce9f899c3510e8eb03960b1d 100644
--- a/src/processors/processor_tracker_feature_trifocal.cpp
+++ b/src/processors/processor_tracker_feature_trifocal.cpp
@@ -39,7 +39,6 @@ ProcessorTrackerFeatureTrifocal::ProcessorTrackerFeatureTrifocal(ProcessorParams
         prev_origin_ptr_(nullptr),
         initialized_(false)
 {
-    setName(_params_tracker_feature_trifocal->name);
     assert(!(params_tracker_feature_trifocal_->yaml_file_params_vision_utils.empty()) && "Missing YAML file with vision_utils parameters!");
     assert(file_exists(params_tracker_feature_trifocal_->yaml_file_params_vision_utils) && "Cannot setup processor: vision_utils' YAML file does not exist.");
 
diff --git a/src/sensor_GPS_fix.cpp b/src/sensor_GPS_fix.cpp
index fa1f46eb16bbeef20789cfdaf7165433910e21d9..311ec92924f34554cce36398b05eea2e1d680fa2 100644
--- a/src/sensor_GPS_fix.cpp
+++ b/src/sensor_GPS_fix.cpp
@@ -4,12 +4,6 @@
 
 namespace wolf {
 
-SensorGPSFix::SensorGPSFix(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _noise) :
-        SensorBase("GPS FIX", _p_ptr, _o_ptr, nullptr, Eigen::VectorXs::Constant(1,_noise))
-{
-    //
-}
-
 SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics) :
         SensorBase("GPS FIX", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), nullptr, _intrinsics.noise_std)
 {
@@ -17,7 +11,8 @@ SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const Intrinsics
            && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
 }
 
-SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) : SensorGPSFix(_extrinsics, *_intrinsics_ptr)
+SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) :
+        SensorGPSFix(_extrinsics, *_intrinsics_ptr)
 {
     //
 }
@@ -34,8 +29,8 @@ SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen:
 {
     assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
             && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
-    StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics, true);
-    SensorGPSFixPtr sen = std::make_shared<SensorGPSFix>(pos_ptr, nullptr, 0);
+    SensorGPSFixPtr sen = std::make_shared<SensorGPSFix>(_extrinsics, std::static_pointer_cast<IntrinsicsGPSFix>(_intrinsics));
+    sen->getPPtr()->fix();
     sen->setName(_unique_name);
     return sen;
 }
diff --git a/src/sensor_GPS_fix.h b/src/sensor_GPS_fix.h
index bef5bb861629a5c3c8894488f2fe902aa18f5cee..9d5bdb01ac5a0bfd826ad536acd149c3905ef7f6 100644
--- a/src/sensor_GPS_fix.h
+++ b/src/sensor_GPS_fix.h
@@ -26,15 +26,6 @@ WOLF_PTR_TYPEDEFS(SensorGPSFix);
 class SensorGPSFix : public SensorBase
 {
     public:
-        /** \brief Constructor with arguments
-         * 
-         * Constructor with arguments
-         * \param _p_ptr StateBlock pointer to the sensor position
-         * \param _o_ptr StateBlock pointer to the sensor orientation
-         * \param _noise noise standard deviation
-         * 
-         **/
-		SensorGPSFix(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _noise);
         SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics);
         SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr);
 
diff --git a/src/sensor_IMU.cpp b/src/sensor_IMU.cpp
index 9e5da5f0ddba954357e7c58492a82947423ff5af..0189ca2d2e8d4dd84aab43ec656a9f4e95a29b36 100644
--- a/src/sensor_IMU.cpp
+++ b/src/sensor_IMU.cpp
@@ -4,24 +4,6 @@
 
 namespace wolf {
 
-SensorIMU::SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsIMU& _params) :
-        SensorBase("IMU", _p_ptr, _o_ptr, std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6s()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, true),
-        a_noise(_params.a_noise),
-        w_noise(_params.w_noise),
-        ab_initial_stdev(_params.ab_initial_stdev),
-        wb_initial_stdev(_params.wb_initial_stdev),
-        ab_rate_stdev(_params.ab_rate_stdev),
-        wb_rate_stdev(_params.wb_rate_stdev)
-{
-    //
-}
-
-SensorIMU::SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr _params) :
-        SensorIMU(_p_ptr, _o_ptr, *_params)
-{
-    //
-}
-
 SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params) :
         SensorIMU(_extrinsics, *_params)
 {
diff --git a/src/sensor_IMU.h b/src/sensor_IMU.h
index 2f5d9baa576f697772800359a46a239170f39144..e809d79fffc923f731a4e477e49dfe292665cb9b 100644
--- a/src/sensor_IMU.h
+++ b/src/sensor_IMU.h
@@ -46,17 +46,6 @@ class SensorIMU : public SensorBase
 
     public:
 
-        /** \brief Constructor with arguments
-         *
-         * Constructor with arguments
-         * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base
-         * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base
-         * \param params IntrinsicsIMU pointer to sensor properties
-         * \param _a_w_biases_ptr StateBlock pointer to the vector of acc and gyro biases
-         *
-         **/
-        SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr _params);
-        SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsIMU& _params);
         SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params);
         SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params);
 
diff --git a/src/sensor_base.h b/src/sensor_base.h
index f01c8ce0b533ad838b33e9a980e92b80bc691d08..992058512a2bd6d29a253653a988ccbe932c8ad1 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -25,9 +25,6 @@ namespace wolf {
 struct IntrinsicsBase
 {
         virtual ~IntrinsicsBase() = default;
-
-        std::string type;
-        std::string name;
 };
 
 class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBase>
diff --git a/src/sensor_camera.cpp b/src/sensor_camera.cpp
index 7a9a9a2c7904b0ea47143c96a4d0bdd1b36c8c50..403ba8dd86676848c1a3ce3148782ccd773f5197 100644
--- a/src/sensor_camera.cpp
+++ b/src/sensor_camera.cpp
@@ -7,23 +7,18 @@
 namespace wolf
 {
 
-SensorCamera::SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, //
-                           int _img_width, int _img_height) :
-        SensorBase("CAMERA", _p_ptr, _o_ptr, _intr_ptr, 2), //
-        img_width_(_img_width), img_height_(_img_height)
-{
-    //
-}
-
 SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const IntrinsicsCamera& _intrinsics) :
-                SensorBase("CAMERA", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(_intrinsics.pinhole_model, true), 1),
+                SensorBase("CAMERA", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(_intrinsics.pinhole_model_raw, true), 1),
                 img_width_(_intrinsics.width), //
                 img_height_(_intrinsics.height), //
                 distortion_(_intrinsics.distortion), //
-                correction_(distortion_.size()+2) // make correction vector of the same size as distortion vector
+                correction_(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_(true)
 {
     assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3D");
-    K_ = setIntrinsicMatrix(_intrinsics.pinhole_model);
+    useRawImages();
     pinhole::computeCorrectionModel(getIntrinsicPtr()->getState(), distortion_, correction_);
 }
 
@@ -71,7 +66,6 @@ SensorBasePtr SensorCamera::create(const std::string& _unique_name, //
 
 // Register in the SensorFactory
 #include "sensor_factory.h"
-//#include "factory.h"
 namespace wolf
 {
 WOLF_REGISTER_SENSOR("CAMERA", SensorCamera)
diff --git a/src/sensor_camera.h b/src/sensor_camera.h
index d8641510db8225e874e06526665199e8bc316d2f..9bdee32534c2d9b752bc6a988c6d4d0b76a0f7f9 100644
--- a/src/sensor_camera.h
+++ b/src/sensor_camera.h
@@ -12,10 +12,11 @@ WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsCamera);
  */
 struct IntrinsicsCamera : public IntrinsicsBase
 {
-        unsigned int width;                 ///< Image width in pixels
-        unsigned int height;                ///< Image height in pixels
-        Eigen::Vector4s pinhole_model;      ///< k = [u_0, v_0, alpha_u, alpha_v]  vector of pinhole intrinsic parameters
-        Eigen::VectorXs distortion;         ///< d = [d_1, d_2, d_3, ...] radial distortion coefficients
+        unsigned int width;                     ///< Image width in pixels
+        unsigned int height;                    ///< Image height in pixels
+        Eigen::Vector4s pinhole_model_raw;      ///< k = [u_0, v_0, alpha_u, alpha_v]  vector of pinhole intrinsic parameters
+        Eigen::Vector4s pinhole_model_rectified;///< k = [u_0, v_0, alpha_u, alpha_v]  vector of pinhole intrinsic parameters
+        Eigen::VectorXs distortion;             ///< d = [d_1, d_2, d_3, ...] radial distortion coefficients
 
         virtual ~IntrinsicsCamera() = default;
 };
@@ -27,26 +28,20 @@ WOLF_PTR_TYPEDEFS(SensorCamera);
 class SensorCamera : public SensorBase
 {
     public:
-        /** \brief Constructor with arguments
-         *
-         * Constructor with arguments
-         * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base
-         * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base
-         * \param _intr_ptr contains intrinsic values for the camera
-         * \param _img_width image height in pixels
-         * \param _img_height image width in pixels
-         *
-         **/
-        SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, int _img_width, int _img_height);
 
         SensorCamera(const Eigen::VectorXs & _extrinsics, const IntrinsicsCamera& _intrinsics);
         SensorCamera(const Eigen::VectorXs & _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr);
 
         virtual ~SensorCamera();
 
-        Eigen::VectorXs getDistortionVector(){return distortion_;}
-        Eigen::VectorXs getCorrectionVector(){return correction_;}
-        Eigen::Matrix3s getIntrinsicMatrix() {return K_;}
+        Eigen::VectorXs getDistortionVector()   { return distortion_; }
+        Eigen::VectorXs getCorrectionVector()   { return correction_; }
+        Eigen::Matrix3s getIntrinsicMatrix()    { return K_; }
+
+        bool isUsingRawImages() { return using_raw_; }
+        bool useRawImages();
+        bool useRectifiedImages();
+
 
         int getImgWidth(){return img_width_;}
         int getImgHeight(){return img_height_;}
@@ -58,7 +53,9 @@ class SensorCamera : public SensorBase
         int img_height_;
         Eigen::VectorXs distortion_;
         Eigen::VectorXs correction_;
+        Eigen::Vector4s pinhole_model_raw_, pinhole_model_rectified_;
         Eigen::Matrix3s K_;
+        bool using_raw_;
 
         virtual Eigen::Matrix3s setIntrinsicMatrix(Eigen::Vector4s _pinhole_model);
 
@@ -71,6 +68,24 @@ class SensorCamera : public SensorBase
 
 };
 
+inline bool SensorCamera::useRawImages()
+{
+    getIntrinsicPtr()->setState(pinhole_model_raw_);
+    K_ = setIntrinsicMatrix(pinhole_model_raw_);
+    using_raw_ = true;
+
+    return true;
+}
+
+inline bool SensorCamera::useRectifiedImages()
+{
+    getIntrinsicPtr()->setState(pinhole_model_rectified_);
+    K_ = setIntrinsicMatrix(pinhole_model_rectified_);
+    using_raw_ = false;
+
+    return true;
+}
+
 } // namespace wolf
 
 #endif // SENSOR_CAMERA_H
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
index fdb7ab4804809072cada12774e8af9a08ed9c3f4..7a03676337872f829653d6cd56f7daaf0e5446fd 100644
--- a/src/sensor_odom_2D.cpp
+++ b/src/sensor_odom_2D.cpp
@@ -4,14 +4,6 @@
 
 namespace wolf {
 
-SensorOdom2D::SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _disp_noise_factor, const Scalar&  _rot_noise_factor) :
-        SensorBase("ODOM 2D", _p_ptr, _o_ptr, nullptr, 2),
-        k_disp_to_disp_(_disp_noise_factor),
-        k_rot_to_rot_(_rot_noise_factor)
-{
-    //
-}
-
 SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics) :
         SensorBase("ODOM 2D", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
         k_disp_to_disp_(_intrinsics.k_disp_to_disp),
diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h
index 4ba0fc37dee5a0d4e487548c894f32fff4fc2a86..f66b9eea4b643f65eca471ef82906e1d0c1ad4e4 100644
--- a/src/sensor_odom_2D.h
+++ b/src/sensor_odom_2D.h
@@ -26,16 +26,6 @@ class SensorOdom2D : public SensorBase
         Scalar k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
 
 	public:
-        /** \brief Constructor with arguments
-         * 
-         * Constructor with arguments
-         * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base
-         * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base
-         * \param _disp_noise_factor displacement noise factor
-         * \param _rot_noise_factor rotation noise factor
-         * 
-         **/
-        SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _disp_noise_factor, const Scalar&  _rot_noise_factor);
         SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics);
         SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics);
 
diff --git a/src/sensor_odom_3D.cpp b/src/sensor_odom_3D.cpp
index c5f3fb388c1ab12635dd8bd0d64bbc506fab3207..5cf426b294ad2241a0cace34ede0b14b8e1080fd 100644
--- a/src/sensor_odom_3D.cpp
+++ b/src/sensor_odom_3D.cpp
@@ -12,18 +12,6 @@
 
 namespace wolf {
 
-SensorOdom3D::SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _o_ptr, IntrinsicsOdom3DPtr _params) :
-        SensorBase("ODOM 3D", _p_ptr, _o_ptr, nullptr, 6),
-        k_disp_to_disp_(_params->k_disp_to_disp),
-        k_disp_to_rot_(_params->k_disp_to_rot),
-        k_rot_to_rot_(_params->k_rot_to_rot),
-        min_disp_var_(_params->min_disp_var),
-        min_rot_var_(_params->min_rot_var)
-{
-    noise_cov_ = (Eigen::Vector6s() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
-    setNoiseCov(noise_cov_); // sets also noise_std_
-}
-
 SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& _intrinsics) :
                         SensorBase("ODOM 3D", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
                         k_disp_to_disp_(_intrinsics.k_disp_to_disp),
diff --git a/src/sensor_odom_3D.h b/src/sensor_odom_3D.h
index 19f037162677691f08b2deff390c42b49ab706fd..b366b99884997fe3cd73a132f435e53086b131db 100644
--- a/src/sensor_odom_3D.h
+++ b/src/sensor_odom_3D.h
@@ -38,14 +38,6 @@ class SensorOdom3D : public SensorBase
         Scalar min_rot_var_;
 
     public:
-        /** \brief Constructor with arguments
-         *
-         * Constructor with arguments
-         * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base
-         * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base
-         * \param _params shared_ptr to a struct with parameters
-         **/
-        SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _q_ptr, IntrinsicsOdom3DPtr params);
         SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& params);
         SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom3DPtr params);
 
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index bfa53c590ea89d99b569acdd088f93356b796ba8..484793812a146123926cfcc6388184c8b822743a 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -102,6 +102,10 @@ wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
 wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
 target_link_libraries(gtest_sensor_base ${PROJECT_NAME})
 
+# shared_from_this test
+wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp)
+target_link_libraries(gtest_shared_from_this ${PROJECT_NAME})
+
 # SolverManager test
 wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
 target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
@@ -210,9 +214,10 @@ target_link_libraries(gtest_odom_2D ${PROJECT_NAME})
 wolf_add_gtest(gtest_odom_3D gtest_odom_3D.cpp)
 target_link_libraries(gtest_odom_3D ${PROJECT_NAME})
 
-# shared_from_this test
-wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp)
-target_link_libraries(gtest_shared_from_this ${PROJECT_NAME})
+# SensorBase test
+wolf_add_gtest(gtest_sensor_camera gtest_sensor_camera.cpp)
+target_link_libraries(gtest_sensor_camera ${PROJECT_NAME})
+
 
 # ------- Now Core classes Serialization ----------
 
diff --git a/src/test/gtest_constraint_autodiff.cpp b/src/test/gtest_constraint_autodiff.cpp
index c5820537f54d0aaf8e251cc4867a88b82f1d0e33..37754df0a2dfcc492195653699ea058949d16c8d 100644
--- a/src/test/gtest_constraint_autodiff.cpp
+++ b/src/test/gtest_constraint_autodiff.cpp
@@ -24,7 +24,10 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
     FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
 
     // SENSOR
-    SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1);
+    IntrinsicsOdom2D intrinsics_odo;
+    intrinsics_odo.k_disp_to_disp = 0.1;
+    intrinsics_odo.k_rot_to_rot = 0.1;
+    SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
 
     // CAPTURE
     CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
@@ -56,7 +59,10 @@ TEST(CaptureAutodiff, ResidualOdom2d)
     FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
 
     // SENSOR
-    SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1);
+    IntrinsicsOdom2D intrinsics_odo;
+    intrinsics_odo.k_disp_to_disp = 0.1;
+    intrinsics_odo.k_rot_to_rot = 0.1;
+    SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
 
     // CAPTURE
     CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
@@ -100,7 +106,10 @@ TEST(CaptureAutodiff, JacobianOdom2d)
     FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
 
     // SENSOR
-    SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1);
+    IntrinsicsOdom2D intrinsics_odo;
+    intrinsics_odo.k_disp_to_disp = 0.1;
+    intrinsics_odo.k_rot_to_rot = 0.1;
+    SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
 
     // CAPTURE
     CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
@@ -179,7 +188,10 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
     FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
 
     // SENSOR
-    SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1);
+    IntrinsicsOdom2D intrinsics_odo;
+    intrinsics_odo.k_disp_to_disp = 0.1;
+    intrinsics_odo.k_rot_to_rot = 0.1;
+    SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
 
     // CAPTURE
     CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
diff --git a/src/test/gtest_constraint_gnss_fix_2D.cpp b/src/test/gtest_constraint_gnss_fix_2D.cpp
index 5c1d3201d10bd410bf2ec53b2edd106cde09cb43..5900482eeabfe29b9cbe64bfe334b3726a3c7f20 100644
--- a/src/test/gtest_constraint_gnss_fix_2D.cpp
+++ b/src/test/gtest_constraint_gnss_fix_2D.cpp
@@ -75,8 +75,6 @@ class ConstraintGnssFix2DTest : public testing::Test
             gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
             std::cout << "gnss sensor installed" << std::endl;
             ProcessorParamsGnssFixPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssFix>();
-            gnss_params_ptr->name = "cool GNSS fix processor";
-            gnss_params_ptr->type = "GNSS FIX";
             gnss_params_ptr->time_tolerance = -1.0;
             gnss_params_ptr->voting_active = true;
             problem_ptr->installProcessor("GNSS FIX", "gnss fix", gnss_sensor_ptr, gnss_params_ptr);
diff --git a/src/test/gtest_constraint_gnss_single_diff_2D.cpp b/src/test/gtest_constraint_gnss_single_diff_2D.cpp
index 19eaf5ae192372c9865a2b8e8d3251bd49acdd59..743b8506ee399ba8e5d91fd42bfab423c56b8d8b 100644
--- a/src/test/gtest_constraint_gnss_single_diff_2D.cpp
+++ b/src/test/gtest_constraint_gnss_single_diff_2D.cpp
@@ -71,7 +71,7 @@ class ConstraintGnssSingleDiff2DTest : public testing::Test
             std::cout << "gnss sensor installed" << std::endl;
 
             // gnss processor
-            ProcessorParamsBasePtr gnss_params_ptr = std::make_shared<ProcessorParamsBase>(true, 1.0, "GNSS SINGLE DIFFERENCES", "cool GNSS single differences processor");
+            ProcessorParamsBasePtr gnss_params_ptr = std::make_shared<ProcessorParamsBase>(true, 1.0);
             problem_ptr->installProcessor("GNSS SINGLE DIFFERENCES", "gnss single difference", gnss_sensor_ptr, gnss_params_ptr);
             std::cout << "gnss processor installed" << std::endl;
 
diff --git a/src/test/gtest_frame_base.cpp b/src/test/gtest_frame_base.cpp
index ae6b78068133529dcf9045572bba5f9715b9a62c..92068e3920ee3c887dd6fd979db131e03e0f4015 100644
--- a/src/test/gtest_frame_base.cpp
+++ b/src/test/gtest_frame_base.cpp
@@ -55,7 +55,8 @@ TEST(FrameBase, LinksBasic)
     ASSERT_FALSE(F->getProblem());
     //    ASSERT_THROW(f->getPreviousFrame(), std::runtime_error);  // protected by assert()
     //    ASSERT_EQ(f->getStatus(), ST_ESTIMATED);                  // protected
-    ASSERT_FALSE(F->getCaptureOf(make_shared<SensorOdom2D>(nullptr, nullptr, 1,1)));
+    SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), IntrinsicsOdom2D());
+    ASSERT_FALSE(F->getCaptureOf(S));
     ASSERT_TRUE(F->getCaptureList().empty());
     ASSERT_TRUE(F->getConstrainedByList().empty());
     ASSERT_EQ(F->getHits() , (unsigned int) 0);
@@ -67,7 +68,10 @@ TEST(FrameBase, LinksToTree)
     // Problem with 2 frames and one motion constraint between them
     ProblemPtr P = Problem::create("PO 2D");
     TrajectoryBasePtr T = P->getTrajectoryPtr();
-    SensorOdom2DPtr S = make_shared<SensorOdom2D>(make_shared<StateBlock>(2), make_shared<StateBlock>(1), 1,1);
+    IntrinsicsOdom2D intrinsics_odo;
+    intrinsics_odo.k_disp_to_disp = 1;
+    intrinsics_odo.k_rot_to_rot = 1;
+    SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
     P->getHardwarePtr()->addSensor(S);
     FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
     T->addFrame(F1);
diff --git a/src/test/gtest_processor_tracker_feature_trifocal.cpp b/src/test/gtest_processor_tracker_feature_trifocal.cpp
index fb5625ccc2d662c31cac0bc9af3b8e52bc5a2db3..941cf0376702941b066e855b446150c9da6e4e42 100644
--- a/src/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/src/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -82,8 +82,11 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
                                                          intr);
 
     ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>();
-    params_tracker_feature_trifocal->name                           = "trifocal";
-    params_tracker_feature_trifocal->voting_active                  = false;
+
+    //    params_tracker_feature_trifocal->name                           = "trifocal";
+    params_tracker_feature_trifocal->pixel_noise_std                = 1.0;
+    params_tracker_feature_trifocal->voting_active                  = true;
+    params_tracker_feature_trifocal->min_features_for_keyframe      = 5;
     params_tracker_feature_trifocal->time_tolerance                 = dt/2;
     params_tracker_feature_trifocal->min_features_for_keyframe      = 5;
     params_tracker_feature_trifocal->pixel_noise_std                = 1.0;
diff --git a/src/test/gtest_sensor_camera.cpp b/src/test/gtest_sensor_camera.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a75431e50f596372604ad49bec5618a9fb0664f1
--- /dev/null
+++ b/src/test/gtest_sensor_camera.cpp
@@ -0,0 +1,123 @@
+/**
+ * \file gtest_sensor_camera.cpp
+ *
+ *  Created on: Feb 7, 2019
+ *      \author: jsola
+ */
+
+
+#include "utils_gtest.h"
+
+#include "sensor_camera.cpp"
+#include "sensor_factory.h"
+
+using namespace wolf;
+
+TEST(SensorCamera, Img_size)
+{
+    Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+    IntrinsicsCamera params;
+    params.width  = 640;
+    params.height = 480;
+    SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
+
+    ASSERT_EQ(cam->getImgWidth() , 640);
+    ASSERT_EQ(cam->getImgHeight(), 480);
+
+    cam->setImgWidth(100);
+    ASSERT_EQ(cam->getImgWidth() , 100);
+
+    cam->setImgHeight(100);
+    ASSERT_EQ(cam->getImgHeight(), 100);
+}
+
+TEST(SensorCamera, Intrinsics_Raw_Rectified)
+{
+    Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+    IntrinsicsCamera params;
+    params.pinhole_model_raw       << 321, 241, 321, 321;
+    params.pinhole_model_rectified << 320, 240, 320, 320;
+    SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
+
+    Eigen::Matrix3s K_raw, K_rectified;
+    K_raw       << 321, 0, 321,    0, 321, 241,    0, 0, 1;
+    K_rectified << 320, 0, 320,    0, 320, 240,    0, 0, 1;
+    Eigen::Vector4s k_raw(321,241,321,321);
+    Eigen::Vector4s k_rectified(320,240,320,320);
+
+    // default is raw
+    ASSERT_TRUE(cam->isUsingRawImages());
+    ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8);
+    ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsicPtr()->getState(), 1e-8);
+
+    cam->useRectifiedImages();
+    ASSERT_FALSE(cam->isUsingRawImages());
+    ASSERT_MATRIX_APPROX(K_rectified, cam->getIntrinsicMatrix(), 1e-8);
+    ASSERT_MATRIX_APPROX(k_rectified, cam->getIntrinsicPtr()->getState(), 1e-8);
+
+    cam->useRawImages();
+    ASSERT_TRUE(cam->isUsingRawImages());
+    ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8);
+    ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsicPtr()->getState(), 1e-8);
+}
+
+TEST(SensorCamera, Distortion)
+{
+    Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+    IntrinsicsCamera params;
+    params.width  = 640;
+    params.height = 480;
+    params.pinhole_model_raw       << 321, 241, 321, 321;
+    params.pinhole_model_rectified << 320, 240, 320, 320;
+    params.distortion = Eigen::Vector3s( -0.3, 0.096, 0 );
+    SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
+
+    Eigen::Vector3s d(-0.3, 0.096, 0);
+
+    ASSERT_MATRIX_APPROX(d, cam->getDistortionVector(), 1e-8);
+}
+
+TEST(SensorCamera, Correction_zero)
+{
+    Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+    IntrinsicsCamera params;
+    params.width  = 640;
+    params.height = 480;
+    params.pinhole_model_raw       << 321, 241, 321, 321;
+    params.pinhole_model_rectified << 320, 240, 320, 320;
+    params.distortion = Eigen::Vector3s( 0, 0, 0 );
+    SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
+
+    Eigen::MatrixXs c(cam->getCorrectionVector().size(), 1);
+    c.setZero();
+
+    ASSERT_GE(cam->getCorrectionVector().size(), cam->getDistortionVector().size());
+    ASSERT_MATRIX_APPROX(c, cam->getCorrectionVector(), 1e-8);
+}
+
+TEST(SensorCamera, create)
+{
+    Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+    IntrinsicsCameraPtr params = std::make_shared<IntrinsicsCamera>();
+    params->width  = 640;
+    params->height = 480;
+    params->pinhole_model_raw       << 321, 241, 321, 321;
+    params->pinhole_model_rectified << 320, 240, 320, 320;
+    params->distortion = Eigen::Vector3s( 0, 0, 0 );
+
+    SensorBasePtr   sen = SensorCamera::create("camera", extrinsics, params);
+
+    ASSERT_NE(sen, nullptr);
+
+    SensorCameraPtr cam = std::static_pointer_cast<SensorCamera>(sen);
+
+    ASSERT_NE(cam, nullptr);
+    ASSERT_EQ(cam->getImgWidth(), 640);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp
index 75e2e405e9d7dacc0ac57f927701fdf4d970878b..e31c957f86b2eac07b3df83e4c9199118eaf60b3 100644
--- a/src/yaml/processor_IMU_yaml.cpp
+++ b/src/yaml/processor_IMU_yaml.cpp
@@ -26,20 +26,10 @@ static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _file
 
     if (config["processor type"].as<std::string>() == "IMU")
     {
-
-        // YAML:: to Eigen::
-        using namespace Eigen;
-        std::string processor_type = config["processor type"]     .as<std::string>();
-        std::string processor_name = config["processor name"]     .as<std::string>();
-
         YAML::Node kf_vote = config["keyframe vote"];
 
         ProcessorParamsIMUPtr params = std::make_shared<ProcessorParamsIMU>();
 
-        params->time_tolerance = config["time tolerance"].as<Scalar>();
-
-        params->type                = processor_type;
-        params->name                = processor_name;
         params->max_time_span       = kf_vote["max time span"]      .as<Scalar>();
         params->max_buff_length     = kf_vote["max buffer length"]  .as<SizeEigen  >();
         params->dist_traveled       = kf_vote["dist traveled"]      .as<Scalar>();
diff --git a/src/yaml/processor_odom_3D_yaml.cpp b/src/yaml/processor_odom_3D_yaml.cpp
index 1b5a3f40db3589a8e51201273a93872cc827d8ce..047e42463a5466b66079ad9dfff9072f9566abac 100644
--- a/src/yaml/processor_odom_3D_yaml.cpp
+++ b/src/yaml/processor_odom_3D_yaml.cpp
@@ -26,18 +26,10 @@ static ProcessorParamsBasePtr createProcessorOdom3DParams(const std::string & _f
 
     if (config["processor type"].as<std::string>() == "ODOM 3D")
     {
-
-        // YAML:: to Eigen::
-        using namespace Eigen;
-        std::string processor_type = config["processor type"]     .as<std::string>();
-        std::string processor_name = config["processor name"]     .as<std::string>();
-
         YAML::Node kf_vote = config["keyframe vote"];
 
         ProcessorParamsOdom3DPtr params = std::make_shared<ProcessorParamsOdom3D>();
 
-        params->type                = processor_type;
-        params->name                = processor_name;
         params->max_time_span       = kf_vote["max time span"]      .as<Scalar>();
         params->max_buff_length     = kf_vote["max buffer length"]  .as<SizeEigen  >();
         params->dist_traveled       = kf_vote["dist traveled"]      .as<Scalar>();
diff --git a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
index 09b543541855b481ee954242a73686f25a08c3d8..438f0de2fa5e3660c9e22f6d61b27b8cecb303f8 100644
--- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
@@ -34,9 +34,6 @@ static ProcessorParamsBasePtr createProcessorParamsTrackerFeatureTrifocal(const
     {
         ProcessorParamsTrackerFeatureTrifocalPtr params = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>();
 
-        params->type                          = config["processor type"].as<std::string>();
-        params->name                          = config["processor name"].as<std::string>();
-
         YAML::Node vision_utils               = config      ["vision_utils"];
         params->yaml_file_params_vision_utils = vision_utils["YAML file params"].as<std::string>();
 
diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp
index 658becb6bf8dd50722454d2c83947357a3e57a2b..a29331becfebfc0d132ac78d13b0fcb7d83ab6f3 100644
--- a/src/yaml/sensor_IMU_yaml.cpp
+++ b/src/yaml/sensor_IMU_yaml.cpp
@@ -27,23 +27,17 @@ static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_y
 
     if (config["sensor type"].as<std::string>() == "IMU")
     {
-
-        // YAML:: to Eigen::
-        using namespace Eigen;
-        std::string sensor_type = config["sensor type"]     .as<std::string>();
-        std::string sensor_name = config["sensor name"]     .as<std::string>();
-
-        YAML::Node variances    = config["motion variances"];
-        YAML::Node kf_vote      = config["keyframe vote"];
+        YAML::Node variances        = config["motion variances"];
+        YAML::Node kf_vote          = config["keyframe vote"];
 
         IntrinsicsIMUPtr params = std::make_shared<IntrinsicsIMU>();
 
-        params->a_noise     = variances["a_noise"]  .as<Scalar>();
-        params->w_noise      = variances["w_noise"]   .as<Scalar>();
-        params->ab_initial_stdev     = variances["ab_initial_stdev"]     .as<Scalar>();
-        params->wb_initial_stdev     = variances["wb_initial_stdev"]     .as<Scalar>();
-        params->ab_rate_stdev        = variances["ab_rate_stdev"]     .as<Scalar>();
-        params->wb_rate_stdev        = variances["wb_rate_stdev"]     .as<Scalar>();
+        params->a_noise             = variances["a_noise"]          .as<Scalar>();
+        params->w_noise             = variances["w_noise"]          .as<Scalar>();
+        params->ab_initial_stdev    = variances["ab_initial_stdev"] .as<Scalar>();
+        params->wb_initial_stdev    = variances["wb_initial_stdev"] .as<Scalar>();
+        params->ab_rate_stdev       = variances["ab_rate_stdev"]    .as<Scalar>();
+        params->wb_rate_stdev       = variances["wb_rate_stdev"]    .as<Scalar>();
 
         return params;
     }
diff --git a/src/yaml/sensor_camera_yaml.cpp b/src/yaml/sensor_camera_yaml.cpp
index 852adc977ac61c465613ed08a6dfd267f2cceedd..22e2b85b8a688ecb5e4067ac3c55e0e68d062cc3 100644
--- a/src/yaml/sensor_camera_yaml.cpp
+++ b/src/yaml/sensor_camera_yaml.cpp
@@ -24,40 +24,58 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
 {
     YAML::Node camera_config = YAML::LoadFile(_filename_dot_yaml);
 
-    if ("CAMERA") //camera_config["sensor type"])
+    //    if (camera_config["sensor type"].as<std::string>() == "CAMERA") // this does not work: camera YAML files are ROS-styled
+    if (camera_config["camera_matrix"]) // check that at least this field exists to validate YAML file of the correct type
     {
 
         // YAML:: to Eigen::
         using namespace Eigen;
-        std::string sensor_type = "CAMERA"; //camera_config["sensor type"]                  .as<std::string>();
-        std::string sensor_name = camera_config["camera_name"]                      .as<std::string>();
         unsigned int width      = camera_config["image_width"]                      .as<unsigned int>();
         unsigned int height     = camera_config["image_height"]                     .as<unsigned int>();
-        VectorXd intrinsic      = camera_config["camera_matrix"]["data"]            .as<VectorXd>();
         VectorXd distortion     = camera_config["distortion_coefficients"]["data"]  .as<VectorXd>();
+        VectorXd intrinsic      = camera_config["camera_matrix"]["data"]            .as<VectorXd>();
+        VectorXd projection     = camera_config["projection_matrix"]["data"]        .as<VectorXd>();
 
         // 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->pinhole_model[0] = intrinsic[2];
-        intrinsics_cam->pinhole_model[1] = intrinsic[5];
-        intrinsics_cam->pinhole_model[2] = intrinsic[0];
-        intrinsics_cam->pinhole_model[3] = intrinsic[4];
+
+        intrinsics_cam->width   = width;
+        intrinsics_cam->height  = height;
+
+        intrinsics_cam->pinhole_model_raw[0] = intrinsic[2];
+        intrinsics_cam->pinhole_model_raw[1] = intrinsic[5];
+        intrinsics_cam->pinhole_model_raw[2] = intrinsic[0];
+        intrinsics_cam->pinhole_model_raw[3] = intrinsic[4];
+
+        intrinsics_cam->pinhole_model_rectified[0] = projection[2];
+        intrinsics_cam->pinhole_model_rectified[1] = projection[6];
+        intrinsics_cam->pinhole_model_rectified[2] = projection[0];
+        intrinsics_cam->pinhole_model_rectified[3] = projection[5];
+
         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!");
+
+        WOLF_WARN_COND( distortion(2) != 0 || distortion(3) != 0 , "Wolf does not handle tangential distortion. Please consider re-calibrating without tangential distortion!");
+
         if (distortion(4) == 0)
-            intrinsics_cam->distortion = distortion.head<2>();
+            if (distortion(1) == 0)
+                if (distortion(0) == 0)
+                    intrinsics_cam->distortion.resize(0);
+                else
+                {
+                    intrinsics_cam->distortion.resize(1);
+                    intrinsics_cam->distortion = distortion.head<1>();
+                }
+            else
+            {
+                intrinsics_cam->distortion.resize(2);
+                intrinsics_cam->distortion = distortion.head<2>();
+            }
         else
         {
-            unsigned int dist_size = distortion.size() - 2;
-            unsigned int dist_tail_size = dist_size - 2;
-            intrinsics_cam->distortion.resize(dist_size);
+            intrinsics_cam->distortion.resize(3);
             intrinsics_cam->distortion.head<2>() = distortion.head<2>();
-            intrinsics_cam->distortion.tail(dist_tail_size) = distortion.tail(dist_tail_size);
+            intrinsics_cam->distortion.tail<1>() = distortion.tail<1>();
         }
-        intrinsics_cam->width = width;
-        intrinsics_cam->height = height;
 
 
         //=========================================
@@ -87,7 +105,7 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
         return intrinsics_cam;
     }
 
-    std::cout << "Bad configuration file. No sensor type found." << std::endl;
+    std::cout << "Bad configuration file. No or bad sensor type found." << std::endl;
     return nullptr;
 }
 
diff --git a/src/yaml/sensor_laser_2D_yaml.cpp b/src/yaml/sensor_laser_2D_yaml.cpp
index 160386e6ea9f3b005ab1ea073ffe6bcf269c3d4c..110c3aa950157b83da46467f3a7d332688e78650 100644
--- a/src/yaml/sensor_laser_2D_yaml.cpp
+++ b/src/yaml/sensor_laser_2D_yaml.cpp
@@ -23,9 +23,9 @@ namespace {
 // intrinsics creator
 IntrinsicsBasePtr createIntrinsicsLaser2D(const std::string& _filename_dot_yaml)
 {
-    // TODO: Parse YAML <-- maybe we want this out of this file?
+    // If required: Parse YAML
+
     IntrinsicsLaser2DPtr params; // dummy
-    params->type = "LASER 2D"; // fill this one just for the fun of it
     return params;
 }
 
diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3D_yaml.cpp
index a6012aeef037ba22493f562969614ff885341aa4..8d1e391c2c061454e11b6b558ca133ed7fe3ea7d 100644
--- a/src/yaml/sensor_odom_3D_yaml.cpp
+++ b/src/yaml/sensor_odom_3D_yaml.cpp
@@ -26,14 +26,7 @@ static IntrinsicsBasePtr createIntrinsicsOdom3D(const std::string & _filename_do
 
     if (config["sensor type"].as<std::string>() == "ODOM 3D")
     {
-
-        // YAML:: to Eigen::
-        using namespace Eigen;
-        std::string sensor_type = config["sensor type"]     .as<std::string>();
-        std::string sensor_name = config["sensor name"]     .as<std::string>();
-
         YAML::Node variances = config["motion variances"];
-        YAML::Node kf_vote = config["keyframe vote"];
 
         IntrinsicsOdom3DPtr params = std::make_shared<IntrinsicsOdom3D>();