diff --git a/CMakeLists.txt b/CMakeLists.txt index 4703ab5d1bb1f1deda1597372654d4f7e6cb159f..f1f567d074179dd6bf6d2a925d12d2caf331dd25 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,7 +75,7 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON) # ============ DEPENDENCIES ============ FIND_PACKAGE(wolfcore REQUIRED CONFIG) FIND_PACKAGE(wolfvision REQUIRED) -FIND_PACKAGE(apriltag REQUIRED) +FIND_PACKAGE(apriltag REQUIRED) # UMichigan apriltag library #============ CONFIG.H ============ set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR}) @@ -101,10 +101,12 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D # ============ HEADERS ============ SET(HDRS_FACTOR -include/${PROJECT_NAME}/factor/factor_apriltag.h +include/${PROJECT_NAME}/factor/factor_apriltag_proj.h ) SET(HDRS_FEATURE include/${PROJECT_NAME}/feature/feature_apriltag.h +include/${PROJECT_NAME}/feature/feature_apriltag_pose.h +include/${PROJECT_NAME}/feature/feature_apriltag_proj.h ) SET(HDRS_LANDMARK include/${PROJECT_NAME}/landmark/landmark_apriltag.h @@ -116,7 +118,6 @@ include/${PROJECT_NAME}/processor/ippe.h # ============ SOURCES ============ SET(SRCS_FEATURE -src/feature/feature_apriltag.cpp ) SET(SRCS_LANDMARK src/landmark/landmark_apriltag.cpp diff --git a/demos/demo_apriltag.cpp b/demos/demo_apriltag.cpp index 4f82abbc66fd1cc307dabc599a86d08b3232bf00..7fecf1b25851fa90339e3e26030e587ef60e077d 100644 --- a/demos/demo_apriltag.cpp +++ b/demos/demo_apriltag.cpp @@ -34,7 +34,7 @@ #include "core/sensor/sensor_camera.h" #include "core/processor/processor_tracker_landmark_apriltag.h" #include "core/capture/capture_image.h" -#include "core/feature/feature_apriltag.h" +#include "core/feature/feature_apriltag_pose.h" // opencv #include @@ -179,7 +179,7 @@ int main(int argc, char *argv[]) auto img = std::static_pointer_cast(cap); for (FeatureBasePtr f : img->getFeatureList()) { - FeatureApriltagPtr fa = std::static_pointer_cast(f); + FeatureApriltagPosePtr fa = std::static_pointer_cast(f); draw_apriltag(img->getImage(), fa->getTagCorners(), 1); } cv::imshow( img->getName(), img->getImage() ); // display original image. diff --git a/demos/processor_tracker_landmark_apriltag.yaml b/demos/processor_tracker_landmark_apriltag.yaml index 465a0dade019d277ecc7ae1575cf6dbbef0e1a28..bafd3b61dab2a05d873f890fbe1ee0e6d632305b 100644 --- a/demos/processor_tracker_landmark_apriltag.yaml +++ b/demos/processor_tracker_landmark_apriltag.yaml @@ -26,7 +26,9 @@ noise: std_z : 0.1 # m std_rpy_degrees : 5 # degrees std_pix: 20 # pixel error - + +use_proj_factor: false + vote: voting active: true min_time_vote: 0 # s @@ -34,8 +36,6 @@ vote: min_features_for_keyframe: 12 nb_vote_for_every_first: 50 -reestimate_last_frame: true # for a better prior on the new keyframe: use only if no motion processor -add_3d_cstr: true # add 3D constraints between the KF so that they do not jump when using apriltag only max_new_features: -1 apply_loss_function: true diff --git a/include/apriltag/factor/factor_apriltag.h b/include/apriltag/factor/factor_apriltag_proj.h similarity index 52% rename from include/apriltag/factor/factor_apriltag.h rename to include/apriltag/factor/factor_apriltag_proj.h index 99a56d9b998a01f9ac75267fa2988d78e23ec989..b077b39b3df9e39309cc2c990ff355dcb58dd2ef 100644 --- a/include/apriltag/factor/factor_apriltag.h +++ b/include/apriltag/factor/factor_apriltag_proj.h @@ -19,40 +19,46 @@ // along with this program. If not, see . // //--------LICENSE_END-------- -#ifndef _FACTOR_APRILTAG_H_ -#define _FACTOR_APRILTAG_H_ - -//Wolf includes -#include "core/common/wolf.h" -#include "core/math/rotations.h" -#include "core/factor/factor_autodiff.h" -#include "core/sensor/sensor_base.h" +#ifndef _FACTOR_APRILTAG_PROJ_H_ +#define _FACTOR_APRILTAG_PROJ_H_ + +// Wolf apriltag #include "apriltag/landmark/landmark_apriltag.h" -#include "apriltag/feature/feature_apriltag.h" +#include "apriltag/feature/feature_apriltag_proj.h" + +// Wolf vision +#include +#include + +//Wolf core +#include +#include +#include +#include namespace wolf { -WOLF_PTR_TYPEDEFS(FactorApriltag); +WOLF_PTR_TYPEDEFS(FactorApriltagProj); -class FactorApriltag : public FactorAutodiff +class FactorApriltagProj : public FactorAutodiff { public: /** \brief Class constructor */ - FactorApriltag( + FactorApriltagProj( + const FeatureApriltagProjPtr& _feature_ptr, const SensorBasePtr& _sensor_ptr, const FrameBasePtr& _frame_ptr, - const LandmarkApriltagPtr& _landmark_other_ptr, - const FeatureApriltagPtr& _feature_ptr, + const LandmarkBasePtr& _landmark_other_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, - FactorStatus _status); + FactorStatus _status = FAC_ACTIVE); /** \brief Class Destructor */ - ~FactorApriltag() override; + ~FactorApriltagProj() override; /** brief : compute the residual from the state blocks being iterated by the solver. **/ @@ -65,7 +71,7 @@ class FactorApriltag : public FactorAutodiff + static Eigen::Matrix pinholeProj(const Eigen::Matrix3d& K, + const Eigen::MatrixBase& p_c_l, + const Eigen::Quaternion& q_c_l, + const Eigen::Vector3d& l_corn); + private: + Eigen::Vector3d l_corn1_; + Eigen::Vector3d l_corn2_; + Eigen::Vector3d l_corn3_; + Eigen::Vector3d l_corn4_; + SensorCameraConstPtr camera_; + Matrix3d K_; }; } // namespace wolf @@ -90,15 +109,15 @@ class FactorApriltag : public FactorAutodiffgetP(), _landmark_other_ptr->getO() ) { - - + double tag_width = _feature_ptr->getTagWidth(); + + // Same order as the 2d corners (anti clockwise, looking at the tag). + // Looking at the tag, the reference frame is + // X = Right, Y = Down, Z = Inside the plane + double s = tag_width/2; + l_corn1_ << -s, s, 0; // bottom left + l_corn2_ << s, s, 0; // bottom right + l_corn3_ << s, -s, 0; // top right + l_corn4_ << -s, -s, 0; // top left + + ////////////////////////////////////// + // Camera matrix + K_ = std::static_pointer_cast(_sensor_ptr)->getIntrinsicMatrix(); } /** \brief Class Destructor */ -FactorApriltag::~FactorApriltag() +inline FactorApriltagProj::~FactorApriltagProj() { // } -template bool FactorApriltag::operator ()( const T* const _p_camera, const T* const _o_camera, const T* const _p_keyframe, const T* const _o_keyframe, const T* const _p_landmark, const T* const _o_landmark, T* _residuals) const + +template +Eigen::Matrix FactorApriltagProj::pinholeProj(const Eigen::Matrix3d& K, + const Eigen::MatrixBase& p_c_l, + const Eigen::Quaternion& q_c_l, + const Eigen::Vector3d& l_corn) +{ + MatrixSizeCheck<3,1>::check(p_c_l); + + typedef typename D1::Scalar T; + Eigen::Matrix h = K.cast() * (p_c_l + q_c_l * l_corn.cast()); + Eigen::Matrix pix; pix << h(0)/h(2), h(1)/h(2); + + return pix; +} + + +template +bool FactorApriltagProj::operator ()(const T* const _p_camera, + const T* const _o_camera, + const T* const _p_keyframe, + const T* const _o_keyframe, + const T* const _p_landmark, + const T* const _o_landmark, + T* _residuals) const { // Maps Eigen::Map> p_r_c(_p_camera); @@ -133,34 +188,31 @@ template bool FactorApriltag::operator ()( const T* const _p_camera, Eigen::Map> q_w_r(_o_keyframe); Eigen::Map> p_w_l(_p_landmark); Eigen::Map> q_w_l(_o_landmark); - Eigen::Map> residuals(_residuals); + Eigen::Map> residuals(_residuals); + // Expected relative camera-lmk transformation // Expected measurement Eigen::Quaternion q_c_w = (q_w_r * q_r_c).conjugate(); Eigen::Quaternion q_c_l = q_c_w * q_w_l; Eigen::Matrix p_c_l = q_c_w * (-(p_w_r + q_w_r * p_r_c) + p_w_l); - // Measurement - Eigen::Vector3d p_c_l_meas(getMeasurement().head<3>()); - Eigen::Quaterniond q_c_l_meas(getMeasurement().data() + 3 ); - Eigen::Quaternion q_l_c_meas = q_c_l_meas.conjugate().cast(); - //Eigen::Matrix p_l_c_meas = -q_l_c_meas * p_c_l_meas.cast(); + ////////////////////////////////////// + // Expected corner projections + Eigen::Matrix corners_exp; + corners_exp.segment(0,2) = pinholeProj(K_, p_c_l, q_c_l, l_corn1_); + corners_exp.segment(2,2) = pinholeProj(K_, p_c_l, q_c_l, l_corn2_); + corners_exp.segment(4,2) = pinholeProj(K_, p_c_l, q_c_l, l_corn3_); + corners_exp.segment(6,2) = pinholeProj(K_, p_c_l, q_c_l, l_corn4_); - // Error - Eigen::Matrix err; - err.head(3) = q_l_c_meas * (p_c_l_meas.cast() - p_c_l); - //err.tail(3) = wolf::log_q(q_l_c_meas * q_c_l); - err.tail(3) = T(2)*(q_l_c_meas * q_c_l).vec(); - - // Residual - residuals = getMeasurementSquareRootInformationUpper().cast() * err; + residuals = getMeasurementSquareRootInformationUpper().cast() * (corners_exp - getMeasurement().cast()); return true; } -Eigen::Vector6d FactorApriltag::residual() const + +inline Eigen::Vector8d FactorApriltagProj::residual() const { - Eigen::Vector6d res; + Eigen::Vector8d res; double * p_camera, * o_camera, * p_frame, * o_frame, * p_tag, * o_tag; p_camera = getCapture()->getSensorP()->getState().data(); o_camera = getCapture()->getSensorO()->getState().data(); @@ -174,11 +226,12 @@ Eigen::Vector6d FactorApriltag::residual() const return res; } -double FactorApriltag::cost() const + +inline double FactorApriltagProj::cost() const { return residual().squaredNorm(); } } // namespace wolf -#endif /* _CONSTRAINT_AUTODIFF_APRILTAG_H_ */ +#endif /* _FACTOR_APRILTAG_PROJ_H_ */ diff --git a/include/apriltag/feature/feature_apriltag.h b/include/apriltag/feature/feature_apriltag.h index 6dbcf73deddcd572b267ded747399bf5ff8946f4..27b7519bc31b6dbd92f0f6b88e450f282d95210e 100644 --- a/include/apriltag/feature/feature_apriltag.h +++ b/include/apriltag/feature/feature_apriltag.h @@ -25,12 +25,6 @@ //Wolf includes #include "core/feature/feature_base.h" -//std includes - -//external library incudes -#include "apriltag/apriltag.h" -#include "apriltag/common/zarray.h" - // opencv #include @@ -38,47 +32,75 @@ namespace wolf { WOLF_PTR_TYPEDEFS(FeatureApriltag); + //class FeatureApriltag class FeatureApriltag : public FeatureBase { public: - FeatureApriltag(const Eigen::Vector7d & _measurement, - const Eigen::Matrix6d & _meas_covariance, - const int _tag_id, - const apriltag_detection_t & _det, - double _rep_error1, - double _rep_error2, - bool _use_rotation, - UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO); + FeatureApriltag( + const std::string _type, + const Eigen::VectorXd & _measurement, + const Eigen::MatrixXd & _meas_covariance, + const int _tag_id, + const Vector8d & _corners_vec, + UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO); ~FeatureApriltag() override; - /** \brief Returns tag id - * - * Returns tag id + /** \brief Return tag id * **/ - double getTagId() const; - - const apriltag_detection_t& getDetection() const; + int getTagId() const; + /** \brief Return vector of tag corners + * + **/ const std::vector& getTagCorners() const; - double getRepError1() const; - double getRepError2() const; - bool getUserotation() const; - private: int tag_id_; std::vector tag_corners_; - apriltag_detection_t detection_; - double rep_error1_; - double rep_error2_; - bool use_rotation_; }; + +inline FeatureApriltag::FeatureApriltag( + const std::string _type, + const Eigen::VectorXd & _measurement, + const Eigen::MatrixXd & _meas_uncertainty, + const int _tag_id, + const Vector8d & _corners_vec, + UncertaintyType _uncertainty_type) : + FeatureBase(_type, _measurement, _meas_uncertainty, _uncertainty_type), + tag_id_(_tag_id), + tag_corners_(4) +{ + setTrackId(_tag_id); // assuming there is a single landmark with this id in the scene + + for (int i = 0; i < 4; i++) + { + tag_corners_[i].x = _corners_vec[2*i]; + tag_corners_[i].y = _corners_vec[2*i+1]; + } +} + +inline FeatureApriltag::~FeatureApriltag() +{ + // +} + +inline int FeatureApriltag::getTagId() const +{ + return tag_id_; +} + +inline const std::vector& FeatureApriltag::getTagCorners() const +{ + return tag_corners_; +} + + } // namespace wolf #endif diff --git a/include/apriltag/feature/feature_apriltag_pose.h b/include/apriltag/feature/feature_apriltag_pose.h new file mode 100644 index 0000000000000000000000000000000000000000..725c42a14cbdcea005731295bb6efffec7b9e4a5 --- /dev/null +++ b/include/apriltag/feature/feature_apriltag_pose.h @@ -0,0 +1,105 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . +// +//--------LICENSE_END-------- +#ifndef FEATURE_APRILTAG_POSE_H_ +#define FEATURE_APRILTAG_POSE_H_ + +// Wolf apriltag +#include "apriltag/feature/feature_apriltag.h" + +// Wolf core +#include + +// UMich apriltag library +#include +#include + +// opencv +#include + +namespace wolf { + +WOLF_PTR_TYPEDEFS(FeatureApriltagPose); + + +//class FeatureApriltagPose +class FeatureApriltagPose : public FeatureApriltag +{ + public: + + FeatureApriltagPose( + const Eigen::Vector7d & _measurement, + const Eigen::Matrix6d & _meas_info, + const int _tag_id, + 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_; +}; + +inline FeatureApriltagPose::FeatureApriltagPose( + const Eigen::Vector7d & _measurement, + const Eigen::Matrix6d & _meas_info, + const int _tag_id, + 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) +{ +} + +inline FeatureApriltagPose::~FeatureApriltagPose() +{ + // +} + +inline double FeatureApriltagPose::getRepError1() const +{ + return rep_error1_; +} +inline double FeatureApriltagPose::getRepError2() const +{ + return rep_error2_; +} +inline bool FeatureApriltagPose::getUserotation() const +{ + return use_rotation_; +} + +} // namespace wolf + +#endif diff --git a/include/apriltag/feature/feature_apriltag_proj.h b/include/apriltag/feature/feature_apriltag_proj.h new file mode 100644 index 0000000000000000000000000000000000000000..60d90fc362fe1d2f88394fa7e4585338c795c895 --- /dev/null +++ b/include/apriltag/feature/feature_apriltag_proj.h @@ -0,0 +1,97 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . +// +//--------LICENSE_END-------- +#ifndef FEATURE_APRILTAG_PROJ_H_ +#define FEATURE_APRILTAG_PROJ_H_ + +// Wolf apriltag +#include "apriltag/feature/feature_apriltag.h" + +// Wolf core +#include + +// UMich apriltag library +#include +#include + +// opencv +#include + +namespace wolf { + +WOLF_PTR_TYPEDEFS(FeatureApriltagProj); + +//class FeatureApriltagProj +class FeatureApriltagProj : public FeatureApriltag +{ + public: + FeatureApriltagProj( + const Eigen::Vector8d & _measurement, + const Eigen::Matrix8d & _meas_covariance, + const int _tag_id, + const double _tag_width, + const Eigen::Vector7d& _pose_pnp, + UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE); + + ~FeatureApriltagProj() override; + + const Eigen::Vector7d& getPosePnp() const; + double getTagWidth() const; + + private: + Eigen::Vector7d pose_pnp_; + double tag_width_; + + +}; + + +inline FeatureApriltagProj::FeatureApriltagProj( + const Eigen::Vector8d & _measurement, + const Eigen::Matrix8d & _meas_covariance, + const int _tag_id, + const double _tag_width, + const Eigen::Vector7d& _pose_pnp, + UncertaintyType _uncertainty_type) : + FeatureApriltag("FeatureApriltagProj", _measurement, _meas_covariance, _tag_id, _measurement, _uncertainty_type), + pose_pnp_(_pose_pnp), + tag_width_(_tag_width) +{ + +} + +inline FeatureApriltagProj::~FeatureApriltagProj() +{ + // +} + +inline const Eigen::Vector7d& FeatureApriltagProj::getPosePnp() const +{ + return pose_pnp_; +} +inline double FeatureApriltagProj::getTagWidth() const +{ + return tag_width_; +} + +} // namespace wolf + +#endif diff --git a/include/apriltag/processor/processor_tracker_landmark_apriltag.h b/include/apriltag/processor/processor_tracker_landmark_apriltag.h index 9158f57f0faaf1d6cd29ba9d7a8b61841d21e6e0..03dea58fe3b373c8afd2b2a2c243a8a395a7193a 100644 --- a/include/apriltag/processor/processor_tracker_landmark_apriltag.h +++ b/include/apriltag/processor/processor_tracker_landmark_apriltag.h @@ -24,7 +24,10 @@ // Wolf apriltag includes #include "apriltag/feature/feature_apriltag.h" +#include "apriltag/feature/feature_apriltag_proj.h" +#include "apriltag/feature/feature_apriltag_pose.h" #include "apriltag/landmark/landmark_apriltag.h" +#include "apriltag/factor/factor_apriltag_proj.h" // IPPE (copy from https://github.com/tobycollins/IPPE) #include "ippe.h" @@ -86,14 +89,15 @@ struct ParamsProcessorTrackerLandmarkApriltag : public ParamsProcessorTrackerLan bool refine_edges_; double std_pix_; + + bool use_proj_factor_; + double min_time_span_; double max_time_span_; int nb_vote_for_every_first_; - bool add_3d_cstr_; double ippe_min_ratio_; double ippe_max_rep_error_; - bool reestimate_last_frame_; ParamsProcessorTrackerLandmarkApriltag() = default; ParamsProcessorTrackerLandmarkApriltag(std::string _unique_name, const ParamsServer& _server): ParamsProcessorTrackerLandmark(_unique_name, _server) @@ -107,17 +111,16 @@ struct ParamsProcessorTrackerLandmarkApriltag : public ParamsProcessorTrackerLan debug_ = _server.getParam(prefix + _unique_name + "/debug"); refine_edges_ = _server.getParam(prefix + _unique_name + "/refine_edges"); std_pix_ = _server.getParam(prefix + _unique_name + "/std_pix"); + use_proj_factor_ = _server.getParam(prefix + _unique_name + "/use_proj_factor"); min_time_span_ = _server.getParam(prefix + _unique_name + "/keyframe_vote/min_time_span"); max_time_span_ = _server.getParam(prefix + _unique_name + "/keyframe_vote/max_time_span"); nb_vote_for_every_first_ = _server.getParam(prefix + _unique_name + "/keyframe_vote/nb_vote_for_every_first"); - add_3d_cstr_ = _server.getParam(prefix + _unique_name + "/add_3d_cstr"); ippe_min_ratio_ = _server.getParam(prefix + _unique_name + "/ippe_min_ratio"); ippe_max_rep_error_ = _server.getParam(prefix + _unique_name + "/ippe_max_rep_error"); - reestimate_last_frame_ = _server.getParam(prefix + _unique_name + "/reestimate_last_frame"); } std::string print() const override { - return ParamsProcessorTrackerLandmark::print() + "\n" + return ParamsProcessorTrackerLandmark::print() + "\n" + "tag_family_: " + tag_family_ + "\n" + "tag_width_default_: " + std::to_string(tag_width_default_) + "\n" + "tag_widths_: " + converter::convert(tag_widths_) + "\n" @@ -127,13 +130,12 @@ struct ParamsProcessorTrackerLandmarkApriltag : public ParamsProcessorTrackerLan + "debug_: " + std::to_string(debug_) + "\n" + "refine_edges_: " + std::to_string(refine_edges_) + "\n" + "std_pix_: " + std::to_string(std_pix_) + "\n" + + "use_proj_factor_: " + std::to_string(use_proj_factor_) + "\n" + "min_time_span_: " + std::to_string(min_time_span_) + "\n" + "max_time_span_: " + std::to_string(max_time_span_) + "\n" + "nb_vote_for_every_first_: " + std::to_string(nb_vote_for_every_first_) + "\n" - + "add_3d_cstr_: " + std::to_string(add_3d_cstr_) + "\n" + "ippe_min_ratio_: " + std::to_string(ippe_min_ratio_) + "\n" - + "ippe_max_rep_error_: " + std::to_string(ippe_max_rep_error_) + "\n" - + "reestimate_last_frame_: " + std::to_string(reestimate_last_frame_) + "\n"; + + "ippe_max_rep_error_: " + std::to_string(ippe_max_rep_error_) + "\n"; } }; @@ -208,8 +210,6 @@ class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark void configure(SensorBasePtr _sensor) override; - void reestimateLastFrame(); - public: double getTagWidth(int _id) const; std::string getTagFamily() const; @@ -251,11 +251,11 @@ class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark apriltag_detector_t* detector_; apriltag_family_t* tag_family_; double std_pix_; ///< pixel error to be propagated to a camera to tag transformation covariance + bool use_proj_factor_; double ippe_min_ratio_; double ippe_max_rep_error_; Matrix3d K_; cv::Mat_ cv_K_; - bool reestimate_last_frame_; int n_reset_; protected: @@ -269,7 +269,6 @@ class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark double max_time_span_; unsigned int min_features_for_keyframe_; int nb_vote_for_every_first_; - bool add_3d_cstr_; int nb_vote_; public: diff --git a/src/feature/feature_apriltag.cpp b/src/feature/feature_apriltag.cpp deleted file mode 100644 index 74e12fcc26722ceffe48999cd3b3a3fd6c72ce00..0000000000000000000000000000000000000000 --- a/src/feature/feature_apriltag.cpp +++ /dev/null @@ -1,86 +0,0 @@ -//--------LICENSE_START-------- -// -// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. -// Authors: Joan Solà Ortega (jsola@iri.upc.edu) -// All rights reserved. -// -// This file is part of WOLF -// WOLF is free software: you can redistribute it and/or modify -// it under the terms of the GNU Lesser General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU Lesser General Public License for more details. -// -// You should have received a copy of the GNU Lesser General Public License -// along with this program. If not, see . -// -//--------LICENSE_END-------- -#include "apriltag/feature/feature_apriltag.h" - -namespace wolf { - -FeatureApriltag::FeatureApriltag(const Eigen::Vector7d & _measurement, - const Eigen::Matrix6d & _meas_uncertainty, - const int _tag_id, - const apriltag_detection_t & _det, - double _rep_error1, - double _rep_error2, - bool _use_rotation, - UncertaintyType _uncertainty_type) : - FeatureBase("FeatureApriltag", _measurement, _meas_uncertainty, _uncertainty_type), - tag_id_ (_tag_id), - tag_corners_(4), - detection_ (_det), - rep_error1_(_rep_error1), - rep_error2_(_rep_error2), - use_rotation_(_use_rotation) -{ - assert(_det.id == _tag_id && "Tag ID and detection ID do not match!"); - setTrackId(_tag_id); - for (int i = 0; i < 4; i++) - { - tag_corners_[i].x = detection_.p[i][0]; - tag_corners_[i].y = detection_.p[i][1]; - } -} - -FeatureApriltag::~FeatureApriltag() -{ - // -} - -double FeatureApriltag::getTagId() const -{ - return tag_id_; -} - -const apriltag_detection_t& FeatureApriltag::getDetection() const -{ - return detection_; -} - -const std::vector& FeatureApriltag::getTagCorners() const -{ - return tag_corners_; -} - -double FeatureApriltag::getRepError1() const -{ - return rep_error1_; -} - -double FeatureApriltag::getRepError2() const -{ - return rep_error2_; -} - -bool FeatureApriltag::getUserotation() const -{ - return use_rotation_; -} - -} // namespace wolf diff --git a/src/landmark/landmark_apriltag.cpp b/src/landmark/landmark_apriltag.cpp index c7a235a4fab9e2ef460ca779f879fa161bbb0bd9..1f73843906fba33ab37669d39a9ea0f59659a776 100644 --- a/src/landmark/landmark_apriltag.cpp +++ b/src/landmark/landmark_apriltag.cpp @@ -32,7 +32,6 @@ LandmarkApriltag::LandmarkApriltag(Eigen::Vector7d& pose, const int& _tagid, con tag_width_(_tag_width) { setDescriptor(Eigen::VectorXd::Constant(1,_tagid)); //change tagid to int ? do not use descriptor vector ? - setId(_tagid); // overwrite lmk ID to match tag_id. } LandmarkApriltag::~LandmarkApriltag() @@ -57,7 +56,6 @@ int LandmarkApriltag::getTagId() const LandmarkBasePtr LandmarkApriltag::create(const YAML::Node& _lmk_node) { // Parse YAML node with lmk info and data - unsigned int id = _lmk_node["id"] .as(); unsigned int tag_id = _lmk_node["tag id"] .as(); double tag_width = _lmk_node["tag width"] .as(); Eigen::Vector3d pos = _lmk_node["position"] .as(); @@ -82,7 +80,6 @@ LandmarkBasePtr LandmarkApriltag::create(const YAML::Node& _lmk_node) // Create a new landmark LandmarkApriltagPtr lmk_ptr = std::make_shared(pose, tag_id, tag_width); - lmk_ptr->setId(id); lmk_ptr->getP()->setFixed(pos_fixed); lmk_ptr->getO()->setFixed(ori_fixed); diff --git a/src/processor/processor_tracker_landmark_apriltag.cpp b/src/processor/processor_tracker_landmark_apriltag.cpp index a47b16b076c18c07a9e94529ec4b559f807acdf6..3efdbc8540b6bf761d378f4da5074a08fba27e15 100644 --- a/src/processor/processor_tracker_landmark_apriltag.cpp +++ b/src/processor/processor_tracker_landmark_apriltag.cpp @@ -31,18 +31,16 @@ ProcessorTrackerLandmarkApriltag::ProcessorTrackerLandmarkApriltag( ParamsProces 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_), + use_proj_factor_(_params_tracker_landmark_apriltag->use_proj_factor_), ippe_min_ratio_(_params_tracker_landmark_apriltag->ippe_min_ratio_), ippe_max_rep_error_(_params_tracker_landmark_apriltag->ippe_max_rep_error_), cv_K_(3,3), - reestimate_last_frame_(_params_tracker_landmark_apriltag->reestimate_last_frame_), n_reset_(0), min_time_span_(_params_tracker_landmark_apriltag->min_time_span_), max_time_span_(_params_tracker_landmark_apriltag->max_time_span_), min_features_for_keyframe_(_params_tracker_landmark_apriltag->min_features_for_keyframe), nb_vote_for_every_first_(_params_tracker_landmark_apriltag->nb_vote_for_every_first_), - add_3d_cstr_(_params_tracker_landmark_apriltag->add_3d_cstr_), nb_vote_(0) - { // configure apriltag detector std::string famname(_params_tracker_landmark_apriltag->tag_family_); @@ -131,47 +129,69 @@ void ProcessorTrackerLandmarkApriltag::preProcess() int tag_id = det->id; double tag_width = getTagWidth(tag_id); // tag width in meters - Eigen::Isometry3d c_M_t; - bool use_rotation = true; ////////////////// // IPPE (Infinitesimal Plane-based Pose Estimation) + // TODO: Quite bad for reprojection factor: we only need to do this question once + // when the landmark is newly discovered. Would require to change the whole logic + // or to check if this landmark is already in the map HERE (which is usually not the logic of preProcess) ////////////////// - Eigen::Isometry3d M_ippe1, M_ippe2, M_april, M_PnP; - double rep_error1; - double rep_error2; + Eigen::Isometry3d M_ippe1, M_ippe2; + double rep_error1, rep_error2; ippePoseEstimation(det, cv_K_, tag_width, M_ippe1, rep_error1, M_ippe2, rep_error2); // If not so sure about whether we have the right solution or not, do not create a feature - use_rotation = ((rep_error2 / rep_error1 > ippe_min_ratio_) && rep_error1 < ippe_max_rep_error_); + bool use_rotation = ((rep_error2 / rep_error1 > ippe_min_ratio_) && rep_error1 < ippe_max_rep_error_); ////////////////// - c_M_t = M_ippe1; + M_ippe1 = M_ippe1; // set the measured pose vector - Eigen::Vector3d translation ( c_M_t.translation() ); // translation vector in apriltag meters - Eigen::Matrix3d cRt = c_M_t.linear(); - Eigen::Vector7d pose; pose << translation, R2q(cRt).coeffs(); - - // compute the covariance - // Eigen::Matrix6d cov = getVarVec().asDiagonal() ; // fixed dummy covariance - Eigen::Matrix6d info = computeInformation(translation, c_M_t.linear(), K_, tag_width, std_pix_); // Lie jacobians covariance - - if (!use_rotation){ - // Put a very high covariance on angles measurements (low info matrix) - info.bottomLeftCorner(3,3) = Eigen::Matrix3d::Zero(); - info.topRightCorner(3,3) = Eigen::Matrix3d::Zero(); - info.bottomRightCorner(3,3) = 0.0001 * Eigen::Matrix3d::Identity(); + Eigen::Vector3d p_c_t ( M_ippe1.translation() ); // translation vector in apriltag meters + Eigen::Matrix3d R_c_t = M_ippe1.linear(); + Eigen::Vector7d pose; pose << p_c_t, R2q(R_c_t).coeffs(); + + // Order of the 2d corners (anti clockwise, looking at the tag). + // Looking at the tag, the reference frame is + // X = Right, Y = Down, Z = Inside the plane + Vector8d corners_vec; corners_vec << det->p[0][0], det->p[0][1], // bottom left + det->p[1][0], det->p[1][1], // bottom right + det->p[2][0], det->p[2][1], // top right + det->p[3][0], det->p[3][1]; // top left + + if (use_proj_factor_) + { + // add to detected features list + detections_incoming_.push_back(std::make_shared(corners_vec, + std_pix_*std_pix_*Eigen::Matrix8d::Identity(), + tag_id, + tag_width, + pose)); } + else + { + // compute the covariance + // Eigen::Matrix6d cov = getVarVec().asDiagonal() ; // fixed dummy covariance + Eigen::Matrix6d info = computeInformation(p_c_t, R_c_t, K_, tag_width, std_pix_); // Lie jacobians covariance + + if (!use_rotation){ + // Put a very high covariance on angles measurements (low info matrix) + info.bottomLeftCorner(3,3) = Eigen::Matrix3d::Zero(); + info.topRightCorner(3,3) = Eigen::Matrix3d::Zero(); + info.bottomRightCorner(3,3) = M_1_PI*M_1_PI * Eigen::Matrix3d::Identity(); // 180 degrees standar deviation + } - // add to detected features list - detections_incoming_.push_back(std::make_shared(pose, - info, - tag_id, - *det, - rep_error1, - rep_error2, - use_rotation, - FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO)); + // add to detected features list + detections_incoming_.push_back(std::make_shared(pose, + info, + tag_id, + corners_vec, + rep_error1, + rep_error2, + use_rotation, + FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO)); + } } + WOLF_INFO("\ndetections_incoming_: ", detections_incoming_.size()) + apriltag_detections_destroy(detections); } @@ -184,21 +204,17 @@ void ProcessorTrackerLandmarkApriltag::ippePoseEstimation(apriltag_detection_t * double &_rep_error2){ // get corners from det + // Order of the 2d corners (anti clockwise, looking at the tag). + // Looking at the tag, the reference frame is + // X = Right, Y = Down, Z = Inside the plane + // bottom left, bottom right, top right ,top left std::vector corners_pix(4); for (int i = 0; i < 4; i++) { corners_pix[i].x = _det->p[i][0]; corners_pix[i].y = _det->p[i][1]; } - std::vector obj_pts; - // Same order as the 2d corners (anti clockwise, looking at the tag). - // Looking at the tag, the reference frame is - // X = Right, Y = Down, Z = Inside the plane - double s = _tag_width/2; - obj_pts.emplace_back(-s, s, 0); // bottom left - obj_pts.emplace_back( s, s, 0); // bottom right - obj_pts.emplace_back( s, -s, 0); // top right - obj_pts.emplace_back(-s, -s, 0); // top left + cv::Mat rvec1, tvec1, rvec2, tvec2; float err1, err2; @@ -236,11 +252,27 @@ void ProcessorTrackerLandmarkApriltag::postProcess() FactorBasePtr ProcessorTrackerLandmarkApriltag::emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) { - return FactorBase::emplace(_feature_ptr, - _feature_ptr, - _landmark_ptr, - shared_from_this(), - params_->apply_loss_function); + auto feat_pose = std::dynamic_pointer_cast(_feature_ptr); + if (feat_pose) + { + return FactorBase::emplace(_feature_ptr, + _feature_ptr, + _landmark_ptr, + shared_from_this(), + params_->apply_loss_function); + } + else + { + auto feat_proj = std::dynamic_pointer_cast(_feature_ptr); + return FactorBase::emplace(feat_proj, + feat_proj, + getSensor(), + feat_proj->getFrame(), + _landmark_ptr, + shared_from_this(), + params_->apply_loss_function); + } + } LandmarkBasePtr ProcessorTrackerLandmarkApriltag::emplaceLandmark(FeatureBasePtr _feature_ptr) @@ -258,8 +290,17 @@ LandmarkBasePtr ProcessorTrackerLandmarkApriltag::emplaceLandmark(FeatureBasePtr Eigen::Isometry3d r_M_c = Eigen::Translation(pos.head(3)) * quat_tmp; // camera to lmk (tag) - pos = _feature_ptr->getMeasurement().head(3); - quat_tmp.coeffs() = _feature_ptr->getMeasurement().tail(4); + if (use_proj_factor_) + { + auto feat_proj = std::dynamic_pointer_cast(_feature_ptr); + pos = feat_proj->getPosePnp().head(3); + quat_tmp.coeffs() = feat_proj->getPosePnp().tail(4); + } + else + { + pos = _feature_ptr->getMeasurement().head(3); + quat_tmp.coeffs() = _feature_ptr->getMeasurement().tail(4); + } Eigen::Isometry3d c_M_t = Eigen::Translation(pos) * quat_tmp; // world to lmk (tag) @@ -272,7 +313,7 @@ LandmarkBasePtr ProcessorTrackerLandmarkApriltag::emplaceLandmark(FeatureBasePtr Vector7d w_pose_t; w_pose_t << pos, quat_tmp.coeffs(); - FeatureApriltagPtr feat_april = std::static_pointer_cast(_feature_ptr); + FeatureApriltagPosePtr feat_april = std::static_pointer_cast(_feature_ptr); int tag_id = feat_april->getTagId(); return LandmarkBase::emplace(getProblem()->getMap(), w_pose_t, tag_id, getTagWidth(tag_id)); @@ -282,31 +323,41 @@ unsigned int ProcessorTrackerLandmarkApriltag::detectNewFeatures(const int& _max const CaptureBasePtr& _capture, FeatureBasePtrList& _features_out) { - // list of landmarks in the map - const LandmarkBasePtrList& landmark_list = getProblem()->getMap()->getLandmarkList(); - for (auto feature_in_image : detections_last_) + // _max_new_features: max number of new features to be detected + // _capture: capture in which are stored the features + // _features_out: filled with new features in last. Empty at the beginning of the call + + // detectNewFeatures is called by processNew() in ProcessorTrackerLandmark + // processNew is only called when a KF is created by the processor or another + // (except for the SECOND_TIME_WITHOUT_KEYFRAME case) + // -> use detections in last frame/capture to detect features that cannot be associated to any landmark, create one accordingly + + // Loop over the detections in last and check + for (auto feat: detections_last_) { // max_new_features reached if (_max_new_features != -1 && _features_out.size() == _max_new_features) break; - bool feature_already_found(false); - - auto feature_april = std::static_pointer_cast(feature_in_image); - - //Loop over the landmark to find is one is associated to feature_in_image - for(auto it = landmark_list.begin(); it != landmark_list.end(); ++it){ - if(std::static_pointer_cast(*it)->getTagId() == feature_april->getTagId()){ - feature_already_found = true; + auto feat_april = std::static_pointer_cast(feat); + bool match_found = false; + // Loop over the landmarks to find if one is associated to the + for(auto lmk: getProblem()->getMap()->getLandmarkList()){ + auto lmk_april = std::dynamic_pointer_cast(lmk); + if (lmk_april and lmk_april->getTagId() == feat_april->getTagId()) + { + match_found = true; break; } } - if (!feature_already_found) + // if the feature was not matched to any existing landmark, add it to the new features list + if (!match_found) { + // remove duplicate features (detections that correspond to the same landmark, even though this should never happen) for (FeatureBasePtrList::iterator it=_features_out.begin(); it != _features_out.end(); ++it) { - if (std::static_pointer_cast(*it)->getTagId() == feature_april->getTagId()) + if (std::static_pointer_cast(*it)->getTagId() == feat_april->getTagId()) { //we have a detection with the same id as the currently processed one. We remove the previous feature from the list for now _features_out.erase(it); @@ -314,15 +365,12 @@ unsigned int ProcessorTrackerLandmarkApriltag::detectNewFeatures(const int& _max break; } } - // discard features that do not have orientation information - if (!feature_april->getUserotation()) - continue; // If the feature is not in the map & not in the list of newly detected features yet then we add it. - _features_out.push_back(feature_in_image); + _features_out.push_back(feat); // link feature (they are created unlinked in preprocess()) - feature_in_image->link(_capture); + feat->link(_capture); } //otherwise we check the next feature } @@ -365,19 +413,21 @@ unsigned int ProcessorTrackerLandmarkApriltag::findLandmarks(const LandmarkBaseP FeatureBasePtrList& _features_out, LandmarkMatchMap& _feature_landmark_correspondences) { - for (auto feature_in_image : detections_incoming_) + for (auto feat : detections_incoming_) { - int tag_id(std::static_pointer_cast(feature_in_image)->getTagId()); + int tag_id(std::static_pointer_cast(feat)->getTagId()); - for (auto landmark_in_ptr : _landmarks_in) + for (auto lmk : _landmarks_in) { - if(std::static_pointer_cast(landmark_in_ptr)->getTagId() == tag_id) + auto lmk_april = std::dynamic_pointer_cast(lmk); + // would be so much easier with an std::map + if(lmk_april and lmk_april->getTagId() == tag_id) { - _features_out.push_back(feature_in_image); + _features_out.push_back(feat); double score(1.0); - LandmarkMatchPtr matched_landmark = std::make_shared(landmark_in_ptr, score); - _feature_landmark_correspondences.emplace ( feature_in_image, matched_landmark ); - feature_in_image->link(_capture); // since all features are created in preProcess() are unlinked + LandmarkMatchPtr matched_landmark = std::make_shared(lmk, score); + _feature_landmark_correspondences.emplace ( feat, matched_landmark ); + feat->link(_capture); // since all features are created in preProcess() are unlinked break; } } @@ -511,130 +561,10 @@ void ProcessorTrackerLandmarkApriltag::advanceDerived() void ProcessorTrackerLandmarkApriltag::resetDerived() { - // BAD -> should be rewritten some other way - if (getProblem()->getMotionProviderMap().empty() && reestimate_last_frame_){ - reestimateLastFrame(); - } - ProcessorTrackerLandmark::resetDerived(); detections_last_ = std::move(detections_incoming_); } -void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){ - // Rewrite the last frame state and landmark state initialised during last frame creation to account - // for a better estimate of the current state using the last landmark detection. - // Otherwise, default behaviour is to take the last KF as an initialization of the new KF which can lead - // to the solver finding local minima - - // When called for the first time, no feature list initialized in ori/last(?) - if (n_reset_ < 1){ - n_reset_ += 1; - return; - } - - // A TESTER - // (getOrigin() != nullptr) - - // Retrieve camera extrinsics - Eigen::Quaterniond last_q_cam(getSensor()->getO()->getState().data()); - Eigen::Isometry3d last_M_cam = Eigen::Translation3d(getSensor()->getP()->getState()) * last_q_cam; - - // get features list of KF linked to origin capture and last capture - FeatureBasePtrList ori_feature_list = getOrigin()->getFeatureList(); - FeatureBasePtrList last_feature_list = getLast()->getFeatureList(); - - if (last_feature_list.size() == 0 || ori_feature_list.size() == 0){ - return; - } - - // Among landmarks detected in origin and last, find the one that has the smallest error ratio (best confidence) - double lowest_ration = 1; // rep_error1/rep_error2 cannot be higher than 1 - FeatureApriltagPtr best_feature; - bool useable_feature = false; - for (auto it_last = last_feature_list.begin(); it_last != last_feature_list.end(); it_last++){ - FeatureApriltagPtr last_feat_ptr = std::static_pointer_cast(*it_last); - for (auto it_ori = ori_feature_list.begin(); it_ori != ori_feature_list.end(); it_ori++){ - FeatureApriltagPtr ori_feat_ptr = std::static_pointer_cast(*it_ori); - if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){ - double ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2(); - //if (ratio < lowest_ration){ - if (last_feat_ptr->getUserotation() && (ratio < lowest_ration)){ - useable_feature = true; - lowest_ration = ratio; - best_feature = last_feat_ptr; - } - } - } - } - // If there is no common feature between the two images, the continuity is broken and - // nothing can be estimated. In the case of aprilslam alone, this result in a broken factor map - if (!useable_feature){ - return; - } - - // std::cout << "Best feature id after: " << best_feature->getTagId() << std::endl; - // Retrieve cam to landmark transform - Eigen::Vector7d cam_pose_lmk = best_feature->getMeasurement(); - Eigen::Quaterniond cam_q_lmk(cam_pose_lmk.segment<4>(3).data()); - Eigen::Isometry3d cam_M_lmk = Eigen::Translation3d(cam_pose_lmk.head(3)) * cam_q_lmk; - - // Get corresponding landmarks in origin/last landmark list - Eigen::Isometry3d w_M_lmk; - LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList(); - // Iterate in reverse order because landmark detected in last are at the end of the list (while still landmarks to discovers) - for (std::list::reverse_iterator it_lmk = lmk_list.rbegin(); it_lmk != lmk_list.rend(); ++it_lmk){ - LandmarkApriltagPtr lmk_ptr = std::dynamic_pointer_cast(*it_lmk); - // the map might contain other types of landmarks so check if the cast is valid - if (lmk_ptr == nullptr){ - continue; - } - - if (lmk_ptr->getTagId() == best_feature->getTagId()){ - Eigen::Vector3d w_t_lmk = lmk_ptr->getP()->getState(); - Eigen::Quaterniond w_q_lmk(lmk_ptr->getO()->getState().data()); - w_M_lmk = Eigen::Translation(w_t_lmk) * w_q_lmk; - } - } - - // Compute last frame estimate - Eigen::Isometry3d w_M_last = w_M_lmk * (last_M_cam * cam_M_lmk).inverse(); - - // Use the w_M_last to overide last key frame state estimation and keyframe estimation - Eigen::Vector3d pos_last = w_M_last.translation(); - Eigen::Quaterniond quat_last(w_M_last.linear()); - getLast()->getFrame()->getP()->setState(pos_last); - getLast()->getFrame()->getO()->setState(quat_last.coeffs()); - - // if (!best_feature->getUserotation()){ - // return; - // } - /////////////////// - // Reestimate position of landmark new in Last - /////////////////// - for (auto it_feat = new_features_last_.begin(); it_feat != new_features_last_.end(); it_feat++){ - FeatureApriltagPtr new_feature_last = std::static_pointer_cast(*it_feat); - - Eigen::Vector7d cam_pose_lmk = new_feature_last->getMeasurement(); - Eigen::Quaterniond cam_q_lmk(cam_pose_lmk.segment<4>(3).data()); - Eigen::Isometry3d cam_M_lmk_new = Eigen::Translation3d(cam_pose_lmk.head(3)) * cam_q_lmk; - Eigen::Isometry3d w_M_lmk = w_M_last * last_M_cam * cam_M_lmk_new; - - for (auto it_lmk = new_landmarks_.begin(); it_lmk != new_landmarks_.end(); ++it_lmk){ - LandmarkApriltagPtr lmk_ptr = std::dynamic_pointer_cast(*it_lmk); - if (lmk_ptr == nullptr){ - continue; - } - if (lmk_ptr->getTagId() == new_feature_last->getTagId()){ - Eigen::Vector3d pos_lmk_last = w_M_lmk.translation(); - Eigen::Quaterniond quat_lmk_last(w_M_lmk.linear()); - lmk_ptr->getP()->setState(pos_lmk_last); - lmk_ptr->getO()->setState(quat_lmk_last.coeffs()); - break; - } - } - } -} - std::string ProcessorTrackerLandmarkApriltag::getTagFamily() const { return tag_family_->name; diff --git a/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp b/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp index ded4f69690a45ed266f6c086b18581cadeae5036..4019560b0672ea11f08c8b11e0bfcb33c9c7b33e 100644 --- a/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp +++ b/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp @@ -79,6 +79,8 @@ static ParamsProcessorBasePtr createParamsProcessorLandmarkApriltag(const std::s YAML::Node noise = config["noise"]; params->std_pix_ = noise["std_pix"] .as(); + params->use_proj_factor_ = config["use_proj_factor"] .as(); + YAML::Node vote = config["vote"]; params->voting_active = vote["voting active"] .as(); params->min_time_span_ = vote["min_time_vote"] .as(); @@ -86,9 +88,6 @@ static ParamsProcessorBasePtr createParamsProcessorLandmarkApriltag(const std::s params->min_features_for_keyframe = vote["min_features_for_keyframe"] .as(); params->nb_vote_for_every_first_ = vote["nb_vote_for_every_first"] .as(); - params->reestimate_last_frame_ = config["reestimate_last_frame"] .as(); - params->add_3d_cstr_ = config["add_3d_cstr"] .as(); - params->max_new_features = config["max_new_features"] .as(); params->apply_loss_function = config["apply_loss_function"] .as(); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index e8b6ff0b16e3cd90e7e90cc039062f13ff545a06..1c0331b4b30ead1d6583d80c5e4927e809090700 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -8,8 +8,10 @@ add_subdirectory(gtest) # # ########################################################### -wolf_add_gtest(gtest_feature_apriltag gtest_feature_apriltag.cpp) +wolf_add_gtest(gtest_features_apriltag gtest_features_apriltag.cpp) wolf_add_gtest(gtest_landmark_apriltag gtest_landmark_apriltag.cpp) wolf_add_gtest(gtest_processor_tracker_landmark_apriltag gtest_processor_tracker_landmark_apriltag.cpp) + +wolf_add_gtest(gtest_factor_apriltag_proj gtest_factor_apriltag_proj.cpp) diff --git a/test/gtest_factor_apriltag.cpp b/test/gtest_factor_apriltag.cpp deleted file mode 100644 index de97ff55e9fe5a64456088d8797be1d5b87ff054..0000000000000000000000000000000000000000 --- a/test/gtest_factor_apriltag.cpp +++ /dev/null @@ -1,415 +0,0 @@ -//--------LICENSE_START-------- -// -// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. -// Authors: Joan Solà Ortega (jsola@iri.upc.edu) -// All rights reserved. -// -// This file is part of WOLF -// WOLF is free software: you can redistribute it and/or modify -// it under the terms of the GNU Lesser General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU Lesser General Public License for more details. -// -// You should have received a copy of the GNU Lesser General Public License -// along with this program. If not, see . -// -//--------LICENSE_END-------- -#include - -#include "core/common/wolf.h" -#include "core/utils/logging.h" - -#include "core/ceres_wrapper/solver_ceres.h" -#include "core/processor/factory_processor.h" - -#include "vision/capture/capture_image.h" - -#include "apriltag/processor/processor_tracker_landmark_apriltag.h" -#include "apriltag/factor/factor_apriltag.h" -#include "apriltag/internal/config.h" - -#include - -using namespace Eigen; -using namespace wolf; -using std::static_pointer_cast; - -//////////////////////////////////////////////////////////////// -/* - * Wrapper class to be able to have setOrigin() and setLast() in ProcessorTrackerLandmarkApriltag - */ -WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkApriltag_Wrapper); -class ProcessorTrackerLandmarkApriltag_Wrapper : public ProcessorTrackerLandmarkApriltag -{ - public: - ProcessorTrackerLandmarkApriltag_Wrapper(ParamsProcessorTrackerLandmarkApriltagPtr _params_tracker_landmark_apriltag) : - ProcessorTrackerLandmarkApriltag(_params_tracker_landmark_apriltag) - { - setType("ProcessorTrackerLandmarkApriltag_Wrapper"); - }; - ~ProcessorTrackerLandmarkApriltag_Wrapper() override{} - void setOriginPtr(const CaptureBasePtr _origin_ptr) { origin_ptr_ = _origin_ptr; } - void setLastPtr (const CaptureBasePtr _last_ptr) { last_ptr_ = _last_ptr; } - void setIncomingPtr (const CaptureBasePtr _incoming_ptr) { incoming_ptr_ = _incoming_ptr; } - - // for factory - static ProcessorBasePtr create(const std::string& _unique_name, const ParamsProcessorBasePtr _params) - { - auto prc_apriltag_params_ = std::static_pointer_cast(_params); - - auto prc_ptr = std::make_shared(prc_apriltag_params_); - - prc_ptr->setName(_unique_name); - - return prc_ptr; - } - -}; -namespace wolf{ -// Register in the Factories -WOLF_REGISTER_PROCESSOR(ProcessorTrackerLandmarkApriltag_Wrapper); -} -//////////////////////////////////////////////////////////////// - -// Use the following in case you want to initialize tests with predefines variables or methods. -class FactorApriltag_class : public testing::Test{ - public: - Vector3d pos_camera, pos_robot, pos_landmark; - Vector3d euler_camera, euler_robot, euler_landmark; - Quaterniond quat_camera, quat_robot, quat_landmark; - Vector4d vquat_camera, vquat_robot, vquat_landmark; // quaternions as vectors - Vector7d pose_camera, pose_robot, pose_landmark; - - ProblemPtr problem; - SolverCeresPtr solver; - - SensorCameraPtr camera; - ProcessorTrackerLandmarkApriltag_WrapperPtr proc_apriltag; - - SensorBasePtr S; - FrameBasePtr F1; - CaptureImagePtr C1; - FeatureApriltagPtr f1; - LandmarkApriltagPtr lmk1; - FactorApriltagPtr c_tag; - apriltag_detection_t det; - - double rep_error1; - double rep_error2; - bool use_rotation; - - Eigen::Matrix6d meas_cov; - - void SetUp() override - { - std::string wolf_root = _WOLF_APRILTAG_ROOT_DIR; - - // configuration - - /* We have three poses to take into account: - * - pose of the camera (extrinsincs) - * - pose of the landmark - * - pose of the robot (Keyframe) - * - * The measurement provides the pose of the landmark wrt camera's current pose in the world. - * Camera's current pose in World is the composition of the robot pose with the camera extrinsics. - * - * The robot has a camera looking forward - * S: pos = (0,0,0), ori = (0, 0, 0) - * - * There is a point at the origin - * P: pos = (0,0,0) - * - * The camera is canonical - * K = Id. - * - * Therefore, P projects exactly at the origin of the camera, - * f: p = (0,0) - * - * We form a Wolf tree with 1 frames F1, 1 capture C1, - * 1 feature f1 (measurement), 1 landmark l and 1 apriltag constraint c1: - * - * Frame F1, Capture C1, feature f1, landmark l1, constraint c1 - * - * The frame pose F1, and the camera pose S - * in the robot frame are variables subject to optimization - * - * We perform a number of tests based on this configuration.*/ - - - // camera is at origin of robot and looking forward - // robot is at (0,0,0) - // landmark is right in front of camera. Its orientation wrt camera is identity. - pos_camera << 0,0,0; - pos_robot << 0,0,0; //robot is at origin - pos_landmark << 0,1,0; - euler_camera << 0,0,0; - //euler_camera << -M_PI_2, 0, -M_PI_2; - euler_robot << 0,0,0; - euler_landmark << 0,0,0; - quat_camera = e2q(euler_camera); - quat_robot = e2q(euler_robot); - quat_landmark = e2q(euler_landmark); - vquat_camera = quat_camera.coeffs(); - vquat_robot = quat_robot.coeffs(); - vquat_landmark = quat_landmark.coeffs(); - pose_camera << pos_camera, vquat_camera; - pose_robot << pos_robot, vquat_robot; - pose_landmark << pos_landmark, vquat_landmark; - - // Build problem - problem = Problem::create("PO", 3); - solver = std::make_shared(problem); - - // Install sensor and processor - S = problem->installSensor("SensorCamera", "canonical", pose_camera, wolf_root + "/demos/camera_params_canonical.yaml"); - camera = std::static_pointer_cast(S); - - ParamsProcessorTrackerLandmarkApriltagPtr params = std::make_shared(); - // Need to set some parameters ? do it now ! - params->tag_family_ = "tag36h11"; - params->time_tolerance = 1; - //params->name = params->tag_family_; - - ProcessorBasePtr proc = problem->installProcessor("ProcessorTrackerLandmarkApriltag_Wrapper", "apriltags_wrapper", camera, params); - proc_apriltag = std::static_pointer_cast(proc); - - // F1 is be origin KF - VectorComposite x0(pose_robot, "PO", {3,4}); - VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)}); - F1 = problem->setPriorFactor(x0, s0, 0.0); - - //emplace capture & set as last and origin - C1 = std::static_pointer_cast(CaptureBase::emplace(F1, 1.0, camera, cv::Mat(2,2,CV_8UC1))); - proc_apriltag->setOriginPtr(C1); - proc_apriltag->setLastPtr(C1); - - // the sensor is at origin as well as the robot. The measurement matches with the pose of the tag wrt camera (and also wrt robot and world) - meas_cov = Eigen::Matrix6d::Identity(); - meas_cov.topLeftCorner(3,3) *= 1e-2; - meas_cov.bottomRightCorner(3,3) *= 1e-3; - int tag_id = 1; - - det.id = tag_id; - det.p[0][0] = 1.0; - det.p[0][1] = -1.0; - det.p[1][0] = 1.0; - det.p[1][1] = 1.0; - det.p[2][0] = -1.0; - det.p[2][1] = 1.0; - det.p[3][0] = -1.0; - det.p[3][1] = -1.0; - - rep_error1 = 0.01; - rep_error2 = 0.1; - use_rotation = true; - - //emplace feature and landmark - f1 = std::static_pointer_cast(FeatureBase::emplace(C1, pose_landmark, meas_cov, det.id, det, rep_error1, rep_error2, use_rotation)); - lmk1 = std::static_pointer_cast(proc_apriltag->emplaceLandmark(f1)); - } -}; - - -TEST_F(FactorApriltag_class, Constructor) -{ - FactorApriltagPtr factor = std::make_shared( - S, - F1, - lmk1, - f1, - nullptr, - false, - FAC_ACTIVE - ); - - ASSERT_TRUE(factor->getType() == "FactorApriltag"); -} - -TEST_F(FactorApriltag_class, Check_tree) -{ - auto factor = FactorBase::emplace(f1, - S, - F1, - lmk1, - f1, - nullptr, - false, - FAC_ACTIVE); - ASSERT_TRUE(problem->check(1)); -} - -TEST_F(FactorApriltag_class, solve_F1_P_perturbated) -{ - auto factor = FactorBase::emplace(f1, - S, - F1, - lmk1, - f1, - nullptr, - false, - FAC_ACTIVE); - ASSERT_TRUE(problem->check(1)); - - // unfix F1, perturbate state - F1->unfix(); - F1->getP()->perturb(); - - std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); -} - -TEST_F(FactorApriltag_class, solve_F1_O_perturbated) -{ - auto factor = FactorBase::emplace(f1, - S, - F1, - lmk1, - f1, - nullptr, - false, - FAC_ACTIVE); - - // unfix F1, perturbate state - F1->unfix(); - F1->getO()->perturb(); - - std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); -} - -TEST_F(FactorApriltag_class, Check_initialization) -{ - auto factor = FactorBase::emplace(f1, - S, - F1, - lmk1, - f1, - nullptr, - false, - FAC_ACTIVE); - - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); - ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); - -} - -TEST_F(FactorApriltag_class, solve_L1_P_perturbated) -{ - auto factor = FactorBase::emplace(f1, - S, - F1, - lmk1, - f1, - nullptr, - false, - FAC_ACTIVE); - - // unfix lmk1, perturbate state - lmk1->unfix(); - lmk1->getP()->perturb(); - - std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); -} - -TEST_F(FactorApriltag_class, solve_L1_O_perturbated) -{ - auto factor = FactorBase::emplace(f1, - S, - F1, - lmk1, - f1, - nullptr, - false, - FAC_ACTIVE); - - // unfix F1, perturbate state - lmk1->unfix(); - lmk1->getO()->perturb(); - - std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); - -} - -TEST_F(FactorApriltag_class, solve_L1_PO_perturbated) -{ - // Change setup - Vector3d p_w_r, p_r_c, p_c_l, p_w_l; - Quaterniond q_w_r, q_r_c, q_c_l, q_w_l; - p_w_r << 1, 2, 3; - p_r_c << 4, 5, 6; - p_c_l << 7, 8, 9; - q_w_r.coeffs() << 1, 2, 3, 4; q_w_r.normalize(); - q_r_c.coeffs() << 4, 5, 6, 7; q_r_c.normalize(); - q_c_l.coeffs() << 7, 8, 9, 0; q_c_l.normalize(); - - q_w_l = q_w_r * q_r_c * q_c_l; - p_w_l = p_w_r + q_w_r * (p_r_c + q_r_c * p_c_l); - - // Change feature (remove and emplace) - Vector7d meas; - meas << p_c_l, q_c_l.coeffs(); - f1->remove(); - f1 = std::static_pointer_cast(FeatureBase::emplace(C1, meas, meas_cov, det.id, det, rep_error1, rep_error2, use_rotation)); - - // emplace factor - auto factor = FactorBase::emplace(f1, - S, - F1, - lmk1, - f1, - nullptr, - false, - FAC_ACTIVE); - - // Change Landmark - lmk1->getP()->setState(p_w_l); - lmk1->getO()->setState(q_w_l.coeffs()); - ASSERT_TRUE(lmk1->getP()->stateUpdated()); - ASSERT_TRUE(lmk1->getO()->stateUpdated()); - - // Change Frame - F1->getP()->setState(p_w_r); - F1->getO()->setState(q_w_r.coeffs()); - F1->fix(); - ASSERT_TRUE(F1->getP()->stateUpdated()); - ASSERT_TRUE(F1->getO()->stateUpdated()); - - // Change sensor extrinsics - S->getP()->setState(p_r_c); - S->getO()->setState(q_r_c.coeffs()); - ASSERT_TRUE(S->getP()->stateUpdated()); - ASSERT_TRUE(S->getO()->stateUpdated()); - - Vector7d t_w_r, t_w_l; - t_w_r << p_w_r, q_w_r.coeffs(); - t_w_l << p_w_l, q_w_l.coeffs(); - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), t_w_r, 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), t_w_l, 1e-6); - - // unfix LMK, perturbate state - lmk1->unfix(); - lmk1->perturb(); - - std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO").transpose(), t_w_r.transpose(), 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO").transpose(), t_w_l.transpose(), 1e-6); - -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_factor_apriltag_proj.cpp b/test/gtest_factor_apriltag_proj.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0e419c72c5e84b1b2f3b4cc61847bc146575decf --- /dev/null +++ b/test/gtest_factor_apriltag_proj.cpp @@ -0,0 +1,330 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . +// +//--------LICENSE_END-------- +#include + + +// Wolf apriltag +#include "apriltag/processor/processor_tracker_landmark_apriltag.h" +#include "apriltag/feature/feature_apriltag_proj.h" +#include "apriltag/factor/factor_apriltag_proj.h" +#include "apriltag/internal/config.h" + +// Wolf vision +#include + +// Wolf core +#include +#include +#include +#include +#include + +// UMich apriltag +#include + + +using namespace Eigen; +using namespace wolf; +using std::static_pointer_cast; + + +Isometry3d pose2iso(const Vector3d& posi, const Quaterniond& quat) +{ + return Translation(posi) * quat; +} + +Isometry3d pose2iso(const Vector7d& pose) +{ + Vector3d posi = pose.segment<3>(0); + Quaterniond quat(pose.segment<4>(3)); + return pose2iso(posi, quat); +} + +Vector7d iso2pose(const Isometry3d& T) +{ + Vector7d pose; + pose << T.translation(), Quaterniond(T.rotation()).coeffs(); + return pose; +} + +Vector7d posiquat2pose(const Vector3d& posi, const Quaterniond& quat) +{ + Vector7d pose; + pose << posi, quat.coeffs(); + return pose; +} + +// Use the following in case you want to initialize tests with predefines variables or methods. +class FactorApriltagProj_class : public testing::Test{ + public: + + // ground truth needed for individual tests + Vector3d p_w_r1, p_w_r2; + Quaterniond q_w_r1, q_w_r2; + Vector3d p_r_c; + Quaterniond q_r_c; + Vector3d p_w_l; + Quaterniond q_w_l; + // measurements + Vector3d p_c1_l; + Quaterniond q_c1_l; + Vector3d p_c2_l; + Quaterniond q_c2_l; + + // WOLF + ProblemPtr problem; + SolverCeresPtr solver; + SensorCameraPtr camera; + + FrameBasePtr F1; + CaptureImagePtr C1; + FeatureApriltagProjPtr f1; + LandmarkApriltagPtr lmk1; + FactorApriltagProjPtr fac_a1; + + // Apriltag + apriltag_detection_t det; + double tag_width; + Matrix3d K; + + // corners in tag frame + Eigen::Vector3d l_corn1; + Eigen::Vector3d l_corn2; + Eigen::Vector3d l_corn3; + Eigen::Vector3d l_corn4; + Vector8d meas1; + Vector8d meas2; + Eigen::Matrix8d meas_cov; + Eigen::Vector7d pose_dummy; + + void SetUp() override + { + std::string wolf_root = _WOLF_APRILTAG_ROOT_DIR; + + // configuration + + /* We have three types of poses to take into account: + * - robot->camera (extrinsincs) p_r_c, q_r_c + * - world->landmark p_w_l, q_w_l + * - world->robot (Keyframe) p_w_r, q_w_r + * + * Factor graph with 2 Keyframes and 1 lmks + * + * + * + * L + * |\ + * | \ + * E----|--\ + * | \ + * KF1 KF2 + * + * Measurements are pixel projections of the landmarks tags corners (8D), associated error is pixel covariance (8x8) + * + */ + + + // camera is at origin of robot and looking forward + // robot is at (0,0,0) + // landmark is right in front of camera. Its orientation wrt camera is identity. + + + // ground truth needed for individual tests + // robot pose 1 + p_w_r1 << 0,0,0; + Vector3d euler_w_r1; euler_w_r1 << 0.0, 0.0, 0.0; + q_w_r1 = e2q(euler_w_r1); + // robot pose 2 + p_w_r2 << 0,-0.1,0; + Vector3d euler_w_r2; euler_w_r2 << 0.1, 0.1, 0.1; + q_w_r2 = e2q(euler_w_r2); + // extrinsics + p_r_c << 0,0,0; + Vector3d euler_r_c; euler_r_c << toRad(-90.0), 0, toRad(-90.0); // put FLOATS otherwise 0 + q_r_c = e2q(euler_r_c); + // landmark (same orientation as camera of robot frame 1 (r1)) + p_w_l << 1,0,0; + Vector3d euler_w_l; euler_w_l << toRad(-90.0), 0, toRad(-90.0); + q_w_l = e2q(euler_w_l); + + // camera landmark pose (measurements) + Isometry3d T_w_r1 = pose2iso(p_w_r1, q_w_r1); + Isometry3d T_w_r2 = pose2iso(p_w_r2, q_w_r2); + Isometry3d T_r_c = pose2iso(p_r_c, q_r_c); + Isometry3d T_w_l = pose2iso(p_w_l, q_w_l); + + Isometry3d T_c1_l = (T_w_r1*T_r_c).inverse() * T_w_l; + Isometry3d T_c2_l = (T_w_r2*T_r_c).inverse() * T_w_l; + Vector7d pose_c1_l = iso2pose(T_c1_l); + Vector7d pose_c2_l = iso2pose(T_c2_l); + p_c1_l = pose_c1_l.segment<3>(0); + q_c1_l = Quaterniond(pose_c1_l.segment<4>(3)); + p_c2_l = pose_c2_l.segment<3>(0); + q_c2_l = Quaterniond(pose_c2_l.segment<4>(3)); + + Vector7d pose_w_r1; pose_w_r1 << p_w_r1, q_w_r1.coeffs() ; + Vector7d pose_w_r2; pose_w_r2 << p_w_r2, q_w_r2.coeffs() ; + Vector7d pose_r_c; pose_r_c << p_r_c, q_r_c.coeffs() ; + Vector7d pose_w_l; pose_w_l << p_w_l, q_w_l.coeffs() ; + + // Build problem + problem = Problem::create("PO", 3); + solver = std::make_shared(problem); + + // Install sensor and processor + SensorBasePtr S = problem->installSensor("SensorCamera", "canonical", pose_r_c, wolf_root + "/demos/camera_params_canonical.yaml"); + camera = std::static_pointer_cast(S); + + Vector4d k = camera->getIntrinsic()->getState(); //[cx cy fx fy] + K << k(2), 0, k(0), + 0, k(3), k(1), + 0, 0, 1; + + // F1 is be origin KF + VectorComposite x0(pose_w_r1, "PO", {3,4}); + VectorComposite s0("PO", {Vector3d(0.01,0.01,0.01), Vector3d(0.01,0.01,0.01)}); + F1 = problem->setPriorFactor(x0, s0, 0.0); + + // emplace dummy capture & set as last and origin + C1 = std::static_pointer_cast(CaptureBase::emplace(F1, 0.0, camera, cv::Mat(2,2,CV_8UC1))); + + meas_cov = Eigen::Matrix8d::Identity(); // pixel noise + + tag_width = 0.2; + lmk1 = LandmarkBase::emplace(problem->getMap(), pose_w_l, 42, tag_width); + + double s = tag_width/2; + l_corn1 << -s, s, 0; // bottom left + l_corn2 << s, s, 0; // bottom right + l_corn3 << s, -s, 0; // top right + l_corn4 << -s, -s, 0; // top left + + // Measurement for first keyframe + meas1 << FactorApriltagProj::pinholeProj(K, p_c1_l, q_c1_l, l_corn1), + FactorApriltagProj::pinholeProj(K, p_c1_l, q_c1_l, l_corn2), + FactorApriltagProj::pinholeProj(K, p_c1_l, q_c1_l, l_corn3), + FactorApriltagProj::pinholeProj(K, p_c1_l, q_c1_l, l_corn4); + + // Measurement for second keyframe + meas2 << FactorApriltagProj::pinholeProj(K, p_c2_l, q_c2_l, l_corn1), + FactorApriltagProj::pinholeProj(K, p_c2_l, q_c2_l, l_corn2), + FactorApriltagProj::pinholeProj(K, p_c2_l, q_c2_l, l_corn3), + FactorApriltagProj::pinholeProj(K, p_c2_l, q_c2_l, l_corn4); + + // dummy pose, not used in the factor, only for some part of the processor + pose_dummy.setZero(); + + } +}; + + +TEST_F(FactorApriltagProj_class, Constructor) +{ + f1 = std::static_pointer_cast(FeatureBase::emplace(C1, meas1, meas_cov, 1, tag_width, pose_dummy)); + FactorApriltagProjPtr factor = std::make_shared( + f1, + camera, + F1, + lmk1, + nullptr, + false + ); + + ASSERT_TRUE(factor->getType() == "FactorApriltagProj"); +} + + + +TEST_F(FactorApriltagProj_class, problem_1KF) +{ + f1 = std::static_pointer_cast(FeatureBase::emplace(C1, meas1, meas_cov, 1, tag_width, pose_dummy)); + + //emplace feature and landmark + auto factor = FactorBase::emplace(f1, f1, camera, F1, lmk1, nullptr, false); + + ASSERT_TRUE(problem->check(0)); + + problem->perturb(); + solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport + + ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), posiquat2pose(p_w_r1, q_w_r1), 1e-6); + ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), posiquat2pose(p_w_l, q_w_l), 1e-6); + + // Reproject solution and compare to measurement + Isometry3d T_w_l_post = pose2iso(lmk1->getState().vector("PO")); + Isometry3d T_w_r1 = pose2iso(p_w_r1, q_w_r1); + Isometry3d T_r_c = pose2iso(p_r_c, q_r_c); + + Isometry3d T_c1_l = (T_w_r1*T_r_c).inverse() * T_w_l_post; + Vector3d p_c1_l = T_c1_l.translation(); + Quaterniond q_c1_l = Quaterniond(T_c1_l.rotation()); + + Vector8d proj_post; + proj_post << FactorApriltagProj::pinholeProj(K, p_c1_l, q_c1_l, l_corn1), + FactorApriltagProj::pinholeProj(K, p_c1_l, q_c1_l, l_corn2), + FactorApriltagProj::pinholeProj(K, p_c1_l, q_c1_l, l_corn3), + FactorApriltagProj::pinholeProj(K, p_c1_l, q_c1_l, l_corn4); + ASSERT_MATRIX_APPROX(proj_post, meas1, 1e-6); +} + + +TEST_F(FactorApriltagProj_class, problem_2KF) +{ + // Create frame and dummy capture for second KF + 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)); + + //emplace feature and landmark + auto factor1 = FactorBase::emplace(f1, f1, camera, F1, lmk1, nullptr, false); + auto factor2 = FactorBase::emplace(f2, f2, camera, F2, lmk1, nullptr, false); + + ASSERT_TRUE(problem->check(0)); + + problem->perturb(0.5); + solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport + + ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), posiquat2pose(p_w_r1, q_w_r1), 1e-6); + ASSERT_MATRIX_APPROX(F2->getState().vector("PO"), posiquat2pose(p_w_r2, q_w_r2), 1e-6); + ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), posiquat2pose(p_w_l, q_w_l), 1e-6); + + // camera->unfixExtrinsics(); // not observable + // camera->getP()->unfix(); // not observable + // camera->getO()->unfix(); // not observable + // problem->perturb(0.1); + // problem->print(4,1,1,1); + // std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport + // std::cout << report << std::endl; + // problem->print(4,1,1,1); + +} + + + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + diff --git a/test/gtest_feature_apriltag.cpp b/test/gtest_feature_apriltag.cpp deleted file mode 100644 index 2cc81b7e2e8da2baa8982f6b94c3ed6fefd5d8cb..0000000000000000000000000000000000000000 --- a/test/gtest_feature_apriltag.cpp +++ /dev/null @@ -1,140 +0,0 @@ -//--------LICENSE_START-------- -// -// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. -// Authors: Joan Solà Ortega (jsola@iri.upc.edu) -// All rights reserved. -// -// This file is part of WOLF -// WOLF is free software: you can redistribute it and/or modify -// it under the terms of the GNU Lesser General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU Lesser General Public License for more details. -// -// You should have received a copy of the GNU Lesser General Public License -// along with this program. If not, see . -// -//--------LICENSE_END-------- -/** - * \file gtest_feature_apriltag.cpp - * - * Created on: Dec 22, 2018 - * \author: jsola - */ - - -#include - -#include "apriltag/feature/feature_apriltag.h" - -using namespace wolf; - -class FeatureApriltag_test : public testing::Test -{ - public: - Eigen::Vector7d pose; - Eigen::Matrix6d cov; - int tag_id; - apriltag_detection_t det; - double rep_error1; - double rep_error2; - bool use_rotation; - - void SetUp() override - { - pose << 1,2,3,4,5,6,7; - cov.setIdentity() * 2.0; - - det.id = 1; - tag_id = det.id; - det.p[0][0] = 1.0; - det.p[0][1] = -1.0; - det.p[1][0] = 1.0; - det.p[1][1] = 1.0; - det.p[2][0] = -1.0; - det.p[2][1] = 1.0; - det.p[3][0] = -1.0; - det.p[3][1] = -1.0; - - rep_error1 = 0.01; - rep_error2 = 0.1; - use_rotation = true; - } -}; - -TEST_F(FeatureApriltag_test, type) -{ - FeatureApriltagPtr f = std::make_shared(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation); - - ASSERT_EQ(f->getType(), "FeatureApriltag"); -} - -TEST_F(FeatureApriltag_test, getTagId) -{ - FeatureApriltagPtr f = std::make_shared(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation); - - ASSERT_EQ(f->getTagId(), 1); -} - -TEST_F(FeatureApriltag_test, getCorners) -{ - FeatureApriltagPtr f = std::make_shared(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation); - - ASSERT_EQ(f->getTagCorners().size(), 4); - - ASSERT_EQ(f->getTagCorners()[0].x, 1.0); - ASSERT_EQ(f->getTagCorners()[0].y, -1.0); - ASSERT_EQ(f->getTagCorners()[1].x, 1.0); - ASSERT_EQ(f->getTagCorners()[1].y, 1.0); - ASSERT_EQ(f->getTagCorners()[2].x, -1.0); - ASSERT_EQ(f->getTagCorners()[2].y, 1.0); - ASSERT_EQ(f->getTagCorners()[3].x, -1.0); - ASSERT_EQ(f->getTagCorners()[3].y, -1.0); -} - -TEST_F(FeatureApriltag_test, getDetection) -{ - FeatureApriltagPtr f = std::make_shared(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation); - - ASSERT_EQ(f->getDetection().id, 1); -} - -TEST_F(FeatureApriltag_test, tagid_detid_equality) -{ - FeatureApriltagPtr f = std::make_shared(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation); - - ASSERT_EQ(f->getDetection().id, f->getTagId()); -} - -TEST_F(FeatureApriltag_test, tagCorners_detection_equality) -{ - FeatureApriltagPtr f = std::make_shared(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation); - - for (int i = 0; igetTagCorners().size(); i++) - { - ASSERT_EQ(f->getTagCorners()[i].x, f->getDetection().p[i][0]); - ASSERT_EQ(f->getTagCorners()[i].y, f->getDetection().p[i][1]); - } -} - -TEST_F(FeatureApriltag_test, getRepErrors) -{ - FeatureApriltagPtr f = std::make_shared(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation); - - double err1 = f->getRepError1(); - double err2 = f->getRepError2(); - - ASSERT_EQ(err1, rep_error1); - ASSERT_EQ(err2, rep_error2); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_features_apriltag.cpp b/test/gtest_features_apriltag.cpp new file mode 100644 index 0000000000000000000000000000000000000000..acf3699bd83b7b57ebc0c57460c811bbed89db98 --- /dev/null +++ b/test/gtest_features_apriltag.cpp @@ -0,0 +1,115 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . +// +//--------LICENSE_END-------- + +#include + +#include "apriltag/feature/feature_apriltag.h" +#include "apriltag/feature/feature_apriltag_pose.h" +#include "apriltag/feature/feature_apriltag_proj.h" + +using namespace wolf; + +class FeatureApriltag_fixture : public testing::Test +{ + public: + Eigen::Vector7d pose; + Eigen::Matrix6d info_pose; + Eigen::Vector8d corners_vec; + Eigen::Matrix8d cov_pix; + int tag_id; + double tag_widh; + double rep_error1; + double rep_error2; + bool use_rotation; + + void SetUp() override + { + pose << 1,2,3,4,5,6,7; + info_pose.setIdentity(); + + corners_vec << 1.0, -1.0, + 1.0, 1.0, + -1.0, 1.0, + -1.0, -1.0; + cov_pix.setIdentity(); + + tag_id = 1; + tag_widh = 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); + ASSERT_EQ(fa_pose->getType(), "FeatureApriltagPose"); + + FeatureApriltagProjPtr fa_proj = std::make_shared(corners_vec, cov_pix, tag_id, tag_widh, pose); + 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)); +} + +TEST_F(FeatureApriltag_fixture, FeatureApriltagPose_getters) +{ + FeatureApriltagPosePtr fa_pose = std::make_shared(pose, info_pose, tag_id, corners_vec, rep_error1, rep_error2, use_rotation); + + ASSERT_EQ(fa_pose->getRepError1(), rep_error1); + ASSERT_EQ(fa_pose->getRepError2(), rep_error2); + ASSERT_EQ(fa_pose->getUserotation(), use_rotation); +} + +TEST_F(FeatureApriltag_fixture, FeatureApriltagProj_getters) +{ + FeatureApriltagProjPtr fa_proj = std::make_shared(corners_vec, cov_pix, tag_id, tag_widh, pose); + + ASSERT_MATRIX_APPROX(fa_proj->getPosePnp(), pose, 1e-6); + ASSERT_EQ(fa_proj->getTagWidth(), tag_widh); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + diff --git a/test/gtest_processor_tracker_landmark_apriltag.cpp b/test/gtest_processor_tracker_landmark_apriltag.cpp index 63fd478ba7c7b7a76129324640efc24517f2897c..79627017e9ce07d199477c530d873890c690e404 100644 --- a/test/gtest_processor_tracker_landmark_apriltag.cpp +++ b/test/gtest_processor_tracker_landmark_apriltag.cpp @@ -27,12 +27,11 @@ #include "core/processor/factory_processor.h" #include "apriltag/processor/processor_tracker_landmark_apriltag.h" -#include "apriltag/feature/feature_apriltag.h" +#include "apriltag/feature/feature_apriltag_pose.h" #include "apriltag/landmark/landmark_apriltag.h" #include "apriltag/internal/config.h" using namespace Eigen; using namespace wolf; -using std::static_pointer_cast; //////////////////////////////////////////////////////////////// @@ -86,7 +85,23 @@ WOLF_REGISTER_PROCESSOR(ProcessorTrackerLandmarkApriltag_Wrapper); */ // Use the following in case you want to initialize tests with predefined variables or methods. class ProcessorTrackerLandmarkApriltag_class : public testing::Test{ + public: + ProcessorTrackerLandmarkApriltag_WrapperPtr prc_apr; + std::string wolf_root; + ProblemPtr problem; + SensorBasePtr sen; + ProcessorBasePtr prc; + FrameBasePtr F1; + CaptureBasePtr C1; + Vector8d corners_vec_; + int tag_id_; + double rep_error1; + double rep_error2; + bool use_rotation; + Vector7d pose_default_; + Matrix6d cov_pose_; + void SetUp() override { wolf_root = _WOLF_APRILTAG_ROOT_DIR; @@ -107,32 +122,21 @@ class ProcessorTrackerLandmarkApriltag_class : public testing::Test{ prc_apr->setOriginPtr(C1); prc_apr->setLastPtr(C1); - det.p[0][0] = 1.0; - det.p[0][1] = -1.0; - det.p[1][0] = 1.0; - det.p[1][1] = 1.0; - det.p[2][0] = -1.0; - det.p[2][1] = 1.0; - det.p[3][0] = -1.0; - det.p[3][1] = -1.0; + tag_id_ = 1; + corners_vec_ << 1.0, -1.0, + 1.0, 1.0, + -1.0, 1.0, + -1.0, -1.0; rep_error1 = 0.01; rep_error2 = 0.1; use_rotation = true; + + // use for tests in which pose value irrelevant + pose_default_ << 0,0,0, 0,0,0,1; + cov_pose_.setIdentity(); } - public: - ProcessorTrackerLandmarkApriltag_WrapperPtr prc_apr; - std::string wolf_root; - ProblemPtr problem; - SensorBasePtr sen; - ProcessorBasePtr prc; - FrameBasePtr F1; - CaptureBasePtr C1; - apriltag_detection_t det; - double rep_error1; - double rep_error2; - bool use_rotation; }; //////////////////////////////////////////////////////////////// @@ -200,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, 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); // 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, 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); // } // } @@ -239,32 +243,14 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeaturesDuplicated) prc_apr->detectNewFeatures(1, C1, features_out); ASSERT_EQ(features_out.size(), 0); - // Some detected features TODO + // Some detected features, pose does not matter for this test FeatureBasePtrList features_in; - Eigen::Vector3d pos; - Eigen::Vector3d ori; //Euler angles in rad - Eigen::Quaterniond quat; - Eigen::Vector7d pose; - Eigen::Matrix6d meas_cov( Matrix6d::Identity() ); - int tag_id; // feature 0 - pos << 0,2,0; - ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0; - quat = e2q(ori); - pose << pos, quat.coeffs(); - tag_id = 0; - det.id = tag_id; - FeatureBasePtr f0 = std::make_shared(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation); - + tag_id_ = 0; + auto f0 = std::make_shared(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation); // feature 1 (with same id of feature 0) - pos << 1,2,0; - ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0; - quat = e2q(ori); - pose << pos, quat.coeffs(); - tag_id = 0; - det.id = tag_id; - FeatureBasePtr f1 = std::make_shared(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation); + auto f1 = std::make_shared(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation); features_in.push_back(f0); features_in.push_back(f1); @@ -278,46 +264,19 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeaturesDuplicated) TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeatures) { - // No detected features - FeatureBasePtrList features_out; - prc_apr->detectNewFeatures(1, C1, features_out); - ASSERT_EQ(features_out.size(), 0); - - // Some detected features TODO + // Some detected features, pose does not matter for this test FeatureBasePtrList features_in; - Eigen::Vector3d pos; - Eigen::Vector3d ori; //Euler angles in rad - Eigen::Quaterniond quat; - Eigen::Vector7d pose; - Eigen::Matrix6d meas_cov( Matrix6d::Identity() ); - int tag_id; + FeatureBasePtrList features_out; // feature 0 - pos << 0,2,0; - ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0; - quat = e2q(ori); - pose << pos, quat.coeffs(); - tag_id = 0; - det.id = tag_id; - FeatureBasePtr f0 = std::make_shared(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation); - + tag_id_ = 0; + auto f0 = std::make_shared(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation); // feature 1 - pos << 1,2,0; - ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0; - quat = e2q(ori); - pose << pos, quat.coeffs(); - tag_id = 1; - det.id = tag_id; - FeatureBasePtr f1 = std::make_shared(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation); - + tag_id_ = 1; + auto f1 = std::make_shared(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation); // feature 2 - pos << 0,2,1; - ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0; - quat = e2q(ori); - pose << pos, quat.coeffs(); - tag_id = 2; - det.id = tag_id; - FeatureBasePtr f2 = std::make_shared(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation); + tag_id_ = 2; + auto f2 = std::make_shared(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation); //we add different features in the list features_in.push_back(f0); @@ -344,17 +303,17 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeatures) WOLF_INFO("detecting...."); prc_apr->detectNewFeatures(2, C1, features_out); ASSERT_EQ(features_out.size(), 1); - ASSERT_EQ(std::static_pointer_cast(features_out.front())->getTagId(), 2); + ASSERT_EQ(std::static_pointer_cast(features_out.front())->getTagId(), 2); } TEST_F(ProcessorTrackerLandmarkApriltag_class, emplaceLandmark) { - Vector7d pose_landmark((Vector7d()<<0,0,0,0,0,0,1).finished()); - det.id = 1; - FeatureBasePtr f1 = FeatureBase::emplace(C1,pose_landmark, Matrix6d::Identity(), 1, det, rep_error1, rep_error2, use_rotation); + 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); LandmarkBasePtr lmk = prc_apr->emplaceLandmark(f1); LandmarkApriltagPtr lmk_april = std::static_pointer_cast(lmk); + ASSERT_TRUE(lmk_april->getMap() != nullptr); ASSERT_TRUE(lmk_april->getType() == "LandmarkApriltag"); ASSERT_MATRIX_APPROX(lmk_april->getState().vector("PO"), pose_landmark, 1e-6); @@ -362,16 +321,20 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, emplaceLandmark) TEST_F(ProcessorTrackerLandmarkApriltag_class, emplaceFactor) { - det.id = 1; - FeatureBasePtr f1 = FeatureBase::emplace(C1,(Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), 1, det, rep_error1, rep_error2, use_rotation); + + auto f1 = FeatureBase::emplace(C1, pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation); LandmarkBasePtr lmk = prc_apr->emplaceLandmark(f1); LandmarkApriltagPtr lmk_april = std::static_pointer_cast(lmk); - FactorBasePtr ctr = prc_apr->emplaceFactor(f1, lmk); + auto st = prc_apr->getSensor()->getO()->getState(); + std::cout << prc_apr->getSensor()->getStructure() << std::endl; + std::cout << st.transpose() << std::endl; + + FactorBasePtr fac = prc_apr->emplaceFactor(f1, lmk); - ASSERT_TRUE(ctr->getFeature() == f1); - ASSERT_TRUE(ctr->getType() == "FactorRelativePose3dWithExtrinsics"); + ASSERT_TRUE(fac->getFeature() == f1); + ASSERT_TRUE(fac->getType() == "FactorRelativePose3dWithExtrinsics"); } TEST_F(ProcessorTrackerLandmarkApriltag_class, computeInformation) @@ -460,6 +423,39 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, computeInformation) } + + + + + + + + +//////////////////////////////// +//////////////////////////////// +// Projection based Factor +//////////////////////////////// +//////////////////////////////// + + + +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); + + LandmarkBasePtr lmk = prc_apr->emplaceLandmark(f1); + LandmarkApriltagPtr lmk_april = std::static_pointer_cast(lmk); + + auto st = prc_apr->getSensor()->getO()->getState(); + + FactorBasePtr fac = prc_apr->emplaceFactor(f1, lmk); + + ASSERT_TRUE(fac->getFeature() == f1); + ASSERT_TRUE(fac->getType() == "FactorApriltagProj"); +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/processor_tracker_landmark_apriltag.yaml b/test/processor_tracker_landmark_apriltag.yaml index 465a0dade019d277ecc7ae1575cf6dbbef0e1a28..40a3ff888799bf8bf95d0b051f426225417559c7 100644 --- a/test/processor_tracker_landmark_apriltag.yaml +++ b/test/processor_tracker_landmark_apriltag.yaml @@ -26,7 +26,9 @@ noise: std_z : 0.1 # m std_rpy_degrees : 5 # degrees std_pix: 20 # pixel error - + +use_proj_factor: false + vote: voting active: true min_time_vote: 0 # s @@ -34,8 +36,6 @@ vote: min_features_for_keyframe: 12 nb_vote_for_every_first: 50 -reestimate_last_frame: true # for a better prior on the new keyframe: use only if no motion processor -add_3d_cstr: true # add 3D constraints between the KF so that they do not jump when using apriltag only max_new_features: -1 apply_loss_function: true