Draft: Resolve "SensorBase: extrinsic and intrinsic initialization and prior"

Closes #381

src/sensor/sensor_base.cpp

 #include "core/sensor/sensor_base.h"
 #include "core/state_block/state_block.h"
+#include "core/state_block/state_angle.h"
 #include "core/state_block/state_quaternion.h"
 #include "core/factor/factor_block_absolute.h"
 #include "core/factor/factor_quaternion_absolute.h"
@@ -30,7 +31,7 @@ SensorBase::SensorBase(const std::string& _type,
                       (key != 'O' ?
                               std::make_shared<StateBlock>(sb_param.state,sb_param.prior_mode == "fix") :
                               (_params->dim == 2 ?
-                                      std::static_pointer_cast<StateBlock>(std::make_shared<StateAngle>(sb_param.state, sb_param.prior_mode == "fix")) :
+                                      std::static_pointer_cast<StateBlock>(std::make_shared<StateAngle>(sb_param.state(0), sb_param.prior_mode == "fix")) :
                                       std::static_pointer_cast<StateBlock>(std::make_shared<StateQuaternion>(sb_param.state,sb_param.prior_mode == "fix")) ) ),
                       sb_param.dynamic);
 
@@ -39,39 +40,6 @@ SensorBase::SensorBase(const std::string& _type,
                             sb_param.state,
                             sb_param.sigma.asDiagonal());
     }
-
-    /*// P
-    addStateBlock('P',
-                  std::make_shared<StateBlock>(_params->state_blocks.at('P').state,
-                                               _params->state_blocks.at('P').prior_mode == "fix"),
-                  _params->state_blocks.at('P').dynamic);
-
-    if (_params->state_blocks.at('P').prior_mode == "factor")
-        addPriorP(_params->state_blocks.at('P').state, _params->state_blocks.at('P').sigma.asDiagonal());
-
-    // O
-    addStateBlock('O',
-                  (_params->dim == 2 ?
-                          std::make_shared<StateAngle>(_params->state_blocks.at('O').state,
-                                                       _params->state_blocks.at('O').prior_mode == "fix") :
-                          std::make_shared<StateQuaternion>(_params->state_blocks.at('O').state,
-                                                            _params->state_blocks.at('O').prior_mode == "fix") ),
-                  _params->o_dynamic);
-
-    if (_params->state_blocks.at('O').prior_mode == "factor")
-        addPriorO(_params->p_state, _params->p_cov);
-
-    // I
-    if (_intrinsics_size != 0)
-    {
-        addStateBlock('I',
-                      std::make_shared<StateBlock>(_params->state_blocks.at('I').state,
-                                                   _params->state_blocks.at('I').prior_mode == "fix"),
-                      _params->state_blocks.at('I').dynamic);
-
-        if (_params->state_blocks.at('I').prior_mode == "factor")
-            addPriorI(_params->state_blocks.at('I').state, _params->state_blocks.at('I').sigma.asDiagonal());
-    }*/
 }
 
 SensorBase::SensorBase(const std::string& _type,