Constraint prior sensor params

Added the functionality of adding an absolute constraint in sensor parameters (extrinsics or intrinsics). The class SensorBase has now two new members:

• Function addParameterPrior(...)
• Attribute std::map<StateBlockPtr,FeatureBasePtr> params_prior_map_

The new function addParameterPrior(...) adds the corresponding feature and constraint (ConstraintBlockAbsolute or ConstraintQuaternionAbsolute).

This MR also reimplemented ConstraintBlockAbsolute generalizing to any state block size and allowing constraining just a segment of the state. Now inheriting from ConstraintAnalytic. Accordingly, SensorBase::addParameterPrior(...) API also allows specifying segment constrains (not allowed in case of StateQuaternion):

void addParameterPrior(const StateBlockPtr& _sb, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx = 0, int size = -1)

• _sb is the constrained state block (must be in SensorBase::state_block_vec_).
• _x and _cov are the prior value and covariance (in case of StateQuaternion, the covariance is 3x3).
• _start_idx and _size specifies the segment of the state that is constrained. Default values allows simple calling without considering it.

The current implementation allows only one constraint per each state block, if SensorBase::addParameterPrior(...) is called two times, the second one is the one that remains.

All tests passed, devel was merged just now.

src/sensor_base.h

@@ -190,13 +190,13 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
                                 unsigned int _start_idx = 0,
                                 int _size = -1);
 
-        void addPPrior(const Eigen::VectorXs& _x,
+        void addPriorP(const Eigen::VectorXs& _x,
                        const Eigen::MatrixXs& _cov,
                        unsigned int _start_idx = 0,
                        int _size = -1);
-        void addOPrior(const Eigen::VectorXs& _x,
+        void addPriorO(const Eigen::VectorXs& _x,
                        const Eigen::MatrixXs& _cov);
-        void addIntrinsicsPrior(const Eigen::VectorXs& _x,
+        void addPriorIntrinsics(const Eigen::VectorXs& _x,
                                 const Eigen::MatrixXs& _cov,
                                 unsigned int _start_idx = 0,
                                 int _size = -1);
@@ -332,7 +332,7 @@ inline void SensorBase::addParameterPrior(const unsigned int _i, const TimeStamp
         addParameterStaticPrior(getStateBlockPtrStatic(_i), _x, _cov, _start_idx, _size);
 }
 
-inline void SensorBase::addParameterPrior(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov,
+inline void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov,
                                           unsigned int _start_idx, int _size)
 {
     CaptureBasePtr cap;
@@ -357,22 +357,22 @@ inline void SensorBase::addOPrior(const TimeStamp _ts, const Eigen::VectorXs& _x
 inline void SensorBase::addIntrinsicsPrior(const TimeStamp _ts, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov,
                                            unsigned int _start_idx, int _size)
 {
-    addParameterPrior(2,_ts, _x, _cov);
+    addPriorParameter(getStateBlockPtrStatic(_i), _x, _cov, _start_idx, _size);
 }
 
-inline void SensorBase::addPPrior(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+inline void SensorBase::addPriorP(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
 {
-    addParameterPrior(0, _x, _cov, _start_idx, _size);
+    addPriorParameter(0, _x, _cov, _start_idx, _size);
 }
 
-inline void SensorBase::addOPrior(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov)
+inline void SensorBase::addPriorO(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov)
 {
-    addParameterPrior(1, _x, _cov);
+    addPriorParameter(1, _x, _cov);
 }
 
-inline void SensorBase::addIntrinsicsPrior(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
 {
-    addParameterPrior(2, _x, _cov);
+    addPriorParameter(2, _x, _cov);
 }
 
 inline SizeEigen SensorBase::getCalibSize() const