diff --git a/include/base/capture/capture_gnss_single_diff.h b/include/base/capture/capture_gnss_single_diff.h index 0c8801a4328a94aabd65771618231df76b842436..0eba8c0c69c697df9356698ebe90c0d08d8fcc83 100644 --- a/include/base/capture/capture_gnss_single_diff.h +++ b/include/base/capture/capture_gnss_single_diff.h @@ -35,7 +35,7 @@ class CaptureGnssSingleDiff : public CaptureBase const Eigen::Vector3s& getData() const; const Eigen::Matrix3s& getDataCovariance() const; void getDataAndCovariance(Eigen::Vector3s& data, Eigen::Matrix3s& data_cov) const; - FrameBasePtr getOriginFramePtr() const; + FrameBasePtr getOriginFrame() const; }; inline const Eigen::Vector3s& CaptureGnssSingleDiff::getData() const @@ -54,7 +54,7 @@ inline void CaptureGnssSingleDiff::getDataAndCovariance(Eigen::Vector3s& data, E data_cov = data_covariance_; } -inline FrameBasePtr CaptureGnssSingleDiff::getOriginFramePtr() const +inline FrameBasePtr CaptureGnssSingleDiff::getOriginFrame() const { return origin_frame_ptr_; } diff --git a/include/base/factor/factor_autodiff_distance_3D.h b/include/base/factor/factor_autodiff_distance_3D.h index 77eb08e2ae049978c7f2978eee3106fcec66ba9c..a2d74f77361d5de2a5e6fc495ab128981b5824e8 100644 --- a/include/base/factor/factor_autodiff_distance_3D.h +++ b/include/base/factor/factor_autodiff_distance_3D.h @@ -21,8 +21,8 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D, FactorAutodiffDistance3D(const FeatureBasePtr& _feat, const FrameBasePtr& _frm_other, const ProcessorBasePtr& _processor_ptr, - bool _apply_loss_function, - FactorStatus _status) : + bool _apply_loss_function=false, + FactorStatus _status=FAC_ACTIVE) : FactorAutodiff("DISTANCE 3D", _frm_other, nullptr, diff --git a/include/base/factor/factor_diff_drive.h b/include/base/factor/factor_diff_drive.h index 09af9aaeb1672d6508aca9d0ab1e2471a0c1c016..89078b139473251b40ec0587d7e3256cbdc85bfd 100644 --- a/include/base/factor/factor_diff_drive.h +++ b/include/base/factor/factor_diff_drive.h @@ -38,7 +38,9 @@ class FactorDiffDrive : INTRINSICS_STATE_BLOCK_SIZE > { using Base = FactorAutodiff<FactorDiffDrive, - RESIDUAL_SIZE, POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE, + RESIDUAL_SIZE, + POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE, + POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE, INTRINSICS_STATE_BLOCK_SIZE>; public: @@ -56,12 +58,12 @@ public: _processor_ptr, _apply_loss_function, _status, - _frame_other_ptr->getPPtr(), - _frame_other_ptr->getOPtr(), - _ftr_ptr->getFramePtr()->getPPtr(), - _ftr_ptr->getFramePtr()->getOPtr(), - _ftr_ptr->getCapturePtr()->getSensorIntrinsicPtr()), - capture_wheel_joint_ptr_(std::static_pointer_cast<CaptureWheelJointPosition>(_ftr_ptr->getCapturePtr())) + _frame_other_ptr->getP(), + _frame_other_ptr->getO(), + _ftr_ptr->getFrame()->getP(), + _ftr_ptr->getFrame()->getO(), + _ftr_ptr->getCapture()->getSensorIntrinsic()), + capture_wheel_joint_ptr_(std::static_pointer_cast<CaptureWheelJointPosition>(_ftr_ptr->getCapture())) { // } diff --git a/include/base/factor/factor_gnss_fix_2D.h b/include/base/factor/factor_gnss_fix_2D.h index ebfe02028a09a2b7150cb297f991fed7385ab50c..70434ec608e060c8ac45c5010ea4a31c6a78bfd0 100644 --- a/include/base/factor/factor_gnss_fix_2D.h +++ b/include/base/factor/factor_gnss_fix_2D.h @@ -19,7 +19,7 @@ class FactorGnssFix2D : public FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1, public: - FactorGnssFix2D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) : + FactorGnssFix2D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1, 1, 1>("GNSS FIX 2D", nullptr, nullptr, @@ -28,13 +28,13 @@ class FactorGnssFix2D : public FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1, _processor_ptr, _apply_loss_function, _status, - _ftr_ptr->getFramePtr()->getPPtr(), - _ftr_ptr->getFramePtr()->getOPtr(), - _sensor_gnss_ptr->getStateBlockPtr(0), - _sensor_gnss_ptr->getEnuMapTranslationStatePtr(), - _sensor_gnss_ptr->getEnuMapRollStatePtr(), - _sensor_gnss_ptr->getEnuMapPitchStatePtr(), - _sensor_gnss_ptr->getEnuMapYawStatePtr()), + _ftr_ptr->getFrame()->getP(), + _ftr_ptr->getFrame()->getO(), + _sensor_gnss_ptr->getStateBlock(0), + _sensor_gnss_ptr->getEnuMapTranslationState(), + _sensor_gnss_ptr->getEnuMapRollState(), + _sensor_gnss_ptr->getEnuMapPitchState(), + _sensor_gnss_ptr->getEnuMapYawState()), sensor_gnss_ptr_(_sensor_gnss_ptr) { WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an active GNSS Fix 2D factor without initializing ENU"); diff --git a/include/base/factor/factor_gnss_fix_3D.h b/include/base/factor/factor_gnss_fix_3D.h index e00a69108dd76f917a3162db285441df12a3eb9c..343958d355d92d2f822708d6c36a13583ed4a90e 100644 --- a/include/base/factor/factor_gnss_fix_3D.h +++ b/include/base/factor/factor_gnss_fix_3D.h @@ -27,13 +27,13 @@ class FactorGnssFix3D : public FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1, _processor_ptr, _apply_loss_function, _status, - _ftr_ptr->getFramePtr()->getPPtr(), - _ftr_ptr->getFramePtr()->getOPtr(), - _sensor_gnss_ptr->getStateBlockPtr(0), - _sensor_gnss_ptr->getEnuMapTranslationStatePtr(), - _sensor_gnss_ptr->getEnuMapRollStatePtr(), - _sensor_gnss_ptr->getEnuMapPitchStatePtr(), - _sensor_gnss_ptr->getEnuMapYawStatePtr()), + _ftr_ptr->getFrame()->getP(), + _ftr_ptr->getFrame()->getO(), + _sensor_gnss_ptr->getStateBlock(0), + _sensor_gnss_ptr->getEnuMapTranslationState(), + _sensor_gnss_ptr->getEnuMapRollState(), + _sensor_gnss_ptr->getEnuMapPitchState(), + _sensor_gnss_ptr->getEnuMapYawState()), sensor_gnss_ptr_(_sensor_gnss_ptr) { WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3D factor without initializing ENU"); diff --git a/include/base/factor/factor_gnss_single_diff_2D.h b/include/base/factor/factor_gnss_single_diff_2D.h index c5f9b6031f0310520aed9660c4bcc144b5eb1306..9766685930d7a9e758a6823e02e8a021b660f60d 100644 --- a/include/base/factor/factor_gnss_single_diff_2D.h +++ b/include/base/factor/factor_gnss_single_diff_2D.h @@ -28,14 +28,14 @@ class FactorGnssSingleDiff2D : public FactorAutodiff<FactorGnssSingleDiff2D, 3, _processor_ptr, _apply_loss_function, _status, - _frame_other_ptr->getPPtr(), - _frame_other_ptr->getOPtr(), - _ftr_ptr->getFramePtr()->getPPtr(), - _ftr_ptr->getFramePtr()->getOPtr(), - _sensor_gnss_ptr->getStateBlockPtr(0), - _sensor_gnss_ptr->getEnuMapRollStatePtr(), - _sensor_gnss_ptr->getEnuMapPitchStatePtr(), - _sensor_gnss_ptr->getEnuMapYawStatePtr()), + _frame_other_ptr->getP(), + _frame_other_ptr->getO(), + _ftr_ptr->getFrame()->getP(), + _ftr_ptr->getFrame()->getO(), + _sensor_gnss_ptr->getStateBlock(0), + _sensor_gnss_ptr->getEnuMapRollState(), + _sensor_gnss_ptr->getEnuMapPitchState(), + _sensor_gnss_ptr->getEnuMapYawState()), sensor_gnss_ptr_(_sensor_gnss_ptr) { WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 2D factor without initializing ENU"); diff --git a/include/base/factor/factor_odom_2D.h b/include/base/factor/factor_odom_2D.h index e45570d79a8debe284310998cc87f48032521a10..95cf7559282c9bd356c53c92cab6f1234172e993 100644 --- a/include/base/factor/factor_odom_2D.h +++ b/include/base/factor/factor_odom_2D.h @@ -16,17 +16,17 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1> { public: FactorOdom2D(const FeatureBasePtr& _ftr_ptr, - const FrameBasePtr& _frame_other_ptr, - const ProcessorBasePtr& _processor_ptr = nullptr, - bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : + const FrameBasePtr& _frame_other_ptr, + const ProcessorBasePtr& _processor_ptr = nullptr, + bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>("ODOM 2D", - _frame_other_ptr, nullptr, nullptr, nullptr, - _processor_ptr, - _apply_loss_function, _status, - _frame_other_ptr->getP(), - _frame_other_ptr->getO(), - _ftr_ptr->getFrame()->getP(), - _ftr_ptr->getFrame()->getO()) + _frame_other_ptr, nullptr, nullptr, nullptr, + _processor_ptr, + _apply_loss_function, _status, + _frame_other_ptr->getP(), + _frame_other_ptr->getO(), + _ftr_ptr->getFrame()->getP(), + _ftr_ptr->getFrame()->getO()) { // } diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h index df83c52cfbb49df5374df9eaad57264cebdb15c8..0e7b541a40e0504de4b47c58a9cb0f9e7d4cb5a4 100644 --- a/include/base/landmark/landmark_polyline_2D.h +++ b/include/base/landmark/landmark_polyline_2D.h @@ -212,7 +212,7 @@ class LandmarkPolyline2D : public LandmarkBase YAML::Node saveToYaml() const; - static void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _sq_dist_tol); + static void tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list, const Scalar& _sq_dist_tol); }; inline StateBlockPtr& LandmarkPolyline2D::firstStateBlock() diff --git a/include/base/processor/processor_gnss_fix.h b/include/base/processor/processor_gnss_fix.h index 8fd8d13469a7ac27b21f711557d9c4e8a0b612ae..396b07b1b211076dadc78c83ba306e51a91c2adb 100644 --- a/include/base/processor/processor_gnss_fix.h +++ b/include/base/processor/processor_gnss_fix.h @@ -36,8 +36,8 @@ class ProcessorGnssFix : public ProcessorBase virtual bool voteForKeyFrame(); private: - void emplaceConstraint(FeatureBasePtr& ftr_ptr); - bool rejectOutlier(ConstraintBasePtr ctr_ptr); + void emplaceFactor(FeatureBasePtr& ftr_ptr); + bool rejectOutlier(FactorBasePtr ctr_ptr); public: static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr); diff --git a/include/base/processor/processor_polyline.h b/include/base/processor/processor_polyline.h index a9f05d94c223e84c8e5784c8a08b9b93fc9e5e08..959c9d59dbc6fa70653b49b70b4d84509cd05c41 100644 --- a/include/base/processor/processor_polyline.h +++ b/include/base/processor/processor_polyline.h @@ -14,8 +14,8 @@ #include "base/feature/feature_polyline_2D.h" #include "base/landmark/landmark_match.h" #include "base/landmark/landmark_polyline_2D.h" -#include "base/constraint/constraint_point_2D.h" -#include "base/constraint/constraint_point_to_line_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/processor/processor_base.h" @@ -51,12 +51,12 @@ class ProcessorPolyline : public ProcessorBase ProcessorParamsPolylinePtr params_; - FeatureBaseList new_features_incoming_, new_features_last_, known_features_incoming_, known_features_last_; - LandmarkBaseList new_landmarks_; ///< List of new detected landmarks + FeatureBasePtrList new_features_incoming_, new_features_last_, known_features_incoming_, known_features_last_; + LandmarkBasePtrList new_landmarks_; ///< List of new detected landmarks LandmarkMatchMap matches_landmark_from_incoming_; LandmarkMatchMap matches_landmark_from_last_; - std::list<LandmarkPolyline2DList> merge_candidates_list_; + std::list<LandmarkPolyline2DPtrList> merge_candidates_list_; Eigen::Matrix2s R_sensor_world_, R_world_sensor_; Eigen::Matrix2s R_robot_sensor_; @@ -85,32 +85,32 @@ class ProcessorPolyline : public ProcessorBase virtual bool voteForKeyFrame(); // Implementation - void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBaseList& _polyline_list); - unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out, LandmarkMatchMap& _feature_landmark_correspondences); + void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list); + unsigned int findLandmarks(const LandmarkBasePtrList& _landmark_list_in, FeatureBasePtrList& _feature_list_out, LandmarkMatchMap& _feature_landmark_correspondences); virtual void createNewLandmarks(); virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr); - virtual void establishConstraints(); - void emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature, + virtual void establishFactors(); + void emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature, LandmarkPolyline2DPtr _polyline_landmark, const int& _ftr_point_id, const int& _lmk_point_id, const int& _lmk_prev_point_id); - void emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature, + void emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature, LandmarkPolyline2DPtr _polyline_landmark, const int& _ftr_point_id, const int& _lmk_point_id); - bool rejectOutlier(ConstraintBasePtr& ctr_ptr); - void classifyPolylineLandmarks(LandmarkBaseList& _lmk_list); + bool rejectOutlier(FactorBasePtr& fac_ptr); + void classifyPolylineLandmarks(LandmarkBasePtrList& _lmk_list); LandmarkMatchPolyline2DPtr tryMatch(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr); LandmarkMatchPolyline2DPtr tryMatch(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const; // Gets/Sets - const FeatureBaseList& getLastNewPolylines() const; - const FeatureBaseList& getLastKnownPolylines() const; + const FeatureBasePtrList& getLastNewPolylines() const; + const FeatureBasePtrList& getLastKnownPolylines() const; virtual void setKeyFrame(CaptureBasePtr _capture_ptr); - virtual CaptureBasePtr getLastPtr(); + virtual CaptureBasePtr getLast(); // Maths void computeTransformations(const TimeStamp& _ts); @@ -122,49 +122,49 @@ class ProcessorPolyline : public ProcessorBase static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr); }; -inline void ProcessorPolyline::emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature, - LandmarkPolyline2DPtr _polyline_landmark, - const int& _ftr_point_id, - const int& _lmk_point_id, - const int& _lmk_prev_point_id) +inline void ProcessorPolyline::emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature, + LandmarkPolyline2DPtr _polyline_landmark, + const int& _ftr_point_id, + const int& _lmk_point_id, + const int& _lmk_prev_point_id) { assert(_polyline_landmark->isValidId(_lmk_point_id) && _polyline_landmark->isValidId(_lmk_prev_point_id)); // CREATE CONSTRAINT -------------------- - ConstraintBasePtr new_ctr = std::make_shared<ConstraintPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id); + FactorBasePtr new_fac = std::make_shared<FactorPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id); // ADD CONSTRAINT -------------------- - _polyline_feature->addConstraint(new_ctr); - _polyline_landmark->addConstrainedBy(new_ctr); + _polyline_feature->addFactor(new_fac); + _polyline_landmark->addConstrainedBy(new_fac); } -inline void ProcessorPolyline::emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature, +inline void ProcessorPolyline::emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature, LandmarkPolyline2DPtr _polyline_landmark, const int& _ftr_point_id, const int& _lmk_point_id) { // CREATE CONSTRAINT -------------------- - ConstraintBasePtr new_ctr = std::make_shared<ConstraintPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id); + FactorBasePtr new_fac = std::make_shared<FactorPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id); // ADD CONSTRAINT -------------------- - _polyline_feature->addConstraint(new_ctr); - _polyline_landmark->addConstrainedBy(new_ctr); + _polyline_feature->addFactor(new_fac); + _polyline_landmark->addConstrainedBy(new_fac); } -inline const FeatureBaseList& ProcessorPolyline::getLastNewPolylines() const +inline const FeatureBasePtrList& ProcessorPolyline::getLastNewPolylines() const { return new_features_last_; } -inline const FeatureBaseList& ProcessorPolyline::getLastKnownPolylines() const +inline const FeatureBasePtrList& ProcessorPolyline::getLastKnownPolylines() const { - if (last_ptr_->getFramePtr()->isKey()) + if (last_ptr_->getFrame()->isKey()) return last_ptr_->getFeatureList(); else return known_features_last_; } -inline CaptureBasePtr ProcessorPolyline::getLastPtr() +inline CaptureBasePtr ProcessorPolyline::getLast() { return last_ptr_; } diff --git a/include/base/processor/processor_tracker_feature.h b/include/base/processor/processor_tracker_feature.h index 97cbb23737fee4bb24a556243ddd24bccb408be9..2f0e7399ca78b42ff2ad5ddb4a4f53a1280fbbdf 100644 --- a/include/base/processor/processor_tracker_feature.h +++ b/include/base/processor/processor_tracker_feature.h @@ -169,7 +169,7 @@ class ProcessorTrackerFeature : public ProcessorTracker /** \brief Establish factors between features in Captures \b last and \b origin */ - virtual void establishConstraints(); + virtual void establishFactors(); public: Track track(SizeStd _trk_id); diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h index 63ef69245d53cb14520d8f5b16759b8b34b230d2..05e3723e3f75e49a8e6917ef8c2c9aa4cd1858f8 100644 --- a/include/base/processor/processor_tracker_feature_corner.h +++ b/include/base/processor/processor_tracker_feature_corner.h @@ -115,7 +115,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_, * the list of newly detected features of the capture last_ptr_. */ - virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out); + virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out); virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr); diff --git a/include/base/processor/processor_tracker_feature_dummy.h b/include/base/processor/processor_tracker_feature_dummy.h index 419c31f5d31b81211c2b9cf15048c559103356ae..d517431b134026fdebeb91d17748bb49471b38cc 100644 --- a/include/base/processor/processor_tracker_feature_dummy.h +++ b/include/base/processor/processor_tracker_feature_dummy.h @@ -65,7 +65,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_, * the list of newly detected features of the capture last_ptr_. */ - virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out); + virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out); virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr); diff --git a/include/base/processor/processor_tracker_feature_image.h b/include/base/processor/processor_tracker_feature_image.h index 25e2c629157033508b32bba6e80c910510ca8e16..3f730b60dfd540cf9fd2a44da40b61240a4d6f9d 100644 --- a/include/base/processor/processor_tracker_feature_image.h +++ b/include/base/processor/processor_tracker_feature_image.h @@ -121,7 +121,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_, * the list of newly detected features of the capture last_ptr_. */ - virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out); + virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out); virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr); diff --git a/include/base/processor/processor_tracker_feature_polyline_2D.h b/include/base/processor/processor_tracker_feature_polyline_2D.h index f883668806fd6501a42b5ad473f65d436b32de8f..7ac9687dfbb5aa8541bdc74306f38d98a2c75696 100644 --- a/include/base/processor/processor_tracker_feature_polyline_2D.h +++ b/include/base/processor/processor_tracker_feature_polyline_2D.h @@ -16,8 +16,8 @@ #include "base/capture/capture_laser_2D.h" #include "base/feature/feature_polyline_2D.h" #include "base/feature/feature_match_polyline_2D.h" -#include "base/constraint/constraint_point_2D.h" -#include "base/constraint/constraint_point_to_line_2D.h" +#include "base/factor/factor_point_2D.h" +#include "base/factor/factor_point_to_line_2D.h" //laser_scan_utils #include "laser_scan_utils/laser_scan.h" @@ -29,8 +29,8 @@ namespace wolf WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline2D); WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline2D); -typedef std::list<FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DList; -typedef std::list<LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DList; +typedef std::list<FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DPtrList; +typedef std::list<LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DPtrList; typedef std::map<Scalar,FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DScalarMap; typedef std::map<Scalar,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScalarMap; @@ -52,10 +52,10 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature ProcessorParamsTrackerFeaturePolyline2DPtr params_tracker_feature_polyline_; laserscanutils::LineFinderIterative line_finder_; - FeatureBaseList all_features_incoming_, all_features_last_; + FeatureBasePtrList all_features_incoming_, all_features_last_; LandmarkMatchPolyline2DMap landmark_match_map_; - LandmarkPolyline2DList modified_lmks_; - std::list<LandmarkPolyline2DList> merge_candidates_list_; + LandmarkPolyline2DPtrList modified_lmks_; + std::list<LandmarkPolyline2DPtrList> merge_candidates_list_; Eigen::Matrix2s R_sensor_world_last_, R_world_sensor_last_; Eigen::Vector2s t_sensor_world_last_, t_world_sensor_last_; @@ -80,8 +80,8 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature * \param _features_incoming_out returned list of features found in \b incoming * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr */ - virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, - FeatureBaseList& _features_incoming_out, + virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, + FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_correspondences); /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. @@ -108,38 +108,40 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature virtual unsigned int processNew(const unsigned int& _max_features); /** \brief Detect new Features - * \param _max_features maximum number of features detected + * \param _max_features maximum number of features detected (-1: unlimited. 0: none) + * \param _features_last_out The list of detected Features. * \return The number of detected Features. * * This function detects Features that do not correspond to known Features/Landmarks in the system. * - * The function sets _features_last_out, the list of newly detected features in Capture last. + * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_, + * the list of newly detected features of the capture last_ptr_. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, - FeatureBaseList& _features_last_out); + virtual unsigned int detectNewFeatures(const int& _max_new_features, + FeatureBasePtrList& _features_last_out); - /** \brief Create a new constraint and link it to the wolf tree + /** \brief Create a new factor and link it to the wolf tree * \param _feature_ptr pointer to the parent Feature * \param _feature_other_ptr pointer to the other feature constrained. * * Implement this method in derived classes. * - * This function creates a constraint of the appropriate type for the derived processor. + * This function creates a factor of the appropriate type for the derived processor. */ - virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, + virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr){ return nullptr; }; - /** \brief Establish constraints between features in Captures \b last and \b origin + /** \brief Establish factors between features in Captures \b last and \b origin */ - virtual void establishConstraints(); + virtual void establishFactors(); - void emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature, + void emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature, LandmarkPolyline2DPtr _polyline_landmark, const int& _ftr_point_id, const int& _lmk_point_id, const int& _lmk_prev_point_id); - void emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature, + void emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature, LandmarkPolyline2DPtr _polyline_landmark, const int& _ftr_point_id, const int& _lmk_point_id); @@ -196,7 +198,7 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature const ProcessorParamsBasePtr _params, const SensorBasePtr _sensor_ptr); - const FeatureBaseList& getLastNewFeatures() const + const FeatureBasePtrList& getLastNewFeatures() const { return all_features_last_; } diff --git a/include/base/processor/processor_tracker_feature_trifocal.h b/include/base/processor/processor_tracker_feature_trifocal.h index 0fc73588391bc5e13bd5a66763cf07b88eebc6e9..03758dbdaecaa24414abfcbf6df272783029f63e 100644 --- a/include/base/processor/processor_tracker_feature_trifocal.h +++ b/include/base/processor/processor_tracker_feature_trifocal.h @@ -94,7 +94,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_, * the list of newly detected features of the capture last_ptr_. */ - virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out) override; + virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out) override; /** \brief Create a new factor and link it to the wolf tree * \param _feature_ptr pointer to the parent Feature diff --git a/include/base/processor/processor_tracker_landmark.h b/include/base/processor/processor_tracker_landmark.h index 35a99280c4a2fc9b72bce7dbc00f6f937bcfa993..b4e079eb723b60aa28e873167f4f4d7fb409cdeb 100644 --- a/include/base/processor/processor_tracker_landmark.h +++ b/include/base/processor/processor_tracker_landmark.h @@ -140,7 +140,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_, * the list of newly detected features of the capture last_ptr_. */ - virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out) = 0; + virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out) = 0; /** \brief Creates a landmark for each of new_features_last_ **/ diff --git a/include/base/processor/processor_tracker_landmark_corner.h b/include/base/processor/processor_tracker_landmark_corner.h index 83be3c837aa8c2f26f57ceb8322a3897303096ad..903f14490f3dcb2c2f79e0900a35fd3e26a71173 100644 --- a/include/base/processor/processor_tracker_landmark_corner.h +++ b/include/base/processor/processor_tracker_landmark_corner.h @@ -144,7 +144,7 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_, * the list of newly detected features of the capture last_ptr_. */ - virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out); + virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out); /** \brief Create one landmark * diff --git a/include/base/processor/processor_tracker_landmark_dummy.h b/include/base/processor/processor_tracker_landmark_dummy.h index c0c3424cb0983f352d6976a7bee0ec07af9bdaa2..a312ce4a08b8b71a2cc9d8519932d5c4074fc653 100644 --- a/include/base/processor/processor_tracker_landmark_dummy.h +++ b/include/base/processor/processor_tracker_landmark_dummy.h @@ -57,7 +57,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_, * the list of newly detected features of the capture last_ptr_. */ - virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out); + virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out); /** \brief Create one landmark * diff --git a/include/base/processor/processor_tracker_landmark_image.h b/include/base/processor/processor_tracker_landmark_image.h index afdc8eff9200218c2856ee397aac879960a146d2..9765efd4be3584479e4322ca268d4de3438a28ba 100644 --- a/include/base/processor/processor_tracker_landmark_image.h +++ b/include/base/processor/processor_tracker_landmark_image.h @@ -137,7 +137,7 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_, * the list of newly detected features of the capture last_ptr_. */ - virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out); + virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out); /** \brief Create one landmark * diff --git a/include/base/sensor/sensor_gnss.h b/include/base/sensor/sensor_gnss.h index 1781dd90023d27430668d4ed2c1b7256b7450e1e..80367f52dcfc0b5ea38ba122835f5e2143696df6 100644 --- a/include/base/sensor/sensor_gnss.h +++ b/include/base/sensor/sensor_gnss.h @@ -43,10 +43,10 @@ class SensorGnss : public SensorBase bool isEnuMapInitialized() const; // ENU_MAP - StateBlockPtr getEnuMapTranslationStatePtr() const; - StateBlockPtr getEnuMapRollStatePtr() const; - StateBlockPtr getEnuMapPitchStatePtr() const; - StateBlockPtr getEnuMapYawStatePtr() const; + StateBlockPtr getEnuMapTranslationState() const; + StateBlockPtr getEnuMapRollState() const; + StateBlockPtr getEnuMapPitchState() const; + StateBlockPtr getEnuMapYawState() const; void initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, const Eigen::Vector3s& _t_ECEF_antenaENU, const Eigen::VectorXs& _pose_MAP_frame2, const Eigen::Vector3s& _t_ECEF_antena2); @@ -72,22 +72,22 @@ inline bool SensorGnss::isEnuMapInitialized() const return ENU_MAP_initialized_; } -inline StateBlockPtr SensorGnss::getEnuMapTranslationStatePtr() const +inline StateBlockPtr SensorGnss::getEnuMapTranslationState() const { return getStateBlockPtrStatic(3); } -inline StateBlockPtr SensorGnss::getEnuMapRollStatePtr() const +inline StateBlockPtr SensorGnss::getEnuMapRollState() const { return getStateBlockPtrStatic(4); } -inline StateBlockPtr SensorGnss::getEnuMapPitchStatePtr() const +inline StateBlockPtr SensorGnss::getEnuMapPitchState() const { return getStateBlockPtrStatic(5); } -inline StateBlockPtr SensorGnss::getEnuMapYawStatePtr() const +inline StateBlockPtr SensorGnss::getEnuMapYawState() const { return getStateBlockPtrStatic(6); } @@ -113,7 +113,7 @@ inline Eigen::Matrix<T,3,3> SensorGnss::computeREnuMap(const T& _r,const T& _p,c // TODO: store R's when T=Scalar //Eigen::Matrix<T,3,3> R_roll_T, R_pitch_T, R_yaw_T; - //if (!getRollStatePtr()->isFixed()) + //if (!getRollState()->isFixed()) // *R_roll_ptr_ = Eigen::AngleAxis<Scalar>(0.25*M_PI, Eigen::Vector3s::UnitX()); } diff --git a/src/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp index e7e5752b0e381d8ede98d7086d843c78b71bfa71..44d3e44f200b5a0b0858c181c160d96e284cc051 100644 --- a/src/examples/test_processor_tracker_feature.cpp +++ b/src/examples/test_processor_tracker_feature.cpp @@ -20,22 +20,22 @@ void print_prc(wolf::ProcessorTrackerFeaturePtr _prc) { using namespace wolf; - auto o = _prc->getOriginPtr(); - auto l = _prc->getLastPtr(); - auto i = _prc->getIncomingPtr(); + auto o = _prc->getOrigin(); + auto l = _prc->getLast(); + auto i = _prc->getIncoming(); std::cout << "o: C" << ( int)( o ? o->id() : -99 ) << " || l: C" << ( int)( l ? l->id() : -99 ) << " || i: C" << ( int)( i ? i->id() : -99 ) << std::endl; - for (auto ftr : _prc->getLastPtr()->getFeatureList()) + for (auto ftr : _prc->getLast()->getFeatureList()) { SizeStd trk_id = ftr->trackId(); std::cout << "Track " << trk_id << " ---------------------" << std::endl; for (auto ftr_pair : _prc->track(trk_id)) { auto f = ftr_pair.second; - auto C = f->getCapturePtr(); + auto C = f->getCapture(); std::cout << "f" << f->id() << " C" << (int)(C ? C->id() : -99) << std::endl; } } diff --git a/src/examples/test_trifocal_optimization.cpp b/src/examples/test_trifocal_optimization.cpp index 93cc5eed6b0fdd7541f36dcf5153c887b59e7c47..4e6ae7113cca24c723ddbd791db1426625950fef 100644 --- a/src/examples/test_trifocal_optimization.cpp +++ b/src/examples/test_trifocal_optimization.cpp @@ -10,14 +10,14 @@ #include <vision_utils.h> //Wolf includes -#include "../processors/processor_tracker_feature_trifocal.h" -#include "../capture_image.h" -#include "../sensor_camera.h" -#include "../ceres_wrapper/ceres_manager.h" -#include "../rotations.h" -#include "../capture_pose.h" -#include "../capture_void.h" -#include "../constraints/constraint_autodiff_distance_3D.h" +#include "base/processor/processor_tracker_feature_trifocal.h" +#include "base/capture/capture_image.h" +#include "base/sensor/sensor_camera.h" +#include "base/ceres_wrapper/ceres_manager.h" +#include "base/rotations.h" +#include "base/capture/capture_pose.h" +#include "base/capture/capture_void.h" +#include "base/factor/factor_autodiff_distance_3D.h" Eigen::VectorXs get_random_state(const double& _LO, const double& _HI) { @@ -122,7 +122,7 @@ int main(int argc, char** argv) camera->process(capture_1); // Verify KFs - FrameBasePtr kf1_check = capture_1->getFramePtr(); + FrameBasePtr kf1_check = capture_1->getFrame(); assert(kf1->id()==kf1_check->id() && "Prior and KF1 are not the same!"); // problem->print(2,0,1,0); @@ -159,12 +159,12 @@ int main(int argc, char** argv) } // ================================================== - // Establish Scale Constraint (to see results scaled) + // Establish Scale Factor (to see results scaled) // ================================================== - std::cout << "================== ADD Scale constraint ========================" << std::endl; + std::cout << "================== ADD Scale factor ========================" << std::endl; - // Distance constraint + // Distance factor Vector1s distance(0.2); // 2x10cm distance -- this fixes the scale Matrix1s dist_cov(0.000001); // 1mm error @@ -175,11 +175,11 @@ int main(int argc, char** argv) ftr_dist = cap_dist->addFeature(make_shared<FeatureBase>("DISTANCE", distance, dist_cov)); - ConstraintBasePtr - ctr_dist = ftr_dist->addConstraint(make_shared<ConstraintAutodiffDistance3D>(ftr_dist, + FactorBasePtr + fac_dist = ftr_dist->addFactor(make_shared<FactorAutodiffDistance3D>(ftr_dist, kfs.at(0), nullptr)); - kfs.at(0)->addConstrainedBy(ctr_dist); + kfs.at(0)->addConstrainedBy(fac_dist); problem->print(1,1,1,0); @@ -195,8 +195,8 @@ int main(int argc, char** argv) problem->print(1,1,1,0); // Print orientation states for all KFs - for (auto kf : problem->getTrajectoryPtr()->getFrameList()) - std::cout << "KF" << kf->id() << " Euler deg " << wolf::q2e(kf->getOPtr()->getState()).transpose()*180.0/3.14159 << std::endl; + for (auto kf : problem->getTrajectory()->getFrameList()) + std::cout << "KF" << kf->id() << " Euler deg " << wolf::q2e(kf->getO()->getState()).transpose()*180.0/3.14159 << std::endl; // =============================================== @@ -205,10 +205,11 @@ int main(int argc, char** argv) // GET COVARIANCES of all states WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); - for (auto kf : problem->getTrajectoryPtr()->getFrameList()) + for (auto kf : problem->getTrajectory()->getFrameList()) if (kf->isKey()) { - Eigen::MatrixXs cov = kf->getCovariance(); + Eigen::MatrixXs cov; + kf->getCovariance(cov); WOLF_TRACE("KF", kf->id(), "_std (sigmas) = ", cov.diagonal().transpose().array().sqrt()); } std::cout << std::endl; @@ -220,7 +221,7 @@ int main(int argc, char** argv) // ADD PERTURBATION std::cout << "================== ADD PERTURBATION ========================" << std::endl; - for (auto kf : problem->getTrajectoryPtr()->getFrameList()) + for (auto kf : problem->getTrajectory()->getFrameList()) { if (kf != kf1) { @@ -228,7 +229,7 @@ int main(int argc, char** argv) Eigen::Vector7s state_perturbed = kf->getState() + perturbation; state_perturbed.segment(3,4).normalize(); kf->setState(state_perturbed); - std::cout << "KF" << kf->id() << " Euler deg " << wolf::q2e(kf->getOPtr()->getState()).transpose()*180.0/3.14159 << std::endl; + std::cout << "KF" << kf->id() << " Euler deg " << wolf::q2e(kf->getO()->getState()).transpose()*180.0/3.14159 << std::endl; } } @@ -246,8 +247,8 @@ int main(int argc, char** argv) std::cout << "================== AFTER SOLVE 2nd TIME ========================" << std::endl; problem->print(1,1,1,0); - for (auto kf : problem->getTrajectoryPtr()->getFrameList()) - std::cout << "KF" << kf->id() << " Euler deg " << wolf::q2e(kf->getOPtr()->getState()).transpose()*180.0/3.14159 << std::endl; + for (auto kf : problem->getTrajectory()->getFrameList()) + std::cout << "KF" << kf->id() << " Euler deg " << wolf::q2e(kf->getO()->getState()).transpose()*180.0/3.14159 << std::endl; cv::waitKey(0); // Wait for a keystroke in the window diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp index 6cc41a85db65fdf5a88e8b8d7a38cab019d94d9b..7abfae36142847df7511e5fd19e94f5f25ab85c0 100644 --- a/src/landmark/landmark_polyline_2D.cpp +++ b/src/landmark/landmark_polyline_2D.cpp @@ -83,8 +83,8 @@ StateBlockPtr LandmarkPolyline2D::getPointStateBlock(int _i) Eigen::MatrixXs LandmarkPolyline2D::computePointsGlobal() const { Eigen::MatrixXs points_global = Eigen::MatrixXs::Ones(3,getNPoints()); - Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(getOPtr()->getState()(0)).matrix(); - Eigen::Vector2s t_world_points = getPPtr()->getState(); + Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(getO()->getState()(0)).matrix(); + Eigen::Vector2s t_world_points = getP()->getState(); for (int i = 0, valid_id = getFirstId(); i < getNPoints(); i++, valid_id = getNextValidId(valid_id)) { @@ -457,7 +457,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id) // If landmark point constrained -> new factor if (fac_point_ptr->getLandmarkPointId() == _remove_id) { - new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeaturePtr()), + new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), fac_point_ptr->getProcessor(), fac_point_ptr->getFeaturePointId(), @@ -483,7 +483,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id) // If landmark point is aux point -> new factor else if (fac_point_line_ptr->getLandmarkPointAuxId() == _remove_id) { - new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeaturePtr()), + new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), fac_point_line_ptr->getProcessor(), fac_point_line_ptr->getFeaturePointId(), @@ -692,20 +692,20 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli classify(pl_class); // Move origin to to the center - getPPtr()->setState((getPointVector(points_ids[0]) + getPointVector(points_ids[2])) / 2.0); + getP()->setState((getPointVector(points_ids[0]) + getPointVector(points_ids[2])) / 2.0); Eigen::Vector2s frame_x = (configuration ? getPointVector(points_ids[0])-getPointVector(points_ids[1]) : getPointVector(points_ids[2])-getPointVector(points_ids[1])); - getOPtr()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0)))); + getO()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0)))); // Unfix origin - getPPtr()->unfix(); - getOPtr()->unfix(); + getP()->unfix(); + getO()->unfix(); //std::cout << "A: " << getPointVector(points_ids[0]).transpose() << std::endl; //std::cout << "B: " << getPointVector(points_ids[1]).transpose() << std::endl; //std::cout << "C: " << getPointVector(points_ids[2]).transpose() << std::endl; //std::cout << "frame_x: " << frame_x.transpose() << std::endl; - //std::cout << "frame position: " << getPPtr()->getState().transpose() << std::endl; - //std::cout << "frame orientation: " << getOPtr()->getState() << std::endl; + //std::cout << "frame position: " << getP()->getState().transpose() << std::endl; + //std::cout << "frame orientation: " << getO()->getState() << std::endl; // Set polyline points to its respective relative positions if (configuration) @@ -819,16 +819,16 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, } // ------------------------- COPY FACTORS - FactorBaseList old_factors_list = _merged_lmk->getConstrainedByList(); + FactorBasePtrList old_factors_list = _merged_lmk->getConstrainedByList(); //std::cout << "\tchanging " << old_factors_list.size() << " factors." << std::endl; FactorBasePtr new_fac_ptr = nullptr; for (auto fac_ptr : old_factors_list) { - //std::cout << "\t\tfactor " << fac_ptr->id() << " to landmark " << fac_ptr->getLandmarkOtherPtr()->id() << std::endl; - assert(fac_ptr->getLandmarkOtherPtr() == _merged_lmk); + //std::cout << "\t\tfactor " << fac_ptr->id() << " to landmark " << fac_ptr->getLandmarkOther()->id() << std::endl; + assert(fac_ptr->getLandmarkOther() == _merged_lmk); FactorPoint2DPtr fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr); - FactortPointToLine2DPtr fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr); + FactorPointToLine2DPtr fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr); // point 2 point if (fac_point_ptr) @@ -836,7 +836,7 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, //std::cout << "\t\tpoint-point factor to id: " << fac_point_ptr->getLandmarkPointId() << std::endl; assert(merged_id_to_id.find(fac_point_ptr->getLandmarkPointId()) != merged_id_to_id.end()); - new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeaturePtr()), + new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), fac_point_ptr->getProcessor(), fac_point_ptr->getFeaturePointId(), @@ -852,7 +852,7 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, assert(merged_id_to_id.find(fac_point_line_ptr->getLandmarkPointId()) != merged_id_to_id.end()); assert(merged_id_to_id.find(fac_point_line_ptr->getLandmarkPointAuxId()) != merged_id_to_id.end()); - new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeaturePtr()), + new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), fac_point_line_ptr->getProcessor(), fac_point_line_ptr->getFeaturePointId(), @@ -873,7 +873,7 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk, //std::cout << "\t\tdeleting factor: " << fac_ptr->id() << std::endl; // add new factor - fac_ptr->getFeaturePtr()->addFactor(new_fac_ptr); + fac_ptr->getFeature()->addFactor(new_fac_ptr); addConstrainedBy(new_fac_ptr); // remove factor @@ -968,7 +968,7 @@ YAML::Node LandmarkPolyline2D::saveToYaml() const return node; } -void LandmarkPolyline2D::tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _sq_dist_tol) +void LandmarkPolyline2D::tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list, const Scalar& _sq_dist_tol) { std::cout << "LandmarkPolyline2D::tryMergeLandmarks\n"; assert(_lmk_list.size() >= 2); diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp index cb5577914e4632f0725126cedb350a01ae63acf7..b8251c3aa78637fdaa55b2a670a774bbadf9fa5d 100644 --- a/src/processor/processor_diff_drive.cpp +++ b/src/processor/processor_diff_drive.cpp @@ -6,14 +6,15 @@ #include "base/capture/capture_velocity.h" #include "base/rotations.h" -//#include "base/factor/factor_odom_2D.h" +#include "base/factor/factor_odom_2D.h" +#include "base/factor/factor_diff_drive.h" #include "base/feature/feature_diff_drive.h" namespace wolf { ProcessorDiffDrive::ProcessorDiffDrive(SensorDiffDrivePtr _sensor_diff_drive_ptr, ProcessorParamsDiffDrivePtr _params) : - ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, (_sensor_diff_drive_ptr->getIntrinsicPtr()->isFixed() ? 0 : 3), _params), + ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, (_sensor_diff_drive_ptr->getIntrinsic()->isFixed() ? 0 : 3), _params), sensor_diff_drive_ptr_(_sensor_diff_drive_ptr), params_motion_diff_drive_(_params) { @@ -218,12 +219,12 @@ CaptureMotionPtr ProcessorDiffDrive::createCapture(const TimeStamp& _ts, FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin) { - FactorOdom2DPtr fac_odom; + FactorBasePtr fac_odom; if (hasCalibration()) - fac_odom = std::make_shared<FactorDiffDrive>(_feature, _capture_origin->getFramePtr(), shared_from_this()); + fac_odom = std::make_shared<FactorDiffDrive>(_feature, _capture_origin->getFrame(), shared_from_this()); else - fac_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFramePtr(), shared_from_this()); + fac_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(), shared_from_this()); _feature->addFactor(fac_odom); _capture_origin->getFrame()->addConstrainedBy(fac_odom); diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp index 6ee88052df6ff322990abc8cd8688dc2031b50e1..1e8cc96d941d5fe3f47ea91ff8e11a3925ecf25d 100644 --- a/src/processor/processor_gnss_fix.cpp +++ b/src/processor/processor_gnss_fix.cpp @@ -1,5 +1,5 @@ -#include "base/constraint/constraint_gnss_fix_2D.h" -#include "base/constraint/constraint_gnss_fix_3D.h" +#include "base/factor/factor_gnss_fix_2D.h" +#include "base/factor/factor_gnss_fix_3D.h" #include "base/feature/feature_gnss_fix.h" #include "base/processor/processor_gnss_fix.h" #include "base/capture/capture_gnss_fix.h" @@ -34,7 +34,7 @@ void ProcessorGnssFix::process(CaptureBasePtr _capture_ptr) //std::cout << "WOLF: gnss fix in ENU coordinates: " << p_enu.transpose() << std::endl; FrameBasePtr new_frame_ptr = nullptr; - bool new_ctr_created = false; + bool new_fac_created = false; // ALREADY CREATED KF PackKeyFramePtr KF_pack = kf_pack_buffer_.selectPack( _capture_ptr, params_->time_tolerance); @@ -62,25 +62,25 @@ void ProcessorGnssFix::process(CaptureBasePtr _capture_ptr) FeatureBasePtr ftr_ptr = last_capture_ptr_->addFeature(std::make_shared<FeatureGnssFix>(last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance())); // EMPLACE CONSTRAINT - emplaceConstraint(ftr_ptr); - new_ctr_created = true; + emplaceFactor(ftr_ptr); + new_fac_created = true; // outlier rejection if (sensor_gnss_ptr_->isEnuDefined() && sensor_gnss_ptr_->isEnuMapInitialized()) - if (rejectOutlier(ftr_ptr->getConstraintList().front())) - new_ctr_created = false; + if (rejectOutlier(ftr_ptr->getFactorList().front())) + new_fac_created = false; // store last KF - if (new_ctr_created) + if (new_fac_created) last_gnss_fix_KF_= new_frame_ptr; - //WOLF_DEBUG("Constraint established"); + //WOLF_DEBUG("Factor established"); } // INITIALIZE ENU IF: // 1 - not initialized - // 2 - constraint established - if (!sensor_gnss_ptr_->isEnuDefined() && new_ctr_created) + // 2 - factor established + if (!sensor_gnss_ptr_->isEnuDefined() && new_fac_created) { sensor_gnss_ptr_->setEcefEnu(last_capture_ptr_->getData()); @@ -89,62 +89,62 @@ void ProcessorGnssFix::process(CaptureBasePtr _capture_ptr) // INITIALIZE ENU_MAP IF 2 NECESSARY CONDITIONS: // 1 - not initialized - // 2 - two constraints established (first and current) - if (!sensor_gnss_ptr_->isEnuMapInitialized() && first_capture_ptr_ && new_ctr_created) + // 2 - two factors established (first and current) + if (!sensor_gnss_ptr_->isEnuMapInitialized() && first_capture_ptr_ && new_fac_created) { - sensor_gnss_ptr_->initializeEnuMap(first_capture_ptr_->getFramePtr()->getState(), first_capture_ptr_->getData(), - last_capture_ptr_->getFramePtr()->getState(), last_capture_ptr_->getData()); + sensor_gnss_ptr_->initializeEnuMap(first_capture_ptr_->getFrame()->getState(), first_capture_ptr_->getData(), + last_capture_ptr_->getFrame()->getState(), last_capture_ptr_->getData()); WOLF_INFO("ProcessorGnssFix: ENU-MAP initialized.") } - // Store the first capture that established a constraint - if (new_ctr_created && first_capture_ptr_ == nullptr) + // Store the first capture that established a factor + if (new_fac_created && first_capture_ptr_ == nullptr) first_capture_ptr_ = last_capture_ptr_; } -void ProcessorGnssFix::emplaceConstraint(FeatureBasePtr& ftr_ptr) +void ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr_ptr) { // CREATE CONSTRAINT -------------------- - //WOLF_DEBUG("creating the constraint..."); - ConstraintBasePtr new_ctr_ptr = nullptr; + //WOLF_DEBUG("creating the factor..."); + FactorBasePtr new_fac_ptr = nullptr; // 2D if (getProblem()->getDim() == 2) - new_ctr_ptr = std::make_shared<ConstraintGnssFix2D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false); + new_fac_ptr = std::make_shared<FactorGnssFix2D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false); // 3D else - new_ctr_ptr = std::make_shared<ConstraintGnssFix3D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false); + new_fac_ptr = std::make_shared<FactorGnssFix3D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false); // ADD CONSTRAINT -------------------- - //WOLF_DEBUG("adding the constraint..."); - ftr_ptr->addConstraint(new_ctr_ptr); + //WOLF_DEBUG("adding the factor..."); + ftr_ptr->addFactor(new_fac_ptr); } -bool ProcessorGnssFix::rejectOutlier(ConstraintBasePtr ctr_ptr) +bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac_ptr) { // Cast feature - auto gnss_ftr_ptr = std::static_pointer_cast<FeatureGnssFix>(ctr_ptr->getFeaturePtr()); + auto gnss_ftr_ptr = std::static_pointer_cast<FeatureGnssFix>(fac_ptr->getFeature()); // copy states - Eigen::VectorXs x(gnss_ftr_ptr->getCapturePtr()->getFramePtr()->getPPtr()->getState()); - Eigen::VectorXs o(gnss_ftr_ptr->getCapturePtr()->getFramePtr()->getPPtr()->getState()); - Eigen::VectorXs x_antena(sensor_gnss_ptr_->getStateBlockPtr(0)->getState()); - Eigen::VectorXs t_ENU_map(sensor_gnss_ptr_->getEnuMapTranslationStatePtr()->getState()); - Eigen::VectorXs roll_ENU_map(sensor_gnss_ptr_->getEnuMapRollStatePtr()->getState()); - Eigen::VectorXs pitch_ENU_map(sensor_gnss_ptr_->getEnuMapPitchStatePtr()->getState()); - Eigen::VectorXs yaw_ENU_map(sensor_gnss_ptr_->getEnuMapYawStatePtr()->getState()); + Eigen::VectorXs x(gnss_ftr_ptr->getCapture()->getFrame()->getP()->getState()); + Eigen::VectorXs o(gnss_ftr_ptr->getCapture()->getFrame()->getP()->getState()); + Eigen::VectorXs x_antena(sensor_gnss_ptr_->getStateBlock(0)->getState()); + Eigen::VectorXs t_ENU_map(sensor_gnss_ptr_->getEnuMapTranslationState()->getState()); + Eigen::VectorXs roll_ENU_map(sensor_gnss_ptr_->getEnuMapRollState()->getState()); + Eigen::VectorXs pitch_ENU_map(sensor_gnss_ptr_->getEnuMapPitchState()->getState()); + Eigen::VectorXs yaw_ENU_map(sensor_gnss_ptr_->getEnuMapYawState()->getState()); // create Scalar* array of a copy of the state Scalar* parameters[7] = {x.data(), o.data(), x_antena.data(), t_ENU_map.data(), roll_ENU_map.data(), pitch_ENU_map.data(), yaw_ENU_map.data()}; // create residuals pointer Eigen::VectorXs residuals(3); - // evaluate the constraint in this state - ctr_ptr->evaluate(parameters, residuals.data(), nullptr); + // evaluate the factor in this state + fac_ptr->evaluate(parameters, residuals.data(), nullptr); // discard if residual too high evaluated at the current estimation if (residuals.norm() > 1e3) { - WOLF_WARN("Discarding GNSS FIX Constraint, considered OUTLIER"); - WOLF_TRACE("Feature: ", ctr_ptr->getMeasurement().transpose(),"\nError: ",(ctr_ptr->getMeasurementSquareRootInformationUpper().inverse()*residuals).transpose()); - ctr_ptr->remove(); + WOLF_WARN("Discarding GNSS FIX Factor, considered OUTLIER"); + WOLF_TRACE("Feature: ", fac_ptr->getMeasurement().transpose(),"\nError: ",(fac_ptr->getMeasurementSquareRootInformationUpper().inverse()*residuals).transpose()); + fac_ptr->remove(); return true; } return false; @@ -165,7 +165,7 @@ bool ProcessorGnssFix::voteForKeyFrame() void ProcessorGnssFix::configure(SensorBasePtr _sensor) { sensor_gnss_ptr_ = std::static_pointer_cast<SensorGnss>(_sensor); - setSensorPtr(_sensor); + setSensor(_sensor); }; ProcessorBasePtr ProcessorGnssFix::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr) diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp index 9a2600a9eab3768bc1cad6c852607505b43fc183..179933a0789e5bc268351593f3fba3eaaf6349ef 100644 --- a/src/processor/processor_gnss_single_diff.cpp +++ b/src/processor/processor_gnss_single_diff.cpp @@ -1,4 +1,4 @@ -#include "base/constraint/constraint_gnss_single_diff_2D.h" +#include "base/factor/factor_gnss_single_diff_2D.h" #include "base/feature/feature_gnss_single_diff.h" #include "base/processor/processor_gnss_single_diff.h" #include "base/capture/capture_gnss_single_diff.h" @@ -30,7 +30,7 @@ void ProcessorGnssSingleDiff::process(CaptureBasePtr _capture_ptr) // ALREADY CREATED KF PackKeyFramePtr KF_pack = kf_pack_buffer_.selectPack( _capture_ptr, params_->time_tolerance); - if (KF_pack && KF_pack->key_frame != last_capture_ptr_->getOriginFramePtr()) + if (KF_pack && KF_pack->key_frame != last_capture_ptr_->getOriginFrame()) { new_frame_ptr = KF_pack->key_frame; WOLF_DEBUG( "PR ",getName()," - capture ", _capture_ptr->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp()); @@ -54,20 +54,20 @@ void ProcessorGnssSingleDiff::process(CaptureBasePtr _capture_ptr) FeatureBasePtr ftr_ptr = last_capture_ptr_->addFeature(std::make_shared<FeatureGnssSingleDiff>(last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance())); // ADD CONSTRAINT - ConstraintBasePtr ctr_ptr; - //WOLF_DEBUG("adding the constraint..."); + FactorBasePtr fac_ptr; + //WOLF_DEBUG("adding the factor..."); // 2D if (getProblem()->getDim() == 2) - ctr_ptr = ftr_ptr->addConstraint(std::make_shared<ConstraintGnssSingleDiff2D>(ftr_ptr, last_capture_ptr_->getOriginFramePtr(), sensor_gnss_ptr_, shared_from_this())); + fac_ptr = ftr_ptr->addFactor(std::make_shared<FactorGnssSingleDiff2D>(ftr_ptr, last_capture_ptr_->getOriginFrame(), sensor_gnss_ptr_, shared_from_this())); // 3D TODO else std::runtime_error("Single Differences in 3D not implemented yet."); - // ctr_ptr = ftr_ptr->addConstraint(std::make_shared<ConstraintGnssSingleDiff3D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this())); + // fac_ptr = ftr_ptr->addFactor(std::make_shared<FactorGnssSingleDiff3D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this())); // add constrained-by to the origin - last_capture_ptr_->getOriginFramePtr()->addConstrainedBy(ctr_ptr); + last_capture_ptr_->getOriginFrame()->addConstrainedBy(fac_ptr); - //WOLF_DEBUG("Constraint established"); + //WOLF_DEBUG("Factor established"); } } @@ -80,7 +80,7 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame() void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor) { sensor_gnss_ptr_ = std::static_pointer_cast<SensorGnss>(_sensor); - setSensorPtr(_sensor); + setSensor(_sensor); }; ProcessorBasePtr ProcessorGnssSingleDiff::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr) diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index ca17b1ea6a5d64f6f1869d1c78426a398859e304..a556c3e8328daacd044b6cf22f90e48d456f97a8 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -554,7 +554,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp { // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion capture_motion = std::static_pointer_cast<CaptureMotion>(capture); - if (capture_motion->getOriginFramePtr() && !capture_motion->getBuffer().get().empty() && _ts >= capture_motion->getBuffer().get().front().ts_) + if (capture_motion->getOriginFrame() && !capture_motion->getBuffer().get().empty() && _ts >= capture_motion->getBuffer().get().front().ts_) // Found time stamp satisfying rule 3 above !! ==> break for loop break; else diff --git a/src/processor/processor_polyline.cpp b/src/processor/processor_polyline.cpp index 901543c14c5603dd74fe1b4208c6b2f2b0bf96d9..38c9794c87182363cbca8eb56346ecf0d830ca39 100644 --- a/src/processor/processor_polyline.cpp +++ b/src/processor/processor_polyline.cpp @@ -70,19 +70,19 @@ void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr) // Create landmarks from new_features_last_ processNew(); - // Establish constraints from last - establishConstraints(); + // Establish factors from last + establishFactors(); } // OTHER TIMES else { - assert(last_ptr_->getFramePtr() && "last_ not linked to any frame in OTHER TIMES"); + assert(last_ptr_->getFrame() && "last_ not linked to any frame in OTHER TIMES"); //WOLF_DEBUG("OTHER TIMES"); - // 1. First we track the known Features and create new constraints as needed + // 1. First we track the known Features and create new factors as needed // Find landmarks in new_features_incoming_ (and extract from it) - findLandmarks(getProblem()->getMapPtr()->getLandmarkList(), known_features_incoming_, matches_landmark_from_incoming_); + findLandmarks(getProblem()->getMap()->getLandmarkList(), known_features_incoming_, matches_landmark_from_incoming_); //WOLF_DEBUG("incoming new_features: " , new_features_incoming_.size()); //WOLF_DEBUG("incoming known features: ", known_features_incoming_.size()); @@ -93,11 +93,11 @@ void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr) // - Last is not KF // - KF has not another capture of the same sensor PackKeyFramePtr KF_pack = kf_pack_buffer_.selectPack( last_ptr_, params_->time_tolerance); - bool KF_has_capture = KF_pack && (KF_pack->key_frame->getCaptureOf(getSensorPtr()) != nullptr); - bool create_KF = !KF_has_capture && !last_ptr_->getFramePtr()->isKey() && voteForKeyFrame() && permittedKeyFrame(); - bool existing_KF = !KF_has_capture && !last_ptr_->getFramePtr()->isKey() && KF_pack; + bool KF_has_capture = KF_pack && (KF_pack->key_frame->getCaptureOf(getSensor()) != nullptr); + bool create_KF = !KF_has_capture && !last_ptr_->getFrame()->isKey() && voteForKeyFrame() && permittedKeyFrame(); + bool existing_KF = !KF_has_capture && !last_ptr_->getFrame()->isKey() && KF_pack; - FrameBasePtr closest_key_frm_to_last = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp()); + FrameBasePtr closest_key_frm_to_last = getProblem()->getTrajectory()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp()); //WOLF_DEBUG("last is KF? ", (last_is_KF ? "YES" : "NO")); @@ -121,24 +121,24 @@ void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr) // Make the last Capture's Frame a KeyFrame setKeyFrame(last_ptr_); - WOLF_DEBUG("PR ",getName() ," - Set KEY to last F", last_ptr_->getFramePtr()->id()); + WOLF_DEBUG("PR ",getName() ," - Set KEY to last F", last_ptr_->getFrame()->id()); } // existing KF within the time tolerance else { // Move incomming to the last F - last_ptr_->getFramePtr()->addCapture(incoming_ptr_); - incoming_ptr_->getFramePtr()->setTimeStamp(incoming_ptr_->getTimeStamp()); + last_ptr_->getFrame()->addCapture(incoming_ptr_); + incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp()); // Move last to the existing KF - last_ptr_->getFramePtr()->unlinkCapture(last_ptr_); + last_ptr_->getFrame()->unlinkCapture(last_ptr_); KF_pack->key_frame->addCapture(last_ptr_); - WOLF_DEBUG("PR ",getName() ," - Last capture ", last_ptr_->id(), " appended to existing KF ", last_ptr_->getFramePtr()->id()); + WOLF_DEBUG("PR ",getName() ," - Last capture ", last_ptr_->id(), " appended to existing KF ", last_ptr_->getFrame()->id()); } - // Establish constraints for last matches - establishConstraints(); + // Establish factors for last matches + establishFactors(); // Reset the Tracker // Move matches and new features from incoming to last @@ -159,7 +159,7 @@ void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr) known_features_last_ = std::move(known_features_incoming_); //WOLF_DEBUG("new_features_last_ " , new_features_last_.size()); - if (last_ptr_->getFramePtr()->isKey()) + if (last_ptr_->getFrame()->isKey()) { //WOLF_DEBUG("Last is in a KF \n"); // Make a non-key-frame to hold incoming @@ -171,8 +171,8 @@ void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr) { //WOLF_DEBUG("Last is NOT in a KF \n"); // Add incoming Capture to the tracker's last Frame - last_ptr_->getFramePtr()->addCapture(incoming_ptr_); - incoming_ptr_->getFramePtr()->setTimeStamp(incoming_ptr_->getTimeStamp()); + last_ptr_->getFrame()->addCapture(incoming_ptr_); + incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp()); // discard last capture last_ptr_->remove(); } @@ -191,7 +191,7 @@ void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr) while (!merge_candidates_list_.empty()) { // load next list of candidates - LandmarkPolyline2DList merge_candidates = std::move(merge_candidates_list_.front()); + LandmarkPolyline2DPtrList merge_candidates = std::move(merge_candidates_list_.front()); merge_candidates_list_.pop_front(); // change already merged lmks with the corresponding remaining lmk @@ -242,7 +242,7 @@ bool ProcessorPolyline::voteForKeyFrame() { assert(matches_landmark_from_last_.find(new_ftr) != matches_landmark_from_last_.end()); assert(matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().size() > 0); - if (std::fabs(last_ptr_->getTimeStamp() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapturePtr()->getTimeStamp()) > params_->loop_time_th) + if (std::fabs(last_ptr_->getTimeStamp() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getTimeStamp()) > params_->loop_time_th) { WOLF_DEBUG("PR ",getName(),"------------- NEW KEY FRAME: Option 2 - Loop closure"); return true; @@ -253,13 +253,13 @@ bool ProcessorPolyline::voteForKeyFrame() // IMPLEMENTATION ///////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void ProcessorPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBaseList& _polyline_list) +void ProcessorPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list) { //WOLF_DEBUG("ProcessorPolyline::extractPolylines: "); // TODO: sort corners by bearing std::list<laserscanutils::Polyline> polylines; - line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensorPtr()))->getScanParams(), polylines); + line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines); //WOLF_DEBUG("Polylines extracted: ", polylines.size()); for (auto&& pl : polylines) @@ -271,8 +271,8 @@ void ProcessorPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, F } } -unsigned int ProcessorPolyline::findLandmarks(const LandmarkBaseList& _landmarks_searched, - FeatureBaseList& _features_found, +unsigned int ProcessorPolyline::findLandmarks(const LandmarkBasePtrList& _landmarks_searched, + FeatureBasePtrList& _features_found, LandmarkMatchMap& _feature_landmark_correspondences) { //std::cout << "ProcessorPolyline::findLandmarks: " << _landmarks_searched.size() << " features: " << new_features_incoming_.size() << std::endl; @@ -295,7 +295,7 @@ unsigned int ProcessorPolyline::findLandmarks(const LandmarkBaseList& _landmarks LandmarkMatchPolyline2DPtr best_match = nullptr; FeaturePolyline2DPtr pl_ftr; LandmarkPolyline2DPtr pl_lmk; - LandmarkPolyline2DList merging_candidates; + LandmarkPolyline2DPtrList merging_candidates; auto feature_it = new_features_incoming_.begin(); // iterate over all polylines features @@ -372,7 +372,7 @@ unsigned int ProcessorPolyline::findLandmarks(const LandmarkBaseList& _landmarks merge_candidates_list_.push_back(std::move(merging_candidates)); } - //std::cout << matches_landmark_from_incoming_.size() << " landmarks found from " << getProblem()->getMapPtr()->getLandmarkList().size() << std::endl; + //std::cout << matches_landmark_from_incoming_.size() << " landmarks found from " << getProblem()->getMap()->getLandmarkList().size() << std::endl; //std::cout << "done" << std::endl; return matches_landmark_from_incoming_.size(); } @@ -440,16 +440,16 @@ LandmarkBasePtr ProcessorPolyline::createLandmark(FeatureBasePtr _feature_ptr) //std::cout << "done" << std::endl; } -void ProcessorPolyline::establishConstraints() +void ProcessorPolyline::establishFactors() { //TODO: update with new index in landmarks - //std::cout << "ProcessorPolyline::establishConstraints" << std::endl; - //std::cout << "\tlast_ id: " << last_ptr_->id() << " linked to frame: " << last_ptr_->getFramePtr()->id() << " is linked to problem? " << (last_ptr_->getProblem() ? "YES" : "NO") << std::endl; - unsigned int N_constraints = 0; + //std::cout << "ProcessorPolyline::establishFactors" << std::endl; + //std::cout << "\tlast_ id: " << last_ptr_->id() << " linked to frame: " << last_ptr_->getFrame()->id() << " is linked to problem? " << (last_ptr_->getProblem() ? "YES" : "NO") << std::endl; + unsigned int N_factors = 0; LandmarkMatchPolyline2DPtr pl_match; FeaturePolyline2DPtr pl_ftr; LandmarkPolyline2DPtr pl_lmk; - LandmarkPolyline2DList modified_lmks; + LandmarkPolyline2DPtrList modified_lmks; for (auto last_ftr : last_ptr_->getFeatureList()) { @@ -647,15 +647,15 @@ void ProcessorPolyline::establishConstraints() assert(pl_match->feature_from_id_ == 0 && "Landmark didn't grow properly"); assert(pl_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly"); assert((pl_lmk->getNPoints() >= pl_ftr->getNPoints() + pl_match->landmark_from_id_ || pl_lmk->isClosed()) && "Landmark didn't grow properly"); - //std::cout << "Establishing constraint between" << std::endl; - //std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapturePtr() ? pl_ftr->getCapturePtr()->id() : 9999999999999999) << " is liked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl; + //std::cout << "Establishing factor between" << std::endl; + //std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapture() ? pl_ftr->getCapture()->id() : 9999999999999999) << " is liked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl; //std::cout << "\tlandmark " << pl_lmk->id() << std::endl; //std::cout << "\tmatch from feature point " << pl_match->feature_from_id_ << std::endl; //std::cout << "\tmatch to feature point " << pl_match->feature_to_id_ << std::endl; //std::cout << "\tmatch from landmark point " << pl_match->landmark_from_id_ << std::endl; //std::cout << "\tmatch to landmark point " << pl_match->landmark_to_id_ << std::endl; - // Constraints for all feature points + // Factors for all feature points int lmk_point_id = pl_match->landmark_from_id_; for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++) @@ -665,47 +665,47 @@ void ProcessorPolyline::establishConstraints() // First not defined point if (ftr_point_id == 0 && !pl_ftr->isFirstDefined()) - // first point to line constraint + // first point to line factor { //std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl; - assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly."); + assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a factor for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly."); - // emplace constraint - emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id)); - N_constraints++; - //std::cout << "constraint added" << std::endl; + // emplace factor + emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id)); + N_factors++; + //std::cout << "factor added" << std::endl; } // Last not defined point else if (ftr_point_id == pl_ftr->getNPoints()-1 && !pl_ftr->isLastDefined()) - // last point to line constraint + // last point to line factor { //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl; - assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly."); + assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a factor for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly."); - // emplace constraint - emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id)); - N_constraints++; - //std::cout << "constraint added" << std::endl; + // emplace factor + emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id)); + N_factors++; + //std::cout << "factor added" << std::endl; } // Defined point else - // point to point constraint + // point to point factor { //std::cout << "point-point: landmark point " << lmk_point_id << std::endl; //std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl; //std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl; //std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl; - emplaceConstraintPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id); - //std::cout << "constraint added" << std::endl; + emplaceFactorPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id); + //std::cout << "factor added" << std::endl; } if (ftr_point_id < pl_ftr->getNPoints()-1) lmk_point_id=pl_lmk->getNextValidId(lmk_point_id); } } - //std::cout << N_constraints << " constraints established, trying to close and classify " << modified_lmks.size() << std::endl; + //std::cout << N_factors << " factors established, trying to close and classify " << modified_lmks.size() << std::endl; // Try to close & classify modified landmarks for (auto lmk_ptr : modified_lmks) @@ -721,15 +721,15 @@ void ProcessorPolyline::establishConstraints() } // Outlier rejection - ConstraintBaseList new_ctr_list; - last_ptr_->getConstraintList(new_ctr_list); - for (auto ctr : new_ctr_list) - rejectOutlier(ctr); + FactorBasePtrList new_fac_list; + last_ptr_->getFactorList(new_fac_list); + for (auto fac : new_fac_list) + rejectOutlier(fac); - //std::cout << "ProcessorPolyline::establishConstraints: done\n"; + //std::cout << "ProcessorPolyline::establishFactors: done\n"; } -void ProcessorPolyline::classifyPolylineLandmarks(LandmarkBaseList& _lmk_list) +void ProcessorPolyline::classifyPolylineLandmarks(LandmarkBasePtrList& _lmk_list) { //std::cout << "ProcessorPolyline::classifyPolylineLandmarks: " << _lmk_list.size() << std::endl; @@ -795,23 +795,23 @@ LandmarkMatchPolyline2DPtr ProcessorPolyline::tryMatch(const Eigen::MatrixXs& _l return lmk_match; } -bool ProcessorPolyline::rejectOutlier(ConstraintBasePtr& ctr_ptr) +bool ProcessorPolyline::rejectOutlier(FactorBasePtr& fac_ptr) { - // Cast Point2Line/Point2Point constraint - auto ctr_p2l_ptr = std::dynamic_pointer_cast<ConstraintPointToLine2D>(ctr_ptr); - auto ctr_p2p_ptr = std::dynamic_pointer_cast<ConstraintPoint2D>(ctr_ptr); + // Cast Point2Line/Point2Point factor + auto fac_p2l_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr); + auto fac_p2p_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr); // Cast feature and landmark - auto pl_ftr_ptr = std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()); - auto pl_lmk_ptr = std::static_pointer_cast<LandmarkPolyline2D>(ctr_ptr->getLandmarkOtherPtr()); + auto pl_ftr_ptr = std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()); + auto pl_lmk_ptr = std::static_pointer_cast<LandmarkPolyline2D>(fac_ptr->getLandmarkOther()); // copy states - Eigen::VectorXs frameP(pl_ftr_ptr->getCapturePtr()->getFramePtr()->getPPtr()->getState()); - Eigen::VectorXs frameO(pl_ftr_ptr->getCapturePtr()->getFramePtr()->getOPtr()->getState()); - Eigen::VectorXs lmkOriginP(pl_lmk_ptr->getPPtr()->getState()); - Eigen::VectorXs lmkOriginO(pl_lmk_ptr->getOPtr()->getState()); - Eigen::VectorXs lmkPoint(pl_lmk_ptr->getPointVector((ctr_p2l_ptr ? ctr_p2l_ptr->getLandmarkPointId() : ctr_p2p_ptr->getLandmarkPointId()))); - Eigen::VectorXs lmkPointAux((ctr_p2l_ptr ? pl_lmk_ptr->getPointVector(ctr_p2l_ptr->getLandmarkPointAuxId()) : Eigen::VectorXs::Zero(1))); + Eigen::VectorXs frameP(pl_ftr_ptr->getCapture()->getFrame()->getP()->getState()); + Eigen::VectorXs frameO(pl_ftr_ptr->getCapture()->getFrame()->getO()->getState()); + Eigen::VectorXs lmkOriginP(pl_lmk_ptr->getP()->getState()); + Eigen::VectorXs lmkOriginO(pl_lmk_ptr->getO()->getState()); + Eigen::VectorXs lmkPoint(pl_lmk_ptr->getPointVector((fac_p2l_ptr ? fac_p2l_ptr->getLandmarkPointId() : fac_p2p_ptr->getLandmarkPointId()))); + Eigen::VectorXs lmkPointAux((fac_p2l_ptr ? pl_lmk_ptr->getPointVector(fac_p2l_ptr->getLandmarkPointAuxId()) : Eigen::VectorXs::Zero(1))); // create Scalar* array of a copy of the state Scalar* parameters[6] = {frameP.data(), @@ -822,16 +822,16 @@ bool ProcessorPolyline::rejectOutlier(ConstraintBasePtr& ctr_ptr) lmkPointAux.data()}; // last wont be used in p2p evaluate() // create residuals pointer - Eigen::VectorXs residuals(ctr_p2l_ptr ? 1 : 2); + Eigen::VectorXs residuals(fac_p2l_ptr ? 1 : 2); - // evaluate the constraint in this state (without jacobians, no need) - ctr_ptr->evaluate(parameters, residuals.data(), nullptr); + // evaluate the factor in this state (without jacobians, no need) + fac_ptr->evaluate(parameters, residuals.data(), nullptr); // discard if residual too high evaluated at the current estimation if (residuals.norm() > 1e3) { - WOLF_WARN((ctr_p2l_ptr ? "Discarding Point 2 Line Polyline Constraint, considered OUTLIER" : "Discarding Point 2 Point Polyline Constraint, considered OUTLIER")); - ctr_ptr->remove(); + WOLF_WARN((fac_p2l_ptr ? "Discarding Point 2 Line Polyline Factor, considered OUTLIER" : "Discarding Point 2 Point Polyline Factor, considered OUTLIER")); + fac_ptr->remove(); return true; } return false; @@ -843,15 +843,15 @@ bool ProcessorPolyline::rejectOutlier(ConstraintBasePtr& ctr_ptr) void ProcessorPolyline::setKeyFrame(CaptureBasePtr _capture_ptr) { - assert(_capture_ptr != nullptr && _capture_ptr->getFramePtr() != nullptr && "ProcessorTracker::setKeyFrame: null capture or capture without frame"); - if (!_capture_ptr->getFramePtr()->isKey()) + assert(_capture_ptr != nullptr && _capture_ptr->getFrame() != nullptr && "ProcessorTracker::setKeyFrame: null capture or capture without frame"); + if (!_capture_ptr->getFrame()->isKey()) { // Set key - _capture_ptr->getFramePtr()->setKey(); + _capture_ptr->getFrame()->setKey(); // Set state to the keyframe - _capture_ptr->getFramePtr()->setState(getProblem()->getState(_capture_ptr->getTimeStamp())); - // Call the new keyframe callback in order to let the other processors to establish their constraints - getProblem()->keyFrameCallback(_capture_ptr->getFramePtr(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_->time_tolerance); + _capture_ptr->getFrame()->setState(getProblem()->getState(_capture_ptr->getTimeStamp())); + // Call the new keyframe callback in order to let the other processors to establish their factors + getProblem()->keyFrameCallback(_capture_ptr->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_->time_tolerance); } } @@ -867,11 +867,11 @@ void ProcessorPolyline::computeTransformations(const TimeStamp& _ts) 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 (getSensorPtr()->isExtrinsicDynamic() || !getSensorPtr()->getPPtr()->isFixed() - || !getSensorPtr()->getOPtr()->isFixed() || !extrinsics_transformation_computed_) + if (getSensor()->isExtrinsicDynamic() || !getSensor()->getP()->isFixed() + || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_) { - t_robot_sensor_ = getSensorPtr()->getPPtr()->getState(); - R_robot_sensor_ = Eigen::Rotation2Ds(getSensorPtr()->getOPtr()->getState()(0)).matrix(); + t_robot_sensor_ = getSensor()->getP()->getState(); + R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix(); extrinsics_transformation_computed_ = true; } diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp index 9218dba63738e88a65e2fcf3572ed2a1b5757237..9a90676b67408ba352704ecb89ce0e06fc0b7838 100644 --- a/src/processor/processor_tracker_feature_corner.cpp +++ b/src/processor/processor_tracker_feature_corner.cpp @@ -107,14 +107,14 @@ bool ProcessorTrackerFeatureCorner::voteForKeyFrame() return incoming_ptr_->getFeatureList().size() < params_tracker_feature_corner_->n_corners_th; } -unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out) +unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out) { // in corners_last_ remain all not tracked corners - _features_incoming_out = std::move(corners_last_); - if (_max_features != -1 && _features_incoming_out.size() > _max_features) - _features_incoming_out.resize(_max_features); + _features_last_out = std::move(corners_last_); + if (_max_features != -1 && _features_last_out.size() > _max_features) + _features_last_out.resize(_max_features); - return _features_incoming_out.size(); + return _features_last_out.size(); } FactorBasePtr ProcessorTrackerFeatureCorner::createFactor(FeatureBasePtr _feature_ptr, diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp index 87655896dd2cc378378dd7a76b6acacf3579dfa3..aea025a2daea5962cf98ed8e0afea2330a8759f8 100644 --- a/src/processor/processor_tracker_feature_dummy.cpp +++ b/src/processor/processor_tracker_feature_dummy.cpp @@ -49,7 +49,7 @@ bool ProcessorTrackerFeatureDummy::voteForKeyFrame() return vote; } -unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out) +unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out) { unsigned int max_features = _max_features; diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp index e6188cc896ce5406480c90478aa617b70147dc11..e8092dafb1454e53bd6ab3eb22345c76e468026a 100644 --- a/src/processor/processor_tracker_feature_image.cpp +++ b/src/processor/processor_tracker_feature_image.cpp @@ -244,7 +244,7 @@ bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _ori } } -unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out) +unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out) { cv::Rect roi; KeyPointVector new_keypoints; diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp index b90877de97f2289987ee405096c1830340e1dd9c..9c2fa6ac19d996cc682613de624af5bdae655846 100644 --- a/src/processor/processor_tracker_feature_polyline_2D.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -23,8 +23,8 @@ ProcessorTrackerFeaturePolyline2D::~ProcessorTrackerFeaturePolyline2D() { } -unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseList& _features_last_in, - FeatureBaseList& _features_incoming_out, +unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBasePtrList& _features_last_in, + FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) { WOLF_DEBUG("PTFP ", getName(), "::trackFeatures ", _features_last_in.size()); @@ -174,7 +174,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const unsigned int& _ T_sensor_world_last.topRightCorner(2,1) = t_sensor_world_last_; // For each new feature: Either create a landmark or match with an existent landmark - LandmarkBaseList new_landmarks; + LandmarkBasePtrList new_landmarks; for (auto ftr_it = std::prev(last_ptr_->getFeatureList().end(),n); ftr_it != last_ptr_->getFeatureList().end(); ftr_it++) @@ -184,7 +184,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const unsigned int& _ WOLF_DEBUG("Processing feature: ", pl_ftr->id()); // Check matching with all existing polyline landmarks - for (auto lmk : getProblem()->getMapPtr()->getLandmarkList()) + for (auto lmk : getProblem()->getMap()->getLandmarkList()) // Check for polylines landmarks that hasn't been just created if (lmk->getType() == "POLYLINE 2D" && std::find(new_landmarks.begin(), new_landmarks.end(), lmk) == new_landmarks.end()) // Store all matches consistent with T_sensor_world_last in best_lmk_matches sorted by difference from T_sensor_world_last @@ -225,7 +225,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const unsigned int& _ // store landmark candidates to be merged if (best_lmk_matches.size() > 1) { - LandmarkPolyline2DList merge_candidates; + LandmarkPolyline2DPtrList merge_candidates; for (auto lmk_match_pair_it : best_lmk_matches) merge_candidates.push_back(std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_pair_it.second->landmark_ptr_)); merge_candidates_list_.push_back(merge_candidates); @@ -261,38 +261,26 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const unsigned int& _ return n; } -unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const unsigned int& _max_new_features, - FeatureBaseList& _features_last_out) +unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const int& _max_new_features, + FeatureBasePtrList& _features_last_out) { WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)"); - int prev_size = _features_last_out.size(); - if (_features_last_out == new_features_last_) - { - _features_last_out.splice(_features_last_out.end(), all_features_last_); + _features_last_out = std::move(all_features_last_); + if (_max_new_features != -1 && _features_last_out.size() > _max_new_features) + _features_last_out.resize(_max_new_features); - // 0 means unlimited new features - if (_max_new_features != 0 && _features_last_out.size() > prev_size + _max_new_features) - { - _features_last_out.resize(prev_size + _max_new_features); - WOLF_DEBUG(_max_new_features, " were provided (max new features)"); - return _max_new_features; - } - WOLF_DEBUG(_features_last_out.size()-prev_size, " were provided"); - return _features_last_out.size()-prev_size; - } - else - std::runtime_error("asking for new features not from last"); + WOLF_DEBUG(_features_last_out.size(), " were provided"); - return 0; + return _features_last_out.size(); } -void ProcessorTrackerFeaturePolyline2D::establishConstraints() +void ProcessorTrackerFeaturePolyline2D::establishFactors() { - WOLF_DEBUG("PTFP ", getName(), "::establishConstraints: "); - unsigned int N_constraints = 0; + WOLF_DEBUG("PTFP ", getName(), "::establishFactors: "); + unsigned int N_factors = 0; - // Create a constraint for each match in last features + // Create a factor for each match in last features auto ftr_it = last_ptr_->getFeatureList().begin(); while (ftr_it != last_ptr_->getFeatureList().end()) { @@ -343,22 +331,22 @@ void ProcessorTrackerFeaturePolyline2D::establishConstraints() modified_lmks_.push_back(pl_lmk); // ESTABLISH CONSTRAINTS - WOLF_DEBUG("\tEstablishing constraints.."); + WOLF_DEBUG("\tEstablishing factors.."); // checks assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly"); assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly"); assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly"); assert(lmk_match->check() && "Landmark didn't grow properly"); - std::cout << "Establishing constraint between" << std::endl; - std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapturePtr() ? pl_ftr->getCapturePtr()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl; + std::cout << "Establishing factor between" << std::endl; + std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapture() ? pl_ftr->getCapture()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl; std::cout << "\tlandmark " << pl_lmk->id() << std::endl; std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl; std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl; std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl; std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl; - // Constraints for all feature points + // Factors for all feature points int lmk_point_id = lmk_match->landmark_from_id_; for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++) { @@ -367,41 +355,41 @@ void ProcessorTrackerFeaturePolyline2D::establishConstraints() // First not defined point if (ftr_point_id == 0 && !pl_ftr->isFirstDefined()) - // first point to line constraint + // first point to line factor { std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl; - assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly."); + assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a factor for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly."); - // emplace constraint - emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id)); - N_constraints++; - std::cout << "constraint added" << std::endl; + // emplace factor + emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id)); + N_factors++; + std::cout << "factor added" << std::endl; } // Last not defined point else if (ftr_point_id == pl_ftr->getNPoints()-1 && !pl_ftr->isLastDefined()) - // last point to line constraint + // last point to line factor { std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getPrevValidId(lmk_point_id) << std::endl; - assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly."); + assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a factor for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly."); - // emplace constraint - emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id)); - N_constraints++; - std::cout << "constraint added" << std::endl; + // emplace factor + emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id)); + N_factors++; + std::cout << "factor added" << std::endl; } // Defined point else - // point to point constraint + // point to point factor { std::cout << "point-point: landmark point " << lmk_point_id << std::endl; //std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl; //std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl; //std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl; - emplaceConstraintPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id); - N_constraints++; - std::cout << "constraint added" << std::endl; + emplaceFactorPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id); + N_factors++; + std::cout << "factor added" << std::endl; } // Next lmk point @@ -411,10 +399,10 @@ void ProcessorTrackerFeaturePolyline2D::establishConstraints() // next ftr ftr_it++; } - std::cout << N_constraints << " constraints established" << std::endl; + std::cout << N_factors << " factors established" << std::endl; } -void ProcessorTrackerFeaturePolyline2D::emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature, +void ProcessorTrackerFeaturePolyline2D::emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature, LandmarkPolyline2DPtr _polyline_landmark, const int& _ftr_point_id, const int& _lmk_point_id, @@ -423,24 +411,24 @@ void ProcessorTrackerFeaturePolyline2D::emplaceConstraintPointToLine(FeaturePoly assert(_polyline_landmark->isValidId(_lmk_point_id) && _polyline_landmark->isValidId(_lmk_prev_point_id)); // CREATE CONSTRAINT -------------------- - ConstraintBasePtr new_ctr = std::make_shared<ConstraintPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id); + FactorBasePtr new_fac = std::make_shared<FactorPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id); // ADD CONSTRAINT -------------------- - _polyline_feature->addConstraint(new_ctr); - _polyline_landmark->addConstrainedBy(new_ctr); + _polyline_feature->addFactor(new_fac); + _polyline_landmark->addConstrainedBy(new_fac); } -void ProcessorTrackerFeaturePolyline2D::emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature, +void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature, LandmarkPolyline2DPtr _polyline_landmark, const int& _ftr_point_id, const int& _lmk_point_id) { // CREATE CONSTRAINT -------------------- - ConstraintBasePtr new_ctr = std::make_shared<ConstraintPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id); + FactorBasePtr new_fac = std::make_shared<FactorPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id); // ADD CONSTRAINT -------------------- - _polyline_feature->addConstraint(new_ctr); - _polyline_landmark->addConstrainedBy(new_ctr); + _polyline_feature->addFactor(new_fac); + _polyline_landmark->addConstrainedBy(new_fac); } LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::createLandmark(FeatureBasePtr _feature_ptr) @@ -696,7 +684,7 @@ void ProcessorTrackerFeaturePolyline2D::preProcess() // TODO: sort corners by bearing std::list<laserscanutils::Polyline> polylines; - line_finder_.findPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_)->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensorPtr()))->getScanParams(), polylines); + line_finder_.findPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_)->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines); WOLF_DEBUG("PTFP ", getName(), " Polylines extracted: ", polylines.size()); for (auto&& pl : polylines) @@ -739,7 +727,7 @@ void ProcessorTrackerFeaturePolyline2D::postProcess() while (!merge_candidates_list_.empty()) { // load next list of candidates - LandmarkPolyline2DList merge_candidates = std::move(merge_candidates_list_.front()); + LandmarkPolyline2DPtrList merge_candidates = std::move(merge_candidates_list_.front()); merge_candidates_list_.pop_front(); // change already merged lmks with the corresponding remaining lmk @@ -817,7 +805,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline // WOLF_DEBUG("last incomming pose ", T2pose2D(_T_last_incoming_prior).transpose()); // WOLF_DEBUG("incomming last pose ", T2pose2D(_T_last_incoming_prior.inverse()).transpose()); // // compute best rigid transformations -// MatchPolyline2DList matches = computeBestRigidTrans(_ftr_I->getPoints(), // <--feature incoming points +// MatchPolyline2DPtrList matches = computeBestRigidTrans(_ftr_I->getPoints(), // <--feature incoming points // _ftr_I->isFirstDefined(), // _ftr_I->isLastDefined(), // false, // <--feature is not closed for sure @@ -920,7 +908,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli } // // compute best rigid transformation matching -// MatchPolyline2DList matches = computeBestRigidTrans(_lmk_ptr->computePointsGlobal(), // <--landmark points in local coordinates +// MatchPolyline2DPtrList matches = computeBestRigidTrans(_lmk_ptr->computePointsGlobal(), // <--landmark points in local coordinates // _lmk_ptr->isFirstDefined(), // _lmk_ptr->isLastDefined(), // _lmk_ptr->isClosed(), @@ -1079,13 +1067,13 @@ void ProcessorTrackerFeaturePolyline2D::computeTransformations() Eigen::Matrix2s R_world_robot_last = Eigen::Rotation2Ds(vehicle_pose_last(2)).matrix(); // robot_sensor (to be computed once if extrinsics are fixed and not dynamic) - if (getSensorPtr()->isExtrinsicDynamic() || - !getSensorPtr()->getPPtr()->isFixed() || - !getSensorPtr()->getOPtr()->isFixed() || + if (getSensor()->isExtrinsicDynamic() || + !getSensor()->getP()->isFixed() || + !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_) { - t_robot_sensor_ = getSensorPtr()->getPPtr()->getState(); - R_robot_sensor_ = Eigen::Rotation2Ds(getSensorPtr()->getOPtr()->getState()(0)).matrix(); + t_robot_sensor_ = getSensor()->getP()->getState(); + R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix(); extrinsics_transformation_computed_ = true; } diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp index fdcb3b1ab6f98ebc3a3cfef57c70abec963d78b7..622f733132daf2ffdf2bc00ce9f21e016da050af 100644 --- a/src/processor/processor_tracker_feature_trifocal.cpp +++ b/src/processor/processor_tracker_feature_trifocal.cpp @@ -81,7 +81,7 @@ bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, con } -unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out) +unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out) { // // DEBUG ===================================== // clock_t debug_tStart; @@ -267,7 +267,7 @@ void ProcessorTrackerFeatureTrifocal::resetDerived() void ProcessorTrackerFeatureTrifocal::preProcess() { // //DEBUG -// WOLF_TRACE("-------- Image ", getIncomingPtr()->id(), " -- t = ", getIncomingPtr()->getTimeStamp(), " s ----------"); +// WOLF_TRACE("-------- Image ", getIncoming()->id(), " -- t = ", getIncoming()->getTimeStamp(), " s ----------"); if (!initialized_) { @@ -395,8 +395,8 @@ void ProcessorTrackerFeatureTrifocal::establishFactors() // get the middle feature of the track using the average of the time stamps FeatureBasePtr ftr_mid = nullptr; - TimeStamp ts_first = ftr_first->getCapturePtr()->getTimeStamp(); - TimeStamp ts_last = ftr_last->getCapturePtr()->getTimeStamp(); + TimeStamp ts_first = ftr_first->getCapture()->getTimeStamp(); + TimeStamp ts_last = ftr_last->getCapture()->getTimeStamp(); Scalar Dt2 = (ts_last - ts_first) / 2.0; TimeStamp ts_ave = ts_first + Dt2; @@ -404,9 +404,9 @@ void ProcessorTrackerFeatureTrifocal::establishFactors() auto track = track_matrix_.track(trk_id); for (auto ftr_it = track.begin() ; ftr_it != track.end() ; ftr_it ++) { - if ( ftr_it->second->getCapturePtr() != nullptr ) // have capture + if ( ftr_it->second->getCapture() != nullptr ) // have capture { - if ( auto kf_mid = ftr_it->second->getCapturePtr()->getFramePtr() ) // have frame + if ( auto kf_mid = ftr_it->second->getCapture()->getFrame() ) // have frame { TimeStamp ts_mid = kf_mid->getTimeStamp(); @@ -424,7 +424,7 @@ void ProcessorTrackerFeatureTrifocal::establishFactors() // Several safety checks assert(ftr_mid != nullptr && "Middle feature is nullptr!"); - assert(ftr_mid->getCapturePtr()->getFramePtr() != nullptr && "Middle feature's frame is nullptr!"); + assert(ftr_mid->getCapture()->getFrame() != nullptr && "Middle feature's frame is nullptr!"); assert(ftr_mid != ftr_first && "First and middle features are the same!"); assert(ftr_mid != ftr_last && "Last and middle features are the same!"); @@ -437,7 +437,7 @@ void ProcessorTrackerFeatureTrifocal::establishFactors() FAC_ACTIVE); // link to wolf tree - fac -> setFeaturePtr (ftr_last); + fac -> setFeature (ftr_last); ftr_first -> addConstrainedBy(fac); ftr_mid -> addConstrainedBy(fac); ftr_last -> addFactor (fac); diff --git a/src/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp index c3cac8003660aa14149fe1fa4ba2e01927772312..24a59a5f535873ea0a7f7a8d8f8545fab9238092 100644 --- a/src/processor/processor_tracker_landmark_corner.cpp +++ b/src/processor/processor_tracker_landmark_corner.cpp @@ -375,14 +375,14 @@ LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBasePtr _f _feature_ptr->getMeasurement()(3)); } -unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out) +unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out) { // in corners_last_ remain all not tracked corners, already computed in preprocess() - _features_incoming_out = std::move(corners_last_); - if (_max_features != -1 && _features_incoming_out.size() > _max_features) - _features_incoming_out.resize(_max_features); + _features_last_out = std::move(corners_last_); + if (_max_features != -1 && _features_last_out.size() > _max_features) + _features_last_out.resize(_max_features); - return _features_incoming_out.size(); + return _features_last_out.size(); } FactorBasePtr ProcessorTrackerLandmarkCorner::createFactor(FeatureBasePtr _feature_ptr, diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp index 9977c26b643689c92bbd2183235cf4ca94d2bf90..967319385bd8e14adbe5e1e665d2a8232a308c2f 100644 --- a/src/processor/processor_tracker_landmark_image.cpp +++ b/src/processor/processor_tracker_landmark_image.cpp @@ -225,7 +225,7 @@ bool ProcessorTrackerLandmarkImage::voteForKeyFrame() // return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe; } -unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out) +unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out) { cv::Rect roi; KeyPointVector new_keypoints; diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp index 5412fd67251498cf3859d035366658fa85f5ef80..09c619c78c1a1f271cae9da86ba19ccd4cb9746c 100644 --- a/src/sensor/sensor_gnss.cpp +++ b/src/sensor/sensor_gnss.cpp @@ -129,23 +129,23 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con if (getProblem()->getDim() == 2) { // compute antena vector (from 1 to 2) in MAP - Eigen::Vector2s t_MAP_antenaENU = _pose_MAP_frameENU.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frameENU(2)) * getPPtr()->getState().head<2>(); - Eigen::Vector2s t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame2(2)) * getPPtr()->getState().head<2>(); + Eigen::Vector2s t_MAP_antenaENU = _pose_MAP_frameENU.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frameENU(2)) * getP()->getState().head<2>(); + Eigen::Vector2s t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame2(2)) * getP()->getState().head<2>(); Eigen::Vector2s v_MAP = t_MAP_antena2 - t_MAP_antenaENU; // initialize yaw Scalar theta_ENU = atan2(v_ENU(1),v_ENU(0)); Scalar theta_MAP = atan2(v_MAP(1),v_MAP(0)); Scalar yaw = theta_ENU-theta_MAP; - this->getEnuMapYawStatePtr()->setState(Eigen::Vector1s(yaw)); + this->getEnuMapYawState()->setState(Eigen::Vector1s(yaw)); // initialize translation Eigen::Vector3s t_ENU_MAP(Eigen::Vector3s::Zero()); - Eigen::Matrix2s R_ENU_MAP = computeREnuMap(getEnuMapRollStatePtr()->getState()(0), - getEnuMapPitchStatePtr()->getState()(0), - getEnuMapYawStatePtr()->getState()(0)).topLeftCorner<2,2>(); + Eigen::Matrix2s R_ENU_MAP = computeREnuMap(getEnuMapRollState()->getState()(0), + getEnuMapPitchState()->getState()(0), + getEnuMapYawState()->getState()(0)).topLeftCorner<2,2>(); t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU; - this->getEnuMapTranslationStatePtr()->setState(t_ENU_MAP); + this->getEnuMapTranslationState()->setState(t_ENU_MAP); //std::cout << "-----------------------------------------\n"; //std::cout << "t_ECEF_antenaENU: " << _t_ECEF_antenaENU.transpose() << std::endl; diff --git a/src/track_matrix.cpp b/src/track_matrix.cpp index 2bab9abb565eb64b009eddb8a147c757b52440bc..51b06c703e716e055f2fff03429da6873ad84b4f 100644 --- a/src/track_matrix.cpp +++ b/src/track_matrix.cpp @@ -225,7 +225,7 @@ Track TrackMatrix::trackAtKeyframes(size_t _track_id) bool TrackMatrix::markKeyframe(CaptureBasePtr _capture) { - if (_capture->getFramePtr() && _capture->getFramePtr()->isKey()) + if (_capture->getFrame() && _capture->getFrame()->isKey()) { auto snap = snapshot(_capture); @@ -236,7 +236,7 @@ bool TrackMatrix::markKeyframe(CaptureBasePtr _capture) { auto track_id = pair_trkid_ftr.first; auto ftr = pair_trkid_ftr.second; - auto ts = _capture->getFramePtr()->getTimeStamp(); + auto ts = _capture->getFrame()->getTimeStamp(); tracks_kf_[track_id][ts] = ftr; } return true; diff --git a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp index 46f01d4b96546507e8aeb4ff4e0d76da9240b68f..db78505d9a69330770ab7fb693e572f01fb96765 100644 --- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp +++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp @@ -51,7 +51,6 @@ static ProcessorParamsBasePtr createProcessorParamsTrackerFeatureTrifocal(const params->n_cells_h = algorithm["grid horiz cells"] .as<int>(); params->n_cells_v = algorithm["grid vert cells"] .as<int>(); params->min_response_new_feature = algorithm["min response new features"] .as<int>(); - params->max_euclidean_distance = algorithm["max euclidean distance"] .as<Scalar>(); params->min_track_length_for_factor = algorithm["min track length for factor"].as<int>(); YAML::Node noise = config["noise"]; diff --git a/test/gtest_factor_gnss_fix_2D.cpp b/test/gtest_factor_gnss_fix_2D.cpp index 3b60dba80e48528337b7197285527758850040cc..a01314ae7b3a7b554178816db74b9a1dcfb20abf 100644 --- a/test/gtest_factor_gnss_fix_2D.cpp +++ b/test/gtest_factor_gnss_fix_2D.cpp @@ -27,29 +27,29 @@ void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr, const Vector1s& o_map_base) { // ENU-MAP - gnss_sensor_ptr->getEnuMapRollStatePtr()->setState(Eigen::Vector1s::Zero()); - gnss_sensor_ptr->getEnuMapPitchStatePtr()->setState(Eigen::Vector1s::Zero()); - gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map); - gnss_sensor_ptr->getEnuMapTranslationStatePtr()->setState(t_enu_map); + gnss_sensor_ptr->getEnuMapRollState()->setState(Eigen::Vector1s::Zero()); + gnss_sensor_ptr->getEnuMapPitchState()->setState(Eigen::Vector1s::Zero()); + gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map); + gnss_sensor_ptr->getEnuMapTranslationState()->setState(t_enu_map); // Antena - gnss_sensor_ptr->getPPtr()->setState(t_base_antena); + gnss_sensor_ptr->getP()->setState(t_base_antena); // Frame - frame_ptr->getPPtr()->setState(t_map_base.head(2)); - frame_ptr->getOPtr()->setState(o_map_base); + frame_ptr->getP()->setState(t_map_base.head(2)); + frame_ptr->getO()->setState(o_map_base); } void fixAllStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr) { // ENU-MAP - gnss_sensor_ptr->getEnuMapRollStatePtr()->fix(); - gnss_sensor_ptr->getEnuMapPitchStatePtr()->fix(); - gnss_sensor_ptr->getEnuMapYawStatePtr()->fix(); - gnss_sensor_ptr->getEnuMapTranslationStatePtr()->fix(); + gnss_sensor_ptr->getEnuMapRollState()->fix(); + gnss_sensor_ptr->getEnuMapPitchState()->fix(); + gnss_sensor_ptr->getEnuMapYawState()->fix(); + gnss_sensor_ptr->getEnuMapTranslationState()->fix(); // Antena - gnss_sensor_ptr->getPPtr()->fix(); + gnss_sensor_ptr->getP()->fix(); // Frame - frame_ptr->getPPtr()->fix(); - frame_ptr->getOPtr()->fix(); + frame_ptr->getP()->fix(); + frame_ptr->getO()->fix(); } void computeParamSizes(const CeresManagerPtr& ceres_mgr_ptr, int& num_params_reduced, int& num_param_blocks_reduced ) @@ -100,10 +100,10 @@ TEST(FactorGnssFix2DTest, configure_tree) ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100; // Configure sensor and processor - gnss_sensor_ptr->getEnuMapTranslationStatePtr()->setState(t_enu_map); - gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map); - gnss_sensor_ptr->getEnuMapRollStatePtr()->fix(); - gnss_sensor_ptr->getEnuMapPitchStatePtr()->fix(); + gnss_sensor_ptr->getEnuMapTranslationState()->setState(t_enu_map); + gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map); + gnss_sensor_ptr->getEnuMapRollState()->fix(); + gnss_sensor_ptr->getEnuMapPitchState()->fix(); gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu)); ProcessorParamsGnssFixPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssFix>(); @@ -138,7 +138,7 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_position) // --------------------------- fixing/unfixing states fixAllStates(gnss_sensor_ptr, frame_ptr); - frame_ptr->getPPtr()->unfix(); + frame_ptr->getP()->unfix(); // --------------------------- distort: map base position Vector3s frm_dist = frame_ptr->getState(); @@ -184,12 +184,12 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_orientation) // --------------------------- fixing/unfixing states fixAllStates(gnss_sensor_ptr, frame_ptr); - frame_ptr->getOPtr()->unfix(); + frame_ptr->getO()->unfix(); // --------------------------- distort: map base orientation - Vector1s frm_dist = frame_ptr->getOPtr()->getState(); + Vector1s frm_dist = frame_ptr->getO()->getState(); frm_dist(0) += 0.18; - frame_ptr->getOPtr()->setState(frm_dist); + frame_ptr->getO()->setState(frm_dist); // --------------------------- update solver ceres_mgr_ptr->update(); @@ -211,7 +211,7 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_orientation) ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution - ASSERT_MATRIX_APPROX(frame_ptr->getOPtr()->getState(), o_map_base, 1e-6); + ASSERT_MATRIX_APPROX(frame_ptr->getO()->getState(), o_map_base, 1e-6); } /* @@ -227,12 +227,12 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_yaw) // --------------------------- fixing/unfixing states fixAllStates(gnss_sensor_ptr, frame_ptr); - gnss_sensor_ptr->getEnuMapYawStatePtr()->unfix(); + gnss_sensor_ptr->getEnuMapYawState()->unfix(); // --------------------------- distort: yaw enu_map - Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYawStatePtr()->getState(); + Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYawState()->getState(); o_enu_map_dist(0) += 0.18; - gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map_dist); + gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map_dist); // --------------------------- update solver ceres_mgr_ptr->update(); @@ -254,7 +254,7 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_yaw) ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution - ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYawStatePtr()->getState(), o_enu_map, 1e-6); + ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYawState()->getState(), o_enu_map, 1e-6); } /* @@ -270,13 +270,13 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_position) // --------------------------- fixing/unfixing states fixAllStates(gnss_sensor_ptr, frame_ptr); - gnss_sensor_ptr->getEnuMapTranslationStatePtr()->unfix();// enu-map yaw translation + gnss_sensor_ptr->getEnuMapTranslationState()->unfix();// enu-map yaw translation // --------------------------- distort: position enu_map - Vector3s t_enu_map_dist = gnss_sensor_ptr->getEnuMapTranslationStatePtr()->getState(); + Vector3s t_enu_map_dist = gnss_sensor_ptr->getEnuMapTranslationState()->getState(); t_enu_map_dist(0) += 0.86; t_enu_map_dist(1) += -0.34; - gnss_sensor_ptr->getEnuMapTranslationStatePtr()->setState(t_enu_map_dist); + gnss_sensor_ptr->getEnuMapTranslationState()->setState(t_enu_map_dist); // --------------------------- update solver ceres_mgr_ptr->update(); @@ -298,7 +298,7 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_position) ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution - ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslationStatePtr()->getState().head(2), t_enu_map.head(2), 1e-6); + ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslationState()->getState().head(2), t_enu_map.head(2), 1e-6); } /* @@ -314,13 +314,13 @@ TEST(FactorGnssFix2DTest, gnss_1_base_antena) // --------------------------- fixing/unfixing states fixAllStates(gnss_sensor_ptr, frame_ptr); - gnss_sensor_ptr->getPPtr()->unfix(); + gnss_sensor_ptr->getP()->unfix(); // --------------------------- distort: base_antena - Vector3s base_antena_dist = gnss_sensor_ptr->getPPtr()->getState(); + Vector3s base_antena_dist = gnss_sensor_ptr->getP()->getState(); base_antena_dist(0) += 1.68; base_antena_dist(0) += -0.48; - gnss_sensor_ptr->getPPtr()->setState(base_antena_dist); + gnss_sensor_ptr->getP()->setState(base_antena_dist); // --------------------------- update solver ceres_mgr_ptr->update(); @@ -342,7 +342,7 @@ TEST(FactorGnssFix2DTest, gnss_1_base_antena) ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution - ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getPPtr()->getState().head(2), t_base_antena.head(2), 1e-6); + ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6); } int main(int argc, char **argv) diff --git a/test/gtest_factor_gnss_single_diff_2D.cpp b/test/gtest_factor_gnss_single_diff_2D.cpp index 55443ed3404c2f020d760856e53fe199e3721662..c0f0021f60a48be7c3e20b3ccad5d021161fa855 100644 --- a/test/gtest_factor_gnss_single_diff_2D.cpp +++ b/test/gtest_factor_gnss_single_diff_2D.cpp @@ -65,8 +65,8 @@ class FactorGnssSingleDiff2DTest : public testing::Test // gnss sensor gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("GNSS", "gnss", t_base_antena, std::make_shared<IntrinsicsBase>())); - gnss_sensor_ptr->getEnuMapRollStatePtr()->fix(); - gnss_sensor_ptr->getEnuMapPitchStatePtr()->fix(); + gnss_sensor_ptr->getEnuMapRollState()->fix(); + gnss_sensor_ptr->getEnuMapPitchState()->fix(); gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu)); std::cout << "gnss sensor installed" << std::endl; @@ -109,7 +109,7 @@ class FactorGnssSingleDiff2DTest : public testing::Test t_ecef_antena2 = R_ecef_enu * R_enu_map * (R_map_base2 * t_base_antena + t_map_base2) + t_ecef_enu; // Reset antena extrinsics - gnss_sensor_ptr->getPPtr()->setState(t_base_antena); + gnss_sensor_ptr->getP()->setState(t_base_antena); // Reset ENU_ECEF gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu)); @@ -136,25 +136,25 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position) gnss_sensor_ptr->process(cap_gnss_ptr); // fixing things - gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map); // enu-map yaw orientation - gnss_sensor_ptr->getEnuMapYawStatePtr()->fix(); - cap_gnss_ptr->getFramePtr()->getOPtr()->setState(o_map_base2); // frame orientation - cap_gnss_ptr->getFramePtr()->getOPtr()->fix(); + gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map); // enu-map yaw orientation + gnss_sensor_ptr->getEnuMapYawState()->fix(); + cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation + cap_gnss_ptr->getFrame()->getO()->fix(); std::cout << "frame1: " << prior_frame_ptr->getState().transpose() << std::endl; // distort frm position - Vector3s frm_dist = cap_gnss_ptr->getFramePtr()->getState(); + Vector3s frm_dist = cap_gnss_ptr->getFrame()->getState(); frm_dist(0) += 18; frm_dist(1) += -58; - cap_gnss_ptr->getFramePtr()->setState(frm_dist); + cap_gnss_ptr->getFrame()->setState(frm_dist); // solve for frm std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); std::cout << report << std::endl; - ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey()); - ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFramePtr()->getState().head(2), t_map_base2.head(2), 1e-6); + ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey()); + ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFrame()->getState().head(2), t_map_base2.head(2), 1e-6); } /* @@ -170,21 +170,21 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_orientation) gnss_sensor_ptr->process(cap_gnss_ptr); // fixing things - gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map); // enu-map yaw orientation - gnss_sensor_ptr->getEnuMapYawStatePtr()->fix(); - cap_gnss_ptr->getFramePtr()->getPPtr()->setState(t_map_base2.head(2)); // frame position - cap_gnss_ptr->getFramePtr()->getPPtr()->fix(); + gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map); // enu-map yaw orientation + gnss_sensor_ptr->getEnuMapYawState()->fix(); + cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position + cap_gnss_ptr->getFrame()->getP()->fix(); // distort frm position - Vector1s frm_dist = cap_gnss_ptr->getFramePtr()->getOPtr()->getState(); + Vector1s frm_dist = cap_gnss_ptr->getFrame()->getO()->getState(); frm_dist(0) += 1.8; - cap_gnss_ptr->getFramePtr()->getOPtr()->setState(frm_dist); + cap_gnss_ptr->getFrame()->getO()->setState(frm_dist); // solve for frm std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET); - ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey()); - ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFramePtr()->getOPtr()->getState(), o_map_base2, 1e-6); + ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey()); + ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFrame()->getO()->getState(), o_map_base2, 1e-6); } /* @@ -200,23 +200,23 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_enu_map_yaw) gnss_sensor_ptr->process(cap_gnss_ptr); // unfixing things - gnss_sensor_ptr->getEnuMapYawStatePtr()->unfix(); + gnss_sensor_ptr->getEnuMapYawState()->unfix(); // fixing things - cap_gnss_ptr->getFramePtr()->getPPtr()->setState(t_map_base2.head(2)); // frame position - cap_gnss_ptr->getFramePtr()->getPPtr()->fix(); - cap_gnss_ptr->getFramePtr()->getOPtr()->setState(o_map_base2); // frame orientation - cap_gnss_ptr->getFramePtr()->getOPtr()->fix(); + cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position + cap_gnss_ptr->getFrame()->getP()->fix(); + cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation + cap_gnss_ptr->getFrame()->getO()->fix(); // distort yaw enu_map - Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYawStatePtr()->getState(); + Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYawState()->getState(); o_enu_map_dist(0) += 1.8; - gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map_dist); + gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map_dist); // solve for frm std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET); - ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey()); - ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYawStatePtr()->getState(), o_enu_map, 1e-6); + ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey()); + ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYawState()->getState(), o_enu_map, 1e-6); } /* @@ -232,26 +232,26 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_base_antena) gnss_sensor_ptr->process(cap_gnss_ptr); // unfixing things - gnss_sensor_ptr->getPPtr()->unfix(); + gnss_sensor_ptr->getP()->unfix(); // fixing things - gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map); // enu-map yaw orientation - gnss_sensor_ptr->getEnuMapYawStatePtr()->fix(); - cap_gnss_ptr->getFramePtr()->getPPtr()->setState(t_map_base2.head(2)); // frame position - cap_gnss_ptr->getFramePtr()->getPPtr()->fix(); - cap_gnss_ptr->getFramePtr()->getOPtr()->setState(o_map_base2); // frame orientation - cap_gnss_ptr->getFramePtr()->getOPtr()->fix(); + gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map); // enu-map yaw orientation + gnss_sensor_ptr->getEnuMapYawState()->fix(); + cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position + cap_gnss_ptr->getFrame()->getP()->fix(); + cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation + cap_gnss_ptr->getFrame()->getO()->fix(); // distort base_antena - Vector3s base_antena_dist = gnss_sensor_ptr->getPPtr()->getState(); + Vector3s base_antena_dist = gnss_sensor_ptr->getP()->getState(); base_antena_dist(0) += 16.8; base_antena_dist(0) += -40.8; - gnss_sensor_ptr->getPPtr()->setState(base_antena_dist); + gnss_sensor_ptr->getP()->setState(base_antena_dist); // solve for frm std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET); - ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey()); - ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getPPtr()->getState().head(2), t_base_antena.head(2), 1e-6); + ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey()); + ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6); } int main(int argc, char **argv) diff --git a/test/gtest_landmark_polyline.cpp b/test/gtest_landmark_polyline.cpp index 2f01d06b0d6b882418d57e9e5f99389cc98ccddb..1ead19daedc9b85bb4ef797309b24778ed4bdefe 100644 --- a/test/gtest_landmark_polyline.cpp +++ b/test/gtest_landmark_polyline.cpp @@ -120,14 +120,14 @@ TEST_F(LandmarkPolylineTest, Classify) ASSERT_TRUE(lmk_ptr->isClosed()); // test points fixed and center unfixed - ASSERT_FALSE(lmk_ptr->getPPtr()->isFixed()); - ASSERT_FALSE(lmk_ptr->getOPtr()->isFixed()); + ASSERT_FALSE(lmk_ptr->getP()->isFixed()); + ASSERT_FALSE(lmk_ptr->getO()->isFixed()); for (auto st_pair : lmk_ptr->getPointStatePtrMap()) ASSERT_TRUE(st_pair.second->isFixed()); // test points are in the same place (compoping with object pose) - Matrix2s R = Rotation2D<Scalar>(lmk_ptr->getOPtr()->getState()(0)).matrix(); - VectorXs t = lmk_ptr->getPPtr()->getState(); + Matrix2s R = Rotation2D<Scalar>(lmk_ptr->getO()->getState()(0)).matrix(); + VectorXs t = lmk_ptr->getP()->getState(); for (unsigned int point_i = 0; point_i < 4; point_i++) ASSERT_MATRIX_APPROX(R*lmk_ptr->getPointVector(point_i)+t, points_groundtruth[i].col(point_i), 1e-9); }