diff --git a/src/constraint_block_absolute.h b/src/constraint_block_absolute.h index 1e901f10451a858d219a7fc39a7e3eb64c73f06a..e3def4493f0444ada07ed097e737610e5459671b 100644 --- a/src/constraint_block_absolute.h +++ b/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) diff --git a/src/problem.cpp b/src/problem.cpp index aefdac77ec51d3995d09ccd4584fc2e60ed818aa..658bd033dad8e177330dfededc49fb4fbe259d90 100644 --- a/src/problem.cpp +++ b/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() << " )"; diff --git a/src/processor_odom_2D.cpp b/src/processor_odom_2D.cpp index 04c73302051c9b20e0a3f1414d776f23f15bc362..9f3877f783d2a20edfb9b820a37e409d09ecc487 100644 --- a/src/processor_odom_2D.cpp +++ b/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); diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp index 9bff803ed457149143ceaec76dd6b56f8bc4d415..947984669e5c7cad26a0ca0c9c7d99732f2652f3 100644 --- a/src/sensor_base.cpp +++ b/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) diff --git a/src/sensor_base.h b/src/sensor_base.h index 1e690affe078716a233fb87e817fe431a9964d76..f01c8ce0b533ad838b33e9a980e92b80bc691d08 100644 --- a/src/sensor_base.h +++ b/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_; 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),""); }