Remove constructors accepting state blocks as API

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
 

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;
 }

src/sensor_GPS_fix.h

@@ -25,15 +25,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);
 

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)
 {

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);
 

src/sensor_camera.cpp

@@ -7,15 +7,6 @@
 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),
-        using_raw_(true)
-{
-    useRawImages();
-}
-
 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_raw, true), 1),
                 img_width_(_intrinsics.width), //
@@ -75,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)

src/sensor_camera.h

@@ -28,17 +28,6 @@ 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);

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),

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);
 

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),

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);
 

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);

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);