diff --git a/src/sensor_base.h b/src/sensor_base.h index 8801f2f05c26164619b4b061e3b8b249155ee1f0..5967ecc51530f6bfbcdf465c6a80dfcc1afdf53d 100644 --- a/src/sensor_base.h +++ b/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); diff --git a/src/test/gtest_param_prior.cpp b/src/test/gtest_param_prior.cpp index 830660489d0708e901ecb5531ef154f1c709b3c6..0048fa2ef7f7a2f09505a7d5dc1d616bfafb8d1f 100644 --- a/src/test/gtest_param_prior.cpp +++ b/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),""); }