Add sensor constructors with extrinsics and intrinsics data types only.

src/sensor_GPS_fix.cpp

@@ -10,6 +10,18 @@ SensorGPSFix::SensorGPSFix(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const dou
     //
 }
 
+SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics) :
+        SensorBase("GPS FIX", std::make_shared<StateBlock>(_extrinsics.head(3)), std::make_shared<StateQuaternion>(_extrinsics.tail(4)), nullptr, _intrinsics.noise_std)
+{
+    assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
+}
+
+SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) : SensorGPSFix(_extrinsics, *_intrinsics_ptr)
+{
+    //
+}
+
+
 SensorGPSFix::~SensorGPSFix()
 {
     //

src/sensor_GPS_fix.h

@@ -12,8 +12,10 @@
 
 namespace wolf {
 
+WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsGPSFix);
 struct IntrinsicsGPSFix : public IntrinsicsBase
 {
+        Eigen::Vector3s noise_std;
         // Empty -- it acts only as a typedef for IntrinsicsBase, but allows future extension with parameters
         virtual ~IntrinsicsGPSFix() = default;
 };
@@ -32,6 +34,8 @@ class SensorGPSFix : public SensorBase
          * 
          **/
 		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);
 
         virtual ~SensorGPSFix();
         

src/sensor_IMU.cpp

@@ -4,18 +4,42 @@
 
 namespace wolf {
 
-SensorIMU::SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr 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, 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)
+{
+    //
+}
+
+SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params) :
+        SensorBase("IMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), 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)
+{
+    assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
+}
+
 
 SensorIMU::~SensorIMU()
 {
@@ -29,11 +53,8 @@ SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::Ve
     // decode extrinsics vector
     assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
 
-    StateBlockPtr pos_ptr  = std::make_shared<StateBlock>(_extrinsics_pq.head(3), true);
-    StateBlockPtr ori_ptr  = std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true);
-
     IntrinsicsIMUPtr params = std::static_pointer_cast<IntrinsicsIMU>(_intrinsics);
-    SensorIMUPtr sen = std::make_shared<SensorIMU>(pos_ptr, ori_ptr, params);
+    SensorIMUPtr sen = std::make_shared<SensorIMU>(_extrinsics_pq, params);
     sen->setName(_unique_name);
     return sen;
 }

src/sensor_IMU.h

@@ -56,6 +56,9 @@ class SensorIMU : public SensorBase
          *
          **/
         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);
 
         Scalar getGyroNoise() const;
         Scalar getAccelNoise() const;

src/sensor_camera.cpp

@@ -15,18 +15,23 @@ SensorCamera::SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBloc
     //
 }
 
-SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const std::shared_ptr<IntrinsicsCamera> _intrinsics_ptr) :
-                SensorBase("CAMERA", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(_intrinsics_ptr->pinhole_model, true), 1),
-                img_width_(_intrinsics_ptr->width), //
-                img_height_(_intrinsics_ptr->height), //
-                distortion_(_intrinsics_ptr->distortion), //
+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),
+                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
 {
     assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3D");
-    K_ = setIntrinsicMatrix(_intrinsics_ptr->pinhole_model);
+    K_ = setIntrinsicMatrix(_intrinsics.pinhole_model);
     pinhole::computeCorrectionModel(getIntrinsicPtr()->getState(), distortion_, correction_);
 }
 
+SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr) :
+        SensorCamera(_extrinsics, *_intrinsics_ptr)
+{
+    //
+}
 
 SensorCamera::~SensorCamera()
 {
@@ -58,13 +63,6 @@ SensorBasePtr SensorCamera::create(const std::string& _unique_name, //
     SensorCameraPtr sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr);
     sen_ptr->setName(_unique_name);
 
-//    std::cout << "Created camera:\n\tintrinsics   : " << sen_ptr->getIntrinsicPtr()->getState().transpose() << std::endl;
-//    std::cout << "\tintrinsic matrix  : " << K_ << std::endl;
-//    std::cout << "\tdistortion  : " << distortion_.transpose() << std::endl;
-//    std::cout << "\tcorrection  : " << correction_.transpose() << std::endl;
-//    std::cout << "\tposition     : " << sen_ptr->getPPtr()->getState().transpose() << std::endl;
-//    std::cout << "\torientation  : " << sen_ptr->getOPtr()->getState().transpose() << std::endl;
-
     return sen_ptr;
 }
 

src/sensor_camera.h

@@ -7,6 +7,7 @@
 namespace wolf
 {
 
+WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsCamera);
 /** Struct of intrinsic camera parameters
  */
 struct IntrinsicsCamera : public IntrinsicsBase
@@ -38,7 +39,8 @@ class SensorCamera : public SensorBase
          **/
         SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, int _img_width, int _img_height);
 
-        SensorCamera(const Eigen::VectorXs & _extrinsics, const std::shared_ptr<IntrinsicsCamera> _intrinsics_ptr);
+        SensorCamera(const Eigen::VectorXs & _extrinsics, const IntrinsicsCamera& _intrinsics);
+        SensorCamera(const Eigen::VectorXs & _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr);
 
         virtual ~SensorCamera();
 

src/sensor_laser_2D.cpp

@@ -23,6 +23,19 @@ SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const l
     //
 }
 
+SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params) :
+        SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8),
+        scan_params_(_params.scan_params)
+{
+    //
+}
+
+SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params) :
+        SensorLaser2D(_p_ptr, _o_ptr, *_params)
+{
+    //
+}
+
 SensorLaser2D::~SensorLaser2D()
 {
     //

src/sensor_laser_2D.h

@@ -54,6 +54,8 @@ class SensorLaser2D : public SensorBase
          *
          **/
         SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params);
+        SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params);
+        SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params);
 
         virtual ~SensorLaser2D();
         

src/sensor_odom_2D.cpp

@@ -12,6 +12,22 @@ SensorOdom2D::SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Sca
     //
 }
 
+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),
+        k_rot_to_rot_(_intrinsics.k_rot_to_rot)
+{
+    assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2D.");
+    //
+}
+
+SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics) :
+        SensorOdom2D(_extrinsics, *_intrinsics)
+{
+    //
+}
+
+
 SensorOdom2D::~SensorOdom2D()
 {
     //
@@ -33,17 +49,20 @@ SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen:
 {
     // decode extrinsics vector
     assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
-    StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
-    StateBlockPtr ori_ptr = std::make_shared<StateAngle>(_extrinsics_po(2), true);
-    // cast intrinsics into derived type
+
     SensorOdom2DPtr odo;
     if (_intrinsics)
     {
         std::shared_ptr<IntrinsicsOdom2D> params = std::static_pointer_cast<IntrinsicsOdom2D>(_intrinsics);
-        odo = std::make_shared<SensorOdom2D>(pos_ptr, ori_ptr, params->k_disp_to_disp, params->k_rot_to_rot);
+        odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params);
     }
     else
-        odo = std::make_shared<SensorOdom2D>(pos_ptr, ori_ptr, 1, 1);
+    {
+        IntrinsicsOdom2D params;
+        params.k_disp_to_disp = 1;
+        params.k_rot_to_rot   = 1;
+        odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params);
+    }
     odo->setName(_unique_name);
     return odo;
 }

src/sensor_odom_2D.h

@@ -35,7 +35,9 @@ class SensorOdom2D : public SensorBase
          * \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(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);
 
         virtual ~SensorOdom2D();
         

src/sensor_odom_3D.cpp

@@ -24,23 +24,46 @@ SensorOdom3D::SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _o_ptr, Intr
     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),
+                        k_disp_to_rot_(_intrinsics.k_disp_to_rot),
+                        k_rot_to_rot_(_intrinsics.k_rot_to_rot),
+                        min_disp_var_(_intrinsics.min_disp_var),
+                        min_rot_var_(_intrinsics.min_rot_var)
+{
+    assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3D.");
+
+    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, IntrinsicsOdom3DPtr _intrinsics) :
+        SensorOdom3D(_extrinsics_pq, *_intrinsics)
+{
+    //
+}
+
+
 SensorOdom3D::~SensorOdom3D()
 {
     //
 }
 
 // Define the factory method
-SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po,
+SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq,
                                  const IntrinsicsBasePtr _intrinsics)
 {
     // decode extrinsics vector
-    assert(_extrinsics_po.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
-    StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(3), true);
-    StateQuaternionPtr ori_ptr = std::make_shared<StateQuaternion>(_extrinsics_po.tail(4), true);
+    assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
+
     // cast intrinsics into derived type
     IntrinsicsOdom3DPtr params = std::static_pointer_cast<IntrinsicsOdom3D>(_intrinsics);
-    SensorBasePtr odo = std::make_shared<SensorOdom3D>(pos_ptr, ori_ptr, params);
+
+    // Call constructor and finish
+    SensorBasePtr odo = std::make_shared<SensorOdom3D>(_extrinsics_pq, params);
     odo->setName(_unique_name);
+
     return odo;
 }
 

src/sensor_odom_3D.h

@@ -47,6 +47,8 @@ class SensorOdom3D : public SensorBase
          * \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);
 
         virtual ~SensorOdom3D();