Calibration

Advance in the IMU code, and create a complete imu preintegration toolbox with imu_tools.h

src/sensor_base.cpp

@@ -6,14 +6,20 @@ namespace wolf {
 
 unsigned int SensorBase::sensor_id_count_ = 0;
 
-SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr,
-                       const unsigned int _noise_size, const bool _extr_dyn) :
+SensorBase::SensorBase(const std::string& _type,
+                       StateBlockPtr _p_ptr,
+                       StateBlockPtr _o_ptr,
+                       StateBlockPtr _intr_ptr,
+                       const unsigned int _noise_size,
+                       const bool _extr_dyn,
+                       const bool _intr_dyn) :
         NodeBase("SENSOR", _type),
         hardware_ptr_(),
         state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
         is_removing_(false),
         sensor_id_(++sensor_id_count_), // simple ID factory
         extrinsic_dynamic_(_extr_dyn),
+        intrinsic_dynamic_(_intr_dyn),
         noise_std_(_noise_size),
         noise_cov_(_noise_size, _noise_size)
 {
@@ -24,14 +30,20 @@ SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBloc
     //
 }
 
-SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr,
-                       const Eigen::VectorXs & _noise_std, const bool _extr_dyn) :
+SensorBase::SensorBase(const std::string& _type,
+                       StateBlockPtr _p_ptr,
+                       StateBlockPtr _o_ptr,
+                       StateBlockPtr _intr_ptr,
+                       const Eigen::VectorXs & _noise_std,
+                       const bool _extr_dyn,
+                       const bool _intr_dyn) :
         NodeBase("SENSOR", _type),
         hardware_ptr_(),
         state_block_vec_(6), // allow for 3 state blocks by default. Resize in derived constructors if needed.
         is_removing_(false),
         sensor_id_(++sensor_id_count_), // simple ID factory
         extrinsic_dynamic_(_extr_dyn),
+        intrinsic_dynamic_(_intr_dyn),
         noise_std_(_noise_std),
         noise_cov_(_noise_std.size(), _noise_std.size())
 {