diff --git a/include/base/capture/capture_base.h b/include/base/capture/capture_base.h index 7643f243ea4b8a05e0acdad82eb4454a8e87a6c4..0082d9d314fa28cf9f26a64b0cb455ab07102244 100644 --- a/include/base/capture/capture_base.h +++ b/include/base/capture/capture_base.h @@ -57,7 +57,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture void setTimeStampToNow(); FrameBasePtr getFrame() const; - void setFramePtr(const FrameBasePtr _frm_ptr); + void setFrame(const FrameBasePtr _frm_ptr); void unlinkFromFrame(){frame_ptr_.reset();} virtual void setProblem(ProblemPtr _problem) final; @@ -69,7 +69,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture void getFactorList(FactorBasePtrList& _fac_list); SensorBasePtr getSensor() const; - virtual void setSensorPtr(const SensorBasePtr sensor_ptr); + virtual void setSensor(const SensorBasePtr sensor_ptr); // constrained by virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr); @@ -79,8 +79,8 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture // State blocks const std::vector<StateBlockPtr>& getStateBlockVec() const; std::vector<StateBlockPtr>& getStateBlockVec(); - StateBlockPtr getStateBlockPtr(unsigned int _i) const; - void setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr); + StateBlockPtr getStateBlock(unsigned int _i) const; + void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr); StateBlockPtr getSensorP() const; StateBlockPtr getSensorO() const; @@ -143,24 +143,24 @@ inline std::vector<StateBlockPtr>& CaptureBase::getStateBlockVec() return state_block_vec_; } -inline void CaptureBase::setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr) +inline void CaptureBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr) { state_block_vec_[_i] = _sb_ptr; } inline StateBlockPtr CaptureBase::getSensorP() const { - return getStateBlockPtr(0); + return getStateBlock(0); } inline StateBlockPtr CaptureBase::getSensorO() const { - return getStateBlockPtr(1); + return getStateBlock(1); } inline StateBlockPtr CaptureBase::getSensorIntrinsic() const { - return getStateBlockPtr(2); + return getStateBlock(2); } inline unsigned int CaptureBase::id() @@ -173,7 +173,7 @@ inline FrameBasePtr CaptureBase::getFrame() const return frame_ptr_.lock(); } -inline void CaptureBase::setFramePtr(const FrameBasePtr _frm_ptr) +inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr) { frame_ptr_ = _frm_ptr; } @@ -203,7 +203,7 @@ inline SensorBasePtr CaptureBase::getSensor() const return sensor_ptr_.lock(); } -inline void CaptureBase::setSensorPtr(const SensorBasePtr sensor_ptr) +inline void CaptureBase::setSensor(const SensorBasePtr sensor_ptr) { sensor_ptr_ = sensor_ptr; } diff --git a/include/base/capture/capture_laser_2D.h b/include/base/capture/capture_laser_2D.h index 9abd6bf4b874a8970d71929355aed095e52c3838..96a434547b5e802376472f472be0aeb0e27af940 100644 --- a/include/base/capture/capture_laser_2D.h +++ b/include/base/capture/capture_laser_2D.h @@ -28,7 +28,7 @@ class CaptureLaser2D : public CaptureBase laserscanutils::LaserScan& getScan(); - void setSensorPtr(const SensorBasePtr sensor_ptr); + void setSensor(const SensorBasePtr sensor_ptr); private: SensorLaser2DPtr laser_ptr_; //specific pointer to sensor laser 2D object @@ -41,9 +41,9 @@ inline laserscanutils::LaserScan& CaptureLaser2D::getScan() return scan_; } -inline void CaptureLaser2D::setSensorPtr(const SensorBasePtr sensor_ptr) +inline void CaptureLaser2D::setSensor(const SensorBasePtr sensor_ptr) { - CaptureBase::setSensorPtr(sensor_ptr); + CaptureBase::setSensor(sensor_ptr); laser_ptr_ = std::static_pointer_cast<SensorLaser2D>(sensor_ptr); } diff --git a/include/base/capture/capture_motion.h b/include/base/capture/capture_motion.h index ab932da224bd610c130f6bc9f108e8d746c75f7e..ed71171e99ef6d221777f1ab89a7e0ba4a8dc0d7 100644 --- a/include/base/capture/capture_motion.h +++ b/include/base/capture/capture_motion.h @@ -97,7 +97,7 @@ class CaptureMotion : public CaptureBase // Origin frame FrameBasePtr getOriginFrame(); - void setOriginFramePtr(FrameBasePtr _frame_ptr); + void setOriginFrame(FrameBasePtr _frame_ptr); // member data: private: @@ -161,7 +161,7 @@ inline FrameBasePtr CaptureMotion::getOriginFrame() return origin_frame_ptr_.lock(); } -inline void CaptureMotion::setOriginFramePtr(FrameBasePtr _frame_ptr) +inline void CaptureMotion::setOriginFrame(FrameBasePtr _frame_ptr) { origin_frame_ptr_ = _frame_ptr; } diff --git a/include/base/capture/capture_wheel_joint_position.h b/include/base/capture/capture_wheel_joint_position.h index 0bade4029f499ba78b6cc71a7f260f05df44a79e..a90cdb205178dd4c36cd302a7793e7e74c668df1 100644 --- a/include/base/capture/capture_wheel_joint_position.h +++ b/include/base/capture/capture_wheel_joint_position.h @@ -155,7 +155,7 @@ protected: // ~CaptureWheelJointPosition() = default; -//// void setSensorPtr(const SensorBasePtr sensor_ptr) override; +//// void setSensor(const SensorBasePtr sensor_ptr) override; // std::size_t getNumWheels() const noexcept // { diff --git a/include/base/factor/factor_autodiff_distance_3D.h b/include/base/factor/factor_autodiff_distance_3D.h index 0eb1fc7570494b4731656034c9fc56025516c9af..77eb08e2ae049978c7f2978eee3106fcec66ba9c 100644 --- a/include/base/factor/factor_autodiff_distance_3D.h +++ b/include/base/factor/factor_autodiff_distance_3D.h @@ -34,7 +34,7 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D, _feat->getFrame()->getP(), _frm_other->getP()) { - setFeaturePtr(_feat); + setFeature(_feat); } virtual ~FactorAutodiffDistance3D() { /* nothing */ } diff --git a/include/base/factor/factor_autodiff_trifocal.h b/include/base/factor/factor_autodiff_trifocal.h index b7e56e0ac17b15d857f05522f42f911de3668291..d7c2091fb2c547b1511d8f5985e66723a24fb247 100644 --- a/include/base/factor/factor_autodiff_trifocal.h +++ b/include/base/factor/factor_autodiff_trifocal.h @@ -152,7 +152,7 @@ FactorAutodiffTrifocal::FactorAutodiffTrifocal(const FeatureBasePtr& _feature_pr camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensor())), sqrt_information_upper(Matrix3s::Zero()) { - setFeaturePtr(_feature_last_ptr); + setFeature(_feature_last_ptr); Matrix3s K_inv = camera_ptr_->getIntrinsicMatrix().inverse(); pixel_canonical_prev_ = K_inv * Vector3s(_feature_prev_ptr ->getMeasurement(0), _feature_prev_ptr ->getMeasurement(1), 1.0); pixel_canonical_origin_ = K_inv * Vector3s(_feature_origin_ptr->getMeasurement(0), _feature_origin_ptr->getMeasurement(1), 1.0); diff --git a/include/base/factor/factor_base.h b/include/base/factor/factor_base.h index ab5e362e8efb4d50e198a324770aaf19d9518f7c..4562d5f75b39995fc6578b8498fc5ee95e223e61 100644 --- a/include/base/factor/factor_base.h +++ b/include/base/factor/factor_base.h @@ -113,7 +113,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa /** \brief Returns a pointer to the feature constrained from **/ FeatureBasePtr getFeature() const; - void setFeaturePtr(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;} + void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;} /** \brief Returns a pointer to its capture **/ @@ -142,22 +142,22 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa /** \brief Returns a pointer to the frame constrained to **/ FrameBasePtr getFrameOther() const { return frame_other_ptr_.lock(); } - void setFrameOtherPtr(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; } + void setFrameOther(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; } /** \brief Returns a pointer to the frame constrained to **/ CaptureBasePtr getCaptureOther() const { return capture_other_ptr_.lock(); } - void setCaptureOtherPtr(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; } + void setCaptureOther(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; } /** \brief Returns a pointer to the feature constrained to **/ FeatureBasePtr getFeatureOther() const { return feature_other_ptr_.lock(); } - void setFeatureOtherPtr(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; } + void setFeatureOther(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; } /** \brief Returns a pointer to the landmark constrained to **/ LandmarkBasePtr getLandmarkOther() const { return landmark_other_ptr_.lock(); } - void setLandmarkOtherPtr(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; } + void setLandmarkOther(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; } /** * @brief getProcessor diff --git a/include/base/factor/factor_point_2D.h b/include/base/factor/factor_point_2D.h index 2034260de9483f592e54de2a1e3f125a12c19879..4d9686e3805c9fb5d337ed033e90a904cbb4ae53 100644 --- a/include/base/factor/factor_point_2D.h +++ b/include/base/factor/factor_point_2D.h @@ -30,11 +30,11 @@ class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2> const ProcessorBasePtr& _processor_ptr, unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorPoint2D,2,2,1,2,1,2>("POINT 2D", - nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), - feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2)) + nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id)), + feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2)) { //std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl; - //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)->getVector().transpose() << std::endl; + //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl; Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU(); measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition diff --git a/include/base/factor/factor_point_to_line_2D.h b/include/base/factor/factor_point_to_line_2D.h index e60c8d3c7ef2677637cb06d86a832064fdaa44ce..d4b891f3ee5d055d3e7500464e9ce59b1780f9f9 100644 --- a/include/base/factor/factor_point_to_line_2D.h +++ b/include/base/factor/factor_point_to_line_2D.h @@ -31,8 +31,8 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1, unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>("POINT TO LINE 2D", - nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id), _lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)), - landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2)) + nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id), _lmk_ptr->getPointStateBlock(_lmk_point_aux_id)), + landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2)) { //std::cout << "FactorPointToLine2D" << std::endl; Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A diff --git a/include/base/feature/feature_base.h b/include/base/feature/feature_base.h index df014203d7f5bb530f192347b4f19136c822de0b..3732bc55247d9d39b90992a07afe7bb04d7f7681 100644 --- a/include/base/feature/feature_base.h +++ b/include/base/feature/feature_base.h @@ -76,7 +76,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature FrameBasePtr getFrame() const; CaptureBasePtr getCapture() const; - void setCapturePtr(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;} + void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;} FactorBasePtr addFactor(FactorBasePtr _co_ptr); FactorBasePtrList& getFactorList(); diff --git a/include/base/frame_base.h b/include/base/frame_base.h index e6e4c14db28b5e5c897d851773b1f2ff4534ec84..be97eb80a333fac1391f7b19bf12d1895e9dbb6d 100644 --- a/include/base/frame_base.h +++ b/include/base/frame_base.h @@ -86,17 +86,17 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase const std::vector<StateBlockPtr>& getStateBlockVec() const; std::vector<StateBlockPtr>& getStateBlockVec(); protected: - StateBlockPtr getStateBlockPtr(unsigned int _i) const; - void setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr); + StateBlockPtr getStateBlock(unsigned int _i) const; + void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr); void resizeStateBlockVec(unsigned int _size); public: StateBlockPtr getP() const; StateBlockPtr getO() const; StateBlockPtr getV() const; - void setPPtr(const StateBlockPtr _p_ptr); - void setOPtr(const StateBlockPtr _o_ptr); - void setVPtr(const StateBlockPtr _v_ptr); + void setP(const StateBlockPtr _p_ptr); + void setO(const StateBlockPtr _o_ptr); + void setV(const StateBlockPtr _v_ptr); void registerNewStateBlocks(); void removeStateBlocks(); @@ -120,7 +120,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase virtual void setProblem(ProblemPtr _problem) final; TrajectoryBasePtr getTrajectory() const; - void setTrajectoryPtr(TrajectoryBasePtr _trj_ptr); + void setTrajectory(TrajectoryBasePtr _trj_ptr); FrameBasePtr getPreviousFrame() const; FrameBasePtr getNextFrame() const; @@ -197,7 +197,7 @@ inline StateBlockPtr FrameBase::getP() const { return state_block_vec_[0]; } -inline void FrameBase::setPPtr(const StateBlockPtr _p_ptr) +inline void FrameBase::setP(const StateBlockPtr _p_ptr) { state_block_vec_[0] = _p_ptr; } @@ -206,7 +206,7 @@ inline StateBlockPtr FrameBase::getO() const { return state_block_vec_[1]; } -inline void FrameBase::setOPtr(const StateBlockPtr _o_ptr) +inline void FrameBase::setO(const StateBlockPtr _o_ptr) { state_block_vec_[1] = _o_ptr; } @@ -216,18 +216,18 @@ inline StateBlockPtr FrameBase::getV() const return state_block_vec_[2]; } -inline void FrameBase::setVPtr(const StateBlockPtr _v_ptr) +inline void FrameBase::setV(const StateBlockPtr _v_ptr) { state_block_vec_[2] = _v_ptr; } -inline StateBlockPtr FrameBase::getStateBlockPtr(unsigned int _i) const +inline StateBlockPtr FrameBase::getStateBlock(unsigned int _i) const { assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!"); return state_block_vec_[_i]; } -inline void FrameBase::setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr) +inline void FrameBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr) { assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!"); state_block_vec_[_i] = _sb_ptr; @@ -266,7 +266,7 @@ inline FactorBasePtrList& FrameBase::getConstrainedByList() return constrained_by_list_; } -inline void FrameBase::setTrajectoryPtr(TrajectoryBasePtr _trj_ptr) +inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr) { trajectory_ptr_ = _trj_ptr; } diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h index f12da14aabfd3dd33c03be8f2bc22fd5de6c2ced..9019ee85efd04f54ddfa920e98413c8aabf3846e 100644 --- a/include/base/landmark/landmark_base.h +++ b/include/base/landmark/landmark_base.h @@ -62,12 +62,12 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma const std::vector<StateBlockPtr>& getStateBlockVec() const; std::vector<StateBlockPtr>& getStateBlockVec(); std::vector<StateBlockPtr> getUsedStateBlockVec() const; - StateBlockPtr getStateBlockPtr(unsigned int _i) const; - void setStateBlockPtr(unsigned int _i, StateBlockPtr _sb_ptr); + StateBlockPtr getStateBlock(unsigned int _i) const; + void setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr); StateBlockPtr getP() const; StateBlockPtr getO() const; - void setPPtr(const StateBlockPtr _p_ptr); - void setOPtr(const StateBlockPtr _o_ptr); + void setP(const StateBlockPtr _p_ptr); + void setO(const StateBlockPtr _o_ptr); virtual void registerNewStateBlocks(); Eigen::VectorXs getState() const; void getState(Eigen::VectorXs& _state) const; @@ -90,7 +90,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma unsigned int getHits() const; FactorBasePtrList& getConstrainedByList(); - void setMapPtr(const MapBasePtr _map_ptr); + void setMap(const MapBasePtr _map_ptr); MapBasePtr getMap(); }; @@ -113,7 +113,7 @@ inline MapBasePtr LandmarkBase::getMap() return map_ptr_.lock(); } -inline void LandmarkBase::setMapPtr(const MapBasePtr _map_ptr) +inline void LandmarkBase::setMap(const MapBasePtr _map_ptr) { map_ptr_ = _map_ptr; } @@ -150,35 +150,35 @@ inline std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec() return state_block_vec_; } -inline StateBlockPtr LandmarkBase::getStateBlockPtr(unsigned int _i) const +inline StateBlockPtr LandmarkBase::getStateBlock(unsigned int _i) const { assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!"); return state_block_vec_[_i]; } -inline void LandmarkBase::setStateBlockPtr(unsigned int _i, StateBlockPtr _sb_ptr) +inline void LandmarkBase::setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr) { state_block_vec_[_i] = _sb_ptr; } inline StateBlockPtr LandmarkBase::getP() const { - return getStateBlockPtr(0); + return getStateBlock(0); } inline StateBlockPtr LandmarkBase::getO() const { - return getStateBlockPtr(1); + return getStateBlock(1); } -inline void LandmarkBase::setPPtr(const StateBlockPtr _st_ptr) +inline void LandmarkBase::setP(const StateBlockPtr _st_ptr) { - setStateBlockPtr(0, _st_ptr); + setStateBlock(0, _st_ptr); } -inline void LandmarkBase::setOPtr(const StateBlockPtr _st_ptr) +inline void LandmarkBase::setO(const StateBlockPtr _st_ptr) { - setStateBlockPtr(1, _st_ptr); + setStateBlock(1, _st_ptr); } inline void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor) diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h index 4414a69f6175f36280656c2df33e4e94697d0d7d..d6b5d51f9edee7b70a7376a2e0771b34c1de612e 100644 --- a/include/base/landmark/landmark_polyline_2D.h +++ b/include/base/landmark/landmark_polyline_2D.h @@ -74,7 +74,7 @@ class LandmarkPolyline2D : public LandmarkBase const Eigen::VectorXs getPointVector(int _i) const; - StateBlockPtr getPointStateBlockPtr(int _i); + StateBlockPtr getPointStateBlock(int _i); /** \brief Gets a vector of all state blocks pointers **/ diff --git a/include/base/problem.h b/include/base/problem.h index d96aae5e29d91ba85d9251d728d89652da32a1c7..84b51c2a6ce9638fef140c84c86240ebb596738d 100644 --- a/include/base/problem.h +++ b/include/base/problem.h @@ -87,7 +87,7 @@ class Problem : public std::enable_shared_from_this<Problem> /** \brief get a sensor pointer by its name * \param _sensor_name The sensor name, as it was installed with installSensor() */ - SensorBasePtr getSensorPtr(const std::string& _sensor_name); + SensorBasePtr getSensor(const std::string& _sensor_name); /** \brief Factory method to install (create, and add to sensor) processors only from its properties * diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h index 88c97e62f00336cf04b17b269cfcee7ca31b4bf2..84fee5343c63ec3c01b28de1dfbc46c37125f337 100644 --- a/include/base/processor/processor_base.h +++ b/include/base/processor/processor_base.h @@ -184,7 +184,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce SensorBasePtr getSensor(); const SensorBasePtr getSensor() const; - void setSensorPtr(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;} + void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;} virtual bool isMotion(); diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h index 6b8d9bc1c31fd8f489dfc40e77ad461c17812920..64ed42ba22597606c616af57c9ba23984ad3b2a7 100644 --- a/include/base/sensor/sensor_base.h +++ b/include/base/sensor/sensor_base.h @@ -94,7 +94,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa virtual void setProblem(ProblemPtr _problem) final; HardwareBasePtr getHardware(); - void setHardwarePtr(const HardwareBasePtr _hw_ptr); + void setHardware(const HardwareBasePtr _hw_ptr); ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr); ProcessorBasePtrList& getProcessorList(); @@ -108,8 +108,8 @@ 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 getStateBlockPtr(unsigned int _i); - StateBlockPtr getStateBlockPtr(unsigned int _i, const TimeStamp& _ts); + StateBlockPtr getStateBlock(unsigned int _i); + StateBlockPtr getStateBlock(unsigned int _i, const TimeStamp& _ts); void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr); void resizeStateBlockVec(unsigned int _size); @@ -118,15 +118,15 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa 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); + StateBlockPtr getP(const TimeStamp _ts); + StateBlockPtr getO(const TimeStamp _ts); + StateBlockPtr getIntrinsic(const TimeStamp _ts); StateBlockPtr getP() ; StateBlockPtr getO(); StateBlockPtr getIntrinsic(); - void setPPtr(const StateBlockPtr _p_ptr); - void setOPtr(const StateBlockPtr _o_ptr); - void setIntrinsicPtr(const StateBlockPtr _intr_ptr); + void setP(const StateBlockPtr _p_ptr); + void setO(const StateBlockPtr _o_ptr); + void setIntrinsic(const StateBlockPtr _intr_ptr); void removeStateBlocks(); void fix(); @@ -263,22 +263,22 @@ inline HardwareBasePtr SensorBase::getHardware() return hardware_ptr_.lock(); } -inline void SensorBase::setPPtr(const StateBlockPtr _p_ptr) +inline void SensorBase::setP(const StateBlockPtr _p_ptr) { setStateBlockPtrStatic(0, _p_ptr); } -inline void SensorBase::setOPtr(const StateBlockPtr _o_ptr) +inline void SensorBase::setO(const StateBlockPtr _o_ptr) { setStateBlockPtrStatic(1, _o_ptr); } -inline void SensorBase::setIntrinsicPtr(const StateBlockPtr _intr_ptr) +inline void SensorBase::setIntrinsic(const StateBlockPtr _intr_ptr) { setStateBlockPtrStatic(2, _intr_ptr); } -inline void SensorBase::setHardwarePtr(const HardwareBasePtr _hw_ptr) +inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr) { hardware_ptr_ = _hw_ptr; } diff --git a/include/base/state_block.h b/include/base/state_block.h index 09811c41e583e1155c7aa59a5ce9e93098b188ee..447e8a0e9b3534f8048f3aa55131bdaed617e0d7 100644 --- a/include/base/state_block.h +++ b/include/base/state_block.h @@ -112,7 +112,7 @@ public: /** \brief Sets a local parametrization **/ - void setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param); + void setLocalParametrization(LocalParametrizationBasePtr _local_param); /** \brief Removes the state_block local parametrization **/ @@ -242,7 +242,7 @@ inline void StateBlock::removeLocalParametrization() local_param_updated_.store(true); } -inline void StateBlock::setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param) +inline void StateBlock::setLocalParametrization(LocalParametrizationBasePtr _local_param) { assert(_local_param != nullptr && "setting a null local parametrization"); local_param_ptr_ = _local_param; diff --git a/include/base/trajectory_base.h b/include/base/trajectory_base.h index 9150f8fa0daeb9f7999f38f35a5db02712a63c10..6ceddf9e6a8742133b01ee9277369a0a3bcaa5be 100644 --- a/include/base/trajectory_base.h +++ b/include/base/trajectory_base.h @@ -41,7 +41,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj FrameBasePtr getLastKeyFrame(); FrameBasePtr findLastKeyFrame(); FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts); - void setLastKeyFramePtr(FrameBasePtr _key_frame_ptr); + void setLastKeyFrame(FrameBasePtr _key_frame_ptr); void sortFrame(FrameBasePtr _frm_ptr); void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place); FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr); @@ -66,7 +66,7 @@ inline FrameBasePtr TrajectoryBase::getLastKeyFrame() return last_key_frame_ptr_; } -inline void TrajectoryBase::setLastKeyFramePtr(FrameBasePtr _key_frame_ptr) +inline void TrajectoryBase::setLastKeyFrame(FrameBasePtr _key_frame_ptr) { last_key_frame_ptr_ = _key_frame_ptr; } diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index 86165e4503302e5898a1ae0c6f2129297d2b2b73..1d2aa58dbd1b0a5b1767def1eb55d8978f729ac4 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -99,7 +99,7 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr) { //std::cout << "Adding feature" << std::endl; feature_list_.push_back(_ft_ptr); - _ft_ptr->setCapturePtr(shared_from_this()); + _ft_ptr->setCapture(shared_from_this()); _ft_ptr->setProblem(getProblem()); return _ft_ptr; } @@ -108,7 +108,7 @@ void CaptureBase::addFeatureList(FeatureBasePtrList& _new_ft_list) { for (FeatureBasePtr feature_ptr : _new_ft_list) { - feature_ptr->setCapturePtr(shared_from_this()); + feature_ptr->setCapture(shared_from_this()); if (getProblem() != nullptr) feature_ptr->setProblem(getProblem()); } @@ -124,11 +124,11 @@ void CaptureBase::getFactorList(FactorBasePtrList& _fac_list) FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr) { constrained_by_list_.push_back(_fac_ptr); - _fac_ptr->setCaptureOtherPtr(shared_from_this()); + _fac_ptr->setCaptureOther(shared_from_this()); return _fac_ptr; } -StateBlockPtr CaptureBase::getStateBlockPtr(unsigned int _i) const +StateBlockPtr CaptureBase::getStateBlock(unsigned int _i) const { if (getSensor()) { @@ -168,7 +168,7 @@ void CaptureBase::removeStateBlocks() { getProblem()->removeStateBlock(sbp); } - setStateBlockPtr(i, nullptr); + setStateBlock(i, nullptr); } } } @@ -177,7 +177,7 @@ void CaptureBase::fix() { for (unsigned int i = 0; i<getStateBlockVec().size(); i++) { - auto sbp = getStateBlockPtr(i); + auto sbp = getStateBlock(i); if (sbp != nullptr) sbp->fix(); } @@ -188,7 +188,7 @@ void CaptureBase::unfix() { for (unsigned int i = 0; i<getStateBlockVec().size(); i++) { - auto sbp = getStateBlockPtr(i); + auto sbp = getStateBlock(i); if (sbp != nullptr) sbp->unfix(); } @@ -199,7 +199,7 @@ void CaptureBase::fixExtrinsics() { for (unsigned int i = 0; i < 2; i++) { - auto sbp = getStateBlockPtr(i); + auto sbp = getStateBlock(i); if (sbp != nullptr) sbp->fix(); } @@ -210,7 +210,7 @@ void CaptureBase::unfixExtrinsics() { for (unsigned int i = 0; i < 2; i++) { - auto sbp = getStateBlockPtr(i); + auto sbp = getStateBlock(i); if (sbp != nullptr) sbp->unfix(); } @@ -221,7 +221,7 @@ void CaptureBase::fixIntrinsics() { for (unsigned int i = 2; i < getStateBlockVec().size(); i++) { - auto sbp = getStateBlockPtr(i); + auto sbp = getStateBlock(i); if (sbp != nullptr) sbp->fix(); } @@ -232,7 +232,7 @@ void CaptureBase::unfixIntrinsics() { for (unsigned int i = 2; i < getStateBlockVec().size(); i++) { - auto sbp = getStateBlockPtr(i); + auto sbp = getStateBlock(i); if (sbp != nullptr) sbp->unfix(); } @@ -254,7 +254,7 @@ SizeEigen CaptureBase::computeCalibSize() const SizeEigen sz = 0; for (unsigned int i = 0; i < state_block_vec_.size(); i++) { - auto sb = getStateBlockPtr(i); + auto sb = getStateBlock(i); if (sb && !sb->isFixed()) sz += sb->getSize(); } @@ -267,7 +267,7 @@ Eigen::VectorXs CaptureBase::getCalibration() const SizeEigen index = 0; for (unsigned int i = 0; i < getStateBlockVec().size(); i++) { - auto sb = getStateBlockPtr(i); + auto sb = getStateBlock(i); if (sb && !sb->isFixed()) { calib.segment(index, sb->getSize()) = sb->getState(); @@ -284,7 +284,7 @@ void CaptureBase::setCalibration(const VectorXs& _calib) SizeEigen index = 0; for (unsigned int i = 0; i < getStateBlockVec().size(); i++) { - auto sb = getStateBlockPtr(i); + auto sb = getStateBlock(i); if (sb && !sb->isFixed()) { sb->setState(_calib.segment(index, sb->getSize())); diff --git a/src/examples/test_factor_imu.cpp b/src/examples/test_factor_imu.cpp index 59d98814f8d1995ad6abf6055a3158caa009a33f..5dde0e16a573a9b8e4e5fb90d6b65e905dcb18e6 100644 --- a/src/examples/test_factor_imu.cpp +++ b/src/examples/test_factor_imu.cpp @@ -54,7 +54,7 @@ int main(int argc, char** argv) // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.) CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity()) ); - imu_ptr->setFramePtr(wolf_problem_ptr_->getTrajectory()->getFrameList().back()); + imu_ptr->setFrame(wolf_problem_ptr_->getTrajectory()->getFrameList().back()); // set variables using namespace std; @@ -87,7 +87,7 @@ int main(int argc, char** argv) delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov(); delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_; std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov); - feat_imu->setCapturePtr(imu_ptr); + feat_imu->setCapture(imu_ptr); //create a factorIMU FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame); @@ -115,7 +115,7 @@ int main(int argc, char** argv) //reset origin of motion to new frame wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame); - imu_ptr->setFramePtr(last_frame); + imu_ptr->setFrame(last_frame); } /// ******************************************************************************************** /// @@ -148,7 +148,7 @@ int main(int argc, char** argv) delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov(); delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_; std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov); - feat_imu->setCapturePtr(imu_ptr); + feat_imu->setCapture(imu_ptr); //create a factorIMU FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame); @@ -176,7 +176,7 @@ int main(int argc, char** argv) //reset origin of motion to new frame wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame); - imu_ptr->setFramePtr(last_frame); + imu_ptr->setFrame(last_frame); } mpu_clock = 0.004046; @@ -206,7 +206,7 @@ int main(int argc, char** argv) delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov(); delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_; std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov); - feat_imu->setCapturePtr(imu_ptr); + feat_imu->setCapture(imu_ptr); //create a factorIMU FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame); diff --git a/src/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp index e9a2af536ec532ed94aa8340d0d4d11810e4089a..7dedb1200e1fcfa14483a848bf5b3032aac3ccd5 100644 --- a/src/examples/test_map_yaml.cpp +++ b/src/examples/test_map_yaml.cpp @@ -32,7 +32,7 @@ void print(MapBase& _map) std::cout << "\nFirst def: " << poly_ptr->isFirstDefined(); std::cout << "\nLast def: " << poly_ptr->isLastDefined(); for (int idx = poly_ptr->getFirstId(); idx <= poly_ptr->getLastId(); idx++) - std::cout << "\n point: " << idx << ": " << poly_ptr->getPointStateBlockPtr(idx)->getState().transpose(); + std::cout << "\n point: " << idx << ": " << poly_ptr->getPointStateBlock(idx)->getState().transpose(); break; } else if (lmk_ptr->getType() == "AHP") diff --git a/src/feature/feature_IMU.cpp b/src/feature/feature_IMU.cpp index 0f811cff599f6bbc62e28dd16b0cc6a82648b02b..b35baf2d0a7dc9191e0a17886aea0a6134b060df 100644 --- a/src/feature/feature_IMU.cpp +++ b/src/feature/feature_IMU.cpp @@ -13,7 +13,7 @@ FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, jacobian_bias_(_dD_db_jacobians) { if (_cap_imu_ptr) - this->setCapturePtr(_cap_imu_ptr); + this->setCapture(_cap_imu_ptr); } FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr): @@ -22,7 +22,7 @@ FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr): gyro_bias_preint_(_cap_imu_ptr->getCalibrationPreint().tail<3>()), jacobian_bias_(_cap_imu_ptr->getJacobianCalib()) { - this->setCapturePtr(_cap_imu_ptr); + this->setCapture(_cap_imu_ptr); } FeatureIMU::~FeatureIMU() diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp index 39061dea53cbe3ee5e63f4ca1b542bd12afed644..b47c956b9f39e574f8a8c019788669bc024efbe7 100644 --- a/src/feature/feature_base.cpp +++ b/src/feature/feature_base.cpp @@ -54,7 +54,7 @@ void FeatureBase::remove() FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr) { factor_list_.push_back(_co_ptr); - _co_ptr->setFeaturePtr(shared_from_this()); + _co_ptr->setFeature(shared_from_this()); _co_ptr->setProblem(getProblem()); // add factor to be added in solver if (getProblem() != nullptr) @@ -75,7 +75,7 @@ FrameBasePtr FeatureBase::getFrame() const FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _fac_ptr) { constrained_by_list_.push_back(_fac_ptr); - _fac_ptr->setFeatureOtherPtr(shared_from_this()); + _fac_ptr->setFeatureOther(shared_from_this()); return _fac_ptr; } diff --git a/src/frame_base.cpp b/src/frame_base.cpp index fcf542af51341dbe6e747ae99f383e76a6304c38..ccd51dc7f71cf6e696ed68c5e77a50f314b5bf45 100644 --- a/src/frame_base.cpp +++ b/src/frame_base.cpp @@ -72,7 +72,7 @@ void FrameBase::remove() removeStateBlocks(); if (getTrajectory()->getLastKeyFrame()->id() == this_F->id()) - getTrajectory()->setLastKeyFramePtr(getTrajectory()->findLastKeyFrame()); + getTrajectory()->setLastKeyFrame(getTrajectory()->findLastKeyFrame()); // std::cout << "Removed F" << id() << std::endl; } @@ -99,14 +99,14 @@ void FrameBase::removeStateBlocks() { for (unsigned int i = 0; i < state_block_vec_.size(); i++) { - StateBlockPtr sbp = getStateBlockPtr(i); + StateBlockPtr sbp = getStateBlock(i); if (sbp != nullptr) { if (getProblem() != nullptr) { getProblem()->removeStateBlock(sbp); } - setStateBlockPtr(i, nullptr); + setStateBlock(i, nullptr); } } } @@ -119,7 +119,7 @@ void FrameBase::setKey() registerNewStateBlocks(); if (getTrajectory()->getLastKeyFrame() == nullptr || getTrajectory()->getLastKeyFrame()->getTimeStamp() < time_stamp_) - getTrajectory()->setLastKeyFramePtr(shared_from_this()); + getTrajectory()->setLastKeyFrame(shared_from_this()); getTrajectory()->sortFrame(shared_from_this()); @@ -279,7 +279,7 @@ FrameBasePtr FrameBase::getNextFrame() const CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr) { capture_list_.push_back(_capt_ptr); - _capt_ptr->setFramePtr(shared_from_this()); + _capt_ptr->setFrame(shared_from_this()); _capt_ptr->setProblem(getProblem()); _capt_ptr->registerNewStateBlocks(); return _capt_ptr; @@ -341,7 +341,7 @@ void FrameBase::getFactorList(FactorBasePtrList& _fac_list) FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr) { constrained_by_list_.push_back(_fac_ptr); - _fac_ptr->setFrameOtherPtr(shared_from_this()); + _fac_ptr->setFrameOther(shared_from_this()); return _fac_ptr; } diff --git a/src/hardware_base.cpp b/src/hardware_base.cpp index 3349005129e275a5dd930cd0927d408149994680..945b412c87cbab6be87a8d7d5536d2850996a14f 100644 --- a/src/hardware_base.cpp +++ b/src/hardware_base.cpp @@ -18,7 +18,7 @@ SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr) { sensor_list_.push_back(_sensor_ptr); _sensor_ptr->setProblem(getProblem()); - _sensor_ptr->setHardwarePtr(shared_from_this()); + _sensor_ptr->setHardware(shared_from_this()); _sensor_ptr->registerNewStateBlocks(); diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index 48f66967cbfc59d2fbe237ca5ae8320070c1789e..d1fb63043ac989955ab8954c9cc9a19754184bec 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -108,14 +108,14 @@ void LandmarkBase::removeStateBlocks() { for (unsigned int i = 0; i < state_block_vec_.size(); i++) { - auto sbp = getStateBlockPtr(i); + auto sbp = getStateBlock(i); if (sbp != nullptr) { if (getProblem() != nullptr) { getProblem()->removeStateBlock(sbp); } - setStateBlockPtr(i, nullptr); + setStateBlock(i, nullptr); } } } @@ -173,7 +173,7 @@ YAML::Node LandmarkBase::saveToYaml() const FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr) { constrained_by_list_.push_back(_fac_ptr); - _fac_ptr->setLandmarkOtherPtr(shared_from_this()); + _fac_ptr->setLandmarkOther(shared_from_this()); return _fac_ptr; } diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp index b31117566ac69542436e471dd1b1639492680a86..d7c5c7d76a3e3b3237805b0b55218f9c258cbeaf 100644 --- a/src/landmark/landmark_polyline_2D.cpp +++ b/src/landmark/landmark_polyline_2D.cpp @@ -26,9 +26,9 @@ LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_pt point_state_ptr_vector_.push_back(std::make_shared<StateBlock>(_points.col(i).head<2>())); if (!first_defined_) - point_state_ptr_vector_.front()->setLocalParametrizationPtr(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[1])); + point_state_ptr_vector_.front()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[1])); if (!last_defined_) - point_state_ptr_vector_.back()->setLocalParametrizationPtr(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[point_state_ptr_vector_.size() - 2])); + point_state_ptr_vector_.back()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[point_state_ptr_vector_.size() - 2])); assert(point_state_ptr_vector_.front()->hasLocalParametrization() ? !first_defined_ : first_defined_); assert(point_state_ptr_vector_.back()->hasLocalParametrization() ? !last_defined_ : last_defined_); @@ -67,7 +67,7 @@ const Eigen::VectorXs LandmarkPolyline2D::getPointVector(int _i) const return point_state_ptr_vector_[_i-first_id_]->getState(); } -StateBlockPtr LandmarkPolyline2D::getPointStateBlockPtr(int _i) +StateBlockPtr LandmarkPolyline2D::getPointStateBlock(int _i) { assert(_i-first_id_ >= 0 && _i-first_id_ <= (int)(point_state_ptr_vector_.size()) && "out of range!"); return point_state_ptr_vector_[_i-first_id_]; @@ -230,7 +230,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id) assert(_remain_id < getLastId() || last_defined_); // take a defined extreme as remaining - StateBlockPtr remove_state = getPointStateBlockPtr(_remove_id); + StateBlockPtr remove_state = getPointStateBlock(_remove_id); std::cout << "state block to remove " << remove_state->getState().transpose() << std::endl; // Change factors from remove_state to remain_state diff --git a/src/map_base.cpp b/src/map_base.cpp index fdfbf11fbb0965d1ebab126539cb4e5c27bb3a2c..d1ad03121691b64052bfab01372010f68de863f5 100644 --- a/src/map_base.cpp +++ b/src/map_base.cpp @@ -30,7 +30,7 @@ MapBase::~MapBase() LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr) { landmark_list_.push_back(_landmark_ptr); - _landmark_ptr->setMapPtr(shared_from_this()); + _landmark_ptr->setMap(shared_from_this()); _landmark_ptr->setProblem(getProblem()); _landmark_ptr->registerNewStateBlocks(); return _landmark_ptr; @@ -41,7 +41,7 @@ void MapBase::addLandmarkList(LandmarkBasePtrList& _landmark_list) LandmarkBasePtrList lmk_list_copy = _landmark_list; //since _landmark_list will be empty after addDownNodeList() for (LandmarkBasePtr landmark_ptr : lmk_list_copy) { - landmark_ptr->setMapPtr(shared_from_this()); + landmark_ptr->setMap(shared_from_this()); landmark_ptr->setProblem(getProblem()); landmark_ptr->registerNewStateBlocks(); } diff --git a/src/problem.cpp b/src/problem.cpp index c410886afb7395cb445c11b22287db4216f2b76a..b9cd0a1e6bd63bfb40a3eb67bda35e1b5c6096b2 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -141,7 +141,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // const std::string& _corresponding_sensor_name, // const std::string& _params_filename) { - SensorBasePtr sen_ptr = getSensorPtr(_corresponding_sensor_name); + SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name); if (sen_ptr == nullptr) throw std::runtime_error("Sensor not found. Cannot bind Processor."); if (_params_filename == "") @@ -154,7 +154,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // } } -SensorBasePtr Problem::getSensorPtr(const std::string& _sensor_name) +SensorBasePtr Problem::getSensor(const std::string& _sensor_name) { auto sen_it = std::find_if(getHardware()->getSensorList().begin(), getHardware()->getSensorList().end(), @@ -746,7 +746,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->getStateBlockPtr(i); + auto sb = S->getStateBlock(i); if (sb) { cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )"; diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 4798ead84ec1c9ebdf1bea82bd2155bf9d17f454..ef5cbb1cd61cfe586cfcda1b573ddfc7056142ae 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -63,7 +63,7 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source, } // Update the existing capture - _capture_source->setOriginFramePtr(_keyframe_target); + _capture_source->setOriginFrame(_keyframe_target); // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer! reintegrateBuffer(_capture_source); diff --git a/src/processor/processor_tracker_landmark_polyline.cpp b/src/processor/processor_tracker_landmark_polyline.cpp index a8037d037e0cad2f3739a4c1766ae86dcdeccc89..c3f91c0278eda1a7a73657f4d18d40710c59f999 100644 --- a/src/processor/processor_tracker_landmark_polyline.cpp +++ b/src/processor/processor_tracker_landmark_polyline.cpp @@ -969,21 +969,21 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBasePtrList& _l // Fix polyline points to its respective relative positions if (configuration) { - polyline_ptr->getPointStateBlockPtr(A_id)->setState(Eigen::Vector2s(object_L[classification], 0)); - polyline_ptr->getPointStateBlockPtr(B_id)->setState(Eigen::Vector2s(0, 0)); - polyline_ptr->getPointStateBlockPtr(C_id)->setState(Eigen::Vector2s(0, object_W[classification])); - polyline_ptr->getPointStateBlockPtr(D_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification])); + polyline_ptr->getPointStateBlock(A_id)->setState(Eigen::Vector2s(object_L[classification], 0)); + polyline_ptr->getPointStateBlock(B_id)->setState(Eigen::Vector2s(0, 0)); + polyline_ptr->getPointStateBlock(C_id)->setState(Eigen::Vector2s(0, object_W[classification])); + polyline_ptr->getPointStateBlock(D_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification])); } else { - polyline_ptr->getPointStateBlockPtr(A_id)->setState(Eigen::Vector2s(0, 0)); - polyline_ptr->getPointStateBlockPtr(B_id)->setState(Eigen::Vector2s(0, object_W[classification])); - polyline_ptr->getPointStateBlockPtr(C_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification])); - polyline_ptr->getPointStateBlockPtr(D_id)->setState(Eigen::Vector2s(object_L[classification], 0)); + polyline_ptr->getPointStateBlock(A_id)->setState(Eigen::Vector2s(0, 0)); + polyline_ptr->getPointStateBlock(B_id)->setState(Eigen::Vector2s(0, object_W[classification])); + polyline_ptr->getPointStateBlock(C_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification])); + polyline_ptr->getPointStateBlock(D_id)->setState(Eigen::Vector2s(object_L[classification], 0)); } for (auto id = polyline_ptr->getFirstId(); id <= polyline_ptr->getLastId(); id++) { - polyline_ptr->getPointStateBlockPtr(id)->fix(); + polyline_ptr->getPointStateBlock(id)->fix(); } } } diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index dd821901cc05ae6cf67b6b35a228d54501717ae3..a30f777fc109da531c0d56237a650ccad0090bf5 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -260,34 +260,34 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) return capture; } -StateBlockPtr SensorBase::getPPtr(const TimeStamp _ts) +StateBlockPtr SensorBase::getP(const TimeStamp _ts) { - return getStateBlockPtr(0, _ts); + return getStateBlock(0, _ts); } -StateBlockPtr SensorBase::getOPtr(const TimeStamp _ts) +StateBlockPtr SensorBase::getO(const TimeStamp _ts) { - return getStateBlockPtr(1, _ts); + return getStateBlock(1, _ts); } -StateBlockPtr SensorBase::getIntrinsicPtr(const TimeStamp _ts) +StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts) { - return getStateBlockPtr(2, _ts); + return getStateBlock(2, _ts); } StateBlockPtr SensorBase::getP() { - return getStateBlockPtr(0); + return getStateBlock(0); } StateBlockPtr SensorBase::getO() { - return getStateBlockPtr(1); + return getStateBlock(1); } StateBlockPtr SensorBase::getIntrinsic() { - return getStateBlockPtr(2); + return getStateBlock(2); } SizeEigen SensorBase::computeCalibSize() const @@ -321,7 +321,7 @@ Eigen::VectorXs SensorBase::getCalibration() const bool SensorBase::process(const CaptureBasePtr capture_ptr) { - capture_ptr->setSensorPtr(shared_from_this()); + capture_ptr->setSensor(shared_from_this()); for (const auto processor : processor_list_) { @@ -334,27 +334,27 @@ bool SensorBase::process(const CaptureBasePtr capture_ptr) ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr) { processor_list_.push_back(_proc_ptr); - _proc_ptr->setSensorPtr(shared_from_this()); + _proc_ptr->setSensor(shared_from_this()); _proc_ptr->setProblem(getProblem()); return _proc_ptr; } -StateBlockPtr SensorBase::getStateBlockPtr(unsigned int _i) +StateBlockPtr SensorBase::getStateBlock(unsigned int _i) { CaptureBasePtr cap; if (isStateBlockDynamic(_i, cap)) - return cap->getStateBlockPtr(_i); + return cap->getStateBlock(_i); return getStateBlockPtrStatic(_i); } -StateBlockPtr SensorBase::getStateBlockPtr(unsigned int _i, const TimeStamp& _ts) +StateBlockPtr SensorBase::getStateBlock(unsigned int _i, const TimeStamp& _ts) { CaptureBasePtr cap; if (isStateBlockDynamic(_i, _ts, cap)) - return cap->getStateBlockPtr(_i); + return cap->getStateBlock(_i); return getStateBlockPtrStatic(_i); } diff --git a/src/state_block.cpp b/src/state_block.cpp index 46411e7c30aefb15f921bb9f9fa9c7a114a6154b..b4427c2aae025a794ae433bcb723011f1110f499 100644 --- a/src/state_block.cpp +++ b/src/state_block.cpp @@ -33,7 +33,7 @@ void StateBlock::setFixed(bool _fixed) // //void StateBlock::removeFromProblem(ProblemPtr _problem_ptr) //{ -// _problem_ptr->removeStateBlockPtr(shared_from_this()); +// _problem_ptr->removeStateBlock(shared_from_this()); //} } diff --git a/src/track_matrix.cpp b/src/track_matrix.cpp index 54c2bd50b3525d9a68ed9db58bcf18a1b7310d02..50ab808912128f28279b9d0ad39b8580f251faca 100644 --- a/src/track_matrix.cpp +++ b/src/track_matrix.cpp @@ -50,7 +50,7 @@ void TrackMatrix::add(size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr _ftr->setTrackId(_track_id); if (_cap != _ftr->getCapture()) - _ftr->setCapturePtr(_cap); + _ftr->setCapture(_cap); tracks_[_track_id].emplace(_cap->getTimeStamp(), _ftr); snapshots_[_cap->id()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present } diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp index cf01f2d788ed6a28390a460ee526e566412c9bb6..5820b99ba5df29f281ee2e4e61c82368a93d3561 100644 --- a/src/trajectory_base.cpp +++ b/src/trajectory_base.cpp @@ -19,7 +19,7 @@ TrajectoryBase::~TrajectoryBase() FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr) { // link up - _frame_ptr->setTrajectoryPtr(shared_from_this()); + _frame_ptr->setTrajectory(shared_from_this()); _frame_ptr->setProblem(getProblem()); if (_frame_ptr->isKey()) diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp index b6f183675277a9638a85189dbafc5940578a1c84..71217c8e6df3dc2cd0a9ce5bc2e73c97ec19c72a 100644 --- a/test/gtest_capture_base.cpp +++ b/test/gtest_capture_base.cpp @@ -107,7 +107,7 @@ TEST(CaptureBase, process) SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2)); CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, nullptr)); ASSERT_DEATH({C->process();},""); // No sensor in the capture should fail - C->setSensorPtr(S); + C->setSensor(S); ASSERT_TRUE(C->process()); // This should not fail (although it does nothing) } diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp index 3f92fc0872e1f1869a998c0acda2fffaad25f3e3..cbe91a6fdf1c276f55bf50e9c32451f0618feb5b 100644 --- a/test/gtest_ceres_manager.cpp +++ b/test/gtest_ceres_manager.cpp @@ -462,7 +462,7 @@ TEST(CeresManager, AddStateBlockLocalParam) // Local param LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>(); - sb_ptr->setLocalParametrizationPtr(local_param_ptr); + sb_ptr->setLocalParametrization(local_param_ptr); // add stateblock P->addStateBlock(sb_ptr); @@ -489,7 +489,7 @@ TEST(CeresManager, RemoveLocalParam) // Local param LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>(); - sb_ptr->setLocalParametrizationPtr(local_param_ptr); + sb_ptr->setLocalParametrization(local_param_ptr); // add stateblock P->addStateBlock(sb_ptr); @@ -530,7 +530,7 @@ TEST(CeresManager, AddLocalParam) // Local param LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>(); - sb_ptr->setLocalParametrizationPtr(local_param_ptr); + sb_ptr->setLocalParametrization(local_param_ptr); // update solver ceres_manager_ptr->update(); @@ -611,7 +611,7 @@ TEST(CeresManager, FactorsUpdateLocalParam) // remove local param LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>(); - F->getO()->setLocalParametrizationPtr(local_param_ptr); + F->getO()->setLocalParametrization(local_param_ptr); // update solver ceres_manager_ptr->update(); diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp index 2e8245e1544c1a51e280a0874a24d400527a1466..82828c6c274a3ed2e72d53f15e8f625583644697 100644 --- a/test/gtest_feature_IMU.cpp +++ b/test/gtest_feature_IMU.cpp @@ -61,7 +61,7 @@ class FeatureIMU_test : public testing::Test // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.) // give the capture a big covariance, otherwise it will be so small that it won't pass following assertions imu_ptr = std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero()); - imu_ptr->setFramePtr(origin_frame); //to get ptr to Frm in processorIMU and then get biases + imu_ptr->setFrame(origin_frame); //to get ptr to Frm in processorIMU and then get biases //process data data_ << 2, 0, 9.8, 0, 0, 0; @@ -90,7 +90,7 @@ class FeatureIMU_test : public testing::Test calib_preint, dD_db_jacobians, imu_ptr); - feat_imu->setCapturePtr(imu_ptr); //associate the feature to a capture + feat_imu->setCapture(imu_ptr); //associate the feature to a capture } diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp index a3e18e59c74962936a906681143f9003fa9d99f0..425f6a96b9a9af4dcdcf5a491a8fb2ac75f98cf3 100644 --- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp +++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp @@ -148,7 +148,7 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated) 1.2, sensor_ptr); // Make 3rd frame last Keyframe - problem->getTrajectory()->setLastKeyFramePtr(F3); + problem->getTrajectory()->setLastKeyFrame(F3); // trigger search for loopclosure processor_ptr_point2d->process(incomming_dummy); @@ -181,7 +181,7 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance) position_covariance_matrix); // Make 3rd frame last Keyframe - problem->getTrajectory()->setLastKeyFramePtr(F3); + problem->getTrajectory()->setLastKeyFrame(F3); // trigger search for loopclosure processor_ptr_ellipse2d->process(incomming_dummy); @@ -193,7 +193,7 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance) ASSERT_EQ(testloops[1]->id(), (unsigned int) 2); // Make 4th frame last Keyframe - problem->getTrajectory()->setLastKeyFramePtr(F4); + problem->getTrajectory()->setLastKeyFrame(F4); // trigger search for loopclosure again processor_ptr_ellipse2d->process(incomming_dummy); diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp index b6b5f2b85717df13c864551845bdf6c1725c0b39..04c7e6d7a06b2d76224c9b4c1df6e5da3a6f7f03 100644 --- a/test/gtest_solver_manager.cpp +++ b/test/gtest_solver_manager.cpp @@ -263,7 +263,7 @@ TEST(SolverManager, AddUpdateLocalParamStateBlock) // Local param LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>(); - sb_ptr->setLocalParametrizationPtr(local_ptr); + sb_ptr->setLocalParametrization(local_ptr); // Fix state block sb_ptr->fix(); @@ -298,7 +298,7 @@ TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock) // Local param LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>(); - sb_ptr->setLocalParametrizationPtr(local_ptr); + sb_ptr->setLocalParametrization(local_ptr); // check flags ASSERT_FALSE(sb_ptr->stateUpdated()); diff --git a/wolf_scripts/substitution.sh b/wolf_scripts/substitution.sh index 3085c1f82dbeb0a8e69be38df3e3115938fa7bd2..04ebdd6be9a348b77d7d94fc75af1725eb336cc1 100755 --- a/wolf_scripts/substitution.sh +++ b/wolf_scripts/substitution.sh @@ -10,7 +10,8 @@ for file in $(ag 'Ptr\(' . -o); do # subs_line=${line}s/${subs}/${subs%List}PtrList/gp # echo $subs_line # sed -n -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/gp' $target - sed -i -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/g' $target + # sed -n -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/gp' $target + sed -i -e $line's/(/(/g' $target done # for file in $(ag -l -g constraint $folder); do