diff --git a/CMakeLists.txt b/CMakeLists.txt index 20472dda112ee4ec7d6883858ddb0d6b981f657d..a5b1e21c2f3510bd97ad176c181cfa3d6d171264 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -581,7 +581,6 @@ IF (laser_scan_utils_FOUND) SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} include/base/processor/processor_tracker_feature_corner.h include/base/processor/processor_tracker_landmark_corner.h - include/base/processor/processor_tracker_landmark_polyline.h ) SET(HDRS_SENSOR ${HDRS_SENSOR} include/base/sensor/sensor_laser_2D.h @@ -590,7 +589,6 @@ IF (laser_scan_utils_FOUND) src/sensor/sensor_laser_2D.cpp src/processor/processor_tracker_feature_corner.cpp src/processor/processor_tracker_landmark_corner.cpp - src/processor/processor_tracker_landmark_polyline.cpp ) ENDIF(laser_scan_utils_FOUND) 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 85e5dbe7f300c705d0b8d7573f7ee36684b6aa15..06a574a67a8ed5dc3712b2c676082058adf9926f 100644 --- a/include/base/problem.h +++ b/include/base/problem.h @@ -95,7 +95,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 994e3dc89b79fd7ac2d602a92ff25713059dc891..e996a382605975ccda456320961ab775e3593583 100644 --- a/include/base/processor/processor_base.h +++ b/include/base/processor/processor_base.h @@ -190,7 +190,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/processor/processor_tracker_landmark_polyline.h b/include/base/processor/processor_tracker_landmark_polyline.h deleted file mode 100644 index a6f902c930d696dc595ae19f133a0df4fd0e00c2..0000000000000000000000000000000000000000 --- a/include/base/processor/processor_tracker_landmark_polyline.h +++ /dev/null @@ -1,248 +0,0 @@ -/* - * processor_tracker_landmark_polyline.h - * - * Created on: May 26, 2016 - * Author: jvallve - */ - -#ifndef SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_ -#define SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_ - -// Wolf includes -#include "base/sensor/sensor_laser_2D.h" -#include "base/capture/capture_laser_2D.h" -#include "base/feature/feature_polyline_2D.h" -#include "base/landmark/landmark_polyline_2D.h" -#include "base/factor/factor_point_2D.h" -#include "base/factor/factor_point_to_line_2D.h" -#include "base/state_block.h" -#include "base/association/association_tree.h" -#include "base/processor/processor_tracker_landmark.h" - -//laser_scan_utils -#include "laser_scan_utils/laser_scan.h" -#include "laser_scan_utils/line_finder_iterative.h" -#include "laser_scan_utils/polyline.h" - -namespace wolf -{ - -//some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level -const Scalar position_error_th_ = 1; -const Scalar min_features_ratio_th_ = 0.5; - -//forward declaration to typedef class pointers -struct LandmarkPolylineMatch; -typedef std::shared_ptr<LandmarkPolylineMatch> LandmarkPolylineMatchPtr; - -//forward declaration to typedef class pointers -struct ProcessorParamsPolyline; -typedef std::shared_ptr<ProcessorParamsPolyline> ProcessorParamsPolylinePtr; - -WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkPolyline); - -// Match Feature - Landmark -struct LandmarkPolylineMatch : public LandmarkMatch -{ - int landmark_match_from_id_; - int feature_match_from_id_; - int landmark_match_to_id_; - int feature_match_to_id_; - -// std::vector<unsigned int> landmark_points_match_; -// std::vector<unsigned int> feature_points_match_; -// std::vector<unsigned int> feature_points_add_front_; -// std::vector<unsigned int> feature_points_add_back_; - - LandmarkPolylineMatch() : - landmark_match_from_id_(0), - feature_match_from_id_(0), - landmark_match_to_id_(0), - feature_match_to_id_(0) - { - // - } - LandmarkPolylineMatch(int _landmark_match_from_id, - int _feature_match_from_id, - int _landmark_match_to_id, - int _feature_match_to_id) : - landmark_match_from_id_(_landmark_match_from_id), - feature_match_from_id_(_feature_match_from_id), - landmark_match_to_id_(_landmark_match_to_id), - feature_match_to_id_(_landmark_match_to_id) - { - // - } -}; - -WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsPolyline); -struct ProcessorParamsPolyline : public ProcessorParamsTrackerLandmark -{ - laserscanutils::LineFinderIterativeParams line_finder_params; - Scalar position_error_th; - unsigned int new_features_th; - unsigned int loop_frames_th; - - // These values below are constant and defined within the class -- provide a setter or accept them at construction time if you need to configure them - // Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees - // Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees; - // Scalar position_error_th_ = 1; - // Scalar min_features_ratio_th_ = 0.5; -}; - -class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark -{ - private: - laserscanutils::LineFinderIterative line_finder_; - ProcessorParamsPolylinePtr params_; - - FeatureBasePtrList polylines_incoming_; - FeatureBasePtrList polylines_last_; - - Eigen::Matrix2s R_sensor_world_, R_world_sensor_; - Eigen::Matrix2s R_robot_sensor_; - Eigen::Matrix2s R_current_prev_; - Eigen::Vector2s t_sensor_world_, t_world_sensor_, t_world_sensor_prev_, t_sensor_world_prev_; - Eigen::Vector2s t_robot_sensor_; - Eigen::Vector2s t_current_prev_; - Eigen::Vector2s t_world_robot_; - bool extrinsics_transformation_computed_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html) - - ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params); - - virtual ~ProcessorTrackerLandmarkPolyline(); - virtual void configure(SensorBasePtr _sensor) { }; - - const FeatureBasePtrList& getLastPolylines() const; - - protected: - - virtual void preProcess(); - void computeTransformations(const TimeStamp& _ts); - virtual void postProcess(); - - void advanceDerived(); - - void resetDerived(); - - /** \brief Find provided landmarks in the incoming capture - * \param _landmarks_in input list of landmarks to be found in incoming - * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in - * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr - */ - virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out, - LandmarkMatchMap& _feature_landmark_correspondences); - - /** \brief Vote for KeyFrame generation - * - * If a KeyFrame criterion is validated, this function returns true, - * meaning that it wants to create a KeyFrame at the \b last Capture. - * - * WARNING! This function only votes! It does not create KeyFrames! - */ - virtual bool voteForKeyFrame(); - - /** \brief Detect new Features - * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_. - * \param _new_features_list The list of detected Features. Defaults to member new_features_list_. - * \return The number of detected Features. - * - * This function detects Features that do not correspond to known Features/Landmarks in the system. - * - * The function sets the member new_features_list_, the list of newly detected features, - * to be used for landmark initialization. - */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out); - - /** \brief Creates a landmark for each of new_features_last_ - **/ - virtual void createNewLandmarks(); - - /** \brief Create one landmark - * - * Implement in derived classes to build the type of landmark you need for this tracker. - */ - virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr); - - /** \brief Establish factors between features in Captures \b last and \b origin - */ - virtual void establishFactors(); - - /** \brief look for known objects in the list of unclassified polylines - */ - void classifyPolilines(LandmarkBasePtrList& _lmk_list); - - /** \brief Create a new factor - * \param _feature_ptr pointer to the Feature to constrain - * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained. - * - * Implement this method in derived classes. - */ - virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr); - - private: - - void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list); - - void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, - Eigen::MatrixXs& expected_feature_cov_); - - Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature, - const Eigen::Matrix2s& _feature_cov, - const Eigen::Vector2s& _expected_feature, - const Eigen::Matrix2s& _expected_feature_cov, - const Eigen::MatrixXs& _mu); - Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B, - bool _A_extreme, bool _B_extreme); - // Factory method - public: - static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr); -}; - -inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params) : - ProcessorTrackerLandmark("TRACKER LANDMARK POLYLINE", _params), - line_finder_(_params->line_finder_params), - params_(_params), - extrinsics_transformation_computed_(false) -{ -} - -inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out) -{ - // already computed since each scan is computed in preprocess() - _features_incoming_out = std::move(polylines_last_); - return _features_incoming_out.size(); -} - -inline void ProcessorTrackerLandmarkPolyline::advanceDerived() -{ - //std::cout << "\tProcessorTrackerLandmarkPolyline::advance:" << std::endl; - //std::cout << "\t\tcorners_last: " << polylines_last_.size() << std::endl; - //std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl; - ProcessorTrackerLandmark::advanceDerived(); - for (auto polyline : polylines_last_) - polyline->remove(); - polylines_last_ = std::move(polylines_incoming_); - //std::cout << "advanced" << std::endl; -} - -inline void ProcessorTrackerLandmarkPolyline::resetDerived() -{ - //std::cout << "\tProcessorTrackerLandmarkPolyline::reset:" << std::endl; - //std::cout << "\t\tcorners_last: " << corners_last_.size() << std::endl; - //std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl; - ProcessorTrackerLandmark::resetDerived(); - polylines_last_ = std::move(polylines_incoming_); -} - -inline const FeatureBasePtrList& ProcessorTrackerLandmarkPolyline::getLastPolylines() const -{ - return polylines_last_; -} - -} // namespace wolf - -#endif /* SRC_PROCESSOR_TRACKER_LASER_H_ */ diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h index d9000252b6f6a8d5ad736ee68205e307bef18a2b..b4f5e233ba5db866a885b84ac35f9ae3002581cd 100644 --- a/include/base/sensor/sensor_base.h +++ b/include/base/sensor/sensor_base.h @@ -95,7 +95,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(); @@ -109,8 +109,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); @@ -119,15 +119,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(); @@ -264,22 +264,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 8bb5bdac7e2164a9ee2e6338fb203383daba3e57..8e682083d4ab8204c38c5d0f874ba5f60c39d78c 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -148,7 +148,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 == "") @@ -166,7 +166,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // const std::string& _corresponding_sensor_name, // const paramsServer& _server) { - 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."); ProcessorBasePtr prc_ptr = NewProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _server, sen_ptr); @@ -184,7 +184,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // return prc_ptr; } -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(), @@ -776,7 +776,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.cpp b/src/processor/processor_tracker.cpp index e502ba296a1a4fee6a355839c9d7a6d52871a54a..afdc9446a7f3dc0cc54c8453bc07c190a821f90e 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -53,7 +53,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) case FIRST_TIME_WITH_PACK : { PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance); - kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() ); + kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() ); WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() ); @@ -124,7 +124,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) case RUNNING_WITH_PACK : { PackKeyFramePtr pack = kf_pack_buffer_.selectPack( last_ptr_ , params_tracker_->time_tolerance); - kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() ); + kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() ); WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() ); @@ -170,6 +170,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) // We create a KF // set KF on last + last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); last_ptr_->getFrame()->setKey(); // make F; append incoming to new F diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp index 670f3aaf776d371bc1db000d84184f5643cc8e2e..0ee10e6f75b5fa6e843421eaba7728edc7b2f4ea 100644 --- a/src/processor/processor_tracker_feature.cpp +++ b/src/processor/processor_tracker_feature.cpp @@ -152,17 +152,6 @@ void ProcessorTrackerFeature::establishFactors() feature_in_last ->addFactor(fac_ptr); feature_in_origin->addConstrainedBy(fac_ptr); - if (fac_ptr != nullptr) // factor links - { - FrameBasePtr frm = fac_ptr->getFrameOther(); - if (frm) - frm->addConstrainedBy(fac_ptr); - CaptureBasePtr cap = fac_ptr->getCaptureOther(); - if (cap) - cap->addConstrainedBy(fac_ptr); - } - - WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(), " origin: " , feature_in_origin->id() , " from last: " , feature_in_last->id() ); diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp index 3d5b75c8fcc6e9ef70ef396c31a6960a5aa3a016..d19610391edb98ee2565c2f477970a11c0088075 100644 --- a/src/processor/processor_tracker_landmark.cpp +++ b/src/processor/processor_tracker_landmark.cpp @@ -137,12 +137,6 @@ void ProcessorTrackerLandmark::establishFactors() { last_feature->addFactor(fac_ptr); lmk->addConstrainedBy(fac_ptr); - FrameBasePtr frm = fac_ptr->getFrameOther(); - if (frm) - frm->addConstrainedBy(fac_ptr); - CaptureBasePtr cap = fac_ptr->getCaptureOther(); - if (cap) - cap->addConstrainedBy(fac_ptr); } } } diff --git a/src/processor/processor_tracker_landmark_polyline.cpp b/src/processor/processor_tracker_landmark_polyline.cpp deleted file mode 100644 index a8037d037e0cad2f3739a4c1766ae86dcdeccc89..0000000000000000000000000000000000000000 --- a/src/processor/processor_tracker_landmark_polyline.cpp +++ /dev/null @@ -1,1020 +0,0 @@ -#include "base/processor/processor_tracker_landmark_polyline.h" - -namespace wolf -{ - -void ProcessorTrackerLandmarkPolyline::preProcess() -{ - //std::cout << "PreProcess: " << std::endl; - - // extract corners of incoming - extractPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_), polylines_incoming_); - - // compute transformations - computeTransformations(incoming_ptr_->getTimeStamp()); - - //std::cout << "PreProcess: incoming new features: " << polylines_incoming_.size() << std::endl; -} - -void ProcessorTrackerLandmarkPolyline::computeTransformations(const TimeStamp& _ts) -{ - Eigen::Vector3s vehicle_pose = getProblem()->getState(_ts); - t_world_robot_ = vehicle_pose.head<2>(); - - // world_robot - Eigen::Matrix2s R_world_robot = Eigen::Rotation2Ds(vehicle_pose(2)).matrix(); - - // robot_sensor (to be computed once if extrinsics are fixed and not dynamic) - if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed() - || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_) - { - t_robot_sensor_ = getSensor()->getP()->getState(); - R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix(); - extrinsics_transformation_computed_ = true; - } - - // global_sensor - R_world_sensor_ = R_world_robot * R_robot_sensor_; - t_world_sensor_ = t_world_robot_ + R_world_robot * t_robot_sensor_; - - // sensor_global - R_sensor_world_ = R_robot_sensor_.transpose() * R_world_robot.transpose(); - t_sensor_world_ = -R_robot_sensor_.transpose() * t_robot_sensor_ -R_sensor_world_ * t_world_robot_ ; - - //std::cout << "t_world_robot_ " << t_world_robot_.transpose() << std::endl; - //std::cout << "t_robot_sensor_ " << t_robot_sensor_.transpose() << std::endl; - //std::cout << "R_robot_sensor_ " << std::endl << R_robot_sensor_ << std::endl; - //std::cout << "t_world_sensor_ " << t_world_sensor_.transpose() << std::endl; - //std::cout << "R_world_sensor_ " << std::endl << R_world_sensor_ << std::endl; - //std::cout << "t_sensor_world_ " << t_sensor_world_.transpose() << std::endl; - //std::cout << "R_sensor_world_ " << std::endl << R_sensor_world_ << std::endl; - -} - -unsigned int ProcessorTrackerLandmarkPolyline::findLandmarks(const LandmarkBasePtrList& _landmarks_searched, - FeatureBasePtrList& _features_found, - LandmarkMatchMap& _feature_landmark_correspondences) -{ - //std::cout << "ProcessorTrackerLandmarkPolyline::findLandmarks: " << _landmarks_searched.size() << " features: " << polylines_incoming_.size() << std::endl; - - // COMPUTING ALL EXPECTED FEATURES - std::map<LandmarkBasePtr, Eigen::MatrixXs> expected_features; - std::map<LandmarkBasePtr, Eigen::MatrixXs> expected_features_covs; - for (auto landmark : _landmarks_searched) - if (landmark->getType() == "POLYLINE 2D") - { - expected_features[landmark] = Eigen::MatrixXs(3, (std::static_pointer_cast<LandmarkPolyline2D>(landmark))->getNPoints()); - expected_features_covs[landmark] = Eigen::MatrixXs(2, 2*(std::static_pointer_cast<LandmarkPolyline2D>(landmark))->getNPoints()); - expectedFeature(landmark, expected_features[landmark], expected_features_covs[landmark]); - } - - // NAIVE NEAREST NEIGHBOR MATCHING - LandmarkPolylineMatchPtr best_match = nullptr; - FeaturePolyline2DPtr polyline_feature; - LandmarkPolyline2DPtr polyline_landmark; - - auto next_feature_it = polylines_incoming_.begin(); - auto feature_it = next_feature_it++; - int max_ftr, max_lmk, max_offset, min_offset, offset, from_ftr, from_lmk, to_ftr, to_lmk, N_overlapped; - - // iterate over all polylines features - while (feature_it != polylines_incoming_.end()) - { - polyline_feature = std::static_pointer_cast<FeaturePolyline2D>(*feature_it); - max_ftr = polyline_feature->getNPoints() - 1; - - // Check with all landmarks - for (auto landmark_it = _landmarks_searched.begin(); landmark_it != _landmarks_searched.end(); landmark_it++) - { - polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(*landmark_it); - - // Open landmark polyline - if (!polyline_landmark->isClosed()) - { - //std::cout << "MATCHING WITH OPEN LANDMARK" << std::endl; - //std::cout << "\tfeature " << polyline_feature->id() << ": 0-" << max_ftr << std::endl; - //std::cout << "\tlandmark " << polyline_landmark->id() << ": 0-" << polyline_landmark->getNPoints() - 1 << std::endl; - max_lmk = polyline_landmark->getNPoints() - 1; - max_offset = max_ftr; - min_offset = -max_lmk; - - // Check all overlapping positions between each feature-landmark pair - for (offset = min_offset; offset <= max_offset; offset++) - { - if (offset == min_offset && !polyline_landmark->isLastDefined() && !polyline_feature->isFirstDefined()) - continue; - - if (offset == max_offset && !polyline_landmark->isFirstDefined() && !polyline_feature->isLastDefined()) - continue; - - from_lmk = std::max(0, -offset); - from_ftr = std::max(0, offset); - N_overlapped = std::min(max_ftr - from_ftr, max_lmk - from_lmk)+1; - to_lmk = from_lmk+N_overlapped-1; - to_ftr = from_ftr+N_overlapped-1; - - //std::cout << "\t\toffset " << offset << std::endl; - //std::cout << "\t\t\tfrom_lmk " << from_lmk << std::endl; - //std::cout << "\t\t\tfrom_ftr " << from_ftr << std::endl; - //std::cout << "\t\t\tN_overlapped " << N_overlapped << std::endl; - - // Compute the squared distance for all overlapped points - Eigen::ArrayXXd d = (polyline_feature->getPoints().block(0,from_ftr, 2,N_overlapped) - - expected_features[*landmark_it].block(0,from_lmk, 2, N_overlapped)).array(); - - Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2); - //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl; - - if (offset != min_offset && offset != max_offset) - { - // Point-to-line first distance - bool from_ftr_not_defined = (from_ftr == 0 && !polyline_feature->isFirstDefined()); - bool from_lmk_not_defined = (from_lmk == 0 && !polyline_landmark->isFirstDefined()); - //std::cout << "\t\tfrom_ftr_not_defined " << from_ftr_not_defined << (from_ftr == 0) << !polyline_feature->isFirstDefined() << std::endl; - //std::cout << "\t\tfrom_lmk_not_defined " << from_lmk_not_defined << (from_lmk == 0) << !polyline_landmark->isFirstDefined() << std::endl; - if (from_ftr_not_defined || from_lmk_not_defined) - { - //std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl; - //std::cout << "\t\t\tA" << expected_features[*landmark_it].col(from_lmk).transpose() << std::endl; - //std::cout << "\t\t\tAaux" << expected_features[*landmark_it].col(from_lmk+1).transpose() << std::endl; - //std::cout << "\t\t\tB" << polyline_feature->getPoints().col(from_ftr).transpose() << std::endl; - dist2(0) = sqDistPointToLine(expected_features[*landmark_it].col(from_lmk), - expected_features[*landmark_it].col(from_lmk+1), - polyline_feature->getPoints().col(from_ftr), - !from_lmk_not_defined, - !from_ftr_not_defined); - } - - // Point-to-line last distance - bool last_ftr_not_defined = !polyline_feature->isLastDefined() && to_ftr == max_ftr; - bool last_lmk_not_defined = !polyline_landmark->isLastDefined() && to_lmk == max_lmk; - //std::cout << "\t\tlast_ftr_not_defined " << last_ftr_not_defined << (to_ftr == max_ftr) << !polyline_feature->isLastDefined() << std::endl; - //std::cout << "\t\tlast_lmk_not_defined " << last_lmk_not_defined << (to_lmk == max_lmk) << !polyline_landmark->isLastDefined() << std::endl; - if (last_ftr_not_defined || last_lmk_not_defined) - { - //std::cout << "\t\t\tLast feature not defined distance to line" << std::endl; - //std::cout << "\t\t\tA" << expected_features[*landmark_it].col(to_lmk).transpose() << std::endl; - //std::cout << "\t\t\tAaux" << expected_features[*landmark_it].col(to_lmk-1).transpose() << std::endl; - //std::cout << "\t\t\tB" << polyline_feature->getPoints().col(to_ftr).transpose() << std::endl; - dist2(N_overlapped-1) = sqDistPointToLine(expected_features[*landmark_it].col(to_lmk), - expected_features[*landmark_it].col(to_lmk-1), - polyline_feature->getPoints().col(to_ftr), - !last_lmk_not_defined, - !last_ftr_not_defined); - } - } - //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl; - - // All squared distances should be witin a threshold - // Choose the most overlapped one - if ((dist2 < params_->position_error_th*params_->position_error_th).all() && (best_match == nullptr || - (N_overlapped >= best_match->feature_match_to_id_-best_match->feature_match_from_id_+1 && - dist2.mean() < best_match->normalized_score_ ))) - { - //std::cout << "BEST MATCH" << std::endl; - best_match = std::make_shared<LandmarkPolylineMatch>(); - best_match->feature_match_from_id_= from_ftr; - best_match->landmark_match_from_id_= from_lmk+polyline_landmark->getFirstId(); - best_match->feature_match_to_id_= from_ftr+N_overlapped-1; - best_match->landmark_match_to_id_= from_lmk+N_overlapped-1+polyline_landmark->getFirstId(); - best_match->landmark_ptr_=polyline_landmark; - best_match->normalized_score_ = dist2.mean(); - } - } - } - // Closed landmark polyline - else - { - if (polyline_feature->getNPoints() > polyline_landmark->getNPoints()) - continue; - - //std::cout << "MATCHING WITH CLOSED LANDMARK" << std::endl; - //std::cout << "\tfeature " << polyline_feature->id() << ": 0-" << max_ftr << std::endl; - //std::cout << "\tlandmark " << polyline_landmark->id() << ": 0-" << polyline_landmark->getNPoints() - 1 << std::endl; - - max_offset = 0; - min_offset = -polyline_landmark->getNPoints() + 1; - - // Check all overlapping positions between each feature-landmark pair - for (offset = min_offset; offset <= max_offset; offset++) - { - from_lmk = -offset; - to_lmk = from_lmk+polyline_feature->getNPoints()-1; - if (to_lmk >= polyline_landmark->getNPoints()) - to_lmk -= polyline_landmark->getNPoints(); - - //std::cout << "\t\toffset " << offset << std::endl; - //std::cout << "\t\t\tfrom_lmk " << from_lmk << std::endl; - //std::cout << "\t\t\tto_lmk " << to_lmk << std::endl; - - // Compute the squared distance for all overlapped points - Eigen::ArrayXXd d = polyline_feature->getPoints().topRows(2).array(); - if (to_lmk > from_lmk) - d -= expected_features[*landmark_it].block(0,from_lmk, 2, polyline_feature->getNPoints()).array(); - else - { - d.leftCols(polyline_landmark->getNPoints()-from_lmk) -= expected_features[*landmark_it].block(0,from_lmk, 2, polyline_landmark->getNPoints()-from_lmk).array(); - d.rightCols(to_lmk+1) -= expected_features[*landmark_it].block(0, 0, 2, to_lmk+1).array(); - } - Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2); - //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl; - - // Point-to-line first distance - if (!polyline_feature->isFirstDefined()) - { - int next_from_lmk = (from_lmk+1 == polyline_landmark->getNPoints() ? 0 : from_lmk+1); - dist2(0) = sqDistPointToLine(expected_features[*landmark_it].col(from_lmk), - expected_features[*landmark_it].col(next_from_lmk), - polyline_feature->getPoints().col(0), - true, - false); - } - - // Point-to-line last distance - if (!polyline_feature->isLastDefined()) - { - int prev_to_lmk = (to_lmk == 0 ? polyline_landmark->getNPoints()-1 : to_lmk-1); - dist2(polyline_feature->getNPoints()-1) = sqDistPointToLine(expected_features[*landmark_it].col(to_lmk), - expected_features[*landmark_it].col(prev_to_lmk), - polyline_feature->getPoints().col(polyline_feature->getNPoints()-1), - true, - false); - } - //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl; - - // All squared distances should be witin a threshold - // Choose the most overlapped one - if ((dist2 < params_->position_error_th*params_->position_error_th).all() && (best_match == nullptr || dist2.mean() < best_match->normalized_score_ )) - { - //std::cout << "BEST MATCH" << std::endl; - best_match = std::make_shared<LandmarkPolylineMatch>(); - best_match->feature_match_from_id_= 0; - best_match->landmark_match_from_id_= from_lmk+polyline_landmark->getFirstId(); - best_match->feature_match_to_id_= polyline_feature->getNPoints()-1; - best_match->landmark_match_to_id_= to_lmk+polyline_landmark->getFirstId(); - best_match->landmark_ptr_=polyline_landmark; - best_match->normalized_score_ = dist2.mean(); - } - } - } - - //std::cout << "landmark " << (*landmark_it)->id() << ": 0-" << max_lmk << " - defined " << polyline_landmark->isFirstDefined() << polyline_landmark->isLastDefined() << std::endl; - //std::cout << "feature " << (*feature_it)->id() << ": 0-" << max_ftr << " - defined " << polyline_feature->isFirstDefined() << polyline_feature->isLastDefined() << std::endl; - //std::cout << expected_features[*landmark_it] << std::endl; - //std::cout << "\tmax_offset " << max_offset << std::endl; - //std::cout << "\tmin_offset " << min_offset << std::endl; - //if (!polyline_landmark->isFirstDefined() && !polyline_feature->isLastDefined()) - // std::cout << "\tLIMITED max offset " << max_offset << std::endl; - //if (!polyline_feature->isFirstDefined() && !polyline_landmark->isLastDefined()) - // std::cout << "\tLIMITED min offset " << min_offset << std::endl; - - } - // Match found for this feature - if (best_match != nullptr) - { - //std::cout << "\tclosest landmark: " << best_match->landmark_ptr_->id() << std::endl; - // match - matches_landmark_from_incoming_[*feature_it] = best_match; - // move corner feature to output list - _features_found.splice(_features_found.end(), polylines_incoming_, feature_it); - // reset match - best_match = nullptr; - } - //else - //{ - // std::cout << "\t-------------------------->NO LANDMARK CLOSE ENOUGH!!!!" << std::endl; - // std::getchar(); - //} - feature_it = next_feature_it++; - } - return matches_landmark_from_incoming_.size(); -} - -bool ProcessorTrackerLandmarkPolyline::voteForKeyFrame() -{ - //std::cout << "------------- ProcessorTrackerLandmarkPolyline::voteForKeyFrame:" << std::endl; - //std::cout << "polylines_last_.size():" << polylines_last_.size()<< std::endl; - // option 1: more than TH new features in last - if (polylines_last_.size() >= params_->new_features_th) - { - std::cout << "------------- NEW KEY FRAME: Option 1 - Enough new features" << std::endl; - //std::cout << "\tnew features in last = " << corners_last_.size() << std::endl; - return true; - } - // option 2: loop closure (if the newest frame from which a matched landmark was observed is old enough) - for (auto new_ftr : new_features_last_) - { - if (last_ptr_->getFrame()->id() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getFrame()->id() > params_->loop_frames_th) - { - std::cout << "------------- NEW KEY FRAME: Option 2 - Loop closure" << std::endl; - return true; - } - } - return false; -} - -void ProcessorTrackerLandmarkPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, - FeatureBasePtrList& _polyline_list) -{ - //std::cout << "ProcessorTrackerLandmarkPolyline::extractPolylines: " << std::endl; - // TODO: sort corners by bearing - std::list<laserscanutils::Polyline> polylines; - - line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines); - - for (auto&& pl : polylines) - { - //std::cout << "new polyline detected: Defined" << pl.first_defined_ << pl.last_defined_ << std::endl; - //std::cout << "covs: " << std::endl << pl.covs_ << std::endl; - _polyline_list.push_back(std::make_shared<FeaturePolyline2D>(pl.points_, pl.covs_, pl.first_defined_, pl.last_defined_)); - //std::cout << "new polyline detected: " << std::endl; - } - //std::cout << _polyline_list.size() << " polylines extracted" << std::endl; -} - -void ProcessorTrackerLandmarkPolyline::expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, - Eigen::MatrixXs& expected_feature_cov_) -{ - assert(_landmark_ptr->getType() == "POLYLINE 2D" && "ProcessorTrackerLandmarkPolyline::expectedFeature: Bad landmark type"); - LandmarkPolyline2DPtr polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(_landmark_ptr); - assert(expected_feature_.cols() == polyline_landmark->getNPoints() && expected_feature_.rows() == 3 && "ProcessorTrackerLandmarkPolyline::expectedFeature: bad expected_feature_ sizes"); - - //std::cout << "ProcessorTrackerLandmarkPolyline::expectedFeature" << std::endl; - //std::cout << "t_world_sensor_: " << t_world_sensor_.transpose() << std::endl; - //std::cout << "R_sensor_world_: " << std::endl << R_sensor_world_ << std::endl; - - expected_feature_ = Eigen::MatrixXs::Zero(3,polyline_landmark->getNPoints()); - expected_feature_cov_ = Eigen::MatrixXs::Zero(2,2*polyline_landmark->getNPoints()); - Eigen::Vector3s col = Eigen::Vector3s::Ones(); - - ////////// global coordinates points - if (polyline_landmark->getClassification() == UNCLASSIFIED) - for (auto i = 0; i < polyline_landmark->getNPoints(); i++) - { - //std::cout << "Point " << i+polyline_landmark->getFirstId() << std::endl; - //std::cout << "First Point " << polyline_landmark->getFirstId() << std::endl; - //std::cout << "Landmark global position: " << polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()).transpose() << std::endl; - - // ------------ expected feature point - col.head<2>() = R_sensor_world_ * (polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()) - t_world_sensor_); - expected_feature_.col(i) = col; - - //std::cout << "Expected point " << i << ": " << expected_feature_.col(i).transpose() << std::endl; - // ------------ expected feature point covariance - // TODO - expected_feature_cov_.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2); - } - - ////////// landmark with origin - else - { - Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(polyline_landmark->getO()->getState()(0)).matrix(); - const Eigen::VectorXs& t_world_points = polyline_landmark->getP()->getState(); - - for (auto i = 0; i < polyline_landmark->getNPoints(); i++) - { - //std::cout << "Point " << i+polyline_landmark->getFirstId() << std::endl; - //std::cout << "First Point " << polyline_landmark->getFirstId() << std::endl; - //std::cout << "Landmark global position: " << polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()).transpose() << std::endl; - - // ------------ expected feature point - col.head<2>() = R_sensor_world_ * (R_world_points * polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()) + t_world_points - t_world_sensor_); - expected_feature_.col(i) = col; - - //std::cout << "Expected point " << i << ": " << expected_feature_.col(i).transpose() << std::endl; - // ------------ expected feature point covariance - // TODO - expected_feature_cov_.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2); - } - } -} - -Eigen::VectorXs ProcessorTrackerLandmarkPolyline::computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature, - const Eigen::Matrix2s& _feature_cov, - const Eigen::Vector2s& _expected_feature, - const Eigen::Matrix2s& _expected_feature_cov, - const Eigen::MatrixXs& _mu) -{ - // ------------------------ d - Eigen::Vector2s d = _feature - _expected_feature; - - // ------------------------ Sigma_d - Eigen::Matrix2s iSigma_d = (_feature_cov + _expected_feature_cov).inverse(); - Eigen::VectorXs squared_mahalanobis_distances(_mu.cols()); - for (unsigned int i = 0; i < _mu.cols(); i++) - { - squared_mahalanobis_distances(i) = (d - _mu.col(i)).transpose() * iSigma_d * (d - _mu.col(i)); - //if ((d - _mu.col(i)).norm() < 1) - //{ - // std::cout << "distance: " << (d - _mu.col(i)).norm() << std::endl; - // std::cout << "iSigma_d: " << std::endl << iSigma_d << std::endl; - // std::cout << "mahalanobis: " << squared_mahalanobis_distances(i) << std::endl; - //} - } - - return squared_mahalanobis_distances; -} - -Scalar ProcessorTrackerLandmarkPolyline::sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, - const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined) -{ - /* Squared distance from B to the line A_aux-A (match evaluated is A-B) - * - * No matter if A_aux is defined or not. - * - * Case 1: B not defined - * - * The projection of B over the line AAaux must be in [A_aux, inf(in A direction)). Otherwise, return squared distance Aaux-B. - * Check: the angle BAauxA is <= 90º: - * (BA)² <= (BAaux)² + (AAaux)² - * - * Case 1.1: A not defined - * - * No more restrictions: return distance line to point. - * - * Case 1.2: A defined - * - * The projection of B over the line AAaux must be in (inf(in Aaux direction), A]. Otherwise, return squared distance A-B. - * Additional check: the angle BAAaux is <= 90º: - * (BAaux)² <= (BA)² + (AAaux)² - * - * Case 2: B is defined and A is not - * - * Returns the distance B-Line if the projection of B to the line is in [A, inf). Otherwise, return squared distance A-B. - * Checks if the angle BAAaux is >= 90º: (BAaux)² >= (BA)² + (AAaux)² - * - * ( Case B and A are defined is not point-to-line, is point to point -> assertion ) - * - */ - - assert((!_A_defined || !_B_defined) && "ProcessorTrackerLandmarkPolyline::sqDistPointToLine: at least one point must not be defined."); - - Scalar AB_sq = (_B-_A).head<2>().squaredNorm(); - Scalar AauxB_sq = (_B-_A_aux).head<2>().squaredNorm(); - Scalar AAaux_sq = (_A_aux-_A).head<2>().squaredNorm(); - - // Case 1 - if (!_B_defined) - { - if (AB_sq <= AauxB_sq + AAaux_sq) - { - // Case 1.1 - if (!_A_defined) - return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line - // Case 1.2 - else if (AauxB_sq <= AB_sq + AAaux_sq) - return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line - } - } - // Case 2 - else if (!_A_defined && _B_defined) - if (AauxB_sq >= AB_sq + AAaux_sq) - return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line - - // Default return A-B squared distance - return (_A.head<2>() - _B.head<2>()).squaredNorm(); -} - -void ProcessorTrackerLandmarkPolyline::createNewLandmarks() -{ - //std::cout << "ProcessorTrackerLandmarkPolyline::createNewLandmarks" << std::endl; - FeaturePolyline2DPtr polyline_ft_ptr; - LandmarkBasePtr new_lmk_ptr; - for (auto new_feature_ptr : new_features_last_) - { - // create new landmark - new_lmk_ptr = createLandmark(new_feature_ptr); - //std::cout << "\tfeature: " << new_feature_ptr->id() << std::endl; - //std::cout << "\tnew_landmark: " << new_lmk_ptr->id() << ": "<< ((LandmarkPolyline2D*)new_lmk_ptr)->getNPoints() << " points" << std::endl; - new_landmarks_.push_back(new_lmk_ptr); - // cast - polyline_ft_ptr = std::static_pointer_cast<FeaturePolyline2D>(new_feature_ptr); - // create new correspondence - LandmarkPolylineMatchPtr match = std::make_shared<LandmarkPolylineMatch>(); - match->feature_match_from_id_= 0; // all points match - match->landmark_match_from_id_ = 0; - match->feature_match_to_id_= polyline_ft_ptr->getNPoints()-1; // all points match - match->landmark_match_to_id_ = polyline_ft_ptr->getNPoints()-1; - match->normalized_score_ = 1.0; // max score - match->landmark_ptr_ = new_lmk_ptr; - matches_landmark_from_last_[new_feature_ptr] = match; - } -} - -LandmarkBasePtr ProcessorTrackerLandmarkPolyline::createLandmark(FeatureBasePtr _feature_ptr) -{ - assert(_feature_ptr->getType() == "POLYLINE 2D"); - //std::cout << "ProcessorTrackerLandmarkPolyline::createLandmark" << std::endl; - //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl; - //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl; - - FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr); - // compute feature global pose - Eigen::MatrixXs points_global = R_world_sensor_ * polyline_ptr->getPoints().topRows<2>() + - t_world_sensor_ * Eigen::VectorXs::Ones(polyline_ptr->getNPoints()).transpose(); - - //std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl; - //std::cout << "Landmark global points: " << std::endl << points_global << std::endl; - //std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl; - - // Create new landmark - return std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true), std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true), points_global, polyline_ptr->isFirstDefined(), polyline_ptr->isLastDefined()); -} - -ProcessorTrackerLandmarkPolyline::~ProcessorTrackerLandmarkPolyline() -{ - while (!polylines_last_.empty()) - { - polylines_last_.front()->remove(); - polylines_last_.pop_front(); // TODO see if this is needed - } - while (!polylines_incoming_.empty()) - { - polylines_incoming_.front()->remove(); - polylines_incoming_.pop_front(); // TODO see if this is needed - } -} - -void ProcessorTrackerLandmarkPolyline::establishFactors() -{ - //TODO: update with new index in landmarks - - //std::cout << "ProcessorTrackerLandmarkPolyline::establishFactors" << std::endl; - LandmarkPolylineMatchPtr polyline_match; - FeaturePolyline2DPtr polyline_feature; - LandmarkPolyline2DPtr polyline_landmark; - - for (auto last_feature : last_ptr_->getFeatureList()) - { - polyline_feature = std::static_pointer_cast<FeaturePolyline2D>(last_feature); - polyline_match = std::static_pointer_cast<LandmarkPolylineMatch>(matches_landmark_from_last_[last_feature]); - polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(polyline_match->landmark_ptr_); - - assert(polyline_landmark != nullptr && polyline_match != nullptr); - - // Modify landmark (only for not closed) - if (!polyline_landmark->isClosed()) - { - //std::cout << std::endl << "MODIFY LANDMARK" << std::endl; - //std::cout << "feature " << polyline_feature->id() << ": " << std::endl; - //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl; - //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl; - //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl; - //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl; - //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl; - //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl; - //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl; - //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl; - //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl; - //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl; - //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl; - - Eigen::MatrixXs points_global = R_world_sensor_ * polyline_feature->getPoints().topRows<2>() + - t_world_sensor_ * Eigen::VectorXs::Ones(polyline_feature->getNPoints()).transpose(); - // GROW/CLOSE LANDMARK - // -----------------Front----------------- - bool check_front_closing = // Sufficient conditions - // condition 1: feature first defined point not matched - (polyline_feature->isFirstDefined() && polyline_match->feature_match_from_id_ > 0) || - // condition 2: feature second point not matched - (polyline_match->feature_match_from_id_ > 1) || - // condition 3: matched front points but feature front point defined and landmark front point not defined - (polyline_match->landmark_match_from_id_ == polyline_landmark->getFirstId() && polyline_feature->isFirstDefined() && !polyline_landmark->isFirstDefined()); - - // Check closing with landmark's last points - if (check_front_closing) - { - //std::cout << "---------------- Trying to close polyline..." << std::endl; - //std::cout << "feature " << polyline_feature->id() << ": " << std::endl; - //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl; - //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl; - //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl; - //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl; - //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl; - //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl; - //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl; - //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl; - //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl; - //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl; - //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl; - - int feat_point_id_matching = polyline_feature->isFirstDefined() ? 0 : 1; - int lmk_last_defined_point = polyline_landmark->getLastId() - (polyline_landmark->isLastDefined() ? 0 : 1); - //std::cout << std::endl << "\tfeat point matching " << feat_point_id_matching << std::endl; - //std::cout << std::endl << "\tlmk last defined point " << lmk_last_defined_point << std::endl; - - for (int id_lmk = lmk_last_defined_point; id_lmk > polyline_match->landmark_match_to_id_; id_lmk--) - { - //std::cout << "\t\tid_lmk " << id_lmk << std::endl; - //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_->position_error_th*params_->position_error_th << std::endl; - - if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_->position_error_th*params_->position_error_th) - { - std::cout << "CLOSING POLYLINE" << std::endl; - - unsigned int N_back_overlapped = polyline_landmark->getLastId() - id_lmk + 1; - int N_feature_new_points = polyline_match->feature_match_from_id_ - feat_point_id_matching - N_back_overlapped; - - // define first point (if not defined and no points have to be merged) - if (!polyline_landmark->isFirstDefined() && N_feature_new_points >= 0) - polyline_landmark->setFirst(points_global.col(polyline_match->feature_match_from_id_), true); - - // add other points (if there are) - if (N_feature_new_points > 0) - polyline_landmark->addPoints(points_global.middleCols(feat_point_id_matching + N_back_overlapped, N_feature_new_points), // points matrix in global coordinates - N_feature_new_points-1, // last index to be added - true, // defined - false); // front (!back) - - // define last point (if not defined and no points have to be merged) - if (!polyline_landmark->isLastDefined() && N_feature_new_points >= 0) - polyline_landmark->setLast(points_global.col(feat_point_id_matching + N_back_overlapped - 1), true); - - // close landmark - polyline_landmark->setClosed(); - - polyline_match->landmark_match_from_id_ = id_lmk - (polyline_feature->isFirstDefined() ? 0 : 1); - polyline_match->feature_match_from_id_ = 0; - break; - } - } - } - // Add new front points (if not matched feature points) - if (polyline_match->feature_match_from_id_ > 0) // && !polyline_landmark->isClosed() - { - assert(!polyline_landmark->isClosed() && "feature not matched points in a closed landmark"); - //std::cout << "Add new front points. Defined: " << polyline_feature->isFirstDefined() << std::endl; - //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl; - //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl; - //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl; - //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl; - - polyline_landmark->addPoints(points_global, // points matrix in global coordinates - polyline_match->feature_match_from_id_-1, // last feature point index to be added - polyline_feature->isFirstDefined(), // is defined - false); // front - - polyline_match->landmark_match_from_id_ = polyline_landmark->getFirstId(); - polyline_match->feature_match_from_id_ = 0; - //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl; - //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl; - //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl; - //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl; - } - // Change first point - else if (polyline_match->landmark_match_from_id_ == polyline_landmark->getFirstId() && !polyline_landmark->isFirstDefined()) - { - //std::cout << "Change first point. Defined: " << polyline_feature->isFirstDefined() << std::endl; - //std::cout << "\tpoint " << polyline_landmark->getFirstId() << ": " << polyline_landmark->getPointVector(polyline_landmark->getFirstId()).transpose() << std::endl; - //std::cout << "\tpoint " << polyline_landmark->getFirstId()+1 << ": " << polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1).transpose() << std::endl; - //std::cout << "\tfeature point: " << points_global.topLeftCorner(2,1).transpose() << std::endl; - - if (// new defined first - polyline_feature->isFirstDefined() || - // new not defined first - (points_global.topLeftCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1)).squaredNorm() > - (points_global.topLeftCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId())).squaredNorm() + - (polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId())).squaredNorm()) - polyline_landmark->setFirst(points_global.leftCols(1), polyline_feature->isFirstDefined()); - - } - // -----------------Back----------------- - bool check_back_closing = // Sufficient conditions - // condition 1: feature last defined point not matched - (polyline_feature->isLastDefined() && polyline_match->feature_match_to_id_ < polyline_feature->getNPoints()-1) || - // condition 2: feature second last point not matched - (polyline_match->feature_match_to_id_ < polyline_feature->getNPoints() - 2) || - // condition 3: matched back points but feature last point defined and landmark last point not defined - (polyline_match->landmark_match_to_id_ == polyline_landmark->getLastId() && polyline_feature->isLastDefined() && !polyline_landmark->isLastDefined()); - - // Necessary condition: still open landmark - check_back_closing = check_back_closing && !polyline_landmark->isClosed(); - - // Check closing with landmark's first points - if (check_back_closing) - { - int feat_point_id_matching = polyline_feature->getNPoints() - (polyline_feature->isLastDefined() ? 1 : 2); - int lmk_first_defined_point = polyline_landmark->getFirstId() + (polyline_landmark->isFirstDefined() ? 0 : 1); - //std::cout << std::endl << "\tfeat point matching " << feat_point_id_matching << std::endl; - //std::cout << std::endl << "\tlmk first defined point " << lmk_first_defined_point << std::endl; - - for (int id_lmk = lmk_first_defined_point; id_lmk < polyline_match->landmark_match_from_id_; id_lmk++) - { - //std::cout << "\t\tid_lmk " << id_lmk << std::endl; - //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_->position_error_th*params_->position_error_th << std::endl; - - if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_->position_error_th*params_->position_error_th) - { - std::cout << "CLOSING POLYLINE" << std::endl; - - unsigned int N_front_overlapped = id_lmk - polyline_landmark->getFirstId() + 1; - int N_feature_new_points = feat_point_id_matching - polyline_match->feature_match_to_id_ - N_front_overlapped; - - // define first point (if not defined and no points have to be merged) - if (!polyline_landmark->isFirstDefined() && N_feature_new_points >= 0) - polyline_landmark->setFirst(points_global.col(feat_point_id_matching - N_front_overlapped + 1), true); - - // add other points (if there are) - if (N_feature_new_points > 0) - polyline_landmark->addPoints(points_global.middleCols(polyline_match->feature_match_to_id_ + 1, N_feature_new_points), // points matrix in global coordinates - N_feature_new_points-1, // last index to be added - true, // defined - false); // front (!back) - - // define last point (if not defined and no points have to be merged) - if (!polyline_landmark->isLastDefined() && N_feature_new_points >= 0) - polyline_landmark->setLast(points_global.col(polyline_match->feature_match_to_id_), true); - - // close landmark - polyline_landmark->setClosed(); - - polyline_match->landmark_match_to_id_ = id_lmk + (polyline_feature->isLastDefined() ? 0 : 1); - polyline_match->feature_match_to_id_ = polyline_feature->getNPoints() - 1; - break; - } - } - } - // Add new back points (if it wasn't closed) - if (polyline_match->feature_match_to_id_ < polyline_feature->getNPoints()-1) - { - assert(!polyline_landmark->isClosed() && "feature not matched points in a closed landmark"); - //std::cout << "Add back points. Defined: " << polyline_feature->isLastDefined() << std::endl; - //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl; - //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl; - //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl; - //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl; - - polyline_landmark->addPoints(points_global, // points matrix in global coordinates - polyline_match->feature_match_to_id_+1, // last feature point index to be added - polyline_feature->isLastDefined(), // is defined - true); // back - - polyline_match->landmark_match_to_id_ = polyline_landmark->getLastId(); - polyline_match->feature_match_to_id_ = polyline_feature->getNPoints()-1; - //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl; - //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl; - //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl; - //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl; - } - // Change last point - else if (polyline_match->landmark_match_to_id_ == polyline_landmark->getLastId() && !polyline_landmark->isLastDefined()) //&& polyline_match->feature_match_to_id_ == polyline_feature->getNPoints()-1 - { - //std::cout << "Change last point. Defined: " << (polyline_feature->isLastDefined() ? 1 : 0) << std::endl; - //std::cout << "\tpoint " << polyline_landmark->getLastId() << ": " << polyline_landmark->getPointVector(polyline_landmark->getLastId()).transpose() << std::endl; - //std::cout << "\tpoint " << polyline_landmark->getLastId()-1 << ": " << polyline_landmark->getPointVector(polyline_landmark->getLastId()-1).transpose() << std::endl; - //std::cout << "\tfeature point: " << points_global.topRightCorner(2,1).transpose() << std::endl; - if (// new defined last - polyline_feature->isLastDefined() || - // new not defined last - (points_global.topRightCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getLastId()-1)).squaredNorm() > - (points_global.topRightCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getLastId())).squaredNorm() + - (polyline_landmark->getPointVector(polyline_landmark->getLastId()-1)-polyline_landmark->getPointVector(polyline_landmark->getLastId())).squaredNorm()) - polyline_landmark->setLast(points_global.rightCols(1), polyline_feature->isLastDefined()); - } - - //std::cout << "MODIFIED LANDMARK" << std::endl; - //std::cout << "feature " << polyline_feature->id() << ": " << std::endl; - //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl; - //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl; - //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl; - //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl; - //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl; - //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl; - //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl; - } - - // ESTABLISH CONSTRAINTS - //std::cout << "ESTABLISH CONSTRAINTS" << std::endl; - //std::cout << "\tfeature " << polyline_feature->id() << std::endl; - //std::cout << "\tlandmark " << polyline_landmark->id() << std::endl; - //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl; - //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl; - //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl; - //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl; - - // Factors for all inner and defined feature points - int lmk_point_id = polyline_match->landmark_match_from_id_; - - for (int ftr_point_id = 0; ftr_point_id < polyline_feature->getNPoints(); ftr_point_id++, lmk_point_id++) - { - if (lmk_point_id > polyline_landmark->getLastId()) - lmk_point_id -= polyline_landmark->getNPoints(); - if (lmk_point_id < polyline_landmark->getFirstId()) - lmk_point_id += polyline_landmark->getNPoints(); - - //std::cout << "feature point " << ftr_point_id << std::endl; - //std::cout << "landmark point " << lmk_point_id << std::endl; - - // First not defined point - if (ftr_point_id == 0 && !polyline_feature->isFirstDefined()) - // first point to line factor - { - int lmk_next_point_id = lmk_point_id+1; - if (lmk_next_point_id > polyline_landmark->getLastId()) - lmk_next_point_id -= polyline_landmark->getNPoints(); - //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_next_point_id << std::endl; - last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(), - ftr_point_id, lmk_point_id, lmk_next_point_id)); - //std::cout << "factor added" << std::endl; - } - - // Last not defined point - else if (ftr_point_id == polyline_feature->getNPoints()-1 && !polyline_feature->isLastDefined()) - // last point to line factor - { - int lmk_prev_point_id = lmk_point_id-1; - if (lmk_prev_point_id < polyline_landmark->getFirstId()) - lmk_prev_point_id += polyline_landmark->getNPoints(); - //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl; - last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(), - ftr_point_id, lmk_point_id, lmk_prev_point_id)); - //std::cout << "factor added" << std::endl; - } - - // Defined point - else - // point to point factor - { - //std::cout << "point-point: landmark point " << lmk_point_id << std::endl; - //std::cout << "landmark first id:" << polyline_landmark->getFirstId() << std::endl; - //std::cout << "landmark last id:" << polyline_landmark->getLastId() << std::endl; - //std::cout << "landmark n points:" << polyline_landmark->getNPoints() << std::endl; - last_feature->addFactor(std::make_shared<FactorPoint2D>(polyline_feature, polyline_landmark, shared_from_this(), - ftr_point_id, lmk_point_id)); - //std::cout << "factor added" << std::endl; - } - } - //std::cout << "Factors established" << std::endl; - } -} - -void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBasePtrList& _lmk_list) -{ - //std::cout << "ProcessorTrackerLandmarkPolyline::classifyPolilines: " << _lmk_list->size() << std::endl; - std::vector<Scalar> object_L({12, 5.9, 1.2}); - std::vector<Scalar> object_W({2.345, 2.345, 0.9}); - std::vector<Scalar> object_D({sqrt(12*12+2.345*2.345), sqrt(5.9*5.9+2.345*2.345), sqrt(0.9*0.9+1.2*1.2)}); - std::vector<LandmarkClassification> object_class({CONTAINER, SMALL_CONTAINER, PALLET}); - - for (auto lmk_ptr : _lmk_list) - if (lmk_ptr->getType() == "POLYLINE 2D") - { - LandmarkPolyline2DPtr polyline_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_ptr); - auto n_defined_points = polyline_ptr->getNPoints() - (polyline_ptr->isFirstDefined() ? 0 : 1) - (polyline_ptr->isLastDefined() ? 0 : 1); - auto A_id = polyline_ptr->getFirstId() + (polyline_ptr->isFirstDefined() ? 0 : 1); - auto B_id = A_id + 1; - auto C_id = B_id + 1; - auto D_id = C_id + 1; - - // necessary conditions - if (polyline_ptr->getClassification() != UNCLASSIFIED || - n_defined_points < 3 || - n_defined_points > 4 ) - continue; - - //std::cout << "landmark " << lmk_ptr->id() << std::endl; - - // consider 3 first defined points - Scalar dAB = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(B_id)).norm(); - Scalar dBC = (polyline_ptr->getPointVector(B_id) - polyline_ptr->getPointVector(C_id)).norm(); - Scalar dAC = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(C_id)).norm(); - - //std::cout << "dAB = " << dAB << " error 1: " << fabs(dAB-CONT_L) << " error 2: " << fabs(dAB-CONT_W) << std::endl; - //std::cout << "dBC = " << dBC << " error 1: " << fabs(dBC-CONT_W) << " error 2: " << fabs(dBC-CONT_L) << std::endl; - //std::cout << "dAC = " << dAC << " error 1&2: " << fabs(dAC-CONT_D) << std::endl; - - auto classification = -1; - bool configuration; - - for (unsigned int i = 0; i < object_L.size(); i++) - { - // check configuration 1 - if(fabs(dAB-object_L[i]) < params_->position_error_th && - fabs(dBC-object_W[i]) < params_->position_error_th && - fabs(dAC-object_D[i]) < params_->position_error_th) - { - configuration = true; - classification = i; - break; - } - - // check configuration 2 - if(fabs(dAB-object_W[i]) < params_->position_error_th && - fabs(dBC-object_L[i]) < params_->position_error_th && - fabs(dAC-object_D[i]) < params_->position_error_th) - { - configuration = false; - classification = i; - break; - } - } - - // any container position fitted - if (classification != -1) - { - // if 4 defined checking - if (n_defined_points == 4) - { - //std::cout << "checking with 4th point... " << std::endl; - - Scalar dAD = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(D_id)).norm(); - Scalar dBD = (polyline_ptr->getPointVector(B_id) - polyline_ptr->getPointVector(D_id)).norm(); - Scalar dCD = (polyline_ptr->getPointVector(C_id) - polyline_ptr->getPointVector(D_id)).norm(); - - // necessary conditions - if (fabs(dAD-dBC) > params_->position_error_th || - fabs(dBD-dAC) > params_->position_error_th || - fabs(dCD-dAB) > params_->position_error_th) - continue; - } - - // if not 4 defined add/define points - else - { - //std::cout << "adding/defining points... " << std::endl; - if (!polyline_ptr->isFirstDefined()) - { - polyline_ptr->defineExtreme(false); - D_id = polyline_ptr->getFirstId(); - } - else if (!polyline_ptr->isLastDefined()) - polyline_ptr->defineExtreme(true); - else - polyline_ptr->addPoint(Eigen::Vector2s::Zero(), true, true); - } - //std::cout << "landmark " << lmk_ptr->id() << " classified as " << object_class[classification] << " in configuration " << configuration << std::endl; - - // Close - polyline_ptr->setClosed(); - - // Classify - polyline_ptr->classify(object_class[classification]); - - // Unfix origin - polyline_ptr->getP()->unfix(); - polyline_ptr->getO()->unfix(); - - // Move origin to B - polyline_ptr->getP()->setState(polyline_ptr->getPointVector((configuration ? B_id : A_id))); - Eigen::Vector2s frame_x = (configuration ? polyline_ptr->getPointVector(A_id)-polyline_ptr->getPointVector(B_id) : polyline_ptr->getPointVector(C_id)-polyline_ptr->getPointVector(B_id)); - polyline_ptr->getO()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0)))); - - //std::cout << "A: " << polyline_ptr->getPointVector(A_id).transpose() << std::endl; - //std::cout << "B: " << polyline_ptr->getPointVector(B_id).transpose() << std::endl; - //std::cout << "C: " << polyline_ptr->getPointVector(C_id).transpose() << std::endl; - //std::cout << "frame_x: " << frame_x.transpose() << std::endl; - //std::cout << "frame position: " << polyline_ptr->getP()->getVector().transpose() << std::endl; - //std::cout << "frame orientation: " << polyline_ptr->getO()->getVector() << std::endl; - - // 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])); - } - 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)); - } - for (auto id = polyline_ptr->getFirstId(); id <= polyline_ptr->getLastId(); id++) - { - polyline_ptr->getPointStateBlockPtr(id)->fix(); - } - } - } -} - -void ProcessorTrackerLandmarkPolyline::postProcess() -{ - //std::cout << "postProcess: " << std::endl; - //std::cout << "New Last features: " << getNewFeaturesListLast().size() << std::endl; - //std::cout << "Last features: " << last_ptr_->getFeatureList().size() << std::endl; - classifyPolilines(getProblem()->getMap()->getLandmarkList()); -} - -FactorBasePtr ProcessorTrackerLandmarkPolyline::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) -{ - // not used - return nullptr; -} - -ProcessorBasePtr ProcessorTrackerLandmarkPolyline::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr) -{ - ProcessorParamsPolylinePtr params = std::static_pointer_cast<ProcessorParamsPolyline>(_params); - ProcessorTrackerLandmarkPolylinePtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkPolyline>(params); - prc_ptr->setName(_unique_name); - return prc_ptr; -} - -} //namespace wolf - -// Register in the SensorFactory -#include "base/processor/processor_factory.h" -namespace wolf { -WOLF_REGISTER_PROCESSOR("POLYLINE", ProcessorTrackerLandmarkPolyline) -} // namespace wolf 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/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index 17f6e562bb242ba315369effd722391f52f7cf4c..493bc9dcd60683a2836811175a6c57c13c63ff98 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -79,6 +79,9 @@ void SolverManager::update() // UPDATE STATE BLOCKS (state, fix or local parameterization) for (auto state_ptr : wolf_problem_->getStateBlockPtrList()) { + if (state_blocks_.find(state_ptr)==state_blocks_.end()) + continue; + assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !"); // state update @@ -106,8 +109,8 @@ void SolverManager::update() } } - assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update"); - assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update"); + //assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update"); + //assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update"); } wolf::ProblemPtr SolverManager::getProblem() 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