updated to new function names

src/sensor_base.h

@@ -143,18 +143,13 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         /** \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 addPriorParameter(const StateBlockPtr& _sb,
-                                     const Eigen::VectorXs& _x,
-                                     const Eigen::MatrixXs& _cov,
-                                     unsigned int _start_idx = 0,
-                                     int _size = -1);
         void addPriorParameter(const unsigned int _i,
                                const Eigen::VectorXs& _x,
                                const Eigen::MatrixXs& _cov,
@@ -292,12 +287,6 @@ inline void SensorBase::setHardwarePtr(const HardwareBasePtr _hw_ptr)
     hardware_ptr_ = _hw_ptr;
 }
 
-inline void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov,
-                                          unsigned int _start_idx, int _size)
-{
-    addPriorParameter(getStateBlockPtrStatic(_i), _x, _cov, _start_idx, _size);
-}
-
 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);

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),"");
 }