diff --git a/include/apriltag/feature/feature_apriltag.h b/include/apriltag/feature/feature_apriltag.h index 27b7519bc31b6dbd92f0f6b88e450f282d95210e..1f7964c92a2d84beff90256f15756ed4e609c014 100644 --- a/include/apriltag/feature/feature_apriltag.h +++ b/include/apriltag/feature/feature_apriltag.h @@ -43,7 +43,9 @@ class FeatureApriltag : public FeatureBase const Eigen::VectorXd & _measurement, const Eigen::MatrixXd & _meas_covariance, const int _tag_id, + double _tag_width, const Vector8d & _corners_vec, + bool _use_rotation, UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO); ~FeatureApriltag() override; @@ -57,10 +59,27 @@ class FeatureApriltag : public FeatureBase **/ const std::vector& getTagCorners() const; + /** \brief Return pose compute by PnP algorithm + * + **/ + virtual Eigen::VectorXd getPosePnp() const = 0; + + /** \brief Return a boolean assessing if the rotation part of the transformation is reliable. + * + **/ + bool getUseRotation() const; + + /** \brief Return the width of the detected tag (from user specification) + * + **/ + double getTagWidth() const; + private: int tag_id_; + double tag_width_; std::vector tag_corners_; + bool use_rotation_; }; @@ -70,11 +89,15 @@ inline FeatureApriltag::FeatureApriltag( const Eigen::VectorXd & _measurement, const Eigen::MatrixXd & _meas_uncertainty, const int _tag_id, + double _tag_width, const Vector8d & _corners_vec, + bool _use_rotation, UncertaintyType _uncertainty_type) : FeatureBase(_type, _measurement, _meas_uncertainty, _uncertainty_type), tag_id_(_tag_id), - tag_corners_(4) + tag_width_(_tag_width), + tag_corners_(4), + use_rotation_(_use_rotation) { setTrackId(_tag_id); // assuming there is a single landmark with this id in the scene @@ -100,6 +123,17 @@ inline const std::vector& FeatureApriltag::getTagCorners() const return tag_corners_; } +inline double FeatureApriltag::getTagWidth() const +{ + return tag_width_; +} + +inline bool FeatureApriltag::getUseRotation() const +{ + return use_rotation_; +} + + } // namespace wolf diff --git a/include/apriltag/feature/feature_apriltag_pose.h b/include/apriltag/feature/feature_apriltag_pose.h index 92d5dae6ccce4e1ba485452f605d44fac0d41f12..d4cc02fc247c761966329b4fdda8d30ad08c3396 100644 --- a/include/apriltag/feature/feature_apriltag_pose.h +++ b/include/apriltag/feature/feature_apriltag_pose.h @@ -44,37 +44,26 @@ class FeatureApriltagPose : public FeatureApriltag FeatureApriltagPose( const Eigen::Vector7d & _measurement, const Eigen::Matrix6d & _meas_info, - const int _tag_id, + int _tag_id, + double _tag_width, const Vector8d & _corners_vec, - double _rep_error1, - double _rep_error2, bool _use_rotation, UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO); ~FeatureApriltagPose() override; - - double getRepError1() const; - double getRepError2() const; - bool getUserotation() const; - private: - double rep_error1_; - double rep_error2_; - bool use_rotation_; + Eigen::VectorXd getPosePnp() const override; + }; inline FeatureApriltagPose::FeatureApriltagPose( const Eigen::Vector7d & _measurement, const Eigen::Matrix6d & _meas_info, const int _tag_id, + double _tag_width, const Vector8d & _corners_vec, - double _rep_error1, - double _rep_error2, bool _use_rotation, UncertaintyType _uncertainty_type) : - FeatureApriltag("FeatureApriltagPose", _measurement, _meas_info, _tag_id, _corners_vec, _uncertainty_type), - rep_error1_(_rep_error1), - rep_error2_(_rep_error2), - use_rotation_(_use_rotation) + FeatureApriltag("FeatureApriltagPose", _measurement, _meas_info, _tag_id, _tag_width, _corners_vec, _use_rotation, _uncertainty_type) { } @@ -83,17 +72,9 @@ inline FeatureApriltagPose::~FeatureApriltagPose() // } -inline double FeatureApriltagPose::getRepError1() const -{ - return rep_error1_; -} -inline double FeatureApriltagPose::getRepError2() const -{ - return rep_error2_; -} -inline bool FeatureApriltagPose::getUserotation() const +inline Eigen::VectorXd FeatureApriltagPose::getPosePnp() const { - return use_rotation_; + return measurement_; } } // namespace wolf diff --git a/include/apriltag/feature/feature_apriltag_proj.h b/include/apriltag/feature/feature_apriltag_proj.h index 661383f7deb55b9e8292eca8955e34057b918c01..3e05a2c7f1f5689c7291984ac2550422992f7e9c 100644 --- a/include/apriltag/feature/feature_apriltag_proj.h +++ b/include/apriltag/feature/feature_apriltag_proj.h @@ -45,18 +45,15 @@ class FeatureApriltagProj : public FeatureApriltag const int _tag_id, const double _tag_width, const Eigen::Vector7d& _pose_pnp, + bool _use_rotation, UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE); ~FeatureApriltagProj() override; - const Eigen::Vector7d& getPosePnp() const; - double getTagWidth() const; + Eigen::VectorXd getPosePnp() const override; private: Eigen::Vector7d pose_pnp_; - double tag_width_; - - }; @@ -66,10 +63,10 @@ inline FeatureApriltagProj::FeatureApriltagProj( const int _tag_id, const double _tag_width, const Eigen::Vector7d& _pose_pnp, + bool _use_rotation, UncertaintyType _uncertainty_type) : - FeatureApriltag("FeatureApriltagProj", _measurement, _meas_covariance, _tag_id, _measurement, _uncertainty_type), - pose_pnp_(_pose_pnp), - tag_width_(_tag_width) + FeatureApriltag("FeatureApriltagProj", _measurement, _meas_covariance, _tag_id, _tag_width, _measurement, _use_rotation, _uncertainty_type), + pose_pnp_(_pose_pnp) { } @@ -79,14 +76,10 @@ inline FeatureApriltagProj::~FeatureApriltagProj() // } -inline const Eigen::Vector7d& FeatureApriltagProj::getPosePnp() const +inline Eigen::VectorXd FeatureApriltagProj::getPosePnp() const { return pose_pnp_; } -inline double FeatureApriltagProj::getTagWidth() const -{ - return tag_width_; -} } // namespace wolf diff --git a/include/apriltag/processor/processor_tracker_landmark_apriltag.h b/include/apriltag/processor/processor_tracker_landmark_apriltag.h index b460b086a0f3cdb5759aaf70da81dde621cead3d..f717b38222d1c2d1961bda5fc72714bd35afddbd 100644 --- a/include/apriltag/processor/processor_tracker_landmark_apriltag.h +++ b/include/apriltag/processor/processor_tracker_landmark_apriltag.h @@ -30,7 +30,7 @@ #include "apriltag/factor/factor_apriltag_proj.h" // IPPE (copy from https://github.com/tobycollins/IPPE) -#include "ippe.h" +#include "ippe.h" /// use opencv solvePNP with square option instead // Wolf vision #include @@ -39,9 +39,10 @@ // Wolf core #include -#include /// REMOVE? +// #include /// REMOVE? #include #include +#include // apriltag detection Michigan library #include @@ -72,7 +73,7 @@ namespace wolf WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerLandmarkApriltag); -struct ParamsProcessorTrackerLandmarkApriltag : public ParamsProcessorTrackerLandmark +struct ParamsProcessorTrackerLandmarkApriltag : public ParamsProcessorTrackerLandmark, public ParamsMotionProvider { //tag parameters std::string tag_family_; @@ -100,7 +101,8 @@ struct ParamsProcessorTrackerLandmarkApriltag : public ParamsProcessorTrackerLan ParamsProcessorTrackerLandmarkApriltag() = default; ParamsProcessorTrackerLandmarkApriltag(std::string _unique_name, const ParamsServer& _server): - ParamsProcessorTrackerLandmark(_unique_name, _server) + ParamsProcessorTrackerLandmark(_unique_name, _server), + ParamsMotionProvider(_unique_name, _server) { tag_family_ = _server.getParam(prefix + _unique_name + "/tag_family"); tag_width_default_ = _server.getParam(prefix + _unique_name + "/tag_width_default"); @@ -143,7 +145,7 @@ struct ParamsProcessorTrackerLandmarkApriltag : public ParamsProcessorTrackerLan WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkApriltag); -class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark +class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark, public MotionProvider { public: @@ -210,6 +212,14 @@ class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark void configure(SensorBasePtr _sensor) override; + //////////////////////////////////////////////// + // MotionProvider virtual methods implementation + TimeStamp getTimeStamp() const override; + VectorComposite getState(const StateStructure& _structure = "") const override; + VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override; + //////////////////////////////////////////////// + + public: double getTagWidth(int _id) const; std::string getTagFamily() const; diff --git a/src/processor/processor_tracker_landmark_apriltag.cpp b/src/processor/processor_tracker_landmark_apriltag.cpp index 88b27e9930430a21c02d42b5d6e380ca65fa04ff..e488a6d59923048fbffd9dd9f11e493585c8fab6 100644 --- a/src/processor/processor_tracker_landmark_apriltag.cpp +++ b/src/processor/processor_tracker_landmark_apriltag.cpp @@ -22,6 +22,7 @@ #include "apriltag/processor/processor_tracker_landmark_apriltag.h" +#include "core/math/SE3.h" #include #include @@ -30,7 +31,8 @@ namespace wolf { // Constructor ProcessorTrackerLandmarkApriltag::ProcessorTrackerLandmarkApriltag( ParamsProcessorTrackerLandmarkApriltagPtr _params_tracker_landmark_apriltag) : - ProcessorTrackerLandmark("ProcessorTrackerLandmarkApriltag", "PO", _params_tracker_landmark_apriltag ), + ProcessorTrackerLandmark("ProcessorTrackerLandmarkApriltag", "PO", _params_tracker_landmark_apriltag), + MotionProvider("PO", _params_tracker_landmark_apriltag), tag_widths_(_params_tracker_landmark_apriltag->tag_widths_), tag_width_default_(_params_tracker_landmark_apriltag->tag_width_default_), std_pix_(_params_tracker_landmark_apriltag->std_pix_), @@ -177,7 +179,8 @@ void ProcessorTrackerLandmarkApriltag::preProcess() std_pix_*std_pix_*Eigen::Matrix8d::Identity(), tag_id, tag_width, - pose)); + pose, + use_rotation)); } else { @@ -196,9 +199,8 @@ void ProcessorTrackerLandmarkApriltag::preProcess() detections_incoming_.push_back(std::make_shared(pose, info, tag_id, + tag_width, corners_vec, - rep_error1, - rep_error2, use_rotation, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO)); } @@ -591,6 +593,94 @@ std::string ProcessorTrackerLandmarkApriltag::getTagFamily() const return tag_family_->name; } + +//////////////////////////////////////// +// MotionProvider methods implementation +//////////////////////////////////////// + + +TimeStamp ProcessorTrackerLandmarkApriltag::getTimeStamp() const +{ + if ( last_ptr_ == nullptr ) + return TimeStamp::Invalid(); + else + return last_ptr_->getTimeStamp(); +} + +VectorComposite ProcessorTrackerLandmarkApriltag::getState(const StateStructure& _structure) const +{ + // compute the state of the frame corresponding to last capture + // matches_landmark_from_last_ contains matches between landmarks already present in the map when + // processing last capture and detections in last capture. + // From the map current estimate and apriltag last features, we can extract the last pose of the robot + + Vector7d pose_c_r = SE3::inverse(getSensor()->getStateVector("PO")); + + // Compute average SE3 pose between using the hyperbolic average (mean of the se(3) twist 6D vectors) + // Neglected: + // - Uncertainty on the landmark estimate + // - Uncertainty on the camera->landmark pose measurements + int nb_use_rot = 0; + // 1. hyperbolic avg on se(3) + Vector6d nu_w_r_avg = Vector6d::Zero(); + // 2. hyperbolic metric avg on so(3) (http://lucaballan.altervista.org/pdfs/IK.pdf slide) + Vector3d t_w_r_avg = Vector3d::Zero(); + Vector3d omg_w_r_avg = Vector3d::Zero(); + // 3. Frobenius/chordal metric avg followed by projection on SO(3) -> not really a diff + // Matrix3d R_w_r_avg = Matrix3d::Zero(); + for (auto match: matches_landmark_from_last_){ + auto feat_a = std::static_pointer_cast(match.first); + // use the pose only if the rotation is not ambiguous + if (feat_a->getUseRotation()){ + auto lmk_a = std::static_pointer_cast (match.second->landmark_ptr_); + + Vector7d pose_l_c = SE3::inverse(feat_a->getPosePnp()); + Vector7d pose_w_l = lmk_a->getStateVector(); + Vector7d pose_w_r = SE3::compose(SE3::compose(pose_w_l, pose_l_c), pose_c_r); + // 1. + nu_w_r_avg += SE3::log(pose_w_r); + // 2. + t_w_r_avg += pose_w_r.head<3>(); + omg_w_r_avg += q2v(Quaterniond(pose_w_r.tail<4>())); + // 3. + // R_w_r_avg += q2R(pose_w_r.tail<4>()); + nb_use_rot += 1; + } + } + if (nb_use_rot == 0){ + return VectorComposite(); + } + // // 1. + // nu_w_r_avg /= nb_use_rot; + // Vector7d pose_avg = SE3::exp(nu_w_r_avg); + + // 2. + t_w_r_avg /= nb_use_rot; + omg_w_r_avg /= nb_use_rot; + Vector7d pose_avg; pose_avg << t_w_r_avg, v2q(omg_w_r_avg).coeffs(); + + // // 3. + // t_w_r_avg /= nb_use_rot; + // R_w_r_avg /= nb_use_rot; + + + return VectorComposite(pose_avg, "PO", {3,4}); + +} + +VectorComposite ProcessorTrackerLandmarkApriltag::getState(const TimeStamp& _ts, const StateStructure& _structure) const +{ + if (true) { + return getState(_structure); + } + + // empty state should be handled by the receiver + return VectorComposite(); +} + + + + } // namespace wolf // Register in the FactorySensor @@ -600,5 +690,4 @@ namespace wolf { WOLF_REGISTER_PROCESSOR(ProcessorTrackerLandmarkApriltag) WOLF_REGISTER_PROCESSOR_AUTO(ProcessorTrackerLandmarkApriltag) -} - +} \ No newline at end of file diff --git a/test/gtest_factor_apriltag_proj.cpp b/test/gtest_factor_apriltag_proj.cpp index c4e16bf02c0c2133c6a94321ab2e403e398c3ec6..8c8e5bd7768e1b9c24fde295aefb169d38733c20 100644 --- a/test/gtest_factor_apriltag_proj.cpp +++ b/test/gtest_factor_apriltag_proj.cpp @@ -97,7 +97,7 @@ class FactorApriltagProj_class : public testing::Test{ FrameBasePtr F1; CaptureImagePtr C1; - FeatureApriltagProjPtr f1; + FeatureApriltagProjPtr f1; LandmarkApriltagPtr lmk1; FactorApriltagProjPtr fac_a1; @@ -239,7 +239,7 @@ class FactorApriltagProj_class : public testing::Test{ TEST_F(FactorApriltagProj_class, Constructor) { - f1 = std::static_pointer_cast(FeatureBase::emplace(C1, meas1, meas_cov, 1, tag_width, pose_dummy)); + f1 = std::static_pointer_cast(FeatureBase::emplace(C1, meas1, meas_cov, 1, tag_width, pose_dummy, true)); FactorApriltagProjPtr factor = std::make_shared( f1, camera, @@ -256,7 +256,7 @@ TEST_F(FactorApriltagProj_class, Constructor) TEST_F(FactorApriltagProj_class, problem_1KF) { - f1 = std::static_pointer_cast(FeatureBase::emplace(C1, meas1, meas_cov, 1, tag_width, pose_dummy)); + f1 = std::static_pointer_cast(FeatureBase::emplace(C1, meas1, meas_cov, 1, tag_width, pose_dummy, true)); //emplace feature and landmark auto factor = FactorBase::emplace(f1, f1, camera, F1, lmk1, nullptr, false); @@ -293,8 +293,8 @@ TEST_F(FactorApriltagProj_class, problem_2KF) FrameBasePtr F2 = problem->emplaceFrame(2, posiquat2pose(p_w_r2, q_w_r2)); CaptureImagePtr C2 = std::static_pointer_cast(CaptureBase::emplace(F2, 1, camera, cv::Mat(2,2,CV_8UC1))); - f1 = std::static_pointer_cast(FeatureBase::emplace(C1, meas1, meas_cov, 1, tag_width, pose_dummy)); - auto f2 = std::static_pointer_cast(FeatureBase::emplace(C2, meas2, meas_cov, 2, tag_width, pose_dummy)); + f1 = std::static_pointer_cast(FeatureBase::emplace(C1, meas1, meas_cov, 1, tag_width, pose_dummy, true)); + auto f2 = std::static_pointer_cast(FeatureBase::emplace(C2, meas2, meas_cov, 2, tag_width, pose_dummy, true)); //emplace feature and landmark auto factor1 = FactorBase::emplace(f1, f1, camera, F1, lmk1, nullptr, false); diff --git a/test/gtest_features_apriltag.cpp b/test/gtest_features_apriltag.cpp index acf3699bd83b7b57ebc0c57460c811bbed89db98..32d3de280fbebef14bd51dd788670dccc0b9018b 100644 --- a/test/gtest_features_apriltag.cpp +++ b/test/gtest_features_apriltag.cpp @@ -36,75 +36,68 @@ class FeatureApriltag_fixture : public testing::Test Eigen::Vector8d corners_vec; Eigen::Matrix8d cov_pix; int tag_id; - double tag_widh; - double rep_error1; - double rep_error2; + double tag_width; bool use_rotation; void SetUp() override { pose << 1,2,3,4,5,6,7; + pose = pose + 42*Eigen::Vector7d::Ones(); info_pose.setIdentity(); corners_vec << 1.0, -1.0, 1.0, 1.0, -1.0, 1.0, -1.0, -1.0; + corners_vec = 42.0*corners_vec; cov_pix.setIdentity(); tag_id = 1; - tag_widh = 0.1; + tag_width = 0.1; - rep_error1 = 0.01; - rep_error2 = 0.1; use_rotation = true; } }; - TEST_F(FeatureApriltag_fixture, type) { - FeatureApriltagPtr fa = std::make_shared("toto", pose, info_pose, tag_id, corners_vec); - ASSERT_EQ(fa->getType(), "toto"); - - FeatureApriltagPosePtr fa_pose = std::make_shared(pose, info_pose, tag_id, corners_vec, rep_error1, rep_error2, use_rotation); + FeatureApriltagPosePtr fa_pose = std::make_shared(pose, info_pose, tag_id, tag_width, corners_vec, use_rotation); ASSERT_EQ(fa_pose->getType(), "FeatureApriltagPose"); - FeatureApriltagProjPtr fa_proj = std::make_shared(corners_vec, cov_pix, tag_id, tag_widh, pose); + FeatureApriltagProjPtr fa_proj = std::make_shared(corners_vec, cov_pix, tag_id, tag_width, pose, use_rotation); ASSERT_EQ(fa_proj->getType(), "FeatureApriltagProj"); } - TEST_F(FeatureApriltag_fixture, FeatureApriltag_getters) { - FeatureApriltagPtr fa = std::make_shared("toto", pose, info_pose, tag_id, corners_vec); - - ASSERT_EQ(fa->getTagId(), 1); - ASSERT_EQ(fa->getTagCorners().at(0).x, corners_vec(0)); - ASSERT_EQ(fa->getTagCorners().at(0).y, corners_vec(1)); - ASSERT_EQ(fa->getTagCorners().at(1).x, corners_vec(2)); - ASSERT_EQ(fa->getTagCorners().at(1).y, corners_vec(3)); - ASSERT_EQ(fa->getTagCorners().at(2).x, corners_vec(4)); - ASSERT_EQ(fa->getTagCorners().at(2).y, corners_vec(5)); - ASSERT_EQ(fa->getTagCorners().at(3).x, corners_vec(6)); - ASSERT_EQ(fa->getTagCorners().at(3).y, corners_vec(7)); + // FeatureApriltag has a pure virtual method, need to instanciate one of the derived classes + FeatureApriltagPosePtr fa_pose = std::make_shared(pose, info_pose, tag_id, tag_width, corners_vec, use_rotation); + + ASSERT_EQ(fa_pose->getTagId(), 1); + ASSERT_EQ(fa_pose->getTagWidth(), tag_width); + ASSERT_EQ(fa_pose->getTagCorners().at(0).x, corners_vec(0)); + ASSERT_EQ(fa_pose->getTagCorners().at(0).y, corners_vec(1)); + ASSERT_EQ(fa_pose->getTagCorners().at(1).x, corners_vec(2)); + ASSERT_EQ(fa_pose->getTagCorners().at(1).y, corners_vec(3)); + ASSERT_EQ(fa_pose->getTagCorners().at(2).x, corners_vec(4)); + ASSERT_EQ(fa_pose->getTagCorners().at(2).y, corners_vec(5)); + ASSERT_EQ(fa_pose->getTagCorners().at(3).x, corners_vec(6)); + ASSERT_EQ(fa_pose->getTagCorners().at(3).y, corners_vec(7)); + ASSERT_EQ(fa_pose->getUseRotation(), use_rotation); } -TEST_F(FeatureApriltag_fixture, FeatureApriltagPose_getters) +TEST_F(FeatureApriltag_fixture, FeatureApriltagProj_getters) { - FeatureApriltagPosePtr fa_pose = std::make_shared(pose, info_pose, tag_id, corners_vec, rep_error1, rep_error2, use_rotation); + FeatureApriltagProjPtr fa_proj = std::make_shared(corners_vec, cov_pix, tag_id, tag_width, pose, use_rotation); - ASSERT_EQ(fa_pose->getRepError1(), rep_error1); - ASSERT_EQ(fa_pose->getRepError2(), rep_error2); - ASSERT_EQ(fa_pose->getUserotation(), use_rotation); + ASSERT_MATRIX_APPROX(fa_proj->getPosePnp(), pose, 1e-6); } -TEST_F(FeatureApriltag_fixture, FeatureApriltagProj_getters) +TEST_F(FeatureApriltag_fixture, FeatureApriltagPose_getters) { - FeatureApriltagProjPtr fa_proj = std::make_shared(corners_vec, cov_pix, tag_id, tag_widh, pose); + FeatureApriltagPosePtr fa_pose = std::make_shared(pose, info_pose, tag_id, tag_width, corners_vec, use_rotation); - ASSERT_MATRIX_APPROX(fa_proj->getPosePnp(), pose, 1e-6); - ASSERT_EQ(fa_proj->getTagWidth(), tag_widh); + ASSERT_MATRIX_APPROX(fa_pose->getPosePnp(), pose, 1e-6); } int main(int argc, char **argv) diff --git a/test/gtest_processor_tracker_landmark_apriltag.cpp b/test/gtest_processor_tracker_landmark_apriltag.cpp index 79627017e9ce07d199477c530d873890c690e404..9d8d68523de2598e67db11d63bc0c4b16dbd2116 100644 --- a/test/gtest_processor_tracker_landmark_apriltag.cpp +++ b/test/gtest_processor_tracker_landmark_apriltag.cpp @@ -204,11 +204,11 @@ TEST(ProcessorTrackerLandmarkApriltag, Constructor) // for (int i=0; i < min_features_for_keyframe; i++){ // det.id = i; -// FeatureBase::emplace(Ca, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation); -// FeatureBase::emplace(Cc, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation); +// FeatureBase::emplace(Ca, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, use_rotation); +// FeatureBase::emplace(Cc, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, use_rotation); // if (i != min_features_for_keyframe-1){ -// FeatureBase::emplace(Cd, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation); -// FeatureBase::emplace(Ce, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation); +// FeatureBase::emplace(Cd, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, use_rotation); +// FeatureBase::emplace(Ce, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, use_rotation); // } // } @@ -248,9 +248,9 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeaturesDuplicated) // feature 0 tag_id_ = 0; - auto f0 = std::make_shared(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation); + auto f0 = std::make_shared(pose_default_, cov_pose_, tag_id_, 0.1, corners_vec_, use_rotation); // feature 1 (with same id of feature 0) - auto f1 = std::make_shared(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation); + auto f1 = std::make_shared(pose_default_, cov_pose_, tag_id_, 0.1, corners_vec_, use_rotation); features_in.push_back(f0); features_in.push_back(f1); @@ -270,13 +270,13 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeatures) // feature 0 tag_id_ = 0; - auto f0 = std::make_shared(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation); + auto f0 = std::make_shared(pose_default_, cov_pose_, tag_id_, 0.1, corners_vec_, use_rotation); // feature 1 tag_id_ = 1; - auto f1 = std::make_shared(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation); + auto f1 = std::make_shared(pose_default_, cov_pose_, tag_id_, 0.1, corners_vec_, use_rotation); // feature 2 tag_id_ = 2; - auto f2 = std::make_shared(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation); + auto f2 = std::make_shared(pose_default_, cov_pose_, tag_id_, 0.1, corners_vec_, use_rotation); //we add different features in the list features_in.push_back(f0); @@ -309,7 +309,7 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeatures) TEST_F(ProcessorTrackerLandmarkApriltag_class, emplaceLandmark) { Vector7d pose_landmark((Vector7d()<<1,2,3,1,0,0,0).finished()); - auto f1 = std::make_shared(pose_landmark, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation); + auto f1 = std::make_shared(pose_landmark, cov_pose_, tag_id_, 0.1, corners_vec_, use_rotation); LandmarkBasePtr lmk = prc_apr->emplaceLandmark(f1); LandmarkApriltagPtr lmk_april = std::static_pointer_cast(lmk); @@ -322,7 +322,7 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, emplaceLandmark) TEST_F(ProcessorTrackerLandmarkApriltag_class, emplaceFactor) { - auto f1 = FeatureBase::emplace(C1, pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation); + auto f1 = FeatureBase::emplace(C1, pose_default_, cov_pose_, tag_id_, 0.1, corners_vec_, use_rotation); LandmarkBasePtr lmk = prc_apr->emplaceLandmark(f1); LandmarkApriltagPtr lmk_april = std::static_pointer_cast(lmk); @@ -427,10 +427,6 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, computeInformation) - - - - //////////////////////////////// //////////////////////////////// // Projection based Factor @@ -443,7 +439,7 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, emplaceFactorProj) { // dummy pose, not used in the factor, only for some part of the processor Vector7d pose_dummy = Vector7d::Zero(); - auto f1 = FeatureBase::emplace(C1, Vector8d::Zero(), Matrix8d::Identity(), tag_id_, 0.1, pose_dummy); + auto f1 = FeatureBase::emplace(C1, Vector8d::Zero(), Matrix8d::Identity(), tag_id_, 0.1, pose_dummy, use_rotation); LandmarkBasePtr lmk = prc_apr->emplaceLandmark(f1); LandmarkApriltagPtr lmk_april = std::static_pointer_cast(lmk);