Merge remote-tracking branch 'origin/feature/constraint_sensor_params' into gnss

src/constraint_block_absolute.h

@@ -35,8 +35,15 @@ class ConstraintBlockAbsolute : public ConstraintAnalytic
          * \param _start_idx (default=-1) the size of the state segment that is constrained, -1 = all the
          *
          */
-        ConstraintBlockAbsolute(StateBlockPtr _sb_ptr, unsigned int _start_idx = 0, int _size = -1, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-            ConstraintAnalytic("BLOCK ABS", _apply_loss_function, _status, _sb_ptr),
+        ConstraintBlockAbsolute(StateBlockPtr _sb_ptr,
+                                unsigned int _start_idx = 0,
+                                int _size = -1,
+                                bool _apply_loss_function = false,
+                                ConstraintStatus _status = CTR_ACTIVE) :
+            ConstraintAnalytic("BLOCK ABS",
+                               _apply_loss_function,
+                               _status,
+                               _sb_ptr),
             sb_size_(_sb_ptr->getSize()),
             sb_constrained_start_(_start_idx),
             sb_constrained_size_(_size == -1 ? sb_size_ : _size)

src/problem.cpp

@@ -750,7 +750,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
                 {
                     if (i==0) cout << "    Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
                     if (i==2) cout << "    Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
-                    auto sb = S->getStateBlockPtrDynamic(i);
+                    auto sb = S->getStateBlockPtr(i);
                     if (sb)
                     {
                         cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";

src/processor_odom_2D.cpp

@@ -21,7 +21,7 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei
     assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size");
     assert(_data_cov.cols() == data_size_ && "Wrong _data_cov size");
 
-    // data  is [dtheta, dr]
+    // data  is [dr, dtheta]
     // delta is [dx, dy, dtheta]
     // motion model is 1/2 turn + straight + 1/2 turn
     _delta(0) = cos(_data(1) / 2) * _data(0);

src/sensor_base.cpp

@@ -82,8 +82,6 @@ void SensorBase::removeStateBlocks()
     }
 }
 
-
-
 void SensorBase::fix()
 {
     for( auto sbp : state_block_vec_)
@@ -144,11 +142,14 @@ void SensorBase::unfixIntrinsics()
     updateCalibSize();
 }
 
-void SensorBase::addParameterPrior(const StateBlockPtr& _sb, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
 {
+    assert(!isStateBlockDynamic(_i) && "SensorBase::addPriorParameter only allowed for static parameters");
+    assert(_i < state_block_vec_.size() && "State block not found");
+
+    StateBlockPtr _sb = getStateBlockPtrStatic(_i);
     bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
 
-    assert(std::find(state_block_vec_.begin(),state_block_vec_.end(),_sb) != state_block_vec_.end() && "adding prior to unknown state block");
     assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) ||
             (is_quaternion && _x.size() == 4 &&_cov.rows() == 3 && _cov.cols() == 3)) && "bad prior/covariance dimensions");
     assert((_size == -1 && _start_idx == 0) || (_size+_start_idx <= _sb->getSize()));
@@ -166,8 +167,8 @@ void SensorBase::addParameterPrior(const StateBlockPtr& _sb, const Eigen::Vector
     }
 
     // remove previous prior (if any)
-    if (params_prior_map_.find(_sb) != params_prior_map_.end())
-        params_prior_map_[_sb]->remove();
+    if (params_prior_map_.find(_i) != params_prior_map_.end())
+        params_prior_map_[_i]->remove();
 
     // create feature
     FeatureBasePtr ftr_prior = std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
@@ -182,7 +183,7 @@ void SensorBase::addParameterPrior(const StateBlockPtr& _sb, const Eigen::Vector
         ftr_prior->addConstraint(std::make_shared<ConstraintBlockAbsolute>(_sb, _start_idx, _size));
 
     // store feature in params_prior_map_
-    params_prior_map_[_sb] = ftr_prior;
+    params_prior_map_[_i] = ftr_prior;
 }
 
 void SensorBase::registerNewStateBlocks()
@@ -261,32 +262,32 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
 
 StateBlockPtr SensorBase::getPPtr(const TimeStamp _ts)
 {
-    return getStateBlockPtrDynamic(0, _ts);
+    return getStateBlockPtr(0, _ts);
 }
 
 StateBlockPtr SensorBase::getOPtr(const TimeStamp _ts)
 {
-    return getStateBlockPtrDynamic(1, _ts);
+    return getStateBlockPtr(1, _ts);
 }
 
 StateBlockPtr SensorBase::getIntrinsicPtr(const TimeStamp _ts)
 {
-    return getStateBlockPtrDynamic(2, _ts);
+    return getStateBlockPtr(2, _ts);
 }
 
 StateBlockPtr SensorBase::getPPtr()
 {
-    return getStateBlockPtrDynamic(0);
+    return getStateBlockPtr(0);
 }
 
 StateBlockPtr SensorBase::getOPtr()
 {
-    return getStateBlockPtrDynamic(1);
+    return getStateBlockPtr(1);
 }
 
 StateBlockPtr SensorBase::getIntrinsicPtr()
 {
-    return getStateBlockPtrDynamic(2);
+    return getStateBlockPtr(2);
 }
 
 SizeEigen SensorBase::computeCalibSize() const
@@ -340,32 +341,62 @@ ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
     return _proc_ptr;
 }
 
-StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i)
+StateBlockPtr SensorBase::getStateBlockPtr(unsigned int _i)
+{
+    CaptureBasePtr cap;
+
+    if (isStateBlockDynamic(_i, cap))
+        return cap->getStateBlockPtr(_i);
+
+    return getStateBlockPtrStatic(_i);
+}
+
+StateBlockPtr SensorBase::getStateBlockPtr(unsigned int _i, const TimeStamp& _ts)
+{
+    CaptureBasePtr cap;
+
+    if (isStateBlockDynamic(_i, _ts, cap))
+        return cap->getStateBlockPtr(_i);
+
+    return getStateBlockPtrStatic(_i);
+}
+
+bool SensorBase::isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap)
 {
     if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
     {
-        CaptureBasePtr cap = lastKeyCapture();
-        if (cap)
-            return cap->getStateBlockPtr(_i);
-        else
-            return getStateBlockPtrStatic(_i);
+        cap = lastKeyCapture();
+
+        return cap != nullptr;
     }
     else
-        return getStateBlockPtrStatic(_i);
+        return false;
 }
 
-StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts)
+bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap)
 {
     if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
     {
-        CaptureBasePtr cap = lastCapture(_ts);
-        if (cap)
-            return cap->getStateBlockPtr(_i);
-        else
-            return getStateBlockPtrStatic(_i);
+        cap = lastCapture(_ts);
+
+        return cap != nullptr;
     }
     else
-        return getStateBlockPtrStatic(_i);
+        return false;
+}
+
+bool SensorBase::isStateBlockDynamic(unsigned int _i)
+{
+    CaptureBasePtr cap;
+
+    return isStateBlockDynamic(_i,cap);
+}
+
+bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts)
+{
+    CaptureBasePtr cap;
+
+    return isStateBlockDynamic(_i,_ts,cap);
 }
 
 void SensorBase::setProblem(ProblemPtr _problem)

src/sensor_base.h

@@ -50,7 +50,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         Eigen::VectorXs noise_std_; // std of sensor noise
         Eigen::MatrixXs noise_cov_; // cov matrix of noise
 
-        std::map<StateBlockPtr,FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (multimap
+        std::map<unsigned int,FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by index in state_block_vec_)
 
     public:
         /** \brief Constructor with noise size
@@ -112,11 +112,16 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         const std::vector<StateBlockPtr>& getStateBlockVec() const;
         std::vector<StateBlockPtr>& getStateBlockVec();
         StateBlockPtr getStateBlockPtrStatic(unsigned int _i) const;
-        StateBlockPtr getStateBlockPtrDynamic(unsigned int _i);
-        StateBlockPtr getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts);
+        StateBlockPtr getStateBlockPtr(unsigned int _i);
+        StateBlockPtr getStateBlockPtr(unsigned int _i, const TimeStamp& _ts);
         void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr);
         void resizeStateBlockVec(unsigned int _size);
 
+        bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap);
+        bool isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap);
+        bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts);
+        bool isStateBlockDynamic(unsigned int _i);
+
         StateBlockPtr getPPtr(const TimeStamp _ts);
         StateBlockPtr getOPtr(const TimeStamp _ts);
         StateBlockPtr getIntrinsicPtr(const TimeStamp _ts);
@@ -134,21 +139,32 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         void unfixExtrinsics();
         void fixIntrinsics();
         void unfixIntrinsics();
+
         /** \brief Add an absolute constraint to a parameter
          *
          * Add an absolute constraint to a parameter
-         * \param _sb state block of the parameter to be constrained
+         * \param _i state block index (in state_block_vec_) of the parameter to be constrained
          * \param _x prior value
          * \param _cov covariance
          * \param _start_idx state segment starting index (not used in quaternions)
          * \param _size state segment size (-1: whole state) (not used in quaternions)
          *
          **/
-        void addParameterPrior(const StateBlockPtr& _sb,
+        void addPriorParameter(const unsigned int _i,
                                const Eigen::VectorXs& _x,
                                const Eigen::MatrixXs& _cov,
                                unsigned int _start_idx = 0,
                                int _size = -1);
+        void addPriorP(const Eigen::VectorXs& _x,
+                       const Eigen::MatrixXs& _cov,
+                       unsigned int _start_idx = 0,
+                       int _size = -1);
+        void addPriorO(const Eigen::VectorXs& _x,
+                       const Eigen::MatrixXs& _cov);
+        void addPriorIntrinsics(const Eigen::VectorXs& _x,
+                                const Eigen::MatrixXs& _cov,
+                                unsigned int _start_idx = 0,
+                                int _size = -1);
 
         SizeEigen getCalibSize() const;
         Eigen::VectorXs getCalibration() const;
@@ -213,7 +229,8 @@ inline StateBlockPtr SensorBase::getStateBlockPtrStatic(unsigned int _i) const
 
 inline void SensorBase::setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr)
 {
-    assert((params_prior_map_.find(state_block_vec_[_i]) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute constraint");
+    assert (_i < state_block_vec_.size() && "Setting a state block pointer out of the vector range!");
+    assert((params_prior_map_.find(_i) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute constraint");
     state_block_vec_[_i] = _sb_ptr;
 }
 
@@ -270,6 +287,21 @@ inline void SensorBase::setHardwarePtr(const HardwareBasePtr _hw_ptr)
     hardware_ptr_ = _hw_ptr;
 }
 
+inline void SensorBase::addPriorP(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+{
+    addPriorParameter(0, _x, _cov, _start_idx, _size);
+}
+
+inline void SensorBase::addPriorO(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov)
+{
+    addPriorParameter(1, _x, _cov);
+}
+
+inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+{
+    addPriorParameter(2, _x, _cov);
+}
+
 inline SizeEigen SensorBase::getCalibSize() const
 {
     return calib_size_;

src/test/gtest_param_prior.cpp

@@ -35,7 +35,7 @@ TEST(ParameterPrior, initial_extrinsics)
 
 TEST(ParameterPrior, prior_p)
 {
-    odom_sensor_ptr_->addParameterPrior(odom_sensor_ptr_->getPPtr(), prior_extrinsics.head(3),Eigen::Matrix3s::Identity());
+    odom_sensor_ptr_->addPriorP(prior_extrinsics.head(3),Eigen::Matrix3s::Identity());
 
     std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
 
@@ -44,7 +44,7 @@ TEST(ParameterPrior, prior_p)
 
 TEST(ParameterPrior, prior_o)
 {
-    odom_sensor_ptr_->addParameterPrior(odom_sensor_ptr_->getOPtr(), prior_extrinsics.tail(4),Eigen::Matrix3s::Identity());
+    odom_sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Eigen::Matrix3s::Identity());
 
     std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
 
@@ -53,7 +53,7 @@ TEST(ParameterPrior, prior_o)
 
 TEST(ParameterPrior, prior_p_overwrite)
 {
-    odom_sensor_ptr_->addParameterPrior(odom_sensor_ptr_->getPPtr(), prior2_extrinsics.head(3),Eigen::Matrix3s::Identity());
+    odom_sensor_ptr_->addPriorP(prior2_extrinsics.head(3),Eigen::Matrix3s::Identity());
 
     std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
 
@@ -62,7 +62,7 @@ TEST(ParameterPrior, prior_p_overwrite)
 
 TEST(ParameterPrior, prior_p_segment)
 {
-    odom_sensor_ptr_->addParameterPrior(odom_sensor_ptr_->getPPtr(), prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2);
+    odom_sensor_ptr_->addPriorP(prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2);
 
     std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
 
@@ -72,7 +72,7 @@ TEST(ParameterPrior, prior_p_segment)
 TEST(ParameterPrior, prior_o_segment)
 {
     // constraining segment of quaternion is not allowed
-    ASSERT_DEATH(odom_sensor_ptr_->addParameterPrior(odom_sensor_ptr_->getOPtr(), prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2),"");
+    ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter(1, prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2),"");
 }