diff --git a/.gitignore b/.gitignore index ee22d7c8146da16da38f11e838fb6523e42dced7..41dfa17a783a996d522f89f7020021e401bd6988 100644 --- a/.gitignore +++ b/.gitignore @@ -28,3 +28,7 @@ src/CMakeCache.txt src/CMakeFiles/cmake.check_cache src/examples/map_apriltag_save.yaml + +\.vscode/ +build_release/ + diff --git a/CMakeLists.txt b/CMakeLists.txt index 1d661f37bae88e329f90d5ac65b279aa50b9a165..ad82bff778206e0165419bdbce74a33eb5abb711 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,12 +32,12 @@ CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") elseif(COMPILER_SUPPORTS_CXX0X) message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") else() - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") endif() if(UNIX) @@ -152,6 +152,10 @@ ENDIF(Ceres_FOUND) +IF(APRILTAG_INCLUDE_DIR) + INCLUDE_DIRECTORIES(${APRILTAG_INCLUDE_DIR}) +ENDIF(APRILTAG_INCLUDE_DIR) + #HEADERS SET(HDRS_COMMON @@ -272,6 +276,12 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT}) IF (GLOG_FOUND) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY}) ENDIF (GLOG_FOUND) +#check if this is done correctly +IF (OPENCV_FOUND AND Apriltag_FOUND) + LINK_LIBRARIES(apriltag m) + TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${APRILTAG_LIBRARY} ${CMAKE_THREAD_LIBS_INIT} ${OPENCV_LDFLAGS} m) +ENDIF(OPENCV_FOUND AND Apriltag_FOUND) + IF (GLOG_FOUND) IF(BUILD_TESTS) MESSAGE("Building tests.") diff --git a/include/IMU/factor/factor_IMU.h b/include/IMU/factor/factor_IMU.h index cfe20af51e0829fe894c83bee7c1b129cdb3e02d..0378abb66e227f4d5de7ac48bb643e0df1ad1f97 100644 --- a/include/IMU/factor/factor_IMU.h +++ b/include/IMU/factor/factor_IMU.h @@ -25,7 +25,7 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6> virtual ~FactorIMU() = default; - /* \brief : compute the residual from the state blocks being iterated by the solver. + /** \brief : compute the residual from the state blocks being iterated by the solver. -> computes the expected measurement -> corrects actual measurement with new bias -> compares the corrected measurement with the expected one @@ -41,8 +41,14 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6> const T* const _v2, const T* const _b2, T* _res) const; - - /* \brief : compute the residual from the state blocks being iterated by the solver. (same as operator()) + + /** \brief Estimation error given the states in the wolf tree + * + */ + Eigen::Vector9s error(); + + + /** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator()) -> computes the expected measurement -> corrects actual measurement with new bias -> compares the corrected measurement with the expected one @@ -71,7 +77,7 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6> const Eigen::MatrixBase<D1> & _wb2, Eigen::MatrixBase<D3> & _res) const; - /* Function expectation(...) + /** Function expectation(...) * params : * Vector3s _p1 : position in imu frame * Vector4s _q1 : orientation quaternion in imu frame @@ -96,7 +102,7 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6> Eigen::QuaternionBase<D4> & _qe, Eigen::MatrixBase<D3> & _ve) const; - /* \brief : return the expected delta given the state blocks in the wolf tree + /** \brief : return the expected delta given the state blocks in the wolf tree */ Eigen::VectorXs expectation() const; @@ -121,7 +127,7 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6> const Scalar dt_; ///< delta-time and delta-time-squared between keyframes const Scalar ab_rate_stdev_, wb_rate_stdev_; //stdev for standard_deviation (= sqrt(variance)) - /* bias covariance and bias residuals + /** bias covariance and bias residuals * * continuous bias covariance matrix for accelerometer would then be A_r = diag(ab_stdev_^2, ab_stdev_^2, ab_stdev_^2) * To compute bias residuals, we will need to do (sqrt(A_r)).inverse() * ab_error @@ -141,10 +147,10 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6> ///////////////////// IMPLEMENTAITON //////////////////////////// inline FactorIMU::FactorIMU(const FeatureIMUPtr& _ftr_ptr, - const CaptureIMUPtr& _cap_origin_ptr, - const ProcessorBasePtr& _processor_ptr, - bool _apply_loss_function, - FactorStatus _status) : + const CaptureIMUPtr& _cap_origin_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status) : FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ... "IMU", _cap_origin_ptr->getFrame(), @@ -183,14 +189,14 @@ inline FactorIMU::FactorIMU(const FeatureIMUPtr& _ftr_ptr, template<typename T> inline bool FactorIMU::operator ()(const T* const _p1, - const T* const _q1, - const T* const _v1, - const T* const _b1, - const T* const _p2, - const T* const _q2, - const T* const _v2, - const T* const _b2, - T* _res) const + const T* const _q1, + const T* const _v1, + const T* const _b1, + const T* const _p2, + const T* const _q2, + const T* const _v2, + const T* const _b2, + T* _res) const { using namespace Eigen; @@ -214,18 +220,42 @@ inline bool FactorIMU::operator ()(const T* const _p1, return true; } +Eigen::Vector9s FactorIMU::error() +{ + Vector6s bias = capture_other_ptr_.lock()->getCalibration(); + + Map<const Vector3s > acc_bias(bias.data()); + Map<const Vector3s > gyro_bias(bias.data() + 3); + + Eigen::Vector9s delta_exp = expectation(); + + Eigen::Vector9s delta_preint = getMeasurement(); + + Eigen::Vector9s delta_step; + + delta_step.head<3>() = dDp_dab_ * (acc_bias - acc_bias_preint_ ) + dDp_dwb_ * (gyro_bias - gyro_bias_preint_); + delta_step.segment<3>(3) = dDq_dwb_ * (gyro_bias - gyro_bias_preint_); + delta_step.tail<3>() = dDv_dab_ * (acc_bias - acc_bias_preint_ ) + dDv_dwb_ * (gyro_bias - gyro_bias_preint_); + + Eigen::VectorXs delta_corr = imu::plus(delta_preint, delta_step); + + Eigen::Vector9s res = imu::diff(delta_exp, delta_corr); + + return res; +} + template<typename D1, typename D2, typename D3> inline bool FactorIMU::residual(const Eigen::MatrixBase<D1> & _p1, - const Eigen::QuaternionBase<D2> & _q1, - const Eigen::MatrixBase<D1> & _v1, - const Eigen::MatrixBase<D1> & _ab1, - const Eigen::MatrixBase<D1> & _wb1, - const Eigen::MatrixBase<D1> & _p2, - const Eigen::QuaternionBase<D2> & _q2, - const Eigen::MatrixBase<D1> & _v2, - const Eigen::MatrixBase<D1> & _ab2, - const Eigen::MatrixBase<D1> & _wb2, - Eigen::MatrixBase<D3> & _res) const + const Eigen::QuaternionBase<D2> & _q1, + const Eigen::MatrixBase<D1> & _v1, + const Eigen::MatrixBase<D1> & _ab1, + const Eigen::MatrixBase<D1> & _wb1, + const Eigen::MatrixBase<D1> & _p2, + const Eigen::QuaternionBase<D2> & _q2, + const Eigen::MatrixBase<D1> & _v2, + const Eigen::MatrixBase<D1> & _ab2, + const Eigen::MatrixBase<D1> & _wb2, + Eigen::MatrixBase<D3> & _res) const { /* Help for the IMU residual function diff --git a/include/IMU/processor/processor_IMU.h b/include/IMU/processor/processor_IMU.h index ad2060a21f024da25d93098d7196378eb8278d03..d94f8a992ce7ce3cf531257b429a1e9f167ec5e3 100644 --- a/include/IMU/processor/processor_IMU.h +++ b/include/IMU/processor/processor_IMU.h @@ -11,7 +11,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU); struct ProcessorParamsIMU : public ProcessorParamsMotion { - // + Scalar unmeasured_perturbation_std_ = 0.00001; // TODO: pass in processor motion? }; WOLF_PTR_TYPEDEFS(ProcessorIMU); @@ -61,6 +61,7 @@ class ProcessorIMU : public ProcessorMotion{ protected: ProcessorParamsIMUPtr params_motion_IMU_; + Eigen::Matrix<Scalar, 9, 9> unmeasured_perturbation_cov_; public: //for factory diff --git a/include/base/factor/factor_autodiff_apriltag.h b/include/base/factor/factor_autodiff_apriltag.h new file mode 100644 index 0000000000000000000000000000000000000000..b23f30e6f5d0bf97eb184412c79d817c66169d68 --- /dev/null +++ b/include/base/factor/factor_autodiff_apriltag.h @@ -0,0 +1,211 @@ +#ifndef _FACTOR_AUTODIFF_APRILTAG_H_ +#define _FACTOR_AUTODIFF_APRILTAG_H_ + +//Wolf includes +#include "base/common/wolf.h" +#include "base/math/rotations.h" +#include "base/factor/factor_autodiff.h" +#include "base/sensor/sensor_base.h" +#include "base/landmark/landmark_apriltag.h" +#include "base/feature/feature_apriltag.h" + +namespace wolf +{ + +WOLF_PTR_TYPEDEFS(FactorAutodiffApriltag); + +class FactorAutodiffApriltag : public FactorAutodiff<FactorAutodiffApriltag, 6, 3, 4, 3, 4, 3, 4> +{ + public: + + /** \brief Class constructor + */ + FactorAutodiffApriltag( + const SensorBasePtr& _sensor_ptr, + const FrameBasePtr& _frame_ptr, + const LandmarkApriltagPtr& _landmark_other_ptr, + const FeatureApriltagPtr& _feature_ptr, + bool _apply_loss_function, + FactorStatus _status); + + /** \brief Class Destructor + */ + virtual ~FactorAutodiffApriltag(); + + /** brief : compute the residual from the state blocks being iterated by the solver. + **/ + template<typename T> + bool 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; + + Eigen::Vector6s residual() const; + Scalar cost() const; + + // print function only for double (not Jet) + template<typename T, int Rows, int Cols> + void print(int kf, int lmk, const std::string s, const Eigen::Matrix<T, Rows, Cols> _M) const + { + // jet prints nothing + } + template<int Rows, int Cols> + void print(int kf, int lmk, const std::string s, const Eigen::Matrix<Scalar, Rows, Cols> _M) const + { + // double prints stuff + WOLF_TRACE("KF", kf, " L", lmk, "; ", s, _M); + } +}; + +} // namespace wolf + +// Include here all headers for this class +//#include <YOUR_HEADERS.h> + +namespace wolf +{ + +FactorAutodiffApriltag::FactorAutodiffApriltag( + const SensorBasePtr& _sensor_ptr, + const FrameBasePtr& _frame_ptr, + const LandmarkApriltagPtr& _landmark_other_ptr, + const FeatureApriltagPtr& _feature_ptr, + bool _apply_loss_function, + FactorStatus _status) : + FactorAutodiff("AUTODIFF APRILTAG", + nullptr, + nullptr, + nullptr, + _landmark_other_ptr, + nullptr, + _apply_loss_function, + _status, + _sensor_ptr->getP(), _sensor_ptr->getO(), + _frame_ptr->getP(), _frame_ptr->getO(), + _landmark_other_ptr->getP(), _landmark_other_ptr->getO() + ) +{ + + +} + +/** \brief Class Destructor + */ +FactorAutodiffApriltag::~FactorAutodiffApriltag() +{ + // +} + +template<typename T> bool FactorAutodiffApriltag::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<const Eigen::Matrix<T,3,1>> p_r_c(_p_camera); + Eigen::Map<const Eigen::Quaternion<T>> q_r_c(_o_camera); + Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_keyframe); + Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_keyframe); + Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_l(_p_landmark); + Eigen::Map<const Eigen::Quaternion<T>> q_w_l(_o_landmark); + Eigen::Map<Eigen::Matrix<T,6,1>> residuals(_residuals); + + // Expected measurement + Eigen::Quaternion<T> q_c_w = (q_w_r * q_r_c).conjugate(); + Eigen::Quaternion<T> q_c_l = q_c_w * q_w_l; + Eigen::Matrix<T,3,1> p_c_l = q_c_w * (-(p_w_r + q_w_r * p_r_c) + p_w_l); + + // Measurement + Eigen::Vector3s p_c_l_meas(getMeasurement().head<3>()); + Eigen::Quaternions q_c_l_meas(getMeasurement().data() + 3 ); + Eigen::Quaternion<T> q_l_c_meas = q_c_l_meas.conjugate().cast<T>(); + //Eigen::Matrix<T,3,1> p_l_c_meas = -q_l_c_meas * p_c_l_meas.cast<T>(); + + // Error + Eigen::Matrix<T, 6, 1> err; + err.head(3) = q_l_c_meas * (p_c_l_meas.cast<T>() - 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<T>() * err; + + /*//states + Eigen::Translation<T,3> p_camera (_p_camera[0] , _p_camera[1] , _p_camera[2]), + p_keyframe (_p_keyframe[0], _p_keyframe[1], _p_keyframe[2]), + p_landmark (_p_landmark[0], _p_landmark[1], _p_landmark[2]); + Eigen::Quaternion<T> q_camera (_o_camera), + q_keyframe (_o_keyframe), + q_landmark (_o_landmark); + + //Measurements T and Q + Eigen::Translation3ds p_measured(getMeasurement().head(3)); + Eigen::Quaternions q_measured(getMeasurement().data() + 3 ); + // landmark wrt camera, measure + Eigen::Transform<T, 3, Eigen::Affine> c_M_l_meas = p_c_l_meas.cast<T>() * q_measured.cast<T>(); + + // Create transformation matrices to compose + // robot wrt world + Eigen::Transform<T, 3, Eigen::Affine> w_M_r = p_keyframe * q_keyframe; + // camera wrt robot + Eigen::Transform<T, 3, Eigen::Affine> r_M_c = p_camera * q_camera; + // landmark wrt world + Eigen::Transform<T, 3, Eigen::Affine> w_M_l = p_landmark * q_landmark; + // landmark wrt camera, estimated + Eigen::Transform<T, 3, Eigen::Affine> c_M_l_est = (w_M_r * r_M_c).inverse() * w_M_l; + + // expectation error, in camera frame + // right-minus + Eigen::Transform<T, 3, Eigen::Affine> c_M_err = c_M_l_meas.inverse() * c_M_l_est; + // opposite of the previous formula and therefore equivalent +// Eigen::Transform<T, 3, Eigen::Affine> c_M_err = c_M_l_est.inverse() * c_M_l_meas; + + + // error + Eigen::Matrix<T, 6, 1> err; + err.block(0,0,3,1) = c_M_err.translation(); + Eigen::Matrix<T, 3, 3> R_err(c_M_err.linear()); + err.block(3,0,3,1) = wolf::log_R(R_err); + + // debug stuff +// int kf = getFeature()->getCapture()->getFrame()->id(); +// int lmk = getLandmarkOther()->id(); +// +// print(kf, lmk, "w_M_c : \n", (w_M_r*r_M_c).matrix()); +// print(kf, lmk, "w_M_l : \n", w_M_l.matrix()); +// print(kf, lmk, "c_M_l_e: \n", c_M_l_est.matrix()); +// print(kf, lmk, "c_M_l_m: \n", c_M_l_meas.matrix()); +// print(kf, lmk, "error : \n", err.transpose().eval()); + + // residual + Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals); + + res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * err; + */ + return true; +} + +Eigen::Vector6s FactorAutodiffApriltag::residual() const +{ + Eigen::Vector6s res; + Scalar * p_camera, * o_camera, * p_frame, * o_frame, * p_tag, * o_tag; + p_camera = getCapture()->getSensorP()->getState().data(); + o_camera = getCapture()->getSensorO()->getState().data(); + p_frame = getCapture()->getFrame()->getP()->getState().data(); + o_frame = getCapture()->getFrame()->getO()->getState().data(); + p_tag = getLandmarkOther()->getP()->getState().data(); + o_tag = getLandmarkOther()->getO()->getState().data(); + + operator() (p_camera, o_camera, p_frame, o_frame, p_tag, o_tag, res.data()); + + return res; +} + +Scalar FactorAutodiffApriltag::cost() const +{ + return residual().squaredNorm(); +} + +} // namespace wolf + +#endif /* _CONSTRAINT_AUTODIFF_APRILTAG_H_ */ diff --git a/include/base/feature/feature_apriltag.h b/include/base/feature/feature_apriltag.h new file mode 100644 index 0000000000000000000000000000000000000000..f2fbb23756ea252365f1ba366d6a16119f2336e6 --- /dev/null +++ b/include/base/feature/feature_apriltag.h @@ -0,0 +1,59 @@ +#ifndef FEATURE_APRILTAG_H_ +#define FEATURE_APRILTAG_H_ + +//Wolf includes +#include "base/feature/feature_base.h" + +//std includes + +//external library incudes +#include "apriltag.h" +#include "common/zarray.h" + +// opencv +#include <opencv2/features2d.hpp> + +namespace wolf { + +WOLF_PTR_TYPEDEFS(FeatureApriltag); + +//class FeatureApriltag +class FeatureApriltag : public FeatureBase +{ + public: + + FeatureApriltag(const Eigen::Vector7s & _measurement, const Eigen::Matrix6s & _meas_covariance, + const int _tag_id, const apriltag_detection_t & _det, + Scalar _rep_error1, Scalar _rep_error2, bool _use_rotation, + UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO); + virtual ~FeatureApriltag(); + + /** \brief Returns tag id + * + * Returns tag id + * + **/ + Scalar getTagId() const; + + const apriltag_detection_t& getDetection() const; + + const std::vector<cv::Point2d>& getTagCorners() const; + + Scalar getRepError1() const; + Scalar getRepError2() const; + bool getUserotation() const; + + + private: + int tag_id_; + std::vector<cv::Point2d> tag_corners_; + apriltag_detection_t detection_; + Scalar rep_error1_; + Scalar rep_error2_; + bool use_rotation_; + +}; + +} // namespace wolf + +#endif diff --git a/include/base/landmark/landmark_apriltag.h b/include/base/landmark/landmark_apriltag.h new file mode 100644 index 0000000000000000000000000000000000000000..8a586d735a8d86d34f91f142e0cc06e6c2c59150 --- /dev/null +++ b/include/base/landmark/landmark_apriltag.h @@ -0,0 +1,63 @@ + +#ifndef LANDMARK_APRILTAG_H_ +#define LANDMARK_APRILTAG_H_ + +//Wolf includes +#include "base/landmark/landmark_base.h" + +#include "base/state_block/state_quaternion.h" + +// Std includes + + +namespace wolf { + +WOLF_PTR_TYPEDEFS(LandmarkApriltag); + +//class LandmarkApriltag +class LandmarkApriltag : public LandmarkBase +{ + public: + /** \brief Constructor with type, time stamp and the position state pointer + * + * Constructor with type, and state pointer + * \param _p_ptr StateBlock shared pointer to the position + * \param _o_ptr StateBlock shared pointer to the orientation + * \param _tagid descriptor of the landmark: id of the tag + * \param _tag_width : total width of the tag (in metres) + * + **/ + LandmarkApriltag(Eigen::Vector7s& pose, const int& _tagid, const Scalar& _tag_width); + + virtual ~LandmarkApriltag(); + + /** \brief Returns tag id + * + * Returns id of the tag + * + **/ + int getTagId() const; + + /** \brief Returns tag width + * + * Returns width of the tag + * + **/ + Scalar getTagWidth() const; + + /** Factory method to create new landmarks from YAML nodes + */ + static LandmarkBasePtr create(const YAML::Node& _lmk_node); + + YAML::Node saveToYaml() const; + + + private: + const int tag_id_; + const Scalar tag_width_; + +}; + +} // namespace wolf + +#endif diff --git a/include/base/processor/ippe.h b/include/base/processor/ippe.h new file mode 100644 index 0000000000000000000000000000000000000000..d445b223f3e4afd4449da4cfe630cda87c329239 --- /dev/null +++ b/include/base/processor/ippe.h @@ -0,0 +1,222 @@ +#ifndef _IPPE_H_ +#define _IPPE_H_ + +#include <opencv2/core.hpp> +#include <opencv2/calib3d.hpp> + +#include <limits> + +#define IPPE_SMALL 1e-7 //a small constant used to test 'small' values close to zero. + +namespace IPPE { + +class PoseSolver { + +public: + + /** + * @brief PoseSolver constructor + */ + PoseSolver(); + + /** + * @brief PoseSolver destructor + */ + ~PoseSolver(); + + /** + * @brief Finds the two possible poses of a planar object given a set of correspondences and their respective reprojection errors. The poses are sorted with the first having the lowest reprojection error. + * @param _objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points + * @param _imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates. + * @param _cameraMatrix Intrinsic camera matrix (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray() + * @param _distCoeffs Intrinsic camera distortion vector (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray() + * @param _rvec1 First rotation solution (3x1 rotation vector) + * @param _tvec1 First translation solution (3x1 vector) + * @param reprojErr1 Reprojection error of first solution + * @param _rvec2 Second rotation solution (3x1 rotation vector) + * @param _tvec2 Second translation solution (3x1 vector) + * @param reprojErr2 Reprojection error of second solution + */ + void solveGeneric(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, + cv::OutputArray _rvec1, cv::OutputArray _tvec1, float& reprojErr1, cv::OutputArray _rvec2, cv::OutputArray _tvec2, float& reprojErr2); + + /** @brief Finds the two possible poses of a square planar object and their respective reprojection errors using IPPE. These poses are sorted so that the first one is the one with the lowest reprojection error. + * + * @param _squareLength The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.) + * @param _imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates. + * @param _cameraMatrix Intrinsic camera matrix (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray() + * @param _distCoeffs Intrinsic camera distortion vector (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray() + * @param _rvec1 First rotation solution (3x1 rotation vector) + * @param _tvec1 First translation solution (3x1 vector) + * @param reprojErr1 Reprojection error of first solution + * @param _rvec2 Second rotation solution (3x1 rotation vector) + * @param _tvec2 Second translation solution (3x1 vector) + * @param reprojErr2 Reprojection error of second solution + */ + void solveSquare(float squareLength, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, + cv::OutputArray _rvec1, cv::OutputArray _tvec1, float& reprojErr1, cv::OutputArray _rvec2, cv::OutputArray _tvec2, float& reprojErr2); + + /** + * @brief Generates the 4 object points of a square planar object + * @param squareLength The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.) + * @param _objectPoints Set of 4 object points (1x4 3-channel double) + */ + void generateSquareObjectCorners3D(double squareLength, cv::OutputArray _objectPoints); + + /** + * @brief Generates the 4 object points of a square planar object, without including the z-component (which is z=0 for all points). + * @param squareLength The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.) + * @param _objectPoints Set of 4 object points (1x4 2-channel double) + */ + void generateSquareObjectCorners2D(double squareLength, cv::OutputArray _objectPoints); + + /** + * @brief Computes the average depth of an object given its pose in camera coordinates + * @param objectPoints: Object points defined in 3D object space + * @param rvec: Rotation component of pose + * @param tvec: Translation component of pose + * @return: average depth of the object + */ + double meanSceneDepth(cv::InputArray objectPoints, cv::InputArray rvec, cv::InputArray tvec); + +private: + /** + * @brief Finds the two possible poses of a planar object given a set of correspondences in normalized pixel coordinates. These poses are **NOT** sorted on reprojection error. Note that the returned poses are object-to-camera transforms, and not camera-to-object transforms. + * @param _objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double). + * @param _normalizedImagePoints Array of corresponding image points in normalized pixel coordinates, 1xN/Nx1 2-channel (float or double). + * @param _Ma First pose solution (unsorted) + * @param _Mb Second pose solution (unsorted) + */ + void solveGeneric(cv::InputArray _objectPoints, cv::InputArray _normalizedImagePoints, cv::OutputArray _Ma, cv::OutputArray _Mb); + + /** + * @brief Finds the two possible poses of a planar object in its canonical position, given a set of correspondences in normalized pixel coordinates. These poses are **NOT** sorted on reprojection error. Note that the returned poses are object-to-camera transforms, and not camera-to-object transforms. + * @param _canonicalObjPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (double) where N is the number of points + * @param _normalizedInputPoints Array of corresponding image points in normalized pixel coordinates, 1xN/Nx1 2-channel (double) where N is the number of points + * @param _H Homography mapping canonicalObjPoints to normalizedInputPoints. + * @param _Ma + * @param _Mb + */ + void solveCanonicalForm(cv::InputArray _canonicalObjPoints, cv::InputArray _normalizedInputPoints, cv::InputArray _H, + cv::OutputArray _Ma, cv::OutputArray _Mb); + + /** @brief Computes the translation solution for a given rotation solution + * @param _objectPoints Array of corresponding object points, 1xN/Nx1 3-channel where N is the number of points + * @param _normalizedImagePoints Array of corresponding image points (undistorted), 1xN/Nx1 2-channel where N is the number of points + * @param _R Rotation solution (3x1 rotation vector) + * @param _t Translation solution Translation solution (3x1 rotation vector) + */ + void computeTranslation(cv::InputArray _objectPoints, cv::InputArray _normalizedImgPoints, cv::InputArray _R, cv::OutputArray _t); + + /** @brief Computes the two rotation solutions from the Jacobian of a homography matrix H at a point (ux,uy) on the object plane. For highest accuracy the Jacobian should be computed at the centroid of the point correspondences (see the IPPE paper for the explanation of this). For a point (ux,uy) on the object plane, suppose the homography H maps (ux,uy) to a point (p,q) in the image (in normalized pixel coordinates). The Jacobian matrix [J00, J01; J10,J11] is the Jacobian of the mapping evaluated at (ux,uy). + * @param j00 Homography jacobian coefficent at (ux,uy) + * @param j01 Homography jacobian coefficent at (ux,uy) + * @param j10 Homography jacobian coefficent at (ux,uy) + * @param j11 Homography jacobian coefficent at (ux,uy) + * @param p the x coordinate of point (ux,uy) mapped into the image (undistorted and normalized position) + * @param q the y coordinate of point (ux,uy) mapped into the image (undistorted and normalized position) + */ + void computeRotations(double j00, double j01, double j10, double j11, double p, double q, cv::OutputArray _R1, cv::OutputArray _R2); + + /** @brief Closed-form solution for the homography mapping with four corner correspondences of a square (it maps source points to target points). The source points are the four corners of a zero-centred squared defined by: + * point 0: [-squareLength / 2.0, squareLength / 2.0] + * point 1: [squareLength / 2.0, squareLength / 2.0] + * point 2: [squareLength / 2.0, -squareLength / 2.0] + * point 3: [-squareLength / 2.0, -squareLength / 2.0] + * + * @param _targetPoints Array of four corresponding target points, 1x4/4x1 2-channel. Note that the points should be ordered to correspond with points 0, 1, 2 and 3. + * @param halfLength the square's half length (i.e. squareLength/2.0) + * @param _H Homograhy mapping the source points to the target points, 3x3 single channel + */ + void homographyFromSquarePoints(cv::InputArray _targetPoints, double halfLength, cv::OutputArray _H); + + /** @brief Fast conversion from a rotation matrix to a rotation vector using Rodrigues' formula + * @param _R Input rotation matrix, 3x3 1-channel (double) + * @param _r Output rotation vector, 3x1/1x3 1-channel (double) + */ + void rot2vec(cv::InputArray _R, cv::OutputArray _r); + + /** + * @brief Takes a set of planar object points and transforms them to 'canonical' object coordinates This is when they have zero mean and are on the plane z=0 + * @param _objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points + * @param _canonicalObjectPoints Object points in canonical coordinates 1xN/Nx1 2-channel (double) + * @param _MobjectPoints2Canonical Transform matrix mapping _objectPoints to _canonicalObjectPoints: 4x4 1-channel (double) + */ + void makeCanonicalObjectPoints(cv::InputArray _objectPoints, cv::OutputArray _canonicalObjectPoints, cv::OutputArray _MobjectPoints2Canonical); + + /** + * @brief Evaluates the Root Mean Squared (RMS) reprojection error of a pose solution. + * @param _objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points + * @param _imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates. + * @param _cameraMatrix Intrinsic camera matrix (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray(). + * @param _distCoeffs Intrinsic camera distortion vector (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray(). + * @param _M Pose matrix from 3D object to camera coordinates: 4x4 1-channel (double) + * @param err RMS reprojection error + */ + void evalReprojError(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, cv::InputArray _M, float& err); + + /** + * @brief Sorts two pose solutions according to their RMS reprojection error (lowest first). + * @param _objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points + * @param _imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates. + * @param _cameraMatrix Intrinsic camera matrix (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray(). + * @param _distCoeffs Intrinsic camera distortion vector (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray(). + * @param _Ma Pose matrix 1: 4x4 1-channel + * @param _Mb Pose matrix 2: 4x4 1-channel + * @param _M1 Member of (Ma,Mb} with lowest RMS reprojection error. Performs deep copy. + * @param _M2 Member of (Ma,Mb} with highest RMS reprojection error. Performs deep copy. + * @param err1 RMS reprojection error of _M1 + * @param err2 RMS reprojection error of _M2 + */ + void sortPosesByReprojError(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, cv::InputArray _Ma, cv::InputArray _Mb, cv::OutputArray _M1, cv::OutputArray _M2, float& err1, float& err2); + + /** + * @brief Finds the rotation _Ra that rotates a vector _a to the z axis (0,0,1) + * @param _a vector: 3x1 mat (double) + * @param _Ra Rotation: 3x3 mat (double) + */ + void rotateVec2ZAxis(cv::InputArray _a, cv::OutputArray _Ra); + + + /** + * @brief Computes the rotation _R that rotates the object points to the plane z=0. This uses the cross-product method with the first three object points. + * @param _objectPoints Array of N>=3 coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points + * @param _R Rotation Mat: 3x3 (double) + * @return success (true) or failure (false) + */ + bool computeObjextSpaceR3Pts(cv::InputArray _objectPoints, cv::OutputArray _R); + + /** + * @brief computeObjextSpaceRSvD Computes the rotation _R that rotates the object points to the plane z=0. This uses the cross-product method with the first three object points. + * @param _objectPointsZeroMean zero-meaned co=planar object points: 3xN matrix (double) where N>=3 + * @param _R Rotation Mat: 3x3 (double) + * @return success (true) or failure (false) + */ + bool computeObjextSpaceRSvD(cv::InputArray _objectPointsZeroMean, cv::OutputArray _R); +}; +} + +namespace HomographyHO { + +/** + * @brief Computes the best-fitting homography matrix from source to target points using Harker and O'Leary's method: + * Harker, M., O'Leary, P., Computation of Homographies, Proceedings of the British Machine Vision Conference 2005, Oxford, England. + * This is not the author's implementation. + * @param srcPoints Array of source points: 1xN/Nx1 2-channel (float or double) where N is the number of points + * @param targPoints Array of target points: 1xN/Nx1 2-channel (float or double) + * @param H Homography from source to target: 3x3 1-channel (double) + */ +void homographyHO(cv::InputArray srcPoints, cv::InputArray targPoints, cv::OutputArray H); + +/** + * @brief Performs data normalization before homography estimation. For details see Hartley, R., Zisserman, A., Multiple View Geometry in Computer Vision, + * Cambridge University Press, Cambridge, 2001 + * @param Data Array of source data points: 1xN/Nx1 2-channel (float or double) where N is the number of points + * @param DataN Normalized data points: 1xN/Nx1 2-channel (float or double) where N is the number of points + * @param T Homogeneous transform from source to normalized: 3x3 1-channel (double) + * @param Ti Homogeneous transform from normalized to source: 3x3 1-channel (double) + */ +void normalizeDataIsotropic(cv::InputArray Data, cv::OutputArray DataN, cv::OutputArray T, cv::OutputArray Ti); +} + +#endif diff --git a/include/base/processor/processor_tracker_landmark_apriltag.h b/include/base/processor/processor_tracker_landmark_apriltag.h new file mode 100644 index 0000000000000000000000000000000000000000..9e0532cf5f869e2b4eedb750f90b996020203bfe --- /dev/null +++ b/include/base/processor/processor_tracker_landmark_apriltag.h @@ -0,0 +1,183 @@ +#ifndef _PROCESSOR_TRACKER_LANDMARK_APRILTAG_H_ +#define _PROCESSOR_TRACKER_LANDMARK_APRILTAG_H_ + +//Wolf includes +#include "base/common/wolf.h" +#include "base/processor/processor_tracker_landmark.h" +#include "base/sensor/sensor_camera.h" +#include "base/factor/factor_autodiff_distance_3D.h" + +// Apriltag +#include <apriltag.h> + +// open cv +#include <opencv/cv.h> + + +namespace wolf +{ + +WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmarkApriltag); + +struct ProcessorParamsTrackerLandmarkApriltag : public ProcessorParamsTrackerLandmark +{ + //tag parameters + std::string tag_family_; + + // tag sizes + Scalar tag_width_default_; + std::map<int, Scalar> tag_widths_; + + //detector parameters + Scalar quad_decimate_; + Scalar quad_sigma_; + unsigned int nthreads_; + bool debug_; + bool refine_edges_; + + Scalar std_xy_, std_z_, std_rpy_; + Scalar std_pix_; + Scalar min_time_vote_; + Scalar max_time_vote_; + int max_features_diff_; + int nb_vote_for_every_first_; + bool enough_info_necessary_; + bool add_3D_cstr_; + Scalar ippe_min_ratio_; + Scalar ippe_max_rep_error_; + + bool reestimate_last_frame_; +}; + + + +WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkApriltag); + +class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark +{ + public: + + + /** \brief Class constructor + */ + ProcessorTrackerLandmarkApriltag( ProcessorParamsTrackerLandmarkApriltagPtr _params_tracker_landmark_apriltag); + + /** \brief Class Destructor + */ + virtual ~ProcessorTrackerLandmarkApriltag(); + + void preProcess(); + void postProcess(); + + /** \brief Find provided landmarks in the incoming capture + * \param _landmark_list_in input list of landmarks to be found in incoming + * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in + * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr + * \return the number of landmarks found + */ + virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmark_list_in, FeatureBasePtrList& _feature_list_out, + LandmarkMatchMap& _feature_landmark_correspondences); + + /** \brief Vote for KeyFrame generation + * + * If a KeyFrame criterion is validated, this function returns true, + * meaning that it wants to create a KeyFrame at the \b last Capture. + * + * WARNING! This function only votes! It does not create KeyFrames! + */ + virtual bool voteForKeyFrame(); + + /** \brief Detect new Features in last Capture + * \param _max_features The maximum number of features to detect. + * \return The number of detected Features. + * + * This function detects Features that do not correspond to known Landmarks in the system. + * + * The function sets the member new_features_list_, the list of newly detected features + * in last_ptr_ to be used for landmark initialization. + */ + virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _feature_list_out); + + /** \brief Create one landmark + * + * Implement in derived classes to build the type of landmark you need for this tracker. + */ + virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr); + + /** \brief Create a new constraint + * \param _feature_ptr pointer to the Feature to constrain + * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained. + * + * Implement this method in derived classes. + */ + virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr); + + virtual void configure(SensorBasePtr _sensor); + + void reestimateLastFrame(); + + // for factory + static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr); + + public: + Scalar getTagWidth(int _id) const; + std::string getTagFamily() const; + Eigen::Vector6s getVarVec(); + FeatureBasePtrList getIncomingDetections() const; + FeatureBasePtrList getLastDetections() const; + Eigen::Affine3d opencvPoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar>, double _tag_width); + Eigen::Affine3d umichPoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar>, double _tag_width); + void ippePoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar>, double _tag_width, + Eigen::Affine3d &_M1, + double &_rep_error1, + Eigen::Affine3d &_M2, + double &_rep_error2); + Eigen::Matrix6s computeInformation(Eigen::Vector3s const &_t, Eigen::Matrix3s const &_R, Eigen::Matrix3s const &_K, double const &_tag_width, double const &_sig_q); + void pinholeHomogeneous(Eigen::Matrix3s const & _K, Eigen::Vector3s const & _t, + Eigen::Matrix3s const & _R, Eigen::Vector3s const & _p, + Eigen::Vector3s &_h, Eigen::Matrix3s &_J_h_T, Eigen::Matrix3s &_J_h_R); + void cornersToPose(const std::vector<cv::Point2d> &_img_pts, + const std::vector<Scalar> &_k_vec, + Eigen::Affine3ds &_M); + + protected: + void advanceDerived(); + void resetDerived(); + + private: + std::map<int, Scalar> tag_widths_; ///< each tag's width indexed by tag_id + Scalar tag_width_default_; ///< all tags widths defaut to this + cv::Mat grayscale_image_; + apriltag_detector_t detector_; + apriltag_family_t tag_family_; + Scalar std_xy_, std_z_, std_rpy_; ///< dummy std values for covariance computation + Scalar std_pix_; ///< pixel error to be propagated to a camera to tag transformation covariance + Scalar ippe_min_ratio_; + Scalar ippe_max_rep_error_; +// Eigen::Affine3ds c_M_ac_; ///< aprilCamera-to-camera transform not used with solvePnP +// double cx_, cy_, fx_, fy_; + Matrix3s K_; + cv::Mat_<Scalar> cv_K_; + bool reestimate_last_frame_; + int n_reset_; + + protected: + FeatureBasePtrList detections_incoming_; ///< detected tags in wolf form, incoming image + FeatureBasePtrList detections_last_; ///< detected tags in wolf form, last image + + + // To be able to access them in unit tests + protected: + Scalar min_time_vote_; + Scalar max_time_vote_; + unsigned int min_features_for_keyframe_; + int max_features_diff_; + int nb_vote_for_every_first_; + bool enough_info_necessary_; + bool add_3D_cstr_; + int nb_vote_; +}; + +} // namespace wolf + +#endif /* _PROCESSOR_TRACKER_LANDMARK_APRILTAG_H_ */ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 78c8303e47195a85b6e6f3c3b84fed68a7959f02..caab481fc894f13fb75ba9e388839589e12dbffa 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -61,12 +61,28 @@ IF (vision_utils_FOUND) FIND_PACKAGE(OpenCV QUIET) ENDIF(vision_utils_FOUND) +# OpenCV +FIND_PACKAGE(OpenCV QUIET) +IF (OPENCV_FOUND) + MESSAGE("opencv Library FOUND: opencv related sources will be built.") +ENDIF(OPENCV_FOUND) + # Cereal FIND_PACKAGE(cereal QUIET) IF(cereal_FOUND) MESSAGE("cereal Library FOUND: cereal related sources will be built.") ENDIF(cereal_FOUND) +# Apriltag +# TODO: write proper files to be able to use find_package with apriltag library +FIND_PATH(APRILTAG_INCLUDE_DIR NAMES apriltag.h PATH_SUFFIXES "apriltag" ${APRILTAG_INCLUDE_PATH}) +FIND_LIBRARY(APRILTAG_LIBRARY NAMES apriltag PATH_SUFFIXES "${CMAKE_LIBRARY_ARCHITECTURE}" "apriltag" ${APRILTAG_LIBRARY_PATH}) + +IF(APRILTAG_LIBRARY) + SET(Apriltag_FOUND TRUE) + MESSAGE("apriltag Library FOUND in ${APRILTAG_LIBRARY}: apriltag related sources will be built.") +ENDIF(APRILTAG_LIBRARY) + # YAML with yaml-cpp INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake) IF(YAMLCPP_FOUND) @@ -157,6 +173,10 @@ IF(vision_utils_FOUND) INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS}) ENDIF(vision_utils_FOUND) +IF(OPENCV_FOUND) + INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS}) +ENDIF(OPENCV_FOUND) + # cereal IF(cereal_FOUND) INCLUDE_DIRECTORIES(${cereal_INCLUDE_DIRS}) @@ -171,9 +191,13 @@ IF(YAMLCPP_FOUND) ENDIF(YAMLCPP_FOUND) IF(GLOG_FOUND) -INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR}) + INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR}) ENDIF(GLOG_FOUND) +IF(APRILTAG_INCLUDE_DIR) + INCLUDE_DIRECTORIES(${APRILTAG_INCLUDE_DIR}) +ENDIF(APRILTAG_INCLUDE_DIR) + #headers SET(HDRS_BASE capture/capture_motion.h @@ -532,6 +556,15 @@ processor/processor_tracker_feature_trifocal.cpp ) ENDIF(vision_utils_FOUND) +IF (OPENCV_FOUND AND Apriltag_FOUND) + SET(HDRS ${HDRS} + landmark_apriltag.h + ) + SET(SRCS ${SRCS} + landmark_apriltag.cpp + ) +ENDIF(OPENCV_FOUND AND Apriltag_FOUND) + # Add the capture sub-directory # ADD_SUBDIRECTORY(captures) @@ -586,6 +619,11 @@ IF(YAMLCPP_FOUND) yaml/processor_image_yaml.cpp yaml/processor_tracker_feature_trifocal_yaml.cpp ) + IF(Apriltag_FOUND) + SET(SRCS ${SRCS} + yaml/processor_tracker_landmark_apriltag_yaml.cpp + ) + ENDIF(Apriltag_FOUND) ENDIF(vision_utils_FOUND) ENDIF(YAMLCPP_FOUND) @@ -632,6 +670,10 @@ IF (OPENCV_FOUND) ENDIF (vision_utils_FOUND) ENDIF (OPENCV_FOUND) +IF (OPENCV_FOUND) + TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS}) +ENDIF (OPENCV_FOUND) + IF (YAMLCPP_FOUND) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY}) ENDIF (YAMLCPP_FOUND) @@ -640,6 +682,12 @@ IF (GLOG_FOUND) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY}) ENDIF (GLOG_FOUND) +#check if this is done correctly +IF (OPENCV_FOUND AND Apriltag_FOUND) + LINK_LIBRARIES(apriltag m) + TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${APRILTAG_LIBRARY} ${CMAKE_THREAD_LIBS_INIT} ${OPENCV_LDFLAGS} m) +ENDIF(OPENCV_FOUND AND Apriltag_FOUND) + #install library install(TARGETS ${PROJECT_NAME}_core DESTINATION lib/iri-algorithms EXPORT ${PROJECT_NAME}_core-targets) install(EXPORT ${PROJECT_NAME}_core-targets DESTINATION lib/iri-algorithms) diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 836caa3d1c337a37eb8680871139251a371353a2..6ee86ed12165a0cc0bc78c93822a0b5a259eddb0 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -120,6 +120,11 @@ IF(vision_utils_FOUND) ADD_EXECUTABLE(test_simple_AHP test_simple_AHP.cpp) TARGET_LINK_LIBRARIES(test_simple_AHP ${PROJECT_NAME}) + IF (APRILTAG_LIBRARY) + ADD_EXECUTABLE(test_apriltag test_apriltag.cpp) + TARGET_LINK_LIBRARIES(test_apriltag ${PROJECT_NAME}) + ENDIF(APRILTAG_LIBRARY) + ENDIF(Ceres_FOUND) # Testing OpenCV functions for projection of points diff --git a/src/examples/camera_Dinesh_LAAS_params.yaml b/src/examples/camera_Dinesh_LAAS_params.yaml new file mode 100644 index 0000000000000000000000000000000000000000..739505d12e8a2dd224b99220ee024ddc8efd9508 --- /dev/null +++ b/src/examples/camera_Dinesh_LAAS_params.yaml @@ -0,0 +1,20 @@ +image_width: 640 +image_height: 480 +camera_name: mono +camera_matrix: + rows: 3 + cols: 3 + data: [809.135074, 0.000000, 335.684471, 0.000000, 809.410030, 257.352121, 0.000000, 0.000000, 1.000000] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.416913, 0.264210, -0.000221, -0.000326, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] +projection_matrix: + rows: 3 + cols: 4 + data: [753.818405, 0.000000, 337.203047, 0.000000, 0.000000, 777.118492, 258.102663, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/src/examples/camera_Dinesh_LAAS_params_notangentrect.yaml b/src/examples/camera_Dinesh_LAAS_params_notangentrect.yaml new file mode 100644 index 0000000000000000000000000000000000000000..dd2f6433f2b60c21b68ebf70b595af981550923c --- /dev/null +++ b/src/examples/camera_Dinesh_LAAS_params_notangentrect.yaml @@ -0,0 +1,20 @@ +image_width: 640 +image_height: 480 +camera_name: mono +camera_matrix: + rows: 3 + cols: 3 + data: [809.135074, 0.000000, 335.684471, 0.000000, 809.410030, 257.352121, 0.000000, 0.000000, 1.000000] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.416913, 0.264210, 0, 0, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] +projection_matrix: + rows: 3 + cols: 4 + data: [753.818405, 0.000000, 337.203047, 0.000000, 0.000000, 777.118492, 258.102663, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/src/examples/camera_logitech_c300_640_480.yaml b/src/examples/camera_logitech_c300_640_480.yaml new file mode 100644 index 0000000000000000000000000000000000000000..5f8a1899b71df3721e6f9722d39bd8e09e34509a --- /dev/null +++ b/src/examples/camera_logitech_c300_640_480.yaml @@ -0,0 +1,22 @@ +image_width: 640 +image_height: 480 +#camera_name: narrow_stereo +camera_name: camera +camera_matrix: + rows: 3 + cols: 3 + data: [711.687376, 0.000000, 323.306816, 0.000000, 710.933260, 232.005822, 0.000000, 0.000000, 1.000000] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [0.067204, -0.141639, 0, 0, 0] +# data: [0.067204, -0.141639, 0.004462, -0.000636, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] +projection_matrix: + rows: 3 + cols: 4 + data: [718.941772, 0.000000, 323.016804, 0.000000, 0.000000, 717.174622, 233.475721, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/src/examples/camera_params_canonical.yaml b/src/examples/camera_params_canonical.yaml index 370b662c9b958d0c4ab528df8ab793567141f685..2508a0bec574125ae9dea63e558528b7211079d1 100644 --- a/src/examples/camera_params_canonical.yaml +++ b/src/examples/camera_params_canonical.yaml @@ -17,4 +17,4 @@ rectification_matrix: projection_matrix: rows: 3 cols: 4 - data: [1, 0, 1, 0, 0, 1, 1, 0, 0, 0, 1, 0] \ No newline at end of file + data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/src/examples/map_apriltag_1.yaml b/src/examples/map_apriltag_1.yaml new file mode 100644 index 0000000000000000000000000000000000000000..d0c6a90707ddd2d15a2f921f244f085ecb337e6f --- /dev/null +++ b/src/examples/map_apriltag_1.yaml @@ -0,0 +1,42 @@ +map name: "Example of map of Apriltag landmarks" + +nlandmarks: 4 # This must match the number of landmarks in the list that follows. Otherwise it is an error. + +landmarks: + + - id : 1 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 1 + tag width: 0.1 + position: [0, 0, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 3 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 3 + tag width: 0.1 + position: [1, 1, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 5 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 5 + tag width: 0.1 + position: [1, 0, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 2 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 2 + tag width: 0.1 + position: [0, 1, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + diff --git a/src/examples/maps/map_apriltag_logitech_1234.yaml b/src/examples/maps/map_apriltag_logitech_1234.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f313d1a11a3f2b4fa239f710cbbea7372747677d --- /dev/null +++ b/src/examples/maps/map_apriltag_logitech_1234.yaml @@ -0,0 +1,46 @@ +map name: "4 tags printed on a A4 sheet vertical recorded at IRI with logitech webcam." + +nlandmarks: 4 # This must match the number of landmarks in the list that follows. Otherwise it is an error. + +###################### +# World frame is considered to be the top left corner of tag id 0. Reference frame is corherent with a camera +# looking straight at the sheet with RBF convention. +###################### +landmarks: + + - id : 0 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 0 + tag width: 0.055 + position: [0.0225, 0.0225, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 1 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 1 + tag width: 0.055 + position: [0.1525, 0.0225, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 2 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 2 + tag width: 0.055 + position: [0.0225, 0.2125, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 3 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 3 + tag width: 0.055 + position: [0.1525, 0.2125, 0] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + diff --git a/src/examples/processor_imu.yaml b/src/examples/processor_imu.yaml index 7e684c8833a6e9e3123863c71366a989b30e4004..8e8a6c8cd0b75c0366a7aa83db4f3d54e4687fa6 100644 --- a/src/examples/processor_imu.yaml +++ b/src/examples/processor_imu.yaml @@ -1,5 +1,7 @@ processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error. processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails +unmeasured perturbation std: 0.00001 +time tolerance: 0.0025 # Time tolerance for joining KFs keyframe vote: max time span: 2.0 # seconds max buffer length: 20000 # motion deltas diff --git a/src/examples/processor_imu_no_vote.yaml b/src/examples/processor_imu_no_vote.yaml index 4f6ad39556cd9a09a215f043d4beb0066d4a37bb..678867b719b191b6ba980a5c712f5164a9f83e28 100644 --- a/src/examples/processor_imu_no_vote.yaml +++ b/src/examples/processor_imu_no_vote.yaml @@ -1,5 +1,7 @@ processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error. processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails +time tolerance: 0.0025 # Time tolerance for joining KFs +unmeasured perturbation std: 0.00001 keyframe vote: max time span: 999999.0 # seconds max buffer length: 999999 # motion deltas diff --git a/src/examples/processor_imu_t1.yaml b/src/examples/processor_imu_t1.yaml index e0c21758c11ed2a684b2f3f2bc2aeb4c557c84ef..b68740d245b4922a4a095b2a0ac1b2ce5becbd52 100644 --- a/src/examples/processor_imu_t1.yaml +++ b/src/examples/processor_imu_t1.yaml @@ -1,5 +1,7 @@ processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error. processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails +unmeasured perturbation std: 0.00001 +time tolerance: 0.0025 # Time tolerance for joining KFs keyframe vote: max time span: 0.9999 # seconds max buffer length: 10000 # motion deltas diff --git a/src/examples/processor_imu_t6.yaml b/src/examples/processor_imu_t6.yaml index e3a4b17df72c957fec49d935ddcd3a9a8c824a96..511a917c7500abbb445c7bfb016737c881dc400a 100644 --- a/src/examples/processor_imu_t6.yaml +++ b/src/examples/processor_imu_t6.yaml @@ -1,5 +1,7 @@ processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error. processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails +unmeasured perturbation std: 0.00001 +time tolerance: 0.0025 # Time tolerance for joining KFs keyframe vote: max time span: 5.9999 # seconds max buffer length: 10000 # motion deltas diff --git a/src/examples/processor_odom_3D.yaml b/src/examples/processor_odom_3D.yaml index aff35c9d2732c6489c1506021874f5325f303678..f501e333800ec1bbb4b7c751a32aa67a99bec74c 100644 --- a/src/examples/processor_odom_3D.yaml +++ b/src/examples/processor_odom_3D.yaml @@ -1,5 +1,6 @@ processor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error. processor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails +time tolerance: 0.01 # seconds keyframe vote: max time span: 0.2 # seconds max buffer length: 10 # motion deltas diff --git a/src/examples/processor_tracker_landmark_apriltag.yaml b/src/examples/processor_tracker_landmark_apriltag.yaml new file mode 100644 index 0000000000000000000000000000000000000000..e8b1fd02dc80ffedefafc44ae3af9898a873cb3b --- /dev/null +++ b/src/examples/processor_tracker_landmark_apriltag.yaml @@ -0,0 +1,57 @@ +processor type: "TRACKER LANDMARK APRILTAG" +processor name: "tracker landmark apriltag example" + +detector parameters: + quad_decimate: 1.5 # doing quad detection at lower resolution to speed things up (see end of file) + quad_sigma: 0.8 # gaussian blur good for noisy images, may be recommended with quad_decimate. Kernel size adapted (see end of this file) + nthreads: 8 # how many thread during tag detection (does not change much?) + debug: false # write some debugging images + refine_edges: true # better edge detection if quad_decimate > 1 (quite inexpensive, almost no diff) + ippe_min_ratio: 3.0 # quite arbitrary, always > 1 (to deactive, set at 0 for example) + ippe_max_rep_error: 2.0 # to deactivate, set at something big (100) + +tag widths: + 0: 0.055 + 1: 0.055 + 2: 0.055 + 3: 0.055 + +tag parameters: + tag_family: "tag36h11" + # tag_black_border: 1 + tag_width_default: 0.165 # used if tag width not specified + + +noise: + std_xy : 0.1 # m + std_z : 0.1 # m + std_rpy_degrees : 5 # degrees + std_pix: 20 # pixel error + +vote: + voting active: true + min_time_vote: 0 # s + max_time_vote: 0 # s + min_features_for_keyframe: 12 + max_features_diff: 17 + nb_vote_for_every_first: 50 + enough_info_necessary: true # create kf if enough information to uniquely determine the KF position (necessary for apriltag_only slam) + +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 + + +# Annexes: +### Quad decimate: the higher, the lower the resolution +# Does nothing if <= 1.0 +# Only values taken into account: +# 1.5, 2, 3, 4 +# 1.5 -> ~*2 speed up + +# Higher values use a "bad" code according to commentaries in the library, smaller do nothing +### Gaussian blur table: +# max sigma kernel size +# 0.499 1 (disabled) +# 0.999 3 +# 1.499 5 +# 1.999 7 diff --git a/src/examples/test_apriltag.cpp b/src/examples/test_apriltag.cpp new file mode 100644 index 0000000000000000000000000000000000000000..75b5804c9df104247d43e5f2b728f2f57f6bd107 --- /dev/null +++ b/src/examples/test_apriltag.cpp @@ -0,0 +1,290 @@ +/** + * \file test_apriltag.cpp + * + * Created on: Dec 14, 2018 + * \author: Dinesh Atchtuhan + */ + +//Wolf +#include "base/wolf.h" +#include "base/rotations.h" +#include "base/problem.h" +#include "base/ceres_wrapper/ceres_manager.h" +#include "base/sensor/sensor_camera.h" +#include "base/processor/processor_tracker_landmark_apriltag.h" +#include "base/capture/capture_image.h" +#include "base/feature/feature_apriltag.h" + +// opencv +#include <opencv2/imgproc/imgproc.hpp> +#include "opencv2/opencv.hpp" + +// Eigen +#include <Eigen/Core> +#include <Eigen/Geometry> + +// std +#include <iostream> +#include <stdlib.h> + + +void draw_apriltag(cv::Mat image, std::vector<cv::Point2d> corners, int thickness=1, bool draw_corners=false); + + +int main(int argc, char *argv[]) +{ + /* + * HOW TO USE ? + * For now, just call the executable and append the list of images to be processed. + * The images must be placed in the root folder of your wolf project. + * Ex: + * ./test_apriltag frame1.jpg frame2.jpg frame3.jpg + */ + + using namespace wolf; + + + // General execution options + const bool APPLY_CONTRAST = false; + const bool IMAGE_OUTPUT = true; + const bool USEMAP = false; + + + WOLF_INFO( "==================== processor apriltag test ======================" ) + + std::string wolf_root = _WOLF_ROOT_DIR; + // Wolf problem + ProblemPtr problem = Problem::create("PO 3D"); + ceres::Solver::Options options; + options.function_tolerance = 1e-6; + options.max_num_iterations = 100; + CeresManagerPtr ceres_manager = std::make_shared<CeresManager>(problem, options); + + + WOLF_INFO( "==================== Configure Problem ======================" ) + Eigen::Vector7s cam_extrinsics; cam_extrinsics << 0,0,0, 0,0,0,1; + SensorBasePtr sen = problem->installSensor("CAMERA", "camera", cam_extrinsics, wolf_root + "/src/examples/camera_logitech_c300_640_480.yaml"); +// SensorBasePtr sen = problem->installSensor("CAMERA", "camera", cam_extrinsics, wolf_root + "/src/examples/camera_Dinesh_LAAS_params_notangentrect.yaml"); + SensorCameraPtr sen_cam = std::static_pointer_cast<SensorCamera>(sen); + ProcessorBasePtr prc = problem->installProcessor("TRACKER LANDMARK APRILTAG", "apriltags", "camera", wolf_root + "/src/examples/processor_tracker_landmark_apriltag.yaml"); + + if (USEMAP){ + problem->loadMap(wolf_root + "/src/examples/maps/map_apriltag_logitech_1234.yaml"); + for (auto lmk : problem->getMap()->getLandmarkList()){ + lmk->fix(); + } + } + + // set prior + Eigen::Matrix6s covariance = Matrix6s::Identity(); + Scalar std_m; + Scalar std_deg; + if (USEMAP){ + std_m = 100; // standard deviation on original translation + std_deg = 180; // standard deviation on original rotation + } + else { + std_m = 0.00001; // standard deviation on original translation + std_deg = 0.00001; // standard deviation on original rotation + } + + covariance.topLeftCorner(3,3) = std_m*std_m * covariance.topLeftCorner(3,3); + covariance.bottomRightCorner(3,3) = (M_TORAD*std_deg)*(M_TORAD*std_deg) * covariance.bottomRightCorner(3,3); + + if (USEMAP){ + FrameBasePtr F1 = problem->setPrior((Vector7s()<<0.08, 0.15, -0.75, 0, 0, 0, 1).finished(), covariance, 0.0, 0.1); + } + else { + FrameBasePtr F1 = problem->setPrior((Vector7s()<<0,0,0,0,0,0,1).finished(), covariance, 0.0, 0.1); + F1->fix(); + } + + // first argument is the name of the program. + // following arguments are path to image (from wolf_root) + const int inputs = argc -1; + WOLF_DEBUG("nb of images: ", inputs); + cv::Mat frame; + Scalar ts(0); + Scalar dt = 1; + + WOLF_INFO( "==================== Main loop ======================" ) + for (int input = 1; input <= inputs; input++) { + std::string path = wolf_root + "/" + argv[input]; + frame = cv::imread(path, CV_LOAD_IMAGE_COLOR); + + if( frame.data ){ //if imread succeeded + + if (APPLY_CONTRAST){ + Scalar alpha = 2.0; // to tune contrast [1-3] + int beta = 0; // to tune lightness [0-100] + // Do the operation new_image(i,j) = alpha*image(i,j) + beta + for( int y = 0; y < frame.rows; y++ ){ + for( int x = 0; x < frame.cols; x++ ){ + for( int c = 0; c < 3; c++ ){ + frame.at<cv::Vec3b>(y,x)[c] = cv::saturate_cast<uchar>( alpha*( frame.at<cv::Vec3b>(y,x)[c] ) + beta ); + } + } + } + } + + CaptureImagePtr cap = std::make_shared<CaptureImage>(ts, sen_cam, frame); + // cap->setType(argv[input]); // only for problem->print() to show img filename + cap->setName(argv[input]); + WOLF_DEBUG("Processing image...", path); + sen->process(cap); + + if (IMAGE_OUTPUT){ + cv::namedWindow( cap->getName(), cv::WINDOW_NORMAL );// Create a window for display. + } + + } + else + WOLF_WARN("could not load image ", path); + + ts += dt; + } + + + if (IMAGE_OUTPUT){ + WOLF_INFO( "==================== Draw all detections ======================" ) + for (auto F : problem->getTrajectory()->getFrameList()) + { + if (F->isKey()) + { + for (auto cap : F->getCaptureList()) + { + if (cap->getType() == "IMAGE") + { + auto img = std::static_pointer_cast<CaptureImage>(cap); + for (FeatureBasePtr f : img->getFeatureList()) + { + FeatureApriltagPtr fa = std::static_pointer_cast<FeatureApriltag>(f); + draw_apriltag(img->getImage(), fa->getTagCorners(), 1); + } + cv::imshow( img->getName(), img->getImage() ); // display original image. + cv::waitKey(1); + } + } + } + } + } + + + +// WOLF_INFO( "==================== Provide perturbed prior ======================" ) +// for (auto kf : problem->getTrajectory()->getFrameList()) +// { +// Vector7s x; +// if (kf->isKey()) +// { +// x.setRandom(); +// x.tail(4).normalize(); +// kf->setState(x); +// } +// } + + WOLF_INFO( "==================== Solve problem ======================" ) + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::FULL); // 0: nothing, 1: BriefReport, 2: FullReport + WOLF_DEBUG(report); + problem->print(3,0,1,1); + + + + WOLF_INFO("============= SOLVED PROBLEM : POS | EULER (DEG) ===============") + for (auto kf : problem->getTrajectory()->getFrameList()) + { + if (kf->isKey()) + for (auto cap : kf->getCaptureList()) + { + if (cap->getType() != "POSE") + { + Vector3s T = kf->getP()->getState(); + Vector4s qv= kf->getO()->getState(); + Vector3s e = M_TODEG * R2e(q2R(qv)); + WOLF_DEBUG("KF", kf->id(), " => ", T.transpose(), " | ", e.transpose()); + } + } + } + for (auto lmk : problem->getMap()->getLandmarkList()) + { + Vector3s T = lmk->getP()->getState(); + Vector4s qv= lmk->getO()->getState(); + Vector3s e = M_TODEG * R2e(q2R(qv)); + WOLF_DEBUG(" L", lmk->id(), " => ", T.transpose(), " | ", e.transpose()); + } + + + // =============================================== + // COVARIANCES =================================== + // =============================================== + // Print COVARIANCES of all states + WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM : POS | QUAT =======") + ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + for (auto kf : problem->getTrajectory()->getFrameList()) + if (kf->isKey()) + { + Eigen::MatrixXs cov = kf->getCovariance(); + WOLF_DEBUG("KF", kf->id(), "_std (sigmas) = ", cov.diagonal().transpose().array().sqrt()); + } + for (auto lmk : problem->getMap()->getLandmarkList()) + { + Eigen::MatrixXs cov = lmk->getCovariance(); + WOLF_DEBUG(" L", lmk->id(), "_std (sigmas) = ", cov.diagonal().transpose().array().sqrt()); + } + std::cout << std::endl; + + + // =============================================== + // SAVE MAP TO YAML ============================== + // =============================================== + // + // problem->saveMap(wolf_root + "/src/examples/map_apriltag_set3_HC.yaml", "set3"); + + if (IMAGE_OUTPUT){ + cv::waitKey(0); + cv::destroyAllWindows(); + } + + return 0; + +} + + +void draw_apriltag(cv::Mat image, std::vector<cv::Point2d> corners, + int thickness, bool draw_corners) { + cv::line(image, corners[0], corners[1], CV_RGB(255, 0, 0), thickness); + cv::line(image, corners[1], corners[2], CV_RGB(0, 255, 0), thickness); + cv::line(image, corners[2], corners[3], CV_RGB(0, 0, 255), thickness); + cv::line(image, corners[3], corners[0], CV_RGB(255, 0, 255), thickness); + + /////// + // Leads to implement other displays + /////// + +// const auto line_type = cv::LINE_AA; +// if (draw_corners) { +// int r = thickness; +// cv::circle(image, cv::Point2i(p[0].x, p[0].y), r, CV_RGB(255, 0, 0), -1, +// line_type); +// cv::circle(image, cv::Point2i(p[1].x, p[1].y), r, CV_RGB(0, 255, 0), -1, +// line_type); +// cv::circle(image, cv::Point2i(p[2].x, p[2].y), r, CV_RGB(0, 0, 255), -1, +// line_type); +// cv::circle(image, cv::Point2i(p[3].x, p[3].y), r, CV_RGB(255, 0, 255), -1, +// line_type); +// } + +// cv::putText(image, std::to_string(apriltag.id), +// cv::Point2f(apriltag.center.x - 5, apriltag.center.y + 5), +// cv::FONT_HERSHEY_SIMPLEX, 1, CV_RGB(255, 0, 255), 2, line_type); + + +} + +//void DrawApriltags(cv::Mat &image, const ApriltagVec &apriltags) { +// for (const auto &apriltag : apriltags) { +//// DrawApriltag(image, apriltag); +// DrawApriltag(image, apriltag, 1); +// } +//} + diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index b090a8a5a4acf010959c4e53bf006855326a5629..dce55b16f3fd730ddaf8b0832736bb79bd30bccb 100644 --- a/src/examples/test_ceres_2_lasers.cpp +++ b/src/examples/test_ceres_2_lasers.cpp @@ -247,7 +247,7 @@ int main(int argc, char** argv) odom_trajectory.head(3) = ground_truth_pose; // Origin Key Frame - FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts); + FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts); // Prior covariance CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1); diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp index 7e83dec251a85252b14d1014114a1089c912ba27..d79bbfaf88853ee8ccec48f5c172ecce2b83e940 100644 --- a/src/examples/test_ceres_2_lasers_polylines.cpp +++ b/src/examples/test_ceres_2_lasers_polylines.cpp @@ -254,7 +254,7 @@ int main(int argc, char** argv) odom_trajectory.head(3) = ground_truth_pose; // Origin Key Frame - FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts); + FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts); // Prior covariance CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1); diff --git a/src/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp index 21c2a8b9495dc92fa43dfae9dd0c87238971125d..5585fe035ca81d0b7ae8442a03cb3ff7c790680a 100644 --- a/src/examples/test_diff_drive.cpp +++ b/src/examples/test_diff_drive.cpp @@ -163,7 +163,7 @@ int main(int argc, char** argv) } // Wolf problem - ProblemPtr wolf_problem_ptr_ = Problem::create("PO 2D"); + ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2); const std::string sensor_name("Main Odometer"); Eigen::VectorXs extrinsics(3); diff --git a/src/examples/test_factor_AHP.cpp b/src/examples/test_factor_AHP.cpp index 10c7610ca7c523cd56689896aeaf4549e155a58b..8509ff7f573c31ccef85ace45db7d5036b44d470 100644 --- a/src/examples/test_factor_AHP.cpp +++ b/src/examples/test_factor_AHP.cpp @@ -21,7 +21,7 @@ int main() //===================================================== // Wolf problem - ProblemPtr wolf_problem_ptr_ = Problem::create("PO 3D"); + ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 3); /* Do this while there aren't extrinsic parameters on the yaml */ Eigen::Vector7s extrinsic_cam; diff --git a/src/examples/test_factor_imu.cpp b/src/examples/test_factor_imu.cpp index 9d7d73379708fe897e8ec0b7bb00e39fda7f8559..36e6bfa52385e37e51a8d1616e3c20640f35b371 100644 --- a/src/examples/test_factor_imu.cpp +++ b/src/examples/test_factor_imu.cpp @@ -80,7 +80,7 @@ int main(int argc, char** argv) //create FrameIMU ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState(); - FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec); + FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec); wolf_problem_ptr_->getTrajectory()->addFrame(last_frame); //create a feature @@ -141,7 +141,7 @@ int main(int argc, char** argv) //create FrameIMU ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState(); - FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec); + FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec); wolf_problem_ptr_->getTrajectory()->addFrame(last_frame); //create a feature @@ -199,7 +199,7 @@ int main(int argc, char** argv) //create FrameIMU ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState(); - FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec); + FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec); wolf_problem_ptr_->getTrajectory()->addFrame(last_frame); //create a feature diff --git a/src/examples/test_image.cpp b/src/examples/test_image.cpp index 842a6a14b6e682da3481a7f3bd51d80e819fb857..3997b9476334d39092ee2eaf5140c5b9a9facfc0 100644 --- a/src/examples/test_image.cpp +++ b/src/examples/test_image.cpp @@ -52,7 +52,7 @@ int main(int argc, char** argv) std::string wolf_root = _WOLF_ROOT_DIR; std::cout << "Wolf root: " << wolf_root << std::endl; - ProblemPtr wolf_problem_ = Problem::create("PO 3D"); + ProblemPtr wolf_problem_ = Problem::create("PO", 3); //===================================================== // Method 1: Use data generated here for sensor and processor diff --git a/src/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp index ea1079d75c7f11cb3de61fe55df12ce3bd39c441..9e01558e3bc013c65d553d7e213785fdacfaaf96 100644 --- a/src/examples/test_kf_callback.cpp +++ b/src/examples/test_kf_callback.cpp @@ -16,7 +16,7 @@ int main() using namespace Eigen; using namespace std; - ProblemPtr problem = Problem::create("PO 2D"); + ProblemPtr problem = Problem::create("PO", 2); SensorBasePtr sen_odo = problem->installSensor ("ODOM 2D", "main odometer", (Vector3s()<<0,0,0).finished(),""); ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>(); diff --git a/src/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp index c4ba44c55669a60a9d188a0217f537be30e31fce..e509c94e2116b2f7bc08e70d866eafb5ed5a9bfb 100644 --- a/src/examples/test_map_yaml.cpp +++ b/src/examples/test_map_yaml.cpp @@ -75,7 +75,7 @@ int main() std::string wolf_config = wolf_root + "/src/examples"; std::cout << "\nWolf directory for configuration files: " << wolf_config << std::endl; - ProblemPtr problem = Problem::create("PO 2D"); + ProblemPtr problem = Problem::create("PO", 2); filename = wolf_config + "/map_polyline_example.yaml"; std::cout << "Reading map from file: " << filename << std::endl; problem->loadMap(filename); diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp index 8103a7ff39f3e1dc55788230bcbded86b56927cc..7cfae793d291d83364261c8996f78d0b64fa3d13 100644 --- a/src/examples/test_processor_odom_3D.cpp +++ b/src/examples/test_processor_odom_3D.cpp @@ -40,7 +40,7 @@ int main (int argc, char** argv) } cout << "Final timestamp tf = " << tf.get() << " s" << endl; - ProblemPtr problem = Problem::create("PO 3D"); + ProblemPtr problem = Problem::create("PO", 3); ceres::Solver::Options ceres_options; // ceres_options.max_num_iterations = 1000; // ceres_options.function_tolerance = 1e-10; diff --git a/src/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp index cd3282ee8f7f04be38cc4b1b8d9bf9b9d4643f07..74340d9e50d8fb1236f0db02c053f39f81807792 100644 --- a/src/examples/test_processor_tracker_feature.cpp +++ b/src/examples/test_processor_tracker_feature.cpp @@ -26,7 +26,7 @@ int main() std::cout << std::endl << "==================== processor tracker feature test ======================" << std::endl; // Wolf problem - ProblemPtr wolf_problem_ptr_ = Problem::create("PO 2D"); + ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2); SensorBasePtr sensor_ptr_ = make_shared<SensorBase>("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp index 05eb1a5194b43dadb453120af570ebbc342fb7e3..5500fdcbb126b413492620e7ea2828738a89300a 100644 --- a/src/examples/test_processor_tracker_landmark.cpp +++ b/src/examples/test_processor_tracker_landmark.cpp @@ -61,19 +61,22 @@ int main() std::cout << std::endl << "==================== processor tracker landmark test ======================" << std::endl; // Wolf problem - ProblemPtr wolf_problem_ptr_ = Problem::create("PO 2D"); - SensorBasePtr sensor_ptr_ = std::make_shared< SensorBase>("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), - std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), - std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); - + ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2); + // SensorBasePtr sensor_ptr_ = std::make_shared< SensorBase>("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), + // std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), + // std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); + auto sensor_ptr_ = SensorBase::emplace<SensorBase>(wolf_problem_ptr_->getHardware(), "ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), + std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), + std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); ProcessorParamsTrackerLandmarkPtr params_trk = std::make_shared<ProcessorParamsTrackerLandmark>(); params_trk->max_new_features = 5; params_trk->min_features_for_keyframe = 7; params_trk->time_tolerance = 0.25; - std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = std::make_shared< ProcessorTrackerLandmarkDummy>(params_trk); - - wolf_problem_ptr_->addSensor(sensor_ptr_); - sensor_ptr_->addProcessor(processor_ptr_); + // std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = std::make_shared< ProcessorTrackerLandmarkDummy>(params_trk); + std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = + std::static_pointer_cast<ProcessorTrackerLandmarkDummy>(ProcessorBase::emplace<ProcessorTrackerLandmarkDummy>(sensor_ptr_, params_trk)); + // wolf_problem_ptr_->addSensor(sensor_ptr_); + // sensor_ptr_->addProcessor(processor_ptr_); std::cout << "sensor & processor created and added to wolf problem" << std::endl; diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp index f3c79633d5b27a3ec3fc84bb52124eb080380993..e8767c4f69d1ece65c6886cf87a2a405fb0d7ca7 100644 --- a/src/examples/test_processor_tracker_landmark_image.cpp +++ b/src/examples/test_processor_tracker_landmark_image.cpp @@ -80,7 +80,7 @@ int main(int argc, char** argv) //===================================================== // Wolf problem - ProblemPtr problem = Problem::create("PO 3D"); + ProblemPtr problem = Problem::create("PO", 3); // ODOM SENSOR AND PROCESSOR SensorBasePtr sensor_base = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml"); @@ -98,7 +98,7 @@ int main(int argc, char** argv) //===================================================== // Origin Key Frame is fixed TimeStamp t = 0; - FrameBasePtr origin_frame = problem->emplaceFrame(KEY_FRAME, (Vector7s()<<1,0,0,0,0,0,1).finished(), t); + FrameBasePtr origin_frame = problem->emplaceFrame(KEY, (Vector7s()<<1,0,0,0,0,0,1).finished(), t); problem->getProcessorMotion()->setOrigin(origin_frame); origin_frame->fix(); diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp index 290e0915cb85612c6c310eb898b7da42badd7104..f4316cb071860fc7340606e9ef0521d0b9a3aea4 100644 --- a/src/examples/test_simple_AHP.cpp +++ b/src/examples/test_simple_AHP.cpp @@ -95,13 +95,13 @@ int main(int argc, char** argv) // ============================================================================================================ /* 1 */ - ProblemPtr problem = Problem::create("PO 3D"); + ProblemPtr problem = Problem::create("PO", 3); // One anchor frame to define the lmk, and a copy to make a factor - FrameBasePtr kf_1 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); - FrameBasePtr kf_2 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_1 = problem->emplaceFrame(KEY,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_2 = problem->emplaceFrame(KEY,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); // and two other frames to observe the lmk - FrameBasePtr kf_3 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0)); - FrameBasePtr kf_4 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_3 = problem->emplaceFrame(KEY,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_4 = problem->emplaceFrame(KEY,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0)); kf_1->fix(); kf_2->fix(); diff --git a/src/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp index b27e1c0070ef55f91a4ddcb592dc30f0e6c0ac77..a1f225eddd560462b6af13e7898990cdaea6d4dc 100644 --- a/src/examples/test_sort_keyframes.cpp +++ b/src/examples/test_sort_keyframes.cpp @@ -31,22 +31,22 @@ int main() { ProblemPtr problem_ptr = Problem::create(FRM_PO_2D); - problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.1)); - FrameBasePtr frm2 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.2)); - FrameBasePtr frm3 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.3)); - problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.4)); - FrameBasePtr frm5 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.5)); - FrameBasePtr frm6 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.6)); + problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.1)); + FrameBasePtr frm2 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.2)); + FrameBasePtr frm3 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.3)); + problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.4)); + FrameBasePtr frm5 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.5)); + FrameBasePtr frm6 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.6)); printFrames(problem_ptr); std::cout << std::endl << "Frame " << frm5->id() << " set KEY" << std::endl; - frm5->setKey(); + frm5->setEstimated(); printFrames(problem_ptr); std::cout << std::endl << "Frame " << frm2->id() << " set KEY" << std::endl; - frm2->setKey(); + frm2->setEstimated(); printFrames(problem_ptr); @@ -56,21 +56,21 @@ int main() printFrames(problem_ptr); std::cout << std::endl << "Frame " << frm3->id() << " set KEY" << std::endl; - frm3->setKey(); + frm3->setEstimated(); printFrames(problem_ptr); std::cout << std::endl << "Frame " << frm6->id() << " set KEY" << std::endl; - frm6->setKey(); + frm6->setEstimated(); printFrames(problem_ptr); - FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.7)); + FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.7)); std::cout << std::endl << "created Key Frame " << frm7->id() << " TS: " << 0.7 << std::endl; printFrames(problem_ptr); - FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.35)); + FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.35)); std::cout << std::endl << "created Key Frame " << frm8->id() << " TS: " << 0.35 << std::endl; printFrames(problem_ptr); diff --git a/src/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp index fa577c15b88f05a9f9f94a7f63eb926f493cf2c7..a521cabd1b5b1276846573df70b53d2b70b6d537 100644 --- a/src/examples/test_sparsification.cpp +++ b/src/examples/test_sparsification.cpp @@ -216,7 +216,7 @@ int main(int argc, char** argv) // ------------------------ START EXPERIMENT ------------------------ // First frame FIXED - last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, Eigen::Vector3s::Zero(),TimeStamp(0)); + last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, Eigen::Vector3s::Zero(),TimeStamp(0)); last_frame_ptr->fix(); bl_problem_ptr->print(4, true, false, true); @@ -238,7 +238,7 @@ int main(int argc, char** argv) Eigen::Vector3s from_pose = frame_from_ptr->getState(); R.topLeftCorner(2,2) = Eigen::Rotation2Ds(from_pose(2)).matrix(); Eigen::Vector3s new_frame_pose = from_pose + R*meas; - last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, new_frame_pose, TimeStamp(double(edge_to))); + last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, new_frame_pose, TimeStamp(double(edge_to))); frame_to_ptr = last_frame_ptr; diff --git a/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp index 9463399ff7843eca655e702d768a7b62fe383528..b87d3c17ec366f3d868aafb4bf4397b17ba92f45 100644 --- a/src/examples/test_wolf_imported_graph.cpp +++ b/src/examples/test_wolf_imported_graph.cpp @@ -130,8 +130,8 @@ int main(int argc, char** argv) bNum.clear(); // add frame to problem - FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); - FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full); wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun); // store diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp index de917e471e6535c626561ab398e8bf00d3637725..99567b5c3fb991e7664b255fc3893df9f027e1c8 100644 --- a/src/examples/test_wolf_prunning.cpp +++ b/src/examples/test_wolf_prunning.cpp @@ -148,8 +148,8 @@ int main(int argc, char** argv) bNum.clear(); // add frame to problem - FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); - FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full); wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun); // store diff --git a/src/feature/feature_apriltag.cpp b/src/feature/feature_apriltag.cpp new file mode 100644 index 0000000000000000000000000000000000000000..18cb89cdf9813a5112b0aa3b7769834f1318fdaf --- /dev/null +++ b/src/feature/feature_apriltag.cpp @@ -0,0 +1,66 @@ + +#include "base/feature/feature_apriltag.h" + +namespace wolf { + +FeatureApriltag::FeatureApriltag(const Eigen::Vector7s & _measurement, + const Eigen::Matrix6s & _meas_uncertainty, + const int _tag_id, + const apriltag_detection_t & _det, + Scalar _rep_error1, + Scalar _rep_error2, + bool _use_rotation, + UncertaintyType _uncertainty_type) : + FeatureBase("APRILTAG", _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() +{ + // +} + +Scalar FeatureApriltag::getTagId() const +{ + return tag_id_; +} + +const apriltag_detection_t& FeatureApriltag::getDetection() const +{ + return detection_; +} + +const std::vector<cv::Point2d>& FeatureApriltag::getTagCorners() const +{ + return tag_corners_; +} + +Scalar FeatureApriltag::getRepError1() const +{ + return rep_error1_; +} + +Scalar 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 new file mode 100644 index 0000000000000000000000000000000000000000..193c979b275c3e2c4a9202b90347ef376ac1f3d2 --- /dev/null +++ b/src/landmark/landmark_apriltag.cpp @@ -0,0 +1,93 @@ + +#include "base/landmark/landmark_apriltag.h" +#include "base/common/factory.h" +#include "base/math/rotations.h" +#include "base/yaml/yaml_conversion.h" + +namespace wolf { + +LandmarkApriltag::LandmarkApriltag(Eigen::Vector7s& pose, const int& _tagid, const Scalar& _tag_width) : + LandmarkBase("APRILTAG", std::make_shared<StateBlock>(pose.head(3)), std::make_shared<StateQuaternion>(pose.tail(4))), + tag_id_(_tagid), + tag_width_(_tag_width) +{ + setDescriptor(Eigen::VectorXs::Constant(1,_tagid)); //change tagid to int ? do not use descriptor vector ? + setId(_tagid); // overwrite lmk ID to match tag_id. +} + +LandmarkApriltag::~LandmarkApriltag() +{ + // +} + + +Scalar LandmarkApriltag::getTagWidth() const +{ + return tag_width_; +} + +int LandmarkApriltag::getTagId() const +{ + return tag_id_; +} + +// LANDMARK SAVE AND LOAD FROM YAML + +// static +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>(); + unsigned int tag_id = _lmk_node["tag id"] .as<unsigned int>(); + Scalar tag_width = _lmk_node["tag width"] .as<Scalar>(); + Eigen::Vector3s pos = _lmk_node["position"] .as<Eigen::Vector3s>(); + bool pos_fixed = _lmk_node["position fixed"] .as<bool>(); + Eigen::Vector4s vquat; + if (_lmk_node["orientation"].size() == 3) + { + // we have been given 3 Euler angles in degrees + Eigen::Vector3s euler = M_TORAD * ( _lmk_node["orientation"] .as<Eigen::Vector3s>() ); + Eigen::Matrix3s R = e2R(euler); + Eigen::Quaternions quat = R2q(R); + vquat = quat.coeffs(); + } + else if (_lmk_node["orientation"].size() == 4) + { + // we have been given a quaternion + vquat = _lmk_node["orientation"] .as<Eigen::Vector4s>(); + } + bool ori_fixed = _lmk_node["orientation fixed"] .as<bool>(); + + Eigen::Vector7s pose; pose << pos, vquat; + + // Create a new landmark + LandmarkApriltagPtr lmk_ptr = std::make_shared<LandmarkApriltag>(pose, tag_id, tag_width); + lmk_ptr->setId(id); + lmk_ptr->getP()->setFixed(pos_fixed); + lmk_ptr->getO()->setFixed(ori_fixed); + + return lmk_ptr; + +} + +YAML::Node LandmarkApriltag::saveToYaml() const +{ + // First base things + YAML::Node node = LandmarkBase::saveToYaml(); + + // Then Apriltag specific things + node["tag id"] = getTagId(); + node["tag width"] = getTagWidth(); + + return node; +} + + +// Register landmark creator +namespace +{ +const bool WOLF_UNUSED registered_lmk_apriltag = LandmarkFactory::get().registerCreator("APRILTAG", LandmarkApriltag::create); +} + + +} // namespace wolf diff --git a/src/processor/ippe.cpp b/src/processor/ippe.cpp new file mode 100644 index 0000000000000000000000000000000000000000..abbd4c4f8653ffbdde69192094151ef6afb53adc --- /dev/null +++ b/src/processor/ippe.cpp @@ -0,0 +1,1050 @@ +#include "base/processor/ippe.h" +#include <opencv2/imgproc.hpp> + +#include <iostream> + +using namespace cv; + +IPPE::PoseSolver::PoseSolver() +{ + +} + +IPPE::PoseSolver::~PoseSolver() +{ + +} + +void IPPE::PoseSolver::solveGeneric(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, + cv::OutputArray _rvec1, cv::OutputArray _tvec1, float& err1, cv::OutputArray _rvec2, cv::OutputArray _tvec2, float& err2) +{ + cv::Mat normalizedImagePoints; //undistored version of imagePoints + + if (_cameraMatrix.empty()) { + //there is no camera matrix and image points are given in normalized pixel coordinates. + _imagePoints.copyTo(normalizedImagePoints); + } + else { + //undistort the image points (i.e. put them in normalized pixel coordinates): + cv::undistortPoints(_imagePoints, normalizedImagePoints, _cameraMatrix, _distCoeffs); + } + + //solve: + cv::Mat Ma, Mb; + solveGeneric(_objectPoints, normalizedImagePoints, Ma, Mb); + + //the two poses computed by IPPE (sorted): + cv::Mat M1, M2; + + //sort poses by reprojection error: + sortPosesByReprojError(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, Ma, Mb, M1, M2, err1, err2); + + //fill outputs + rot2vec(M1.colRange(0, 3).rowRange(0, 3), _rvec1); + rot2vec(M2.colRange(0, 3).rowRange(0, 3), _rvec2); + + M1.colRange(3, 4).rowRange(0, 3).copyTo(_tvec1); + M2.colRange(3, 4).rowRange(0, 3).copyTo(_tvec2); +} + +void IPPE::PoseSolver::solveGeneric(cv::InputArray _objectPoints, cv::InputArray _normalizedInputPoints, + cv::OutputArray _Ma, cv::OutputArray _Mb) +{ + + //argument checking: + size_t n = _objectPoints.rows() * _objectPoints.cols(); //number of points + int objType = _objectPoints.type(); + int type_input = _normalizedInputPoints.type(); + assert((objType == CV_32FC3) | (objType == CV_64FC3)); + assert((type_input == CV_32FC2) | (type_input == CV_64FC2)); + assert((_objectPoints.rows() == 1) | (_objectPoints.cols() == 1)); + assert((_objectPoints.rows() >= 4) | (_objectPoints.cols() >= 4)); + assert((_normalizedInputPoints.rows() == 1) | (_normalizedInputPoints.cols() == 1)); + assert(static_cast<size_t>(_objectPoints.rows() * _objectPoints.cols()) == n); + + cv::Mat normalizedInputPoints; + if (type_input == CV_32FC2) { + _normalizedInputPoints.getMat().convertTo(normalizedInputPoints, CV_64FC2); + } + else { + normalizedInputPoints = _normalizedInputPoints.getMat(); + } + + cv::Mat objectInputPoints; + if (type_input == CV_32FC3) { + _objectPoints.getMat().convertTo(objectInputPoints, CV_64FC3); + } + else { + objectInputPoints = _objectPoints.getMat(); + } + + cv::Mat canonicalObjPoints; + cv::Mat MmodelPoints2Canonical; + + //transform object points to the canonical position (zero centred and on the plane z=0): + makeCanonicalObjectPoints(objectInputPoints, canonicalObjPoints, MmodelPoints2Canonical); + + //compute the homography mapping the model's points to normalizedInputPoints + cv::Mat H; + HomographyHO::homographyHO(canonicalObjPoints, _normalizedInputPoints, H); + + //now solve + cv::Mat MaCanon, MbCanon; + solveCanonicalForm(canonicalObjPoints, normalizedInputPoints, H, MaCanon, MbCanon); + + //transform computed poses to account for canonical transform: + cv::Mat Ma = MaCanon * MmodelPoints2Canonical; + cv::Mat Mb = MbCanon * MmodelPoints2Canonical; + + //output poses: + Ma.copyTo(_Ma); + Mb.copyTo(_Mb); +} + +void IPPE::PoseSolver::solveCanonicalForm(cv::InputArray _canonicalObjPoints, cv::InputArray _normalizedInputPoints, cv::InputArray _H, + cv::OutputArray _Ma, cv::OutputArray _Mb) +{ + + _Ma.create(4, 4, CV_64FC1); + _Mb.create(4, 4, CV_64FC1); + + cv::Mat Ma = _Ma.getMat(); + cv::Mat Mb = _Mb.getMat(); + cv::Mat H = _H.getMat(); + + //initialise poses: + Ma.setTo(0); + Ma.at<double>(3, 3) = 1; + Mb.setTo(0); + Mb.at<double>(3, 3) = 1; + + //Compute the Jacobian J of the homography at (0,0): + double j00, j01, j10, j11, v0, v1; + + j00 = H.at<double>(0, 0) - H.at<double>(2, 0) * H.at<double>(0, 2); + j01 = H.at<double>(0, 1) - H.at<double>(2, 1) * H.at<double>(0, 2); + j10 = H.at<double>(1, 0) - H.at<double>(2, 0) * H.at<double>(1, 2); + j11 = H.at<double>(1, 1) - H.at<double>(2, 1) * H.at<double>(1, 2); + + //Compute the transformation of (0,0) into the image: + v0 = H.at<double>(0, 2); + v1 = H.at<double>(1, 2); + + //compute the two rotation solutions: + cv::Mat Ra = Ma.colRange(0, 3).rowRange(0, 3); + cv::Mat Rb = Mb.colRange(0, 3).rowRange(0, 3); + computeRotations(j00, j01, j10, j11, v0, v1, Ra, Rb); + + //for each rotation solution, compute the corresponding translation solution: + cv::Mat ta = Ma.colRange(3, 4).rowRange(0, 3); + cv::Mat tb = Mb.colRange(3, 4).rowRange(0, 3); + computeTranslation(_canonicalObjPoints, _normalizedInputPoints, Ra, ta); + computeTranslation(_canonicalObjPoints, _normalizedInputPoints, Rb, tb); +} + +void IPPE::PoseSolver::solveSquare(float squareLength, InputArray _imagePoints, InputArray _cameraMatrix, InputArray _distCoeffs, + OutputArray _rvec1, OutputArray _tvec1, float& err1, OutputArray _rvec2, OutputArray _tvec2, float& err2) +{ + + //allocate outputs: + _rvec1.create(3, 1, CV_64FC1); + _tvec1.create(3, 1, CV_64FC1); + _rvec2.create(3, 1, CV_64FC1); + _tvec2.create(3, 1, CV_64FC1); + + cv::Mat normalizedInputPoints; //undistored version of imagePoints + cv::Mat objectPoints2D; + + //generate the object points: + generateSquareObjectCorners2D(squareLength, objectPoints2D); + + + cv::Mat H; //homography from canonical object points to normalized pixels + + + if (_cameraMatrix.empty()) { + //this means imagePoints are defined in normalized pixel coordinates, so just copy it: + _imagePoints.copyTo(normalizedInputPoints); + } + else { + //undistort the image points (i.e. put them in normalized pixel coordinates). + cv::undistortPoints(_imagePoints, normalizedInputPoints, _cameraMatrix, _distCoeffs); + } + + //compute H + homographyFromSquarePoints(normalizedInputPoints, squareLength / 2.0f, H); + + //now solve + cv::Mat Ma, Mb; + solveCanonicalForm(objectPoints2D, normalizedInputPoints, H, Ma, Mb); + + //sort poses according to reprojection error: + cv::Mat M1, M2; + cv::Mat objectPoints3D; + generateSquareObjectCorners3D(squareLength, objectPoints3D); + sortPosesByReprojError(objectPoints3D, _imagePoints, _cameraMatrix, _distCoeffs, Ma, Mb, M1, M2, err1, err2); + + //fill outputs + rot2vec(M1.colRange(0, 3).rowRange(0, 3), _rvec1); + rot2vec(M2.colRange(0, 3).rowRange(0, 3), _rvec2); + + M1.colRange(3, 4).rowRange(0, 3).copyTo(_tvec1); + M2.colRange(3, 4).rowRange(0, 3).copyTo(_tvec2); +} + +void IPPE::PoseSolver::generateSquareObjectCorners3D(double squareLength, OutputArray _objectPoints) +{ + _objectPoints.create(1, 4, CV_64FC3); + cv::Mat objectPoints = _objectPoints.getMat(); + objectPoints.ptr<Vec3d>(0)[0] = Vec3d(-squareLength / 2.0, squareLength / 2.0, 0.0); + objectPoints.ptr<Vec3d>(0)[1] = Vec3d(squareLength / 2.0, squareLength / 2.0, 0.0); + objectPoints.ptr<Vec3d>(0)[2] = Vec3d(squareLength / 2.0, -squareLength / 2.0, 0.0); + objectPoints.ptr<Vec3d>(0)[3] = Vec3d(-squareLength / 2.0, -squareLength / 2.0, 0.0); +} + +void IPPE::PoseSolver::generateSquareObjectCorners2D(double squareLength, OutputArray _objectPoints) +{ + _objectPoints.create(1, 4, CV_64FC2); + cv::Mat objectPoints = _objectPoints.getMat(); + objectPoints.ptr<Vec2d>(0)[0] = Vec2d(-squareLength / 2.0, squareLength / 2.0); + objectPoints.ptr<Vec2d>(0)[1] = Vec2d(squareLength / 2.0, squareLength / 2.0); + objectPoints.ptr<Vec2d>(0)[2] = Vec2d(squareLength / 2.0, -squareLength / 2.0); + objectPoints.ptr<Vec2d>(0)[3] = Vec2d(-squareLength / 2.0, -squareLength / 2.0); +} + +double IPPE::PoseSolver::meanSceneDepth(InputArray _objectPoints, InputArray _rvec, InputArray _tvec) +{ + assert((_objectPoints.type() == CV_64FC3) | (_objectPoints.type() == CV_64FC3)); + + size_t n = _objectPoints.rows() * _objectPoints.cols(); + Mat R; + Mat q; + Rodrigues(_rvec, R); + double zBar = 0; + + for (size_t i = 0; i < n; i++) { + cv::Mat p(_objectPoints.getMat().at<Point3d>(i)); + q = R * p + _tvec.getMat(); + double z; + if (q.depth() == CV_64FC1) { + z = q.at<double>(2); + } + else { + z = static_cast<double>(q.at<float>(2)); + } + zBar += z; + + //if (z <= 0) { + // std::cout << "Warning: object point " << i << " projects behind the camera! This should not be allowed. " << std::endl; + //} + } + return zBar / static_cast<double>(n); +} + +void IPPE::PoseSolver::rot2vec(InputArray _R, OutputArray _r) +{ + assert(_R.type() == CV_64FC1); + assert(_R.rows() == 3); + assert(_R.cols() == 3); + + _r.create(3, 1, CV_64FC1); + + cv::Mat R = _R.getMat(); + cv::Mat rvec = _r.getMat(); + + double trace = R.at<double>(0, 0) + R.at<double>(1, 1) + R.at<double>(2, 2); + double w_norm = acos((trace - 1.0) / 2.0); + double c0, c1, c2; + double eps = std::numeric_limits<float>::epsilon(); + double d = 1 / (2 * sin(w_norm)) * w_norm; + if (w_norm < eps) //rotation is the identity + { + rvec.setTo(0); + } + else { + c0 = R.at<double>(2, 1) - R.at<double>(1, 2); + c1 = R.at<double>(0, 2) - R.at<double>(2, 0); + c2 = R.at<double>(1, 0) - R.at<double>(0, 1); + rvec.at<double>(0) = d * c0; + rvec.at<double>(1) = d * c1; + rvec.at<double>(2) = d * c2; + } +} + +void IPPE::PoseSolver::computeTranslation(InputArray _objectPoints, InputArray _normalizedImgPoints, InputArray _R, OutputArray _t) +{ + //This is solved by building the linear system At = b, where t corresponds to the (unknown) translation. + //This is then inverted with the associated normal equations to give t = inv(transpose(A)*A)*transpose(A)*b + //For efficiency we only store the coefficients of (transpose(A)*A) and (transpose(A)*b) + + assert(_objectPoints.type() == CV_64FC2); + assert(_normalizedImgPoints.type() == CV_64FC2); + assert(_R.type() == CV_64FC1); + + assert((_R.rows() == 3) & (_R.cols() == 3)); + assert((_objectPoints.rows() == 1) | (_objectPoints.cols() == 1)); + assert((_normalizedImgPoints.rows() == 1) | (_normalizedImgPoints.cols() == 1)); + size_t n = _normalizedImgPoints.rows() * _normalizedImgPoints.cols(); + assert(n == static_cast<size_t>(_objectPoints.rows() * _objectPoints.cols())); + + cv::Mat objectPoints = _objectPoints.getMat(); + cv::Mat imgPoints = _normalizedImgPoints.getMat(); + + _t.create(3, 1, CV_64FC1); + + cv::Mat R = _R.getMat(); + + //coefficients of (transpose(A)*A) + double ATA00 = n; + double ATA02 = 0; + double ATA11 = n; + double ATA12 = 0; + double ATA20 = 0; + double ATA21 = 0; + double ATA22 = 0; + + //coefficients of (transpose(A)*b) + double ATb0 = 0; + double ATb1 = 0; + double ATb2 = 0; + + //S gives inv(transpose(A)*A)/det(A)^2 + double S00, S01, S02; + double S10, S11, S12; + double S20, S21, S22; + + double rx, ry, rz; + double a2; + double b2; + double bx, by; + + //now loop through each point and increment the coefficients: + for (size_t i = 0; i < n; i++) { + rx = R.at<double>(0, 0) * objectPoints.at<Vec2d>(i)(0) + R.at<double>(0, 1) * objectPoints.at<Vec2d>(i)(1); + ry = R.at<double>(1, 0) * objectPoints.at<Vec2d>(i)(0) + R.at<double>(1, 1) * objectPoints.at<Vec2d>(i)(1); + rz = R.at<double>(2, 0) * objectPoints.at<Vec2d>(i)(0) + R.at<double>(2, 1) * objectPoints.at<Vec2d>(i)(1); + + a2 = -imgPoints.at<Vec2d>(i)(0); + b2 = -imgPoints.at<Vec2d>(i)(1); + + ATA02 = ATA02 + a2; + ATA12 = ATA12 + b2; + ATA20 = ATA20 + a2; + ATA21 = ATA21 + b2; + ATA22 = ATA22 + a2 * a2 + b2 * b2; + + bx = -a2 * rz - rx; + by = -b2 * rz - ry; + + ATb0 = ATb0 + bx; + ATb1 = ATb1 + by; + ATb2 = ATb2 + a2 * bx + b2 * by; + } + + double detAInv = 1.0 / (ATA00 * ATA11 * ATA22 - ATA00 * ATA12 * ATA21 - ATA02 * ATA11 * ATA20); + + //construct S: + S00 = ATA11 * ATA22 - ATA12 * ATA21; + S01 = ATA02 * ATA21; + S02 = -ATA02 * ATA11; + S10 = ATA12 * ATA20; + S11 = ATA00 * ATA22 - ATA02 * ATA20; + S12 = -ATA00 * ATA12; + S20 = -ATA11 * ATA20; + S21 = -ATA00 * ATA21; + S22 = ATA00 * ATA11; + + //solve t: + Mat t = _t.getMat(); + t.at<double>(0) = detAInv * (S00 * ATb0 + S01 * ATb1 + S02 * ATb2); + t.at<double>(1) = detAInv * (S10 * ATb0 + S11 * ATb1 + S12 * ATb2); + t.at<double>(2) = detAInv * (S20 * ATb0 + S21 * ATb1 + S22 * ATb2); +} + +void IPPE::PoseSolver::computeRotations(double j00, double j01, double j10, double j11, double p, double q, OutputArray _R1, OutputArray _R2) +{ + //This is fairly optimized code which makes it hard to understand. The matlab code is certainly easier to read. + _R1.create(3, 3, CV_64FC1); + _R2.create(3, 3, CV_64FC1); + + double a00, a01, a10, a11, ata00, ata01, ata11, b00, b01, b10, b11, binv00, binv01, binv10, binv11; + //double rv00, rv01, rv02, rv10, rv11, rv12, rv20, rv21, rv22; + double rtilde00, rtilde01, rtilde10, rtilde11; + double rtilde00_2, rtilde01_2, rtilde10_2, rtilde11_2; + double b0, b1, gamma, dtinv; + double sp; + + Mat Rv; + cv::Mat v(3,1,CV_64FC1); + v.at<double>(0) = p; + v.at<double>(1) = q; + v.at<double>(2) = 1; + rotateVec2ZAxis(v,Rv); + Rv = Rv.t(); + + + //setup the 2x2 SVD decomposition: + double rv00, rv01, rv02; + double rv10, rv11, rv12; + double rv20, rv21, rv22; + rv00 = Rv.at<double>(0,0); + rv01 = Rv.at<double>(0,1); + rv02 = Rv.at<double>(0,2); + + rv10 = Rv.at<double>(1,0); + rv11 = Rv.at<double>(1,1); + rv12 = Rv.at<double>(1,2); + + rv20 = Rv.at<double>(2,0); + rv21 = Rv.at<double>(2,1); + rv22 = Rv.at<double>(2,2); + + b00 = rv00 - p * rv20; + b01 = rv01 - p * rv21; + b10 = rv10 - q * rv20; + b11 = rv11 - q * rv21; + + dtinv = 1.0 / ((b00 * b11 - b01 * b10)); + + binv00 = dtinv * b11; + binv01 = -dtinv * b01; + binv10 = -dtinv * b10; + binv11 = dtinv * b00; + + a00 = binv00 * j00 + binv01 * j10; + a01 = binv00 * j01 + binv01 * j11; + a10 = binv10 * j00 + binv11 * j10; + a11 = binv10 * j01 + binv11 * j11; + + //compute the largest singular value of A: + ata00 = a00 * a00 + a01 * a01; + ata01 = a00 * a10 + a01 * a11; + ata11 = a10 * a10 + a11 * a11; + + gamma = sqrt(0.5 * (ata00 + ata11 + sqrt((ata00 - ata11) * (ata00 - ata11) + 4.0 * ata01 * ata01))); + + //reconstruct the full rotation matrices: + rtilde00 = a00 / gamma; + rtilde01 = a01 / gamma; + rtilde10 = a10 / gamma; + rtilde11 = a11 / gamma; + + rtilde00_2 = rtilde00 * rtilde00; + rtilde01_2 = rtilde01 * rtilde01; + rtilde10_2 = rtilde10 * rtilde10; + rtilde11_2 = rtilde11 * rtilde11; + + b0 = sqrt(-rtilde00_2 - rtilde10_2 + 1); + b1 = sqrt(-rtilde01_2 - rtilde11_2 + 1); + sp = (-rtilde00 * rtilde01 - rtilde10 * rtilde11); + + if (sp < 0) { + b1 = -b1; + } + + //store results: + Mat R1 = _R1.getMat(); + Mat R2 = _R2.getMat(); + + R1.at<double>(0, 0) = (rtilde00)*rv00 + (rtilde10)*rv01 + (b0)*rv02; + R1.at<double>(0, 1) = (rtilde01)*rv00 + (rtilde11)*rv01 + (b1)*rv02; + R1.at<double>(0, 2) = (b1 * rtilde10 - b0 * rtilde11) * rv00 + (b0 * rtilde01 - b1 * rtilde00) * rv01 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv02; + R1.at<double>(1, 0) = (rtilde00)*rv10 + (rtilde10)*rv11 + (b0)*rv12; + R1.at<double>(1, 1) = (rtilde01)*rv10 + (rtilde11)*rv11 + (b1)*rv12; + R1.at<double>(1, 2) = (b1 * rtilde10 - b0 * rtilde11) * rv10 + (b0 * rtilde01 - b1 * rtilde00) * rv11 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv12; + R1.at<double>(2, 0) = (rtilde00)*rv20 + (rtilde10)*rv21 + (b0)*rv22; + R1.at<double>(2, 1) = (rtilde01)*rv20 + (rtilde11)*rv21 + (b1)*rv22; + R1.at<double>(2, 2) = (b1 * rtilde10 - b0 * rtilde11) * rv20 + (b0 * rtilde01 - b1 * rtilde00) * rv21 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv22; + + R2.at<double>(0, 0) = (rtilde00)*rv00 + (rtilde10)*rv01 + (-b0) * rv02; + R2.at<double>(0, 1) = (rtilde01)*rv00 + (rtilde11)*rv01 + (-b1) * rv02; + R2.at<double>(0, 2) = (b0 * rtilde11 - b1 * rtilde10) * rv00 + (b1 * rtilde00 - b0 * rtilde01) * rv01 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv02; + R2.at<double>(1, 0) = (rtilde00)*rv10 + (rtilde10)*rv11 + (-b0) * rv12; + R2.at<double>(1, 1) = (rtilde01)*rv10 + (rtilde11)*rv11 + (-b1) * rv12; + R2.at<double>(1, 2) = (b0 * rtilde11 - b1 * rtilde10) * rv10 + (b1 * rtilde00 - b0 * rtilde01) * rv11 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv12; + R2.at<double>(2, 0) = (rtilde00)*rv20 + (rtilde10)*rv21 + (-b0) * rv22; + R2.at<double>(2, 1) = (rtilde01)*rv20 + (rtilde11)*rv21 + (-b1) * rv22; + R2.at<double>(2, 2) = (b0 * rtilde11 - b1 * rtilde10) * rv20 + (b1 * rtilde00 - b0 * rtilde01) * rv21 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv22; +} + + +void IPPE::PoseSolver::homographyFromSquarePoints(InputArray _targetPoints, double halfLength, OutputArray H_) +{ + + assert((_targetPoints.type() == CV_32FC2) | (_targetPoints.type() == CV_64FC2)); + + cv::Mat pts = _targetPoints.getMat(); + H_.create(3, 3, CV_64FC1); + Mat H = H_.getMat(); + + double p1x, p1y; + double p2x, p2y; + double p3x, p3y; + double p4x, p4y; + + if (_targetPoints.type() == CV_32FC2) { + p1x = -pts.at<Vec2f>(0)(0); + p1y = -pts.at<Vec2f>(0)(1); + + p2x = -pts.at<Vec2f>(1)(0); + p2y = -pts.at<Vec2f>(1)(1); + + p3x = -pts.at<Vec2f>(2)(0); + p3y = -pts.at<Vec2f>(2)(1); + + p4x = -pts.at<Vec2f>(3)(0); + p4y = -pts.at<Vec2f>(3)(1); + } + else { + p1x = -pts.at<Vec2d>(0)(0); + p1y = -pts.at<Vec2d>(0)(1); + + p2x = -pts.at<Vec2d>(1)(0); + p2y = -pts.at<Vec2d>(1)(1); + + p3x = -pts.at<Vec2d>(2)(0); + p3y = -pts.at<Vec2d>(2)(1); + + p4x = -pts.at<Vec2d>(3)(0); + p4y = -pts.at<Vec2d>(3)(1); + } + + //analytic solution: + double detsInv = -1 / (halfLength * (p1x * p2y - p2x * p1y - p1x * p4y + p2x * p3y - p3x * p2y + p4x * p1y + p3x * p4y - p4x * p3y)); + + H.at<double>(0, 0) = detsInv * (p1x * p3x * p2y - p2x * p3x * p1y - p1x * p4x * p2y + p2x * p4x * p1y - p1x * p3x * p4y + p1x * p4x * p3y + p2x * p3x * p4y - p2x * p4x * p3y); + H.at<double>(0, 1) = detsInv * (p1x * p2x * p3y - p1x * p3x * p2y - p1x * p2x * p4y + p2x * p4x * p1y + p1x * p3x * p4y - p3x * p4x * p1y - p2x * p4x * p3y + p3x * p4x * p2y); + H.at<double>(0, 2) = detsInv * halfLength * (p1x * p2x * p3y - p2x * p3x * p1y - p1x * p2x * p4y + p1x * p4x * p2y - p1x * p4x * p3y + p3x * p4x * p1y + p2x * p3x * p4y - p3x * p4x * p2y); + H.at<double>(1, 0) = detsInv * (p1x * p2y * p3y - p2x * p1y * p3y - p1x * p2y * p4y + p2x * p1y * p4y - p3x * p1y * p4y + p4x * p1y * p3y + p3x * p2y * p4y - p4x * p2y * p3y); + H.at<double>(1, 1) = detsInv * (p2x * p1y * p3y - p3x * p1y * p2y - p1x * p2y * p4y + p4x * p1y * p2y + p1x * p3y * p4y - p4x * p1y * p3y - p2x * p3y * p4y + p3x * p2y * p4y); + H.at<double>(1, 2) = detsInv * halfLength * (p1x * p2y * p3y - p3x * p1y * p2y - p2x * p1y * p4y + p4x * p1y * p2y - p1x * p3y * p4y + p3x * p1y * p4y + p2x * p3y * p4y - p4x * p2y * p3y); + H.at<double>(2, 0) = -detsInv * (p1x * p3y - p3x * p1y - p1x * p4y - p2x * p3y + p3x * p2y + p4x * p1y + p2x * p4y - p4x * p2y); + H.at<double>(2, 1) = detsInv * (p1x * p2y - p2x * p1y - p1x * p3y + p3x * p1y + p2x * p4y - p4x * p2y - p3x * p4y + p4x * p3y); + H.at<double>(2, 2) = 1.0; +} + +void IPPE::PoseSolver::makeCanonicalObjectPoints(InputArray _objectPoints, OutputArray _canonicalObjPoints, OutputArray _MmodelPoints2Canonical) +{ + + int objType = _objectPoints.type(); + assert((objType == CV_64FC3) | (objType == CV_32FC3)); + + size_t n = _objectPoints.rows() * _objectPoints.cols(); + + _canonicalObjPoints.create(1, n, CV_64FC2); + _MmodelPoints2Canonical.create(4, 4, CV_64FC1); + + cv::Mat objectPoints = _objectPoints.getMat(); + cv::Mat canonicalObjPoints = _canonicalObjPoints.getMat(); + + cv::Mat UZero(3, n, CV_64FC1); + + double xBar = 0; + double yBar = 0; + double zBar = 0; + bool isOnZPlane = true; + for (size_t i = 0; i < n; i++) { + double x, y, z; + if (objType == CV_32FC3) { + x = static_cast<double>(objectPoints.at<Vec3f>(i)[0]); + y = static_cast<double>(objectPoints.at<Vec3f>(i)[1]); + z = static_cast<double>(objectPoints.at<Vec3f>(i)[2]); + } + else { + x = objectPoints.at<Vec3d>(i)[0]; + y = objectPoints.at<Vec3d>(i)[1]; + z = objectPoints.at<Vec3d>(i)[2]; + + if (abs(z) > IPPE_SMALL) { + isOnZPlane = false; + } + } + xBar += x; + yBar += y; + zBar += z; + + UZero.at<double>(0, i) = x; + UZero.at<double>(1, i) = y; + UZero.at<double>(2, i) = z; + } + xBar = xBar / (double)n; + yBar = yBar / (double)n; + zBar = zBar / (double)n; + + for (size_t i = 0; i < n; i++) { + UZero.at<double>(0, i) -= xBar; + UZero.at<double>(1, i) -= yBar; + UZero.at<double>(2, i) -= zBar; + } + + cv::Mat MCenter(4, 4, CV_64FC1); + MCenter.setTo(0); + MCenter.at<double>(0, 0) = 1; + MCenter.at<double>(1, 1) = 1; + MCenter.at<double>(2, 2) = 1; + MCenter.at<double>(3, 3) = 1; + + MCenter.at<double>(0, 3) = -xBar; + MCenter.at<double>(1, 3) = -yBar; + MCenter.at<double>(2, 3) = -zBar; + + if (isOnZPlane) { + //MmodelPoints2Canonical is given by MCenter + MCenter.copyTo(_MmodelPoints2Canonical); + for (size_t i = 0; i < n; i++) { + canonicalObjPoints.at<Vec2d>(i)[0] = UZero.at<double>(0, i); + canonicalObjPoints.at<Vec2d>(i)[1] = UZero.at<double>(1, i); + } + } + else { + cv::Mat UZeroAligned(3, n, CV_64FC1); + cv::Mat R; //rotation that rotates objectPoints to the plane z=0 + + if (!computeObjextSpaceR3Pts(objectPoints,R)) + { + //we could not compute R, problably because there is a duplicate point in {objectPoints(0),objectPoints(1),objectPoints(2)}. So we compute it with the SVD (which is slower): + computeObjextSpaceRSvD(UZero,R); + } + + UZeroAligned = R * UZero; + + for (size_t i = 0; i < n; i++) { + canonicalObjPoints.at<Vec2d>(i)[0] = UZeroAligned.at<double>(0, i); + canonicalObjPoints.at<Vec2d>(i)[1] = UZeroAligned.at<double>(1, i); + assert(abs(UZeroAligned.at<double>(2, i))<=IPPE_SMALL); + } + + cv::Mat MRot(4, 4, CV_64FC1); + MRot.setTo(0); + MRot.at<double>(3, 3) = 1; + + R.copyTo(MRot.colRange(0, 3).rowRange(0, 3)); + cv::Mat Mb = MRot * MCenter; + Mb.copyTo(_MmodelPoints2Canonical); + } +} + +void IPPE::PoseSolver::evalReprojError(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, cv::InputArray _M, float& err) +{ + cv::Mat projectedPoints; + cv::Mat imagePoints = _imagePoints.getMat(); + cv::Mat r; + rot2vec(_M.getMat().colRange(0, 3).rowRange(0, 3), r); + + if (_cameraMatrix.empty()) { + //there is no camera matrix and image points are in normalized pixel coordinates + cv::Mat K(3, 3, CV_64FC1); + K.setTo(0); + K.at<double>(0, 0) = 1; + K.at<double>(1, 1) = 1; + K.at<double>(2, 2) = 1; + cv::Mat kc; + cv::projectPoints(_objectPoints, r, _M.getMat().colRange(3, 4).rowRange(0, 3), K, kc, projectedPoints); + } + else { + cv::projectPoints(_objectPoints, r, _M.getMat().colRange(3, 4).rowRange(0, 3), _cameraMatrix, _distCoeffs, projectedPoints); + } + + err = 0; + size_t n = _objectPoints.rows() * _objectPoints.cols(); + + float dx, dy; + for (size_t i = 0; i < n; i++) { + if (projectedPoints.depth() == CV_32FC1) { + dx = projectedPoints.at<Vec2f>(i)[0] - imagePoints.at<Vec2f>(i)[0]; +// dy = projectedPoints.at<Vec2f>(i)[0] - imagePoints.at<Vec2f>(i)[0]; + dy = projectedPoints.at<Vec2f>(i)[1] - imagePoints.at<Vec2f>(i)[1]; + } + else { + dx = projectedPoints.at<Vec2d>(i)[0] - imagePoints.at<Vec2d>(i)[0]; +// dy = projectedPoints.at<Vec2d>(i)[0] - imagePoints.at<Vec2d>(i)[0]; + dy = projectedPoints.at<Vec2d>(i)[1] - imagePoints.at<Vec2d>(i)[1]; + } + + err += dx * dx + dy * dy; + } + err = sqrt(err / (2.0f * n)); +} + +void IPPE::PoseSolver::sortPosesByReprojError(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, cv::InputArray _Ma, cv::InputArray _Mb, cv::OutputArray _M1, cv::OutputArray _M2, float& err1, float& err2) +{ + float erra, errb; + evalReprojError(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _Ma, erra); + evalReprojError(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _Mb, errb); + if (erra < errb) { + err1 = erra; + _Ma.copyTo(_M1); + + err2 = errb; + _Mb.copyTo(_M2); + } + else { + err1 = errb; + _Mb.copyTo(_M1); + + err2 = erra; + _Ma.copyTo(_M2); + } +} + +void HomographyHO::normalizeDataIsotropic(cv::InputArray _Data, cv::OutputArray _DataN, cv::OutputArray _T, cv::OutputArray _Ti) +{ + cv::Mat Data = _Data.getMat(); + int numPoints = Data.rows * Data.cols; + assert((Data.rows == 1) | (Data.cols == 1)); + assert((Data.channels() == 2) | (Data.channels() == 3)); + assert(numPoints >= 4); + + int dataType = _Data.type(); + assert((dataType == CV_64FC2) | (dataType == CV_64FC3) | (dataType == CV_32FC2) | (dataType == CV_32FC3)); + + _DataN.create(2, numPoints, CV_64FC1); + + _T.create(3, 3, CV_64FC1); + _Ti.create(3, 3, CV_64FC1); + + cv::Mat DataN = _DataN.getMat(); + cv::Mat T = _T.getMat(); + cv::Mat Ti = _Ti.getMat(); + + _T.setTo(0); + _Ti.setTo(0); + + double xm, ym; + int numChannels = Data.channels(); + + xm = 0; + ym = 0; + for (int i = 0; i < numPoints; i++) { + if (numChannels == 2) { + if (dataType == CV_32FC2) { + xm = xm + Data.at<Vec2f>(i)[0]; + ym = ym + Data.at<Vec2f>(i)[1]; + } + else { + xm = xm + Data.at<Vec2d>(i)[0]; + ym = ym + Data.at<Vec2d>(i)[1]; + } + } + else { + if (dataType == CV_32FC3) { + xm = xm + Data.at<Vec3f>(i)[0]; + ym = ym + Data.at<Vec3f>(i)[1]; + } + else { + xm = xm + Data.at<Vec3d>(i)[0]; + ym = ym + Data.at<Vec3d>(i)[1]; + } + } + } + xm = xm / (double)numPoints; + ym = ym / (double)numPoints; + + double kappa = 0; + double xh, yh; + + for (int i = 0; i < numPoints; i++) { + + if (numChannels == 2) { + if (dataType == CV_32FC2) { + xh = Data.at<Vec2f>(i)[0] - xm; + yh = Data.at<Vec2f>(i)[1] - ym; + } + else { + xh = Data.at<Vec2d>(i)[0] - xm; + yh = Data.at<Vec2d>(i)[1] - ym; + } + } + else { + if (dataType == CV_32FC3) { + xh = Data.at<Vec3f>(i)[0] - xm; + yh = Data.at<Vec3f>(i)[1] - ym; + } + else { + xh = Data.at<Vec3d>(i)[0] - xm; + yh = Data.at<Vec3d>(i)[1] - ym; + } + } + + DataN.at<double>(0, i) = xh; + DataN.at<double>(1, i) = yh; + kappa = kappa + xh * xh + yh * yh; + } + double beta = sqrt(2 * numPoints / kappa); + DataN = DataN * beta; + + T.at<double>(0, 0) = 1.0 / beta; + T.at<double>(1, 1) = 1.0 / beta; + + T.at<double>(0, 2) = xm; + T.at<double>(1, 2) = ym; + + T.at<double>(2, 2) = 1; + + Ti.at<double>(0, 0) = beta; + Ti.at<double>(1, 1) = beta; + + Ti.at<double>(0, 2) = -beta * xm; + Ti.at<double>(1, 2) = -beta * ym; + + Ti.at<double>(2, 2) = 1; +} + +void HomographyHO::homographyHO(cv::InputArray _srcPoints, cv::InputArray _targPoints, cv::OutputArray _H) +{ + + _H.create(3, 3, CV_64FC1); + cv::Mat H = _H.getMat(); + + cv::Mat DataA, DataB, TA, TAi, TB, TBi; + + HomographyHO::normalizeDataIsotropic(_srcPoints, DataA, TA, TAi); + HomographyHO::normalizeDataIsotropic(_targPoints, DataB, TB, TBi); + + int n = DataA.cols; + assert(n == DataB.cols); + + cv::Mat C1(1, n, CV_64FC1); + cv::Mat C2(1, n, CV_64FC1); + cv::Mat C3(1, n, CV_64FC1); + cv::Mat C4(1, n, CV_64FC1); + + cv::Mat Mx(n, 3, CV_64FC1); + cv::Mat My(n, 3, CV_64FC1); + + double mC1, mC2, mC3, mC4; + mC1 = 0; + mC2 = 0; + mC3 = 0; + mC4 = 0; + + for (int i = 0; i < n; i++) { + C1.at<double>(0, i) = -DataB.at<double>(0, i) * DataA.at<double>(0, i); + C2.at<double>(0, i) = -DataB.at<double>(0, i) * DataA.at<double>(1, i); + C3.at<double>(0, i) = -DataB.at<double>(1, i) * DataA.at<double>(0, i); + C4.at<double>(0, i) = -DataB.at<double>(1, i) * DataA.at<double>(1, i); + + mC1 = mC1 + C1.at<double>(0, i); + mC2 = mC2 + C2.at<double>(0, i); + mC3 = mC3 + C3.at<double>(0, i); + mC4 = mC4 + C4.at<double>(0, i); + } + + mC1 = mC1 / n; + mC2 = mC2 / n; + mC3 = mC3 / n; + mC4 = mC4 / n; + + for (int i = 0; i < n; i++) { + Mx.at<double>(i, 0) = C1.at<double>(0, i) - mC1; + Mx.at<double>(i, 1) = C2.at<double>(0, i) - mC2; + Mx.at<double>(i, 2) = -DataB.at<double>(0, i); + + My.at<double>(i, 0) = C3.at<double>(0, i) - mC3; + My.at<double>(i, 1) = C4.at<double>(0, i) - mC4; + My.at<double>(i, 2) = -DataB.at<double>(1, i); + } + + cv::Mat DataAT, DataADataAT, DataADataATi, Pp, Bx, By, Ex, Ey, D; + + cv::transpose(DataA, DataAT); + DataADataAT = DataA * DataAT; + double dt = DataADataAT.at<double>(0, 0) * DataADataAT.at<double>(1, 1) - DataADataAT.at<double>(0, 1) * DataADataAT.at<double>(1, 0); + + DataADataATi = cv::Mat(2, 2, CV_64FC1); + DataADataATi.at<double>(0, 0) = DataADataAT.at<double>(1, 1) / dt; + DataADataATi.at<double>(0, 1) = -DataADataAT.at<double>(0, 1) / dt; + DataADataATi.at<double>(1, 0) = -DataADataAT.at<double>(1, 0) / dt; + DataADataATi.at<double>(1, 1) = DataADataAT.at<double>(0, 0) / dt; + + Pp = DataADataATi * DataA; + + Bx = Pp * Mx; + By = Pp * My; + + Ex = DataAT * Bx; + Ey = DataAT * By; + + D = cv::Mat(2 * n, 3, CV_64FC1); + cv::Mat DT, DDT; + + for (int i = 0; i < n; i++) { + D.at<double>(i, 0) = Mx.at<double>(i, 0) - Ex.at<double>(i, 0); + D.at<double>(i, 1) = Mx.at<double>(i, 1) - Ex.at<double>(i, 1); + D.at<double>(i, 2) = Mx.at<double>(i, 2) - Ex.at<double>(i, 2); + + D.at<double>(i + n, 0) = My.at<double>(i, 0) - Ey.at<double>(i, 0); + D.at<double>(i + n, 1) = My.at<double>(i, 1) - Ey.at<double>(i, 1); + D.at<double>(i + n, 2) = My.at<double>(i, 2) - Ey.at<double>(i, 2); + } + + cv::transpose(D, DT); + DDT = DT * D; + + cv::Mat S, U, V, h12, h45; + double h3, h6; + + cv::eigen(DDT, S, U); + + cv::Mat h789(3, 1, CV_64FC1); + h789.at<double>(0, 0) = U.at<double>(2, 0); + h789.at<double>(1, 0) = U.at<double>(2, 1); + h789.at<double>(2, 0) = U.at<double>(2, 2); + + h12 = -Bx * h789; + h45 = -By * h789; + + h3 = -(mC1 * h789.at<double>(0, 0) + mC2 * h789.at<double>(1, 0)); + h6 = -(mC3 * h789.at<double>(0, 0) + mC4 * h789.at<double>(1, 0)); + + H.at<double>(0, 0) = h12.at<double>(0, 0); + H.at<double>(0, 1) = h12.at<double>(1, 0); + H.at<double>(0, 2) = h3; + + H.at<double>(1, 0) = h45.at<double>(0, 0); + H.at<double>(1, 1) = h45.at<double>(1, 0); + H.at<double>(1, 2) = h6; + + H.at<double>(2, 0) = h789.at<double>(0, 0); + H.at<double>(2, 1) = h789.at<double>(1, 0); + H.at<double>(2, 2) = h789.at<double>(2, 0); + + H = TB * H * TAi; + H = H / H.at<double>(2, 2); +} + + +void IPPE::PoseSolver::rotateVec2ZAxis(InputArray _a, OutputArray _Ra) +{ + _Ra.create(3,3,CV_64FC1); + Mat Ra = _Ra.getMat(); + + double ax = _a.getMat().at<double>(0); + double ay = _a.getMat().at<double>(1); + double az = _a.getMat().at<double>(2); + + double nrm = sqrt(ax*ax + ay*ay + az*az); + ax = ax/nrm; + ay = ay/nrm; + az = az/nrm; + + double c = az; + + if (abs(1.0+c)< std::numeric_limits<float>::epsilon()) + { + Ra.setTo(0.0); + Ra.at<double>(0,0) = 1.0; + Ra.at<double>(1,1) = 1.0; + Ra.at<double>(2,2) = -1.0; + } + else + { + double d = 1.0/(1.0+c); + double ax2 = ax*ax; + double ay2 = ay*ay; + double axay = ax*ay; + + Ra.at<double>(0,0) = - ax2*d + 1.0; + Ra.at<double>(0,1) = -axay*d; + Ra.at<double>(0,2) = -ax; + + Ra.at<double>(1,0) = -axay*d; + Ra.at<double>(1,1) = - ay2*d + 1.0; + Ra.at<double>(1,2) = -ay; + + Ra.at<double>(2,0) = ax; + Ra.at<double>(2,1) = ay; + Ra.at<double>(2,2) = 1.0 - (ax2 + ay2)*d; + } + + +} + +bool IPPE::PoseSolver::computeObjextSpaceR3Pts(InputArray _objectPoints, OutputArray R) +{ + bool ret; //return argument + double p1x,p1y,p1z; + double p2x,p2y,p2z; + double p3x,p3y,p3z; + + cv::Mat objectPoints = _objectPoints.getMat(); +// size_t n = objectPoints.rows*objectPoints.cols; + if (objectPoints.type() == CV_32FC3) + { + p1x = objectPoints.at<Vec3f>(0)[0]; + p1y = objectPoints.at<Vec3f>(0)[1]; + p1z = objectPoints.at<Vec3f>(0)[2]; + + p2x = objectPoints.at<Vec3f>(1)[0]; + p2y = objectPoints.at<Vec3f>(1)[1]; + p2z = objectPoints.at<Vec3f>(1)[2]; + + p3x = objectPoints.at<Vec3f>(2)[0]; + p3y = objectPoints.at<Vec3f>(2)[1]; + p3z = objectPoints.at<Vec3f>(2)[2]; + } + else + { + p1x = objectPoints.at<Vec3d>(0)[0]; + p1y = objectPoints.at<Vec3d>(0)[1]; + p1z = objectPoints.at<Vec3d>(0)[2]; + + p2x = objectPoints.at<Vec3d>(1)[0]; + p2y = objectPoints.at<Vec3d>(1)[1]; + p2z = objectPoints.at<Vec3d>(1)[2]; + + p3x = objectPoints.at<Vec3d>(2)[0]; + p3y = objectPoints.at<Vec3d>(2)[1]; + p3z = objectPoints.at<Vec3d>(2)[2]; + } + + double nx = (p1y - p2y)*(p1z - p3z) - (p1y - p3y)*(p1z - p2z); + double ny = (p1x - p3x)*(p1z - p2z) - (p1x - p2x)*(p1z - p3z); + double nz = (p1x - p2x)*(p1y - p3y) - (p1x - p3x)*(p1y - p2y); + + double nrm = sqrt(nx*nx+ ny*ny + nz*nz); + if (nrm>IPPE_SMALL) + { + nx = nx/nrm; + ny = ny/nrm; + nz = nz/nrm; + cv::Mat v(3,1,CV_64FC1); + v.at<double>(0) = nx; + v.at<double>(1) = ny; + v.at<double>(2) = nz; + rotateVec2ZAxis(v,R); + ret = true; + } + else + { + ret = false; + } + return ret; +} + +bool IPPE::PoseSolver::computeObjextSpaceRSvD(InputArray _objectPointsZeroMean, OutputArray _R) +{ + bool ret; //return argument + _R.create(3,3,CV_64FC1); + cv::Mat R = _R.getMat(); + + //we could not compute R with the first three points, so lets use the SVD + cv::SVD s; + cv::Mat W, U, VT; + s.compute(_objectPointsZeroMean.getMat() * _objectPointsZeroMean.getMat().t(), W, U, VT); + double s3 = W.at<double>(2); + double s2 = W.at<double>(1); + + //check if points are coplanar: + assert(s3 / s2 < IPPE_SMALL); + + R = U.t(); + if (cv::determinant(R) < 0) { //this ensures R is a rotation matrix and not a general unitary matrix: + R.at<double>(2, 0) = -R.at<double>(2, 0); + R.at<double>(2, 1) = -R.at<double>(2, 1); + R.at<double>(2, 2) = -R.at<double>(2, 2); + } + ret = true; + return ret; +} + diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp index 252f5fc777d39b83b5130c430372af89cc39e539..ac38037cd0750ec93b58821017ee579d34b559ac 100644 --- a/src/processor/processor_IMU.cpp +++ b/src/processor/processor_IMU.cpp @@ -13,6 +13,7 @@ ProcessorIMU::ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU) : jacobian_delta_preint_.setIdentity(9,9); // dDp'/dDp, dDv'/dDv, all zeros jacobian_delta_.setIdentity(9,9); // jacobian_calib_.setZero(9,6); + unmeasured_perturbation_cov_ = pow(params_motion_IMU_->unmeasured_perturbation_std_, 2.0) * Eigen::Matrix<Scalar, 9, 9>::Identity(); } ProcessorIMU::~ProcessorIMU() @@ -35,8 +36,6 @@ ProcessorBasePtr ProcessorIMU::create(const std::string& _unique_name, const Pro bool ProcessorIMU::voteForKeyFrame() { - if(!isVotingActive()) - return false; // time span if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_motion_IMU_->max_time_span) { @@ -202,24 +201,25 @@ CaptureMotionPtr ProcessorIMU::createCapture(const TimeStamp& _ts, FeatureBasePtr ProcessorIMU::createFeature(CaptureMotionPtr _capture_motion) { - FeatureIMUPtr key_feature_ptr = std::make_shared<FeatureIMU>( + FeatureIMUPtr feature = std::make_shared<FeatureIMU>( _capture_motion->getBuffer().get().back().delta_integr_, - _capture_motion->getBuffer().get().back().delta_integr_cov_, + _capture_motion->getBuffer().get().back().delta_integr_cov_ + unmeasured_perturbation_cov_, _capture_motion->getBuffer().getCalibrationPreint(), _capture_motion->getBuffer().get().back().jacobian_calib_); - return key_feature_ptr; + return feature; } FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) { CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin); FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion); - FactorIMUPtr fac_imu = std::make_shared<FactorIMU>(ftr_imu, cap_imu, shared_from_this()); + // FactorIMUPtr fac_imu = std::make_shared<FactorIMU>(ftr_imu, cap_imu, shared_from_this()); + auto fac_imu = FactorBase::emplace<FactorIMU>(_feature_motion, ftr_imu, cap_imu, shared_from_this()); // link ot wolf tree - _feature_motion->addFactor(fac_imu); - _capture_origin->addConstrainedBy(fac_imu); - _capture_origin->getFrame()->addConstrainedBy(fac_imu); + // _feature_motion->addFactor(fac_imu); + // _capture_origin->addConstrainedBy(fac_imu); + // _capture_origin->getFrame()->addConstrainedBy(fac_imu); return fac_imu; } diff --git a/src/processor/processor_tracker_landmark_apriltag.cpp b/src/processor/processor_tracker_landmark_apriltag.cpp new file mode 100644 index 0000000000000000000000000000000000000000..61d2ded5bc0c23f1d6e6a2e5feda948f88975fd3 --- /dev/null +++ b/src/processor/processor_tracker_landmark_apriltag.cpp @@ -0,0 +1,669 @@ +#include "base/processor/processor_tracker_landmark_apriltag.h" + +#include "base/capture/capture_image.h" +#include "base/sensor/sensor_camera.h" +#include "base/math/rotations.h" +#include "base/feature/feature_apriltag.h" +#include "base/factor/factor_autodiff_apriltag.h" +#include "base/landmark/landmark_apriltag.h" +#include "base/state_block/state_quaternion.h" +#include "base/math/pinhole_tools.h" + +// April tags +#include "common/homography.h" +#include "common/zarray.h" + +#include <tag16h5.h> +#include <tag25h9.h> +#include <tag36h11.h> +#include <tagCircle21h7.h> +#include <tagCircle49h12.h> +#include <tagCustom48h12.h> +#include <tagStandard41h12.h> +#include <tagStandard52h13.h> + +#include "base/processor/ippe.h" + +// #include "opencv2/opencv.hpp" +#include <opencv2/imgproc/imgproc.hpp> +#include <opencv2/core/eigen.hpp> + +namespace wolf { + + +// Constructor +ProcessorTrackerLandmarkApriltag::ProcessorTrackerLandmarkApriltag( ProcessorParamsTrackerLandmarkApriltagPtr _params_tracker_landmark_apriltag) : + ProcessorTrackerLandmark("TRACKER LANDMARK APRILTAG", _params_tracker_landmark_apriltag ), + tag_widths_(_params_tracker_landmark_apriltag->tag_widths_), + tag_width_default_(_params_tracker_landmark_apriltag->tag_width_default_), + std_xy_ (_params_tracker_landmark_apriltag->std_xy_ ), + std_z_ (_params_tracker_landmark_apriltag->std_z_ ), + std_rpy_(_params_tracker_landmark_apriltag->std_rpy_), + std_pix_(_params_tracker_landmark_apriltag->std_pix_), + 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_vote_(_params_tracker_landmark_apriltag->min_time_vote_), + max_time_vote_(_params_tracker_landmark_apriltag->max_time_vote_), + min_features_for_keyframe_(_params_tracker_landmark_apriltag->min_features_for_keyframe), + max_features_diff_(_params_tracker_landmark_apriltag->max_features_diff_), + nb_vote_for_every_first_(_params_tracker_landmark_apriltag->nb_vote_for_every_first_), + enough_info_necessary_(_params_tracker_landmark_apriltag->enough_info_necessary_), + 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_); + if (famname == "tag16h5") + tag_family_ = *tag16h5_create(); + else if (famname == "tag25h9") + tag_family_ = *tag25h9_create(); + else if (famname == "tag36h11") + tag_family_ = *tag36h11_create(); + else if (famname == "tagCircle21h7") + tag_family_ = *tagCircle21h7_create(); + else if (famname == "tagCircle49h12") + tag_family_ = *tagCircle49h12_create(); + else if (famname == "tagCustom48h12") + tag_family_ = *tagCustom48h12_create(); + else if (famname == "tagStandard41h12") + tag_family_ = *tagStandard41h12_create(); + else if (famname == "tagStandard52h13") + tag_family_ = *tagStandard52h13_create(); + else { + WOLF_ERROR(famname, ": Unrecognized tag family name. Use e.g. \"tag36h11\"."); + exit(-1); + } + + // tag_family_.black_border = _params_tracker_landmark_apriltag->tag_black_border_; // not anymore in apriltag 3 + + detector_ = *apriltag_detector_create(); + apriltag_detector_add_family(&detector_, &tag_family_); + + detector_.quad_decimate = _params_tracker_landmark_apriltag->quad_decimate_; + detector_.quad_sigma = _params_tracker_landmark_apriltag->quad_sigma_; + detector_.nthreads = _params_tracker_landmark_apriltag->nthreads_; + detector_.debug = _params_tracker_landmark_apriltag->debug_; + detector_.refine_edges = _params_tracker_landmark_apriltag->refine_edges_; +} + +// Destructor +ProcessorTrackerLandmarkApriltag::~ProcessorTrackerLandmarkApriltag() +{ + // destroy raw pointers in detector_ + //apriltag_detector_destroy(&detector_); cannot be used because it is trying to free() the detector_ itself that is not implemented as a raw pointer in our case + timeprofile_destroy(detector_.tp); + apriltag_detector_clear_families(&detector_); + zarray_destroy(detector_.tag_families); + workerpool_destroy(detector_.wp); + + //free raw pointers in tag_family_ + free(tag_family_.name); + free(tag_family_.codes); +} + + +ProcessorBasePtr ProcessorTrackerLandmarkApriltag::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr) +{ + std::shared_ptr<ProcessorParamsTrackerLandmarkApriltag> prc_apriltag_params; + if (_params) + prc_apriltag_params = std::static_pointer_cast<ProcessorParamsTrackerLandmarkApriltag>(_params); + else + prc_apriltag_params = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>(); + + ProcessorTrackerLandmarkApriltagPtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkApriltag>(prc_apriltag_params); + prc_ptr->setName(_unique_name); + return prc_ptr; +} + +void ProcessorTrackerLandmarkApriltag::preProcess() +{ + //clear wolf detections so that new ones will be stored inside + detections_incoming_.clear(); + + // The image is assumed to be of color BGR2 type + cv::cvtColor(std::static_pointer_cast<CaptureImage>(incoming_ptr_)->getImage(), grayscale_image_, cv::COLOR_BGR2GRAY); + + //detect tags in incoming image + // Make an image_u8_t header for the Mat data + image_u8_t im = { .width = grayscale_image_.cols, + .height = grayscale_image_.rows, + .stride = grayscale_image_.cols, + .buf = grayscale_image_.data + }; + + // run Apriltag detector + zarray_t *detections = apriltag_detector_detect(&detector_, &im); + // loop all detections + for (int i = 0; i < zarray_size(detections); i++) { + apriltag_detection_t *det; + zarray_get(detections, i, &det); + + int tag_id = det->id; + Scalar tag_width = getTagWidth(tag_id); // tag width in meters + + Eigen::Affine3ds c_M_t; + bool use_rotation = true; + ////////////////// + // IPPE (Infinitesimal Plane-based Pose Estimation) + ////////////////// + Eigen::Affine3ds M_ippe1, M_ippe2, M_april, M_PnP; + Scalar rep_error1; + Scalar 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_); + ////////////////// + c_M_t = M_ippe1; + + // set the measured pose vector + Eigen::Vector3s translation ( c_M_t.translation() ); // translation vector in apriltag meters + Eigen::Vector7s pose; + pose << translation, R2q(c_M_t.linear()).coeffs(); + + // compute the covariance + // Eigen::Matrix6s cov = getVarVec().asDiagonal() ; // fixed dummy covariance + Eigen::Matrix6s 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::Matrix3s::Zero(); + info.topRightCorner(3,3) = Eigen::Matrix3s::Zero(); + info.bottomRightCorner(3,3) = 0.0001 * Eigen::Matrix3s::Identity(); + } + + // add to detected features list + detections_incoming_.push_back(std::make_shared<FeatureApriltag>(pose, info, tag_id, *det, rep_error1, rep_error2, use_rotation, + FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO)); + } + + apriltag_detections_destroy(detections); +} + +void ProcessorTrackerLandmarkApriltag::ippePoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar> _K, double _tag_width, + Eigen::Affine3d &_M1, + double &_rep_error1, + Eigen::Affine3d &_M2, + double &_rep_error2){ + + // get corners from det + std::vector<cv::Point2d> 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<cv::Point3d> 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 + Scalar 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; + IPPE::PoseSolver pose_solver; + + pose_solver.solveSquare(_tag_width, corners_pix, _K, cv::Mat(), + rvec1, tvec1, err1, + rvec2, tvec2, err2); + + _rep_error1 = err1; + _rep_error2 = err2; + + // Puts the result in a Eigen affine Transform + cv::Matx33d rmat1; + cv::Rodrigues(rvec1, rmat1); + Eigen::Matrix3s R_eigen1; cv2eigen(rmat1, R_eigen1); + Eigen::Vector3s t_eigen1; cv2eigen(tvec1, t_eigen1); + _M1.matrix().block(0,0,3,3) = R_eigen1; + _M1.matrix().block(0,3,3,1) = t_eigen1; + + cv::Matx33d rmat2; + cv::Rodrigues(rvec2, rmat2); + Eigen::Matrix3s R_eigen2; cv2eigen(rmat2, R_eigen2); + Eigen::Vector3s t_eigen2; cv2eigen(tvec2, t_eigen2); + _M2.matrix().block(0,0,3,3) = R_eigen2; + _M2.matrix().block(0,3,3,1) = t_eigen2; +} + + +void ProcessorTrackerLandmarkApriltag::postProcess() +{ + +} + +FactorBasePtr ProcessorTrackerLandmarkApriltag::createFactor(FeatureBasePtr _feature_ptr, + LandmarkBasePtr _landmark_ptr) +{ + FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>( + getSensor(), + getLast()->getFrame(), + std::static_pointer_cast<LandmarkApriltag>(_landmark_ptr), + std::static_pointer_cast<FeatureApriltag> (_feature_ptr ), + true, + FAC_ACTIVE + ); + return constraint; +} + +LandmarkBasePtr ProcessorTrackerLandmarkApriltag::createLandmark(FeatureBasePtr _feature_ptr) +{ + // world to rob + Vector3s pos = getLast()->getFrame()->getP()->getState(); + Quaternions quat (getLast()->getFrame()->getO()->getState().data()); + Eigen::Affine3ds w_M_r = Eigen::Translation<Scalar,3>(pos.head(3)) * quat; + + // rob to camera + pos = getSensor()->getP()->getState(); + quat.coeffs() = getSensor()->getO()->getState(); + Eigen::Affine3ds r_M_c = Eigen::Translation<Scalar,3>(pos.head(3)) * quat; + + // camera to lmk (tag) + pos = _feature_ptr->getMeasurement().head(3); + quat.coeffs() = _feature_ptr->getMeasurement().tail(4); + Eigen::Affine3ds c_M_t = Eigen::Translation<Scalar,3>(pos) * quat; + + // world to lmk (tag) + Eigen::Affine3ds w_M_t = w_M_r * r_M_c * c_M_t; + + // make 7-vector for lmk (tag) pose + pos = w_M_t.translation(); + quat = w_M_t.linear(); + Vector7s w_pose_t; + w_pose_t << pos, quat.coeffs(); + + FeatureApriltagPtr feat_april = std::static_pointer_cast<FeatureApriltag>(_feature_ptr); + int tag_id = feat_april->getTagId(); + + LandmarkApriltagPtr new_landmark = std::make_shared<LandmarkApriltag>(w_pose_t, tag_id, getTagWidth(tag_id)); + + return new_landmark; +} + +unsigned int ProcessorTrackerLandmarkApriltag::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _new_features_last) +{ + LandmarkBasePtrList& landmark_list = getProblem()->getMap()->getLandmarkList(); + for (auto feature_in_image : detections_last_) + { + bool feature_already_found(false); + // list of landmarks in the map + + //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<LandmarkApriltag>(*it)->getTagId() == std::static_pointer_cast<FeatureApriltag>(feature_in_image)->getTagId()){ + feature_already_found = true; + break; + } + } + + if (!feature_already_found) + { + for (FeatureBasePtrList::iterator it=_new_features_last.begin(); it != _new_features_last.end(); ++it) + if (std::static_pointer_cast<FeatureApriltag>(*it)->getTagId() == std::static_pointer_cast<FeatureApriltag>(feature_in_image)->getTagId()) + { + //we have a detection with the same id as the currently processed one. We remove the previous feature from the list for now + _new_features_last.erase(it); + //it should not be possible two detection with the same id before getting there so we can stop here. + break; + } + // discard features that do not have orientation information + if (!std::static_pointer_cast<FeatureApriltag>(feature_in_image)->getUserotation()) + continue; + + // If the feature is not in the map & not in the list of newly detected features yet then we add it. + _new_features_last.push_back(feature_in_image); + } //otherwise we check the next feature + } + + return _new_features_last.size(); +} + +bool ProcessorTrackerLandmarkApriltag::voteForKeyFrame() +{ + // Necessary conditions + bool more_in_last = getLast()->getFeatureList().size() >= min_features_for_keyframe_; + // Vote for every image processed at the beginning, bypasses the others + if (more_in_last && (nb_vote_ < nb_vote_for_every_first_)){ + nb_vote_++; + return true; + } + // Check if enough information is provided by the measurements to determine uniquely the position of the KF + // Is only activated when enough_info_necessary_ is set to true + bool enough_info; + if (enough_info_necessary_){ + int nb_userot = 0; + int nb_not_userot = 0; + for (auto it_feat = getLast()->getFeatureList().begin(); it_feat != getLast()->getFeatureList().end(); it_feat++){ + FeatureApriltagPtr feat_apr_ptr = std::static_pointer_cast<FeatureApriltag>(*it_feat); + if (feat_apr_ptr->getUserotation()){ + nb_userot++; + } + else{ + nb_not_userot++; + } + } + // Condition on wether enough information is provided by the measurements: + // Position + rotation OR more that 3 3D translations (3 gives 2 symmetric solutions) + enough_info = (nb_userot > 0) || (nb_not_userot > 3); + } + else{ + enough_info = true; + } + Scalar dt_incoming_origin = getIncoming()->getTimeStamp().get() - getOrigin()->getTimeStamp().get(); + bool more_than_min_time_vote = dt_incoming_origin > min_time_vote_; + + // Possible decision factors + bool too_long_since_last_KF = dt_incoming_origin > max_time_vote_; + bool less_in_incoming = getIncoming()->getFeatureList().size() < min_features_for_keyframe_; + int diff = getOrigin()->getFeatureList().size() - getIncoming()->getFeatureList().size(); + bool too_big_feature_diff = (abs(diff) >= max_features_diff_); + + // Final decision logic + bool vote = enough_info && more_than_min_time_vote && more_in_last && (less_in_incoming || too_long_since_last_KF || too_big_feature_diff); + return vote; +} + +unsigned int ProcessorTrackerLandmarkApriltag::findLandmarks(const LandmarkBasePtrList& _landmark_list_in, + FeatureBasePtrList& _feature_list_out, + LandmarkMatchMap& _feature_landmark_correspondences) +{ + for (auto feature_in_image : detections_incoming_) + { + int tag_id(std::static_pointer_cast<FeatureApriltag>(feature_in_image)->getTagId()); + + for (auto landmark_in_ptr : _landmark_list_in) + { + if(std::static_pointer_cast<LandmarkApriltag>(landmark_in_ptr)->getTagId() == tag_id) + { + _feature_list_out.push_back(feature_in_image); + Scalar score(1.0); + LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(landmark_in_ptr, score); //TODO: smarter score + _feature_landmark_correspondences.emplace ( feature_in_image, matched_landmark ); + break; + } + } + } + + return _feature_list_out.size(); +} + +wolf::Scalar ProcessorTrackerLandmarkApriltag::getTagWidth(int _id) const +{ + if (tag_widths_.find(_id) != tag_widths_.end()) + return tag_widths_.at(_id); + else + return tag_width_default_; +} + +Eigen::Vector6s ProcessorTrackerLandmarkApriltag::getVarVec() +{ + Eigen::Vector6s var_vec; + var_vec << std_xy_*std_xy_, std_xy_*std_xy_, std_z_*std_z_, std_rpy_*std_rpy_, std_rpy_*std_rpy_, std_rpy_*std_rpy_; + + return var_vec; +} + +Eigen::Matrix6s ProcessorTrackerLandmarkApriltag::computeInformation(Eigen::Vector3s const &t, Eigen::Matrix3s const &R, Eigen::Matrix3s const &K, Scalar const &tag_width, double const &sig_q) +{ + // 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 + Scalar s = tag_width/2; + Eigen::Vector3s p1; p1 << -s, s, 0; // bottom left + Eigen::Vector3s p2; p2 << s, s, 0; // bottom right + Eigen::Vector3s p3; p3 << s, -s, 0; // top right + Eigen::Vector3s p4; p4 << -s, -s, 0; // top left + std::vector<Eigen::Vector3s> pvec = {p1, p2, p3, p4}; + std::vector<Eigen::Vector2s> proj_pix_vec; proj_pix_vec.resize(4); + + // Initialize jacobian matrices + Eigen::Matrix<Scalar, 8, 6> J_u_TR = Eigen::Matrix<Scalar, 8, 6>::Zero(); // 2N x 6 jac + Eigen::Vector3s h; + Eigen::Matrix3s J_h_T, J_h_R; + Eigen::Vector2s eu; // 2D pixel coord, not needed + Eigen::Matrix<Scalar, 3, 6> J_h_TR; + Eigen::Matrix<Scalar, 2, 3> J_u_h; + for (int i=0; i < pvec.size(); i++){ + // Pinhole projection to non normalized homogeneous coordinates in pixels (along with jacobians) + pinholeHomogeneous(K, t, R, pvec[i], h, J_h_T, J_h_R); + // 3 x 6 tag to camera translation|rotation jacobian + J_h_TR << J_h_T, J_h_R; + // Euclidianization Jacobian + pinhole::projectPointToNormalizedPlane(h, eu, J_u_h); + // Fill jacobian for ith corner + J_u_TR.block(2*i, 0, 2, 6) = J_u_h * J_h_TR; + proj_pix_vec[i] = eu; + } + + // Pixel uncertainty covariance matrix + Eigen::Matrix<Scalar, 8, 8> pixel_cov = pow(sig_q, 2) * Eigen::Matrix<Scalar, 8, 8>::Identity(); + // 6 x 6 translation|rotation information matrix computed with covariance propagation formula (inverted) + Eigen::Matrix6s transformation_info = (J_u_TR.transpose() * pixel_cov.inverse() * J_u_TR); // Wolf jac + + return transformation_info; + +} + +void ProcessorTrackerLandmarkApriltag::pinholeHomogeneous(Eigen::Matrix3s const & K, Eigen::Vector3s const & t, + Eigen::Matrix3s const & R, Eigen::Vector3s const & p, + Eigen::Vector3s &h, Eigen::Matrix3s &J_h_T, Eigen::Matrix3s &J_h_R) +{ + // Pinhole projection + jacobians + h = K * (t + R * p); + J_h_T = K; + Eigen::Matrix3s p_hat; + p_hat << 0, -p(2), p(1), + p(2), 0, -p(0), + -p(1), p(0), 0; + J_h_R = -K * R * p_hat; +} + +FeatureBasePtrList ProcessorTrackerLandmarkApriltag::getIncomingDetections() const +{ + return detections_incoming_; +} + +FeatureBasePtrList ProcessorTrackerLandmarkApriltag::getLastDetections() const +{ + return detections_last_; +} + +void ProcessorTrackerLandmarkApriltag::configure(SensorBasePtr _sensor) +{ + SensorCameraPtr sen_cam_ptr = std::static_pointer_cast<SensorCamera>(_sensor); + sen_cam_ptr->useRectifiedImages(); + + // get camera intrinsic parameters + Eigen::Vector4s k(_sensor->getIntrinsic()->getState()); //[cx cy fx fy] + + // Intrinsic matrices for opencv and eigen: + + cv_K_ << k(2), 0, k(0), + 0, k(3), k(1), + 0, 0, 1; + + K_ << k(2), 0, k(0), + 0, k(3), k(1), + 0, 0, 1; + +} + +void ProcessorTrackerLandmarkApriltag::advanceDerived() +{ + ProcessorTrackerLandmark::advanceDerived(); + detections_last_ = std::move(detections_incoming_); +} + +void ProcessorTrackerLandmarkApriltag::resetDerived() +{ + // Add 3D distance constraint between 2 frames + if (getProblem()->getProcessorMotion() == nullptr && add_3D_cstr_){ + if ((getOrigin() != nullptr) && + (getOrigin()->getFrame() != nullptr) && + (getOrigin() != getLast()) && + (getOrigin()->getFrame() != getLast()->getFrame()) + ) + { + + FrameBasePtr ori_frame = getOrigin()->getFrame(); + Eigen::Vector1s dist_meas; dist_meas << 0.0; + double dist_std = 0.5; + Eigen::Matrix1s cov0(dist_std*dist_std); + + CaptureBasePtr capt3D = std::make_shared<CaptureBase>("Dist", getLast()->getTimeStamp()); + getLast()->getFrame()->addCapture(capt3D); + FeatureBasePtr feat_dist = capt3D->addFeature(std::make_shared<FeatureBase>("Dist", dist_meas, cov0)); + FactorAutodiffDistance3DPtr cstr = std::make_shared<FactorAutodiffDistance3D>(feat_dist, ori_frame, nullptr, false, FAC_ACTIVE); + feat_dist->addFactor(cstr); + ori_frame->addConstrainedBy(cstr); + } + } + + if ((getProblem()->getProcessorMotion() == nullptr) && 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::Quaternions last_q_cam(getSensor()->getO()->getState().data()); + Eigen::Affine3ds last_M_cam = Eigen::Translation3ds(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(); + + // std::cout << "last_feature_list.size(): " << last_feature_list.size() << std::endl; + // std::cout << "ori_feature_list.size(): " << ori_feature_list.size() << std::endl; + 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) + Scalar 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<FeatureApriltag>(*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<FeatureApriltag>(*it_ori); + if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){ + Scalar 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; + // std::cout << "Best feature id: " << best_feature->getTagId() << std::endl; + } + } + } + } + // 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::Vector7s cam_pose_lmk = best_feature->getMeasurement(); + Eigen::Quaternions cam_q_lmk(cam_pose_lmk.segment<4>(3).data()); + Eigen::Affine3ds cam_M_lmk = Eigen::Translation3ds(cam_pose_lmk.head(3)) * cam_q_lmk; + + // Get corresponding landmarks in origin/last landmark list + Eigen::Affine3ds 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<LandmarkBasePtr>::reverse_iterator it_lmk = lmk_list.rbegin(); it_lmk != lmk_list.rend(); ++it_lmk){ + LandmarkApriltagPtr lmk_ptr = std::dynamic_pointer_cast<LandmarkApriltag>(*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::Vector3s w_t_lmk = lmk_ptr->getP()->getState(); + Eigen::Quaternions w_q_lmk(lmk_ptr->getO()->getState().data()); + w_M_lmk = Eigen::Translation<Scalar,3>(w_t_lmk) * w_q_lmk; + } + } + + // Compute last frame estimate + Eigen::Affine3ds 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::Vector3s pos_last = w_M_last.translation(); + Eigen::Quaternions 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<FeatureApriltag>(*it_feat); + + Eigen::Vector7s cam_pose_lmk = new_feature_last->getMeasurement(); + Eigen::Quaternions cam_q_lmk(cam_pose_lmk.segment<4>(3).data()); + Eigen::Affine3ds cam_M_lmk_new = Eigen::Translation3ds(cam_pose_lmk.head(3)) * cam_q_lmk; + Eigen::Affine3ds 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<LandmarkApriltag>(*it_lmk); + if (lmk_ptr == nullptr){ + continue; + } + if (lmk_ptr->getTagId() == new_feature_last->getTagId()){ + Eigen::Vector3s pos_lmk_last = w_M_lmk.translation(); + Eigen::Quaternions 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; +} + +} // namespace wolf + +// Register in the SensorFactory +#include "base/processor/processor_factory.h" + +namespace wolf +{ +WOLF_REGISTER_PROCESSOR("TRACKER LANDMARK APRILTAG", ProcessorTrackerLandmarkApriltag) +} + diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp index 5058b70fc75b3925b9219e521b45652fc024c62a..424dcf77873eff3fcafcb87bd11bc11dd0cc7659 100644 --- a/src/yaml/processor_IMU_yaml.cpp +++ b/src/yaml/processor_IMU_yaml.cpp @@ -29,13 +29,15 @@ static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _file YAML::Node kf_vote = config["keyframe vote"]; ProcessorParamsIMUPtr params = std::make_shared<ProcessorParamsIMU>(); + params->time_tolerance = config["time tolerance"] .as<Scalar>(); + params->unmeasured_perturbation_std_ = config["unmeasured perturbation std"].as<Scalar>(); + params->unmeasured_perturbation_std = config["unmeasured perturbation std"].as<Scalar>(); params->max_time_span = kf_vote["max time span"] .as<Scalar>(); params->max_buff_length = kf_vote["max buffer length"] .as<SizeEigen >(); params->dist_traveled = kf_vote["dist traveled"] .as<Scalar>(); params->angle_turned = kf_vote["angle turned"] .as<Scalar>(); params->voting_active = kf_vote["voting_active"] .as<bool>(); - return params; } diff --git a/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp b/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp new file mode 100644 index 0000000000000000000000000000000000000000..70a44fc672b41eaacd09fd5dc36b7d642bb283cf --- /dev/null +++ b/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp @@ -0,0 +1,92 @@ +/** + * \file processor_tracker_landmark_apriltag_yaml.cpp + * + * Created on: Dec 6, 2018 + * \author: jsola + */ + + +// wolf +#include "base/processor/processor_tracker_landmark_apriltag.h" +#include "base/yaml/yaml_conversion.h" +#include "base/common/factory.h" + +// yaml-cpp library +#include <yaml-cpp/yaml.h> + +namespace wolf +{ + +namespace +{ +static ProcessorParamsBasePtr createProcessorParamsLandmarkApriltag(const std::string & _filename_dot_yaml) +{ + YAML::Node config = YAML::LoadFile(_filename_dot_yaml); + + if (config.IsNull()) + { + WOLF_ERROR("Invalid YAML file!"); + return nullptr; + } + else if (config["processor type"].as<std::string>() == "TRACKER LANDMARK APRILTAG") + { + ProcessorParamsTrackerLandmarkApriltagPtr params = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>(); + + YAML::Node detector_parameters = config["detector parameters"]; + params->quad_decimate_ = detector_parameters["quad_decimate"] .as<Scalar>(); + params->quad_sigma_ = detector_parameters["quad_sigma"] .as<Scalar>(); + params->nthreads_ = detector_parameters["nthreads"] .as<int>(); + params->debug_ = detector_parameters["debug"] .as<bool>(); + params->refine_edges_ = detector_parameters["refine_edges"] .as<bool>(); + params->ippe_min_ratio_ = detector_parameters["ippe_min_ratio"] .as<Scalar>(); + params->ippe_max_rep_error_ = detector_parameters["ippe_max_rep_error"] .as<Scalar>(); + + YAML::Node tag_parameters = config["tag parameters"]; + params->tag_family_ = tag_parameters["tag_family"] .as<std::string>(); + // params->tag_black_border_ = tag_parameters["tag_black_border"] .as<int>(); + params->tag_width_default_ = tag_parameters["tag_width_default"] .as<Scalar>(); + + // read list of tag widths + YAML::Node tag_widths = config["tag widths"]; + for (auto pair_id_width : tag_widths) + { + int tag_id = pair_id_width.first .as<int>(); + Scalar tag_width = pair_id_width.second .as<Scalar>(); + params->tag_widths_.emplace(tag_id, tag_width); + } + + YAML::Node noise = config["noise"]; + params->std_xy_ = noise["std_xy"] .as<Scalar>(); + params->std_z_ = noise["std_z"] .as<Scalar>(); + params->std_rpy_ = M_TORAD * noise["std_rpy_degrees"] .as<Scalar>(); + params->std_pix_ = noise["std_pix"] .as<Scalar>(); + + YAML::Node vote = config["vote"]; + params->voting_active = vote["voting active"] .as<bool>(); + params->min_time_vote_ = vote["min_time_vote"] .as<Scalar>(); + params->max_time_vote_ = vote["max_time_vote"] .as<Scalar>(); + params->min_features_for_keyframe = vote["min_features_for_keyframe"] .as<unsigned int>(); + params->max_features_diff_ = vote["max_features_diff"] .as<int>(); + params->nb_vote_for_every_first_ = vote["nb_vote_for_every_first"] .as<int>(); + params->enough_info_necessary_ = vote["enough_info_necessary"] .as<bool>(); + + params->reestimate_last_frame_ = config["reestimate_last_frame"] .as<bool>(); + params->add_3D_cstr_ = config["add_3D_cstr"] .as<bool>(); + + return params; + } + else + { + WOLF_ERROR("Wrong processor type! Should be \"TRACKER LANDMARK APRILTAG\""); + return nullptr; + } + return nullptr; +} + +// Register in the SensorFactory +const bool WOLF_UNUSED registered_prc_apriltag = ProcessorParamsFactory::get().registerCreator("TRACKER LANDMARK APRILTAG", createProcessorParamsLandmarkApriltag); +const bool WOLF_UNUSED registered_prc_apriltag_wrapper = ProcessorParamsFactory::get().registerCreator("TRACKER LANDMARK APRILTAG WRAPPER", createProcessorParamsLandmarkApriltag); + +} // namespace [unnamed] + +} // namespace wolf diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp index 224a153e1936cf4eab7680b1fa7a2475df952b03..f61cb7398d3e64f5f047c93d62d1953696c71d62 100644 --- a/test/gtest_IMU.cpp +++ b/test/gtest_IMU.cpp @@ -87,7 +87,7 @@ class Process_Factor_IMU : public testing::Test string wolf_root = _WOLF_ROOT_DIR; //===================================== SETTING PROBLEM - problem = Problem::create("POV 3D"); + problem = Problem::create("POV", 3); // CERES WRAPPER ceres::Solver::Options ceres_options; @@ -480,7 +480,7 @@ class Process_Factor_IMU : public testing::Test virtual void buildProblem() { // ===================================== SET KF in Wolf tree - FrameBasePtr KF = problem->emplaceFrame(KEY_FRAME, x1_exact, t); + FrameBasePtr KF = problem->emplaceFrame(KEY, x1_exact, t); // ===================================== IMU CALLBACK problem->keyFrameCallback(KF, nullptr, dt/2); diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp deleted file mode 100644 index a6146b6615adcd9bea24d3c12fab7d70ee013a12..0000000000000000000000000000000000000000 --- a/test/gtest_SE3.cpp +++ /dev/null @@ -1,236 +0,0 @@ -/** - * \file gtest_SE3.cpp - * - * Created on: Feb 2, 2019 - * \author: jsola - */ - - -#include "core/math/SE3.h" -#include "utils_gtest.h" - - - -using namespace Eigen; -using namespace wolf; -using namespace three_D; - -TEST(SE3, exp_0) -{ - Vector6s tau = Vector6s::Zero(); - Vector7s pose; pose << 0,0,0, 0,0,0,1; - - ASSERT_MATRIX_APPROX(pose, exp_SE3(tau), 1e-8); -} - -TEST(SE3, log_I) -{ - Vector7s pose; pose << 0,0,0, 0,0,0,1; - Vector6s tau = Vector6s::Zero(); - - ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8); -} - -TEST(SE3, exp_log) -{ - Vector6s tau = Vector6s::Random() / 5.0; - Vector7s pose = exp_SE3(tau); - - ASSERT_MATRIX_APPROX(tau, log_SE3(exp_SE3(tau)), 1e-8); - ASSERT_MATRIX_APPROX(pose, exp_SE3(log_SE3(pose)), 1e-8); -} - -TEST(SE3, exp_of_multiple) -{ - Vector6s tau, tau2, tau3; - Vector7s pose, pose2, pose3; - tau = Vector6s::Random() / 5; - pose << exp_SE3(tau); - tau2 = 2*tau; - tau3 = 3*tau; - pose2 = compose(pose, pose); - pose3 = compose(pose2, pose); - - // 1 - ASSERT_MATRIX_APPROX(exp_SE3(tau), pose, 1e-8); - - // 2 - ASSERT_MATRIX_APPROX(exp_SE3(tau2), pose2, 1e-8); - - // 3 - ASSERT_MATRIX_APPROX(exp_SE3(tau3), pose3, 1e-8); -} - -TEST(SE3, log_of_power) -{ - Vector6s tau, tau2, tau3; - Vector7s pose, pose2, pose3; - tau = Vector6s::Random() / 5; - pose << exp_SE3(tau); - tau2 = 2*tau; - tau3 = 3*tau; - pose2 = compose(pose, pose); - pose3 = compose(pose2, pose); - - // 1 - ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8); - - // 2 - ASSERT_MATRIX_APPROX(tau2, log_SE3(pose2), 1e-8); - - // 3 - ASSERT_MATRIX_APPROX(tau3, log_SE3(pose3), 1e-8); -} - -TEST(SE3, interpolate_I_xyz) -{ - Vector7s p1, p2, p_pre; - - p1 << 0,0,0, 0,0,0,1; - p2 << 1,2,3, 0,0,0,1; - Scalar t = 0.2; - - interpolate(p1, p2, t, p_pre); - - Vector7s p_gt; p_gt << 0.2,0.4,0.6, 0,0,0,1; - - ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8); -} - -TEST(SE3, interpolate_xyz_xyz) -{ - Vector7s p1, p2, p_pre; - - p1 << 1,2,3, 0,0,0,1; - p2 << 2,4,6, 0,0,0,1; - Scalar t = 0.2; - - interpolate(p1, p2, t, p_pre); - - Vector7s p_gt; p_gt << 1.2,2.4,3.6, 0,0,0,1; - - ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8); -} - -TEST(SE3, interpolate_I_qx) -{ - Vector7s p1, p2, p_pre; - - p1 << 0,0,0, 0,0,0,1; - p2 << 0,0,0, 1,0,0,0; - Scalar t = 0.5; - - interpolate(p1, p2, t, p_pre); - - Vector7s p_gt; p_gt << 0,0,0, sqrt(2)/2,0,0,sqrt(2)/2; - - ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8); -} - -TEST(SE3, interpolate_I_qy) -{ - Vector7s p1, p2, p_pre; - - p1 << 0,0,0, 0,0,0,1; - p2 << 0,0,0, 0,1,0,0; - Scalar t = 0.5; - - interpolate(p1, p2, t, p_pre); - - Vector7s p_gt; p_gt << 0,0,0, 0,sqrt(2)/2,0,sqrt(2)/2; - - ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8); -} - -TEST(SE3, interpolate_I_qz) -{ - Vector7s p1, p2, p_pre; - - p1 << 0,0,0, 0,0,0,1; - p2 << 0,0,0, 0,0,1,0; - Scalar t = 0.5; - - interpolate(p1, p2, t, p_pre); - - Vector7s p_gt; p_gt << 0,0,0, 0,0,sqrt(2)/2,sqrt(2)/2; - - ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8); -} - -TEST(SE3, interpolate_I_qxyz) -{ - Vector7s p1, p2, dp, p_pre, p_gt; - Vector3s da; - - da.setRandom(); da /= 5; - - p1 << 0,0,0, 0,0,0,1; - dp << 0,0,0, exp_q(da).coeffs(); - p_gt = compose(p1, dp); - p2 = compose(p_gt, dp); - Scalar t = 0.5; - - interpolate(p1, p2, t, p_pre); - - ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8); -} - -TEST(SE3, interpolate_half) -{ - Vector7s p1, p2, dp, p_pre, p_gt; - Vector6s da; - - p1.setRandom(); p1.tail(4).normalize(); - - da.setRandom(); da /= 5; // small rotation - dp << exp_SE3(da); - - // compose double, then interpolate 1/2 - - p_gt = compose(p1, dp); - p2 = compose(p_gt, dp); - - Scalar t = 0.5; - interpolate(p1, p2, t, p_pre); - - ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8); -} - -TEST(SE3, interpolate_third) -{ - Vector7s p1, p2, dp, dp2, dp3, p_pre, p_gt; - Vector6s da; - - p1.setRandom(); p1.tail(4).normalize(); - - da.setRandom(); da /= 5; // small rotation - dp << exp_SE3(da); - dp2 = compose(dp, dp); - dp3 = compose(dp2, dp); - - // compose triple, then interpolate 1/3 - - p_gt = compose(p1, dp); - p2 = compose(p1, dp3); - - Scalar t = 1.0/3.0; - interpolate(p1, p2, t, p_pre); - - ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8); -} - -TEST(SE3, toSE3_I) -{ - Vector7s pose; pose << 0,0,0, 0,0,0,1; - Matrix4s R; - toSE3(pose, R); - - ASSERT_MATRIX_APPROX(R, Matrix4s::Identity(), 1e-8); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp deleted file mode 100644 index 7d31c89997afb7c89c5830fa7e10926a32e04276..0000000000000000000000000000000000000000 --- a/test/gtest_capture_base.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/* - * gtest_capture_base.cpp - * - * Created on: Apr 11, 2017 - * Author: jsola - */ - -#include "utils_gtest.h" - -#include "core/capture/capture_base.h" -#include "core/state_block/state_angle.h" - -using namespace wolf; -using namespace Eigen; - -TEST(CaptureBase, ConstructorNoSensor) -{ - CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2 - - ASSERT_EQ(C->getTimeStamp(), 1.2); - ASSERT_FALSE(C->getFrame()); - ASSERT_FALSE(C->getProblem()); - ASSERT_FALSE(C->getSensor()); -} - -TEST(CaptureBase, ConstructorWithSensor) -{ - SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2)); - CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5 - ASSERT_EQ(C->getTimeStamp(), 1.5); - ASSERT_FALSE(C->getFrame()); - ASSERT_FALSE(C->getProblem()); - ASSERT_TRUE(C->getSensor()); - ASSERT_FALSE(C->getSensorP()); - ASSERT_FALSE(C->getSensorO()); -} - -TEST(CaptureBase, Static_sensor_params) -{ - StateBlockPtr p(std::make_shared<StateBlock>(2)); - StateBlockPtr o(std::make_shared<StateAngle>() ); - StateBlockPtr i(std::make_shared<StateBlock>(2)); - SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", p, o, i, 2)); - CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5 - - // query sensor blocks - ASSERT_EQ(S->getP(), p); - ASSERT_EQ(S->getO(), o); - ASSERT_EQ(S->getIntrinsic(), i); - - // query capture blocks - ASSERT_EQ(C->getSensorP(), p); - ASSERT_EQ(C->getSensorO(), o); - ASSERT_EQ(C->getSensorIntrinsic(), i); -} - -TEST(CaptureBase, Dynamic_sensor_params) -{ - StateBlockPtr p(std::make_shared<StateBlock>(2)); - StateBlockPtr o(std::make_shared<StateAngle>() ); - StateBlockPtr i(std::make_shared<StateBlock>(2)); - SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2, true, true)); // last 2 'true' mark dynamic - CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5 - - // query sensor blocks -- need KFs to find some Capture with the params - // ASSERT_EQ(S->getP(), p); - // ASSERT_EQ(S->getO(), o); - // ASSERT_EQ(S->getIntrinsic(), i); - - // query capture blocks - ASSERT_EQ(C->getSensorP(), p); - ASSERT_EQ(C->getSensorO(), o); - ASSERT_EQ(C->getSensorIntrinsic(), i); -} - -TEST(CaptureBase, addFeature) -{ - CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2 - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity())); - ASSERT_FALSE(C->getFeatureList().empty()); - ASSERT_EQ(C->getFeatureList().front(), f); -} - -TEST(CaptureBase, addFeatureList) -{ - CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2 - FeatureBasePtr f_first = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity())); - ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1); - - // make a list to add - std::list<FeatureBasePtr> fl; - for (int i = 0; i<3; i++) - { - fl.push_back(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity())); - } - FeatureBasePtr f_last = fl.back(); - - C->addFeatureList((fl)); - ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 4); - ASSERT_EQ(fl.size(), (unsigned int) 0); // features have been moved, not copied - ASSERT_EQ(C->getFeatureList().front(), f_first); - ASSERT_EQ(C->getFeatureList().back(), f_last); -} - -TEST(CaptureBase, process) -{ - SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2)); - CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, nullptr)); - ASSERT_DEATH({C->process();},""); // No sensor in the capture should fail - C->setSensor(S); - ASSERT_TRUE(C->process()); // This should not fail (although it does nothing) -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp deleted file mode 100644 index 0d4e8d18213c84942438244bbf74070c90235796..0000000000000000000000000000000000000000 --- a/test/gtest_ceres_manager.cpp +++ /dev/null @@ -1,637 +0,0 @@ -/* - * gtest_ceres_manager.cpp - * - * Created on: Jun, 2018 - * Author: jvallve - */ - -#include "utils_gtest.h" -#include "core/utils/logging.h" - -#include "core/problem/problem.h" -#include "core/sensor/sensor_base.h" -#include "core/state_block/state_block.h" -#include "core/capture/capture_void.h" -#include "core/factor/factor_pose_2D.h" -#include "core/factor/factor_quaternion_absolute.h" -#include "core/solver/solver_manager.h" -#include "core/ceres_wrapper/ceres_manager.h" -#include "core/state_block/local_parametrization_angle.h" -#include "core/state_block/local_parametrization_quaternion.h" - -#include "ceres/ceres.h" - -#include <iostream> - -using namespace wolf; -using namespace Eigen; - -WOLF_PTR_TYPEDEFS(CeresManagerWrapper); -class CeresManagerWrapper : public CeresManager -{ - public: - CeresManagerWrapper(const ProblemPtr& wolf_problem) : - CeresManager(wolf_problem) - { - }; - - bool isStateBlockRegisteredCeresManager(const StateBlockPtr& st) - { - return ceres_problem_->HasParameterBlock(SolverManager::getAssociatedMemBlockPtr(st)); - }; - - bool isStateBlockRegisteredSolverManager(const StateBlockPtr& st) - { - return state_blocks_.find(st)!=state_blocks_.end(); - }; - - bool isStateBlockFixed(const StateBlockPtr& st) - { - return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st)); - }; - - int numStateBlocks() - { - return ceres_problem_->NumParameterBlocks(); - }; - - int numFactors() - { - return ceres_problem_->NumResidualBlocks(); - }; - - bool isFactorRegistered(const FactorBasePtr& fac_ptr) const - { - return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end(); - }; - - bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) - { - return state_blocks_local_param_.find(st) != state_blocks_local_param_.end() && - state_blocks_local_param_.at(st)->getLocalParametrization() == local_param && - ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get(); - }; - - bool hasLocalParametrization(const StateBlockPtr& st) const - { - return state_blocks_local_param_.find(st) != state_blocks_local_param_.end(); - }; - -}; - -TEST(CeresManager, Create) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // check double ointers to branches - ASSERT_EQ(P, ceres_manager_ptr->getProblem()); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, AddStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, DoubleAddStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // update solver - ceres_manager_ptr->update(); - - // add stateblock again - P->addStateBlock(sb_ptr); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, UpdateStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock unfixed - ASSERT_FALSE(ceres_manager_ptr->isStateBlockFixed(sb_ptr)); - - // Fix frame - sb_ptr->fix(); - - // update solver manager - ceres_manager_ptr->update(); - - // check stateblock fixed - ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr)); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, AddUpdateStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // Fix state block - sb_ptr->fix(); - - // update solver manager - ceres_manager_ptr->update(); - - // check stateblock fixed - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr)); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, RemoveStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // update solver - ceres_manager_ptr->update(); - - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); - - // remove state_block - P->removeStateBlock(sb_ptr); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, AddRemoveStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // remove state_block - P->removeStateBlock(sb_ptr); - - // update solver - ceres_manager_ptr->update(); - - // check no stateblocks - ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); - ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, RemoveUpdateStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // update solver - ceres_manager_ptr->update(); - - // remove state_block - P->removeStateBlock(sb_ptr); - - // update solver - ceres_manager_ptr->update(); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, DoubleRemoveStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // remove state_block - P->removeStateBlock(sb_ptr); - - // update solver - ceres_manager_ptr->update(); - - // remove state_block - P->removeStateBlock(sb_ptr); - - // update solver manager - ceres_manager_ptr->update(); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, AddFactor) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); - - // update solver - ceres_manager_ptr->update(); - - // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 1); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, DoubleAddFactor) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); - - // add factor again - P->addFactor(c); - - // update solver - ceres_manager_ptr->update(); - - // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 1); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, RemoveFactor) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); - - // update solver - ceres_manager_ptr->update(); - - // remove factor - P->removeFactor(c); - - // update solver - ceres_manager_ptr->update(); - - // check factor - ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 0); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, AddRemoveFactor) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); - - ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c); - - // remove factor - P->removeFactor(c); - - ASSERT_TRUE(P->getFactorNotificationMap().empty()); - - // update solver - ceres_manager_ptr->update(); - - // check factor - ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 0); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, DoubleRemoveFactor) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); - - // update solver - ceres_manager_ptr->update(); - - // remove factor - P->removeFactor(c); - - // update solver - ceres_manager_ptr->update(); - - // remove factor - P->removeFactor(c); - - ASSERT_DEATH({ - // update solver - ceres_manager_ptr->update();},""); - - // check factor - ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 0); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, AddStateBlockLocalParam) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector1s state; state << 1; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // Local param - LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>(); - sb_ptr->setLocalParametrization(local_param_ptr); - - // add stateblock - P->addStateBlock(sb_ptr); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr)); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, RemoveLocalParam) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector1s state; state << 1; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // Local param - LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>(); - sb_ptr->setLocalParametrization(local_param_ptr); - - // add stateblock - P->addStateBlock(sb_ptr); - - // update solver - ceres_manager_ptr->update(); - - // Remove local param - sb_ptr->removeLocalParametrization(); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr)); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, AddLocalParam) -{ - ProblemPtr P = Problem::create("PO 2D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create State block - Vector1s state; state << 1; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr)); - - // Local param - LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>(); - sb_ptr->setLocalParametrization(local_param_ptr); - - // update solver - ceres_manager_ptr->update(); - - // check stateblock - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(sb_ptr)); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr)); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, FactorsRemoveLocalParam) -{ - ProblemPtr P = Problem::create("PO 3D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) 2 factors quaternion - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); - FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); - - // update solver - ceres_manager_ptr->update(); - - // check local param - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO())); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization())); - - // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1)); - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 2); - - // remove local param - F->getO()->removeLocalParametrization(); - - // update solver - ceres_manager_ptr->update(); - - // check local param - ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(F->getO())); - - // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1)); - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 2); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -TEST(CeresManager, FactorsUpdateLocalParam) -{ - ProblemPtr P = Problem::create("PO 3D"); - CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); - - // Create (and add) 2 factors quaternion - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); - FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); - - // update solver - ceres_manager_ptr->update(); - - // check local param - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO())); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization())); - - // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1)); - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 2); - - // remove local param - LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>(); - F->getO()->setLocalParametrization(local_param_ptr); - - // update solver - ceres_manager_ptr->update(); - - // check local param - ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO())); - ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),local_param_ptr)); - - // check factor - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1)); - ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2)); - ASSERT_EQ(ceres_manager_ptr->numFactors(), 2); - - // run ceres manager check - ceres_manager_ptr->check(); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp deleted file mode 100644 index c7c68b4f3a2df00fab257fb4f47646d429f361be..0000000000000000000000000000000000000000 --- a/test/gtest_eigen_predicates.cpp +++ /dev/null @@ -1,178 +0,0 @@ -#include "utils_gtest.h" - -#include "core/utils/eigen_predicates.h" - -TEST(TestEigenPredicates, TestEigenDynPredZero) -{ - Eigen::MatrixXs A, B, C; - - A = Eigen::MatrixXs::Zero(4,4); - B = Eigen::MatrixXs::Random(4,4); - C = Eigen::MatrixXs::Ones(4,4) * (wolf::Constants::EPS/2.); - - EXPECT_TRUE(wolf::pred_zero(A, wolf::Constants::EPS)); - EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS)); - EXPECT_TRUE(wolf::pred_zero(C, wolf::Constants::EPS)); - -// PRINT_TEST_FINISHED; -} - -TEST(TestEigenPredicates, TestEigenFixPredZero) -{ - Eigen::MatrixXs A, B, C; - - A = Eigen::Matrix4s::Zero(); - B = Eigen::Matrix4s::Random(); - C = Eigen::Matrix4s::Ones() * (wolf::Constants::EPS/2.); - - EXPECT_TRUE(wolf::pred_zero(A, wolf::Constants::EPS)); - EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS)); - EXPECT_TRUE(wolf::pred_zero(C, wolf::Constants::EPS)); - -// PRINT_TEST_FINISHED; -} - -TEST(TestEigenPredicates, TestEigenDynPredDiffZero) -{ - Eigen::MatrixXs A, B, C; - - A = Eigen::MatrixXs::Random(4,4); - B = Eigen::MatrixXs::Random(4,4); - C = A; - - EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS)); - EXPECT_FALSE(wolf::pred_diff_zero(A, B, wolf::Constants::EPS)); - -// PRINT_TEST_FINISHED; -} - -TEST(TestEigenPredicates, TestEigenFixPredDiffZero) -{ - Eigen::MatrixXs A, B, C; - - A = Eigen::Matrix4s::Random(); - B = Eigen::Matrix4s::Random(); - C = A; - - EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS)); - EXPECT_FALSE(wolf::pred_diff_zero(A, B, wolf::Constants::EPS)); - -// PRINT_TEST_FINISHED; -} - -TEST(TestEigenPredicates, TestEigenDynPredDiffNorm) -{ - Eigen::MatrixXs A, B, C; - - A = Eigen::MatrixXs::Random(4,4); - B = Eigen::MatrixXs::Random(4,4); - C = A; - - EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS)); - EXPECT_FALSE(wolf::pred_diff_norm_zero(A, B, wolf::Constants::EPS)); - -// PRINT_TEST_FINISHED; -} - -TEST(TestEigenPredicates, TestEigenFixPredDiffNorm) -{ - Eigen::MatrixXs A, B, C; - - A = Eigen::Matrix4s::Random(); - B = Eigen::Matrix4s::Random(); - C = A; - - EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS)); - EXPECT_FALSE(wolf::pred_diff_norm_zero(A, B, wolf::Constants::EPS)); - -// PRINT_TEST_FINISHED; -} - -TEST(TestEigenPredicates, TestEigenDynPredIsApprox) -{ - Eigen::MatrixXs A, B, C; - - A = Eigen::MatrixXs::Random(4,4); - B = Eigen::MatrixXs::Random(4,4); - C = A; - - EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS)); - EXPECT_FALSE(wolf::pred_is_approx(A, B, wolf::Constants::EPS)); - -// PRINT_TEST_FINISHED; -} - -TEST(TestEigenPredicates, TestEigenFixPredIsApprox) -{ - Eigen::MatrixXs A, B, C; - - A = Eigen::Matrix4s::Random(); - B = Eigen::Matrix4s::Random(); - C = A; - - EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS)); - EXPECT_FALSE(wolf::pred_is_approx(A, B, wolf::Constants::EPS)); - -// PRINT_TEST_FINISHED; -} - -TEST(TestEigenPredicates, TestEigenPredQuatIsApprox) -{ - Eigen::Quaternions A, B, C; - - /// @todo which version of Eigen provides this ? -// A = Eigen::Quaternions::UnitRandom(); - - A.coeffs() = Eigen::Vector4s::Random().normalized(); - B.coeffs() = Eigen::Vector4s::Random().normalized(); - C = A; - - EXPECT_TRUE(wolf::pred_quat_is_approx(A, C, wolf::Constants::EPS)); - EXPECT_FALSE(wolf::pred_quat_is_approx(A, B, wolf::Constants::EPS)); - -// PRINT_TEST_FINISHED; -} - -TEST(TestEigenPredicates, TestEigenPredQuatIsIdentity) -{ - Eigen::Quaternions A, B; - - A = Eigen::Quaternions::Identity(); - B.coeffs() = Eigen::Vector4s::Random().normalized(); - - EXPECT_TRUE(wolf::pred_quat_identity(A, wolf::Constants::EPS)); - EXPECT_FALSE(wolf::pred_quat_identity(B, wolf::Constants::EPS)); - -// PRINT_TEST_FINISHED; -} - -TEST(TestEigenPredicates, TestEigenPredAngleIsApprox) -{ - wolf::Scalar a = M_PI; - wolf::Scalar b = -M_PI; - wolf::Scalar c = 0; - - EXPECT_TRUE(wolf::pred_angle_is_approx(a, b, wolf::Constants::EPS)); - EXPECT_FALSE(wolf::pred_angle_is_approx(a, c, wolf::Constants::EPS)); - -// PRINT_TEST_FINISHED; -} - -TEST(TestEigenPredicates, TestEigenPredAngleIsZero) -{ - wolf::Scalar a = 0; - wolf::Scalar b = M_PI; - wolf::Scalar c = 2.*M_PI; - - EXPECT_TRUE(wolf::pred_angle_zero(a, wolf::Constants::EPS)); - EXPECT_FALSE(wolf::pred_angle_zero(b, wolf::Constants::EPS)); - EXPECT_TRUE(wolf::pred_angle_zero(c, wolf::Constants::EPS)); - -// PRINT_TEST_FINISHED; -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp index bb5fc915c202cc274bd63d2a7928391c312886dd..49ff47590f31c2a49e67117e4dd1f727a14500cb 100644 --- a/test/gtest_factor_IMU.cpp +++ b/test/gtest_factor_IMU.cpp @@ -58,7 +58,7 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV 3D"); + wolf_problem_ptr_ = Problem::create("POV", 3); // CERES WRAPPER ceres::Solver::Options ceres_options; @@ -144,7 +144,7 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV 3D"); + wolf_problem_ptr_ = Problem::create("POV", 3); // CERES WRAPPER ceres::Solver::Options ceres_options; @@ -228,7 +228,7 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV 3D"); + wolf_problem_ptr_ = Problem::create("POV", 3); // CERES WRAPPER ceres::Solver::Options ceres_options; @@ -312,7 +312,7 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV 3D"); + wolf_problem_ptr_ = Problem::create("POV", 3); // CERES WRAPPER ceres::Solver::Options ceres_options; @@ -398,7 +398,7 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV 3D"); + wolf_problem_ptr_ = Problem::create("POV", 3); // CERES WRAPPER ceres::Solver::Options ceres_options; @@ -485,7 +485,7 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV 3D"); + wolf_problem_ptr_ = Problem::create("POV", 3); // CERES WRAPPER ceres::Solver::Options ceres_options; @@ -568,7 +568,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV 3D"); + wolf_problem_ptr_ = Problem::create("POV", 3); // CERES WRAPPER ceres::Solver::Options ceres_options; @@ -661,7 +661,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV 3D"); + wolf_problem_ptr_ = Problem::create("POV", 3); // CERES WRAPPER ceres::Solver::Options ceres_options; @@ -762,7 +762,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV 3D"); + wolf_problem_ptr_ = Problem::create("POV", 3); // CERES WRAPPER ceres::Solver::Options ceres_options; @@ -864,7 +864,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test //===================================================== SETTING PROBLEM // WOLF PROBLEM - problem = Problem::create("POV 3D"); + problem = Problem::create("POV", 3); // CERES WRAPPER ceres::Solver::Options ceres_options; @@ -1045,7 +1045,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV 3D"); + wolf_problem_ptr_ = Problem::create("POV", 3); // CERES WRAPPER ceres::Solver::Options ceres_options; @@ -1186,7 +1186,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV 3D"); + wolf_problem_ptr_ = Problem::create("POV", 3); // CERES WRAPPER ceres::Solver::Options ceres_options; @@ -2457,10 +2457,13 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i Eigen::MatrixXs featureFix_cov(6,6); featureFix_cov = Eigen::MatrixXs::Identity(6,6); featureFix_cov(5,5) = 0.1; - CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr)); - FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov)); - FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix))); - + // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr)); + auto capfix = CaptureBase::emplace<CaptureMotion>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr); + // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov)); + auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov); + // FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix))); + FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix)); + //prepare problem for solving origin_KF->getP()->fix(); origin_KF->getO()->unfix(); @@ -2515,10 +2518,13 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_ Eigen::MatrixXs featureFix_cov(6,6); featureFix_cov = Eigen::MatrixXs::Identity(6,6); featureFix_cov(5,5) = 0.1; - CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr)); - FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov)); - FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix))); - + // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr)); + auto capfix = CaptureBase::emplace<CaptureMotion>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr); + // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov)); + auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov); + // FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix))); + FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix)); + //prepare problem for solving origin_KF->getP()->fix(); origin_KF->getO()->unfix(); diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp deleted file mode 100644 index 2343910a733b60a4ebf500cc4bb706c892ebb0bf..0000000000000000000000000000000000000000 --- a/test/gtest_factor_absolute.cpp +++ /dev/null @@ -1,124 +0,0 @@ -/** - * \file gtest_factor_absolute.cpp - * - * Created on: Dec 9, 2017 - * \author: datchuth - */ - -#include "utils_gtest.h" -#include "core/factor/factor_block_absolute.h" -#include "core/factor/factor_quaternion_absolute.h" -#include "core/capture/capture_motion.h" - -#include "core/ceres_wrapper/ceres_manager.h" - -using namespace Eigen; -using namespace wolf; -using std::cout; -using std::endl; - -Vector10s pose9toPose10(Vector9s _pose9) -{ - return (Vector10s() << _pose9.head<3>() , v2q(_pose9.segment<3>(3)).coeffs(), _pose9.tail<3>()).finished(); -} - -// Input pose9 and covariance -Vector9s pose(Vector9s::Random()); -Vector10s pose10 = pose9toPose10(pose); -Eigen::Matrix<wolf::Scalar, 9, 9> data_cov = 0.2 * Eigen::Matrix<Scalar,9,9>::Identity(); - -// perturbated priors -Vector10s x0 = pose9toPose10(pose + Vector9s::Random()*0.25); - -// Problem and solver -ProblemPtr problem_ptr = Problem::create("POV 3D"); -CeresManager ceres_mgr(problem_ptr); - -// Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0)); - -// Capture, feature and factor -CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS", 0, nullptr, pose10, 10, 9, nullptr)); - -//////////////////////////////////////////////////////// -/* In the tests below, we check that FactorBlockAbsolute and FactorQuaternionAbsolute are working fine - * These are absolute factors related to a specific part of the frame's state - * Both features and factors are created in the TEST(). Hence, tests will not interfere each others. - */ - -TEST(FactorBlockAbs, fac_block_abs_p) -{ - FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>())); - fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP())); - - ASSERT_TRUE(problem_ptr->check(0)); - - // Unfix frame 0, perturb frm0 - frm0->unfix(); - frm0->setState(x0); - - // solve for frm0 - std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); - - //only orientation is constrained - ASSERT_MATRIX_APPROX(frm0->getP()->getState(), pose10.head<3>(), 1e-6); -} - -TEST(FactorBlockAbs, fac_block_abs_p_tail2) -{ - FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>())); - fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP(),1,2)); - - // Unfix frame 0, perturb frm0 - frm0->unfix(); - frm0->setState(x0); - - // solve for frm0 - std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); - - //only orientation is constrained - ASSERT_MATRIX_APPROX(frm0->getP()->getState().tail<2>(), pose10.segment<2>(1), 1e-6); -} - -TEST(FactorBlockAbs, fac_block_abs_v) -{ - FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>())); - fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getV())); - - ASSERT_TRUE(problem_ptr->check(0)); - - // Unfix frame 0, perturb frm0 - frm0->unfix(); - frm0->setState(x0); - - // solve for frm0 - std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); - - //only velocity is constrained - ASSERT_MATRIX_APPROX(frm0->getV()->getState(), pose10.tail<3>(), 1e-6); -} - -TEST(FactorQuatAbs, fac_block_abs_o) -{ - FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3))); - fea0->addFactor(std::make_shared<FactorQuaternionAbsolute>(fea0->getFrame()->getO())); - - ASSERT_TRUE(problem_ptr->check(0)); - - // Unfix frame 0, perturb frm0 - frm0->unfix(); - frm0->setState(x0); - - // solve for frm0 - std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); - - //only velocity is constrained - ASSERT_MATRIX_APPROX(frm0->getO()->getState(), pose10.segment<4>(3), 1e-6); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp deleted file mode 100644 index 1ee57dda9eab55618f330530427dc962d203e509..0000000000000000000000000000000000000000 --- a/test/gtest_factor_autodiff.cpp +++ /dev/null @@ -1,241 +0,0 @@ -/* - * gtest_factor_autodiff.cpp - * - * Created on: May 24 2017 - * Author: jvallve - */ - -#include "utils_gtest.h" - -#include "core/sensor/sensor_odom_2D.h" -#include "core/capture/capture_void.h" -#include "core/feature/feature_odom_2D.h" -#include "core/factor/factor_odom_2D.h" -#include "core/factor/factor_odom_2D_analytic.h" -#include "core/factor/factor_autodiff.h" - -using namespace wolf; -using namespace Eigen; - -TEST(CaptureAutodiff, ConstructorOdom2d) -{ - FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1))); - FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1))); - - // SENSOR - IntrinsicsOdom2D intrinsics_odo; - intrinsics_odo.k_disp_to_disp = 0.1; - intrinsics_odo.k_rot_to_rot = 0.1; - SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); - - // CAPTURE - CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); - fr2_ptr->addCapture(capture_ptr); - - // FEATURE - FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity()); - capture_ptr->addFeature(feature_ptr); - - // CONSTRAINT - FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); - feature_ptr->addFactor(factor_ptr); - fr1_ptr->addConstrainedBy(factor_ptr); - - ASSERT_TRUE(factor_ptr->getFeature()); - ASSERT_TRUE(factor_ptr->getFeature()->getCapture()); - ASSERT_TRUE(factor_ptr->getFeature()->getCapture()->getFrame()); - ASSERT_TRUE(factor_ptr->getFeature()->getCapture()->getSensor()); - ASSERT_TRUE(factor_ptr->getFrameOther()); -} - -TEST(CaptureAutodiff, ResidualOdom2d) -{ - Eigen::Vector3s f1_pose, f2_pose; - f1_pose << 2,1,M_PI; - f2_pose << 3,5,3*M_PI/2; - - FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>()))); - FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); - - // SENSOR - IntrinsicsOdom2D intrinsics_odo; - intrinsics_odo.k_disp_to_disp = 0.1; - intrinsics_odo.k_rot_to_rot = 0.1; - SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); - - // CAPTURE - CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); - fr2_ptr->addCapture(capture_ptr); - - // FEATURE - Eigen::Vector3s d; - d << -1, -4, M_PI/2; - FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity()); - capture_ptr->addFeature(feature_ptr); - - // CONSTRAINT - FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); - feature_ptr->addFactor(factor_ptr); - fr1_ptr->addConstrainedBy(factor_ptr); - - // EVALUATE - - Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState(); - Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState(); - Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState(); - Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState(); - - std::vector<Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()}); - - double const* const* parameters = states_ptr.data(); - Eigen::VectorXs residuals(factor_ptr->getSize()); - factor_ptr->evaluate(parameters, residuals.data(), nullptr); - - ASSERT_TRUE(residuals.maxCoeff() < 1e-9); - ASSERT_TRUE(residuals.minCoeff() > -1e-9); -} - -TEST(CaptureAutodiff, JacobianOdom2d) -{ - Eigen::Vector3s f1_pose, f2_pose; - f1_pose << 2,1,M_PI; - f2_pose << 3,5,3*M_PI/2; - - FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>()))); - FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); - - // SENSOR - IntrinsicsOdom2D intrinsics_odo; - intrinsics_odo.k_disp_to_disp = 0.1; - intrinsics_odo.k_rot_to_rot = 0.1; - SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); - - // CAPTURE - CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); - fr2_ptr->addCapture(capture_ptr); - - // FEATURE - Eigen::Vector3s d; - d << -1, -4, M_PI/2; - FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity()); - capture_ptr->addFeature(feature_ptr); - - // CONSTRAINT - FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); - feature_ptr->addFactor(factor_ptr); - fr1_ptr->addConstrainedBy(factor_ptr); - - // COMPUTE JACOBIANS - - const Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState(); - const Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState(); - const Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState(); - const Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState(); - - std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()}); - - std::vector<Eigen::MatrixXs> Jauto; - Eigen::VectorXs residuals(factor_ptr->getSize()); - factor_ptr->evaluate(states_ptr, residuals, Jauto); - - std::cout << Jauto.size() << std::endl; - - // ANALYTIC JACOBIANS - Eigen::MatrixXs J0(3,2); - J0 << -cos(f1_pose(2)), -sin(f1_pose(2)), - sin(f1_pose(2)), -cos(f1_pose(2)), - 0, 0; - ASSERT_MATRIX_APPROX(J0, Jauto[0], wolf::Constants::EPS); - //ASSERT_TRUE((J0-Jauto[0]).maxCoeff() < 1e-9 && (J0-Jauto[0]).minCoeff() > -1e-9); - - Eigen::MatrixXs J1(3,1); - J1 << -(f2_pose(0) - f1_pose(0)) * sin(f1_pose(2)) + (f2_pose(1) - f1_pose(1)) * cos(f1_pose(2)), - -(f2_pose(0) - f1_pose(0)) * cos(f1_pose(2)) - (f2_pose(1) - f1_pose(1)) * sin(f1_pose(2)), - -1; - ASSERT_MATRIX_APPROX(J1, Jauto[1], wolf::Constants::EPS); - //ASSERT_TRUE((J1-Jauto[1]).maxCoeff() < 1e-9 && (J1-Jauto[1]).minCoeff() > -1e-9); - - Eigen::MatrixXs J2(3,2); - J2 << cos(f1_pose(2)), sin(f1_pose(2)), - -sin(f1_pose(2)), cos(f1_pose(2)), - 0, 0; - ASSERT_MATRIX_APPROX(J2, Jauto[2], wolf::Constants::EPS); - //ASSERT_TRUE((J2-Jauto[2]).maxCoeff() < 1e-9 && (J2-Jauto[2]).minCoeff() > -1e-9); - - Eigen::MatrixXs J3(3,1); - J3 << 0, 0, 1; - ASSERT_MATRIX_APPROX(J3, Jauto[3], wolf::Constants::EPS); - //ASSERT_TRUE((J3-Jauto[3]).maxCoeff() < 1e-9 && (J3-Jauto[3]).minCoeff() > -1e-9); - -// std::cout << "autodiff J " << 0 << ":\n" << Jauto[0] << std::endl; -// std::cout << "analytic J " << 0 << ":\n" << J0 << std::endl; -// std::cout << "autodiff J " << 1 << ":\n" << Jauto[1] << std::endl; -// std::cout << "analytic J " << 1 << ":\n" << J1 << std::endl; -// std::cout << "autodiff J " << 2 << ":\n" << Jauto[2] << std::endl; -// std::cout << "analytic J " << 2 << ":\n" << J2 << std::endl; -// std::cout << "autodiff J " << 3 << ":\n" << Jauto[3] << std::endl; -// std::cout << "analytic J " << 3 << ":\n" << J3 << std::endl; -} - -TEST(CaptureAutodiff, AutodiffVsAnalytic) -{ - Eigen::Vector3s f1_pose, f2_pose; - f1_pose << 2,1,M_PI; - f2_pose << 3,5,3*M_PI/2; - - FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>()))); - FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); - - // SENSOR - IntrinsicsOdom2D intrinsics_odo; - intrinsics_odo.k_disp_to_disp = 0.1; - intrinsics_odo.k_rot_to_rot = 0.1; - SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); - - // CAPTURE - CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); - fr2_ptr->addCapture(capture_ptr); - - // FEATURE - Eigen::Vector3s d; - d << -1, -4, M_PI/2; - FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity()); - capture_ptr->addFeature(feature_ptr); - - // CONSTRAINTS - FactorOdom2DPtr fac_autodiff_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); - feature_ptr->addFactor(fac_autodiff_ptr); - fr1_ptr->addConstrainedBy(fac_autodiff_ptr); - FactorOdom2DAnalyticPtr fac_analytic_ptr = std::make_shared<FactorOdom2DAnalytic>(feature_ptr, fr1_ptr); - feature_ptr->addFactor(fac_analytic_ptr); - fr1_ptr->addConstrainedBy(fac_analytic_ptr); - - // COMPUTE JACOBIANS - - const Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState(); - const Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState(); - const Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState(); - const Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState(); - - std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()}); - - std::vector<Eigen::MatrixXs> Jautodiff, Janalytic; - Eigen::VectorXs residuals(fac_autodiff_ptr->getSize()); - clock_t t = clock(); - fac_autodiff_ptr->evaluate(states_ptr, residuals, Jautodiff); - std::cout << "autodiff evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl; - t = clock(); - //TODO FactorAnalytic::evaluate -// fac_analytic_ptr->evaluate(states_ptr, residuals, Janalytic); -// std::cout << "analytic evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl; -// -// for (auto i = 0; i < Jautodiff.size(); i++) -// ASSERT_MATRIX_APPROX(Jautodiff[i], Janalytic[i], wolf::Constants::EPS); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp deleted file mode 100644 index 8085afc31425c6f3a236b810937106d3f8032c05..0000000000000000000000000000000000000000 --- a/test/gtest_factor_autodiff_distance_3D.cpp +++ /dev/null @@ -1,112 +0,0 @@ -/** - * \file gtest_factor_autodiff_distance_3D.cpp - * - * Created on: Apr 10, 2018 - * \author: jsola - */ - -#include "core/factor/factor_autodiff_distance_3D.h" -#include "core/problem/problem.h" -#include "core/utils/logging.h" -#include "core/ceres_wrapper/ceres_manager.h" -#include "core/math/rotations.h" - -#include "utils_gtest.h" - -using namespace wolf; -using namespace Eigen; - -class FactorAutodiffDistance3D_Test : public testing::Test -{ - public: - Vector3s pos1, pos2; - Vector3s euler1, euler2; - Quaternions quat1, quat2; - Vector4s vquat1, vquat2; // quaternions as vectors - Vector7s pose1, pose2; - - FrameBasePtr F1, F2; - CaptureBasePtr C2; - FeatureBasePtr f2; - FactorAutodiffDistance3DPtr c2; - - Vector1s dist; - Matrix1s dist_cov; - - ProblemPtr problem; - CeresManagerPtr ceres_manager; - - void SetUp() - { - pos1 << 1, 0, 0; - pos2 << 0, 1, 0; - euler1 << 0, 0, M_PI; //euler1 *= M_TORAD; - euler2 << 0, 0, -M_PI_2; //euler2 *= M_TORAD; - quat1 = e2q(euler1); - quat2 = e2q(euler2); - vquat1 = quat1.coeffs(); - vquat2 = quat2.coeffs(); - pose1 << pos1, vquat1; - pose2 << pos2, vquat2; - - dist = Vector1s(sqrt(2.0)); - dist_cov = Matrix1s(0.01); - - problem = Problem::create("PO 3D"); - ceres_manager = std::make_shared<CeresManager>(problem); - - F1 = problem->emplaceFrame (KEY_FRAME, pose1, 1.0); - - F2 = problem->emplaceFrame (KEY_FRAME, pose2, 2.0); - C2 = std::make_shared<CaptureBase>("Base", 1.0); - F2->addCapture(C2); - f2 = std::make_shared<FeatureBase>("Dist", dist, dist_cov); - C2->addFeature(f2); - c2 = std::make_shared<FactorAutodiffDistance3D>(f2, F1, nullptr, false, FAC_ACTIVE); - f2->addFactor(c2); - F1->addConstrainedBy(c2); - - } - -}; - -TEST_F(FactorAutodiffDistance3D_Test, ground_truth) -{ - Scalar res; - - c2->operator ()(pos1.data(), pos2.data(), &res); - - ASSERT_NEAR(res, 0.0, 1e-8); -} - -TEST_F(FactorAutodiffDistance3D_Test, expected_residual) -{ - Scalar measurement = 1.400; - - f2->setMeasurement(Vector1s(measurement)); - - Scalar res_expected = (measurement - (pos2-pos1).norm()) * c2->getMeasurementSquareRootInformationUpper()(0,0); - - Scalar res; - c2->operator ()(pos1.data(), pos2.data(), &res); - - ASSERT_NEAR(res, res_expected, 1e-8); -} - -TEST_F(FactorAutodiffDistance3D_Test, solve) -{ - Scalar measurement = 1.400; - f2->setMeasurement(Vector1s(measurement)); - - std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET); - - // Check distance between F1 and F2 positions -- must match the measurement - ASSERT_NEAR( (F1->getP()->getState() - F2->getP()->getState()).norm(), measurement, 1e-10); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp deleted file mode 100644 index 2ba22aeefbac8171f74302b1d8e53530cf5c9116..0000000000000000000000000000000000000000 --- a/test/gtest_factor_autodiff_trifocal.cpp +++ /dev/null @@ -1,948 +0,0 @@ -#include "utils_gtest.h" - -#include "core/utils/logging.h" - -#include "core/ceres_wrapper/ceres_manager.h" -#include "core/processor/processor_tracker_feature_trifocal.h" -#include "core/capture/capture_image.h" -#include "core/factor/factor_autodiff_trifocal.h" - -using namespace Eigen; -using namespace wolf; - -class FactorAutodiffTrifocalTest : public testing::Test{ - public: - Vector3s pos1, pos2, pos3, pos_cam, point; - Vector3s euler1, euler2, euler3, euler_cam; - Quaternions quat1, quat2, quat3, quat_cam; - Vector4s vquat1, vquat2, vquat3, vquat_cam; // quaternions as vectors - Vector7s pose1, pose2, pose3, pose_cam; - - ProblemPtr problem; - CeresManagerPtr ceres_manager; - - SensorCameraPtr camera; - ProcessorTrackerFeatureTrifocalPtr proc_trifocal; - - SensorBasePtr S; - FrameBasePtr F1, F2, F3; - CaptureImagePtr I1, I2, I3; - FeatureBasePtr f1, f2, f3; - FactorAutodiffTrifocalPtr c123; - - virtual void SetUp() override - { - std::string wolf_root = _WOLF_ROOT_DIR; - - // configuration - /* - * We have three robot poses, in three frames, with cameras C1, C2, C3 - * looking towards the origin of coordinates: - * - * Z - * | - * ________ C3 - * / | / - * --------- /| C2 - * | / | - * |____________/_ | ___ Y - * / | / - * / | / - * --------- C1 |/ - * | / | - * --------- - * / - * Y - * - * Each robot pose is at one axis, facing the origin: - * F1: pos = (1,0,0), ori = (0,0,pi) - * F2: pos = (0,1,0), ori = (0,0,-pi/2) - * F3: pos = (0,0,1), ori = (0,pi/2,pi) - * - * The robot has a camera looking forward - * S: pos = (0,0,0), ori = (-pi/1, 0, -pi/1) - * - * 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 on each camera, - * creating three features: - * f1: p1 = (0,0) - * f2: p2 = (0,0) - * f3: p3 = (0,0) - * - * We form a Wolf tree with three frames, three captures, - * three features, and one trifocal factor: - * - * Frame F1, Capture C1, feature f1 - * Frame F2, Capture C2, feature f2 - * Frame F3, Capture C3, feature f3, factor c123 - * - * The three frame poses F1, F2, F3 and the camera pose S - * in the robot frame are variables subject to optimization - * - * We perform a number of tests based on this configuration. - */ - - // all frames look to the origin - pos1 << 1, 0, 0; - pos2 << 0, 1, 0; - pos3 << 0, 0, 1; - euler1 << 0, 0 , M_PI ; - euler2 << 0, 0 , -M_PI_2 ; - euler3 << 0, M_PI_2, M_PI ; - quat1 = e2q(euler1); - quat2 = e2q(euler2); - quat3 = e2q(euler3); - vquat1 = quat1.coeffs(); - vquat2 = quat2.coeffs(); - vquat3 = quat3.coeffs(); - pose1 << pos1, vquat1; - pose2 << pos2, vquat2; - pose3 << pos3, vquat3; - - // camera at the robot origin looking forward - pos_cam << 0, 0, 0; - euler_cam << -M_PI_2, 0, -M_PI_2;// euler_cam *= M_TORAD; - quat_cam = e2q(euler_cam); - vquat_cam = quat_cam.coeffs(); - pose_cam << pos_cam, vquat_cam; - - // Build problem - problem = Problem::create("PO 3D"); - ceres::Solver::Options options; - ceres_manager = std::make_shared<CeresManager>(problem, options); - - // Install sensor and processor - S = problem->installSensor("CAMERA", "canonical", pose_cam, wolf_root + "/src/examples/camera_params_canonical.yaml"); - camera = std::static_pointer_cast<SensorCamera>(S); - - ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>(); - params_tracker_feature_trifocal_trifocal->time_tolerance = 1.0/2; - params_tracker_feature_trifocal_trifocal->max_new_features = 5; - params_tracker_feature_trifocal_trifocal->min_features_for_keyframe = 5; - params_tracker_feature_trifocal_trifocal->yaml_file_params_vision_utils = wolf_root + "/src/examples/vision_utils_active_search.yaml"; - - ProcessorBasePtr proc = problem->installProcessor("TRACKER FEATURE TRIFOCAL", "trifocal", camera, params_tracker_feature_trifocal_trifocal); - proc_trifocal = std::static_pointer_cast<ProcessorTrackerFeatureTrifocal>(proc); - - // Add three viewpoints with frame, capture and feature - Vector2s pix(0,0); - Matrix2s pix_cov; pix_cov.setIdentity(); - - F1 = problem->emplaceFrame(KEY_FRAME, pose1, 1.0); - I1 = std::make_shared<CaptureImage>(1.0, camera, cv::Mat(2,2,CV_8UC1)); - F1-> addCapture(I1); - f1 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin - I1-> addFeature(f1); - - F2 = problem->emplaceFrame(KEY_FRAME, pose2, 2.0); - I2 = std::make_shared<CaptureImage>(2.0, camera, cv::Mat(2,2,CV_8UC1)); - F2-> addCapture(I2); - f2 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin - I2-> addFeature(f2); - - F3 = problem->emplaceFrame(KEY_FRAME, pose3, 3.0); - I3 = std::make_shared<CaptureImage>(3.0, camera, cv::Mat(2,2,CV_8UC1)); - F3-> addCapture(I3); - f3 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin - I3-> addFeature(f3); - - // trifocal factor - c123 = std::make_shared<FactorAutodiffTrifocal>(f1, f2, f3, proc_trifocal, false, FAC_ACTIVE); - f3 ->addFactor (c123); - f1 ->addConstrainedBy(c123); - f2 ->addConstrainedBy(c123); - } -}; - -TEST_F(FactorAutodiffTrifocalTest, expectation) -{ - // Homogeneous transform C2 wrt C1 - Matrix4s _c1Hc2; _c1Hc2 << - 0, 0, -1, 1, - 0, 1, 0, 0, - 1, 0, 0, 1, - 0, 0, 0, 1; - - // rotation and translation - Matrix3s _c1Rc2 = _c1Hc2.block(0,0,3,3); - Vector3s _c1Tc2 = _c1Hc2.block(0,3,3,1); - - // Essential matrix, ground truth (fwd and bkwd) - Matrix3s _c1Ec2 = wolf::skew(_c1Tc2) * _c1Rc2; - Matrix3s _c2Ec1 = _c1Ec2.transpose(); - - // Expected values - vision_utils::TrifocalTensor tensor; - Matrix3s c2Ec1; - c123->expectation(pos1, quat1, pos2, quat2, pos3, quat3, pos_cam, quat_cam, tensor, c2Ec1); - - // check trilinearities - - // Elements computed using the tensor - Matrix3s T0, T1, T2; - tensor.getLayers(T0,T1,T2); - Vector3s _m1 (0,0,1), - _m2 (0,0,1), - _m3 (0,0,1); // ground truth - Matrix3s m1Tt = _m1(0)*T0.transpose() - + _m1(1)*T1.transpose() - + _m1(2)*T2.transpose(); - - // Projective line: l = (nx ny dn), with (nx ny): normal vector; dn: distance to origin times norm(nx,ny) - Vector3s _l2(0,1,0), - _p2(1,0,0), - _p3(0,1,0); // ground truth - Vector3s l2; - l2 = c2Ec1 * _m1; - - // check epipolar lines (check only director vectors for equal direction) - ASSERT_MATRIX_APPROX(l2/l2(1), _l2/_l2(1), 1e-8); - - // check perpendicular lines (check only director vectors for orthogonal direction) - ASSERT_NEAR(l2(0)*_p2(0) + l2(1)*_p2(1), 0, 1e-8); - - // Verify trilinearities - - // Point-line-line - Matrix1s pll = _p3.transpose() * m1Tt * _p2; - ASSERT_TRUE(pll(0)<1e-5); - - // Point-line-point - Vector3s plp = wolf::skew(_m3) * m1Tt * _p2; - ASSERT_MATRIX_APPROX(plp, Vector3s::Zero(), 1e-8); - - // Point-point-line - Vector3s ppl = _p3.transpose() * m1Tt * wolf::skew(_m2); - ASSERT_MATRIX_APPROX(ppl, Vector3s::Zero(), 1e-8); - - // Point-point-point - Matrix3s ppp = wolf::skew(_m3) * m1Tt * wolf::skew(_m2); - ASSERT_MATRIX_APPROX(ppp, Matrix3s::Zero(), 1e-8); - - // check epipolars - ASSERT_MATRIX_APPROX(c2Ec1/c2Ec1(0,1), _c2Ec1/_c2Ec1(0,1), 1e-8); -} - -TEST_F(FactorAutodiffTrifocalTest, residual) -{ - vision_utils::TrifocalTensor tensor; - Matrix3s c2Ec1; - Vector3s residual; - - // Nominal values - c123->expectation(pos1, quat1, pos2, quat2, pos3, quat3, pos_cam, quat_cam, tensor, c2Ec1); - residual = c123->residual(tensor, c2Ec1); - - ASSERT_MATRIX_APPROX(residual, Vector3s::Zero(), 1e-8); -} - -TEST_F(FactorAutodiffTrifocalTest, error_jacobians) -{ - vision_utils::TrifocalTensor tensor; - Matrix3s c2Ec1; - Vector3s residual, residual_pert; - Vector3s pix0, pert, pix_pert; - Scalar epsilon = 1e-8; - - // Nominal values - c123->expectation(pos1, quat1, pos2, quat2, pos3, quat3, pos_cam, quat_cam, tensor, c2Ec1); - residual = c123->residual(tensor, c2Ec1); - - Matrix<Scalar, 3, 3> J_e_m1, J_e_m2, J_e_m3, J_r_m1, J_r_m2, J_r_m3; - c123->error_jacobians(tensor, c2Ec1, J_e_m1, J_e_m2, J_e_m3); - J_r_m1 = c123->getSqrtInformationUpper() * J_e_m1; - J_r_m2 = c123->getSqrtInformationUpper() * J_e_m2; - J_r_m3 = c123->getSqrtInformationUpper() * J_e_m3; - - // numerical jacs - Matrix<Scalar,3,3> Jn_r_m1, Jn_r_m2, Jn_r_m3; - - // jacs wrt m1 - pix0 = c123->getPixelCanonicalPrev(); - for (int i=0; i<3; i++) - { - pert.setZero(); - pert(i) = epsilon; - pix_pert = pix0 + pert; - c123->setPixelCanonicalPrev(pix_pert); // m1 - residual_pert = c123->residual(tensor, c2Ec1); - - Jn_r_m1.col(i) = (residual_pert - residual) / epsilon; - } - c123->setPixelCanonicalPrev(pix0); - - ASSERT_MATRIX_APPROX(J_r_m1, Jn_r_m1, 1e-6); - - // jacs wrt m2 - pix0 = c123->getPixelCanonicalOrigin(); - for (int i=0; i<3; i++) - { - pert.setZero(); - pert(i) = epsilon; - pix_pert = pix0 + pert; - c123->setPixelCanonicalOrigin(pix_pert); // m2 - residual_pert = c123->residual(tensor, c2Ec1); - - Jn_r_m2.col(i) = (residual_pert - residual) / epsilon; - } - c123->setPixelCanonicalOrigin(pix0); - - ASSERT_MATRIX_APPROX(J_r_m2, Jn_r_m2, 1e-6); - - // jacs wrt m3 - pix0 = c123->getPixelCanonicalLast(); - for (int i=0; i<3; i++) - { - pert.setZero(); - pert(i) = epsilon; - pix_pert = pix0 + pert; - c123->setPixelCanonicalLast(pix_pert); // m3 - residual_pert = c123->residual(tensor, c2Ec1); - - Jn_r_m3.col(i) = (residual_pert - residual) / epsilon; - } - c123->setPixelCanonicalLast(pix0); - - ASSERT_MATRIX_APPROX(J_r_m3, Jn_r_m3, 1e-6); - -} - -TEST_F(FactorAutodiffTrifocalTest, operator_parenthesis) -{ - Vector3s res; - - // Factor with exact states should give zero residual - c123->operator ()(pos1.data(), vquat1.data(), - pos2.data(), vquat2.data(), - pos3.data(), vquat3.data(), - pos_cam.data(), vquat_cam.data(), - res.data()); - - ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); -} - -TEST_F(FactorAutodiffTrifocalTest, solve_F1) -{ - F1->setState(pose1); - F2->setState(pose2); - F3->setState(pose3); - S ->getP()->setState(pos_cam); - S ->getO()->setState(vquat_cam); - // Residual with prior - - Vector3s res; - - Eigen::VectorXs F1_p = F1->getP()->getState(); - Eigen::VectorXs F1_o = F1->getO()->getState(); - Eigen::VectorXs F2_p = F2->getP()->getState(); - Eigen::VectorXs F2_o = F2->getO()->getState(); - Eigen::VectorXs F3_p = F3->getP()->getState(); - Eigen::VectorXs F3_o = F3->getO()->getState(); - Eigen::VectorXs S_p = S ->getP()->getState(); - Eigen::VectorXs S_o = S ->getO()->getState(); - - c123->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - WOLF_DEBUG("Initial state: ", F1->getState().transpose()); - WOLF_DEBUG("residual before perturbing: ", res.transpose()); - ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); - - // Residual with perturbated state - - Vector7s pose_perturbated = F1->getState() + 0.1 * Vector7s::Random(); - pose_perturbated.segment(3,4).normalize(); - F1->setState(pose_perturbated); - - F1_p = F1->getP()->getState(); - F1_o = F1->getO()->getState(); - - c123->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose()); - WOLF_DEBUG("residual before solve: ", res.transpose()); - - // Residual with solved state - - S ->fixExtrinsics(); - F1->unfix(); - F2->fix(); - F3->fix(); - - std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); - - F1_p = F1->getP()->getState(); - F1_o = F1->getO()->getState(); - F2_p = F2->getP()->getState(); - F2_o = F2->getO()->getState(); - F3_p = F3->getP()->getState(); - F3_o = F3->getO()->getState(); - S_p = S ->getP()->getState(); - S_o = S ->getO()->getState(); - - c123->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - WOLF_DEBUG("solved state: ", F1->getState().transpose()); - WOLF_DEBUG("residual after solve: ", res.transpose()); - - WOLF_DEBUG(report, " AND UNION"); - - ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); - -} - -TEST_F(FactorAutodiffTrifocalTest, solve_F2) -{ - F1->setState(pose1); - F2->setState(pose2); - F3->setState(pose3); - S ->getP()->setState(pos_cam); - S ->getO()->setState(vquat_cam); - - // Residual with prior - - Vector3s res; - - Eigen::VectorXs F1_p = F1->getP()->getState(); - Eigen::VectorXs F1_o = F1->getO()->getState(); - Eigen::VectorXs F2_p = F2->getP()->getState(); - Eigen::VectorXs F2_o = F2->getO()->getState(); - Eigen::VectorXs F3_p = F3->getP()->getState(); - Eigen::VectorXs F3_o = F3->getO()->getState(); - Eigen::VectorXs S_p = S ->getP()->getState(); - Eigen::VectorXs S_o = S ->getO()->getState(); - - c123->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - WOLF_DEBUG("Initial state: ", F2->getState().transpose()); - WOLF_DEBUG("residual before perturbing: ", res.transpose()); - ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); - - // Residual with perturbated state - - Vector7s pose_perturbated = F2->getState() + 0.1 * Vector7s::Random(); - pose_perturbated.segment(3,4).normalize(); - F2->setState(pose_perturbated); - - F2_p = F2->getP()->getState(); - F2_o = F2->getO()->getState(); - - c123->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose()); - WOLF_DEBUG("residual before solve: ", res.transpose()); - - // Residual with solved state - - S ->fixExtrinsics(); - F1->fix(); - F2->unfix(); - F3->fix(); - - std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); - - F1_p = F1->getP()->getState(); - F1_o = F1->getO()->getState(); - F2_p = F2->getP()->getState(); - F2_o = F2->getO()->getState(); - F3_p = F3->getP()->getState(); - F3_o = F3->getO()->getState(); - S_p = S ->getP()->getState(); - S_o = S ->getO()->getState(); - - c123->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - WOLF_DEBUG("solved state: ", F2->getState().transpose()); - WOLF_DEBUG("residual after solve: ", res.transpose()); - - WOLF_DEBUG(report, " AND UNION"); - - ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); - -} - -TEST_F(FactorAutodiffTrifocalTest, solve_F3) -{ - F1->setState(pose1); - F2->setState(pose2); - F3->setState(pose3); - S ->getP()->setState(pos_cam); - S ->getO()->setState(vquat_cam); - - // Residual with prior - - Vector3s res; - - Eigen::VectorXs F1_p = F1->getP()->getState(); - Eigen::VectorXs F1_o = F1->getO()->getState(); - Eigen::VectorXs F2_p = F2->getP()->getState(); - Eigen::VectorXs F2_o = F2->getO()->getState(); - Eigen::VectorXs F3_p = F3->getP()->getState(); - Eigen::VectorXs F3_o = F3->getO()->getState(); - Eigen::VectorXs S_p = S ->getP()->getState(); - Eigen::VectorXs S_o = S ->getO()->getState(); - - c123->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - WOLF_DEBUG("Initial state: ", F3->getState().transpose()); - WOLF_DEBUG("residual before perturbing: ", res.transpose()); - ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); - - // Residual with perturbated state - - Vector7s pose_perturbated = F3->getState() + 0.1 * Vector7s::Random(); - pose_perturbated.segment(3,4).normalize(); - F3->setState(pose_perturbated); - - F3_p = F3->getP()->getState(); - F3_o = F3->getO()->getState(); - - c123->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose()); - WOLF_DEBUG("residual before solve: ", res.transpose()); - ASSERT_NEAR(res(2), 0, 1e-8); // Epipolar c2-c1 should be respected when perturbing F3 - - // Residual with solved state - - S ->fixExtrinsics(); - F1->fix(); - F2->fix(); - F3->unfix(); - - std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); - - F1_p = F1->getP()->getState(); - F1_o = F1->getO()->getState(); - F2_p = F2->getP()->getState(); - F2_o = F2->getO()->getState(); - F3_p = F3->getP()->getState(); - F3_o = F3->getO()->getState(); - S_p = S ->getP()->getState(); - S_o = S ->getO()->getState(); - - c123->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - WOLF_DEBUG("solved state: ", F3->getState().transpose()); - WOLF_DEBUG("residual after solve: ", res.transpose()); - - WOLF_DEBUG(report, " AND UNION"); - - ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); - -} - -TEST_F(FactorAutodiffTrifocalTest, solve_S) -{ - F1->setState(pose1); - F2->setState(pose2); - F3->setState(pose3); - S ->getP()->setState(pos_cam); - S ->getO()->setState(vquat_cam); - - // Residual with prior - - Vector3s res; - - Eigen::VectorXs F1_p = F1->getP()->getState(); - Eigen::VectorXs F1_o = F1->getO()->getState(); - Eigen::VectorXs F2_p = F2->getP()->getState(); - Eigen::VectorXs F2_o = F2->getO()->getState(); - Eigen::VectorXs F3_p = F3->getP()->getState(); - Eigen::VectorXs F3_o = F3->getO()->getState(); - Eigen::VectorXs S_p = S ->getP()->getState(); - Eigen::VectorXs S_o = S ->getO()->getState(); - - c123->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - WOLF_DEBUG("Initial state: ", S->getP()->getState().transpose(), " ", S->getO()->getState().transpose()); - WOLF_DEBUG("residual before perturbing: ", res.transpose()); - ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); - - // Residual with perturbated state - - Vector3s pos_perturbated = pos_cam + 0.1 * Vector3s::Random(); - Vector4s ori_perturbated = vquat_cam + 0.1 * Vector4s::Random(); - ori_perturbated.normalize(); - Vector7s pose_perturbated; pose_perturbated << pos_perturbated, ori_perturbated; - S->getP()->setState(pos_perturbated); - S->getO()->setState(ori_perturbated); - - S_p = S->getP()->getState(); - S_o = S->getO()->getState(); - - c123->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose()); - WOLF_DEBUG("residual before solve: ", res.transpose()); - - // Residual with solved state - - S ->unfixExtrinsics(); - F1->fix(); - F2->fix(); - F3->fix(); - - std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); - - F1_p = F1->getP()->getState(); - F1_o = F1->getO()->getState(); - F2_p = F2->getP()->getState(); - F2_o = F2->getO()->getState(); - F3_p = F3->getP()->getState(); - F3_o = F3->getO()->getState(); - S_p = S ->getP()->getState(); - S_o = S ->getO()->getState(); - - c123->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - WOLF_DEBUG("solved state: ", S->getP()->getState().transpose(), " ", S->getO()->getState().transpose()); - WOLF_DEBUG("residual after solve: ", res.transpose()); - - WOLF_DEBUG(report, " AND UNION"); - - ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); - -} - -class FactorAutodiffTrifocalMultiPointTest : public FactorAutodiffTrifocalTest -{ - /* - * In this test class we add 8 more points and perform optimization on the camera frames. - * - * C1 is not optimized as it acts as the reference - * S is not optimized either as observability is compromised - * C2.pos is fixed to set the unobservable scale - * C2.ori and all C3 are optimized - * - */ - - public: - std::vector<FeatureBasePtr> fv1, fv2, fv3; - std::vector<FactorAutodiffTrifocalPtr> cv123; - - virtual void SetUp() override - { - FactorAutodiffTrifocalTest::SetUp(); - - Matrix<Scalar, 2, 9> c1p_can; - c1p_can << - 0, -1/3.00, -1/3.00, 1/3.00, 1/3.00, -1.0000, -1.0000, 1.0000, 1.0000, - 0, 1/3.00, -1/3.00, 1/3.00, -1/3.00, 1.0000, -1.0000, 1.0000, -1.0000; - - Matrix<Scalar, 2, 9> c2p_can; - c2p_can << - 0, 1/3.00, 1/3.00, 1.0000, 1.0000, -1/3.00, -1/3.00, -1.0000, -1.0000, - 0, 1/3.00, -1/3.00, 1.0000, -1.0000, 1/3.00, -1/3.00, 1.0000, -1.0000; - - Matrix<Scalar, 2, 9> c3p_can; - c3p_can << - 0, -1/3.00, -1.0000, 1/3.00, 1.0000, -1/3.00, -1.0000, 1/3.00, 1.0000, - 0, -1/3.00, -1.0000, -1/3.00, -1.0000, 1/3.00, 1.0000, 1/3.00, 1.0000; - - Matrix2s pix_cov; pix_cov.setIdentity(); //pix_cov *= 1e-4; - - // for i==0 we already have them - fv1.push_back(f1); - fv2.push_back(f2); - fv3.push_back(f3); - cv123.push_back(c123); - for (size_t i=1; i<c1p_can.cols() ; i++) - { - fv1.push_back(std::make_shared<FeatureBase>("PIXEL", c1p_can.col(i), pix_cov)); - I1->addFeature(fv1.at(i)); - - fv2.push_back(std::make_shared<FeatureBase>("PIXEL", c2p_can.col(i), pix_cov)); - I2->addFeature(fv2.at(i)); - - fv3.push_back(std::make_shared<FeatureBase>("PIXEL", c3p_can.col(i), pix_cov)); - I3->addFeature(fv3.at(i)); - - cv123.push_back(std::make_shared<FactorAutodiffTrifocal>(fv1.at(i), fv2.at(i), fv3.at(i), proc_trifocal, false, FAC_ACTIVE)); - fv3.at(i)->addFactor(cv123.at(i)); - fv1.at(i)->addConstrainedBy(cv123.at(i)); - fv2.at(i)->addConstrainedBy(cv123.at(i)); - } - - } - -}; - -TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point) -{ - /* - * In this test we add 8 more points and perform optimization on the camera frames. - * - * C1 is not optimized as it acts as the reference - * S is not optimized either as observability is compromised - * C2.pos is fixed to set the unobservable scale - * C2.ori and all C3 are optimized - * - */ - - S ->getP()->fix(); // do not calibrate sensor pos - S ->getO()->fix(); // do not calibrate sensor ori - F1->getP()->fix(); // Cam 1 acts as reference - F1->getO()->fix(); // Cam 1 acts as reference - F2->getP()->fix(); // This fixes the scale - F2->getO()->unfix(); // Estimate Cam 2 ori - F3->getP()->unfix(); // Estimate Cam 3 pos - F3->getO()->unfix(); // Estimate Cam 3 ori - - // Perturbate states, keep scale - F1->getP()->setState( pos1 ); - F1->getO()->setState( vquat1 ); - F2->getP()->setState( pos2 ); // this fixes the scale - F2->getO()->setState((vquat2 + 0.2*Vector4s::Random()).normalized()); - F3->getP()->setState( pos3 + 0.2*Vector3s::Random()); - F3->getO()->setState((vquat3 + 0.2*Vector4s::Random()).normalized()); - - std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); - - // Print results - WOLF_DEBUG("report: ", report); - problem->print(1,0,1,0); - - // Evaluate final states - ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2 , 1e-10); - ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-10); - ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3 , 1e-10); - ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-10); - - Eigen::VectorXs F1_p = F1->getP()->getState(); - Eigen::VectorXs F1_o = F1->getO()->getState(); - Eigen::VectorXs F2_p = F2->getP()->getState(); - Eigen::VectorXs F2_o = F2->getO()->getState(); - Eigen::VectorXs F3_p = F3->getP()->getState(); - Eigen::VectorXs F3_o = F3->getO()->getState(); - Eigen::VectorXs S_p = S ->getP()->getState(); - Eigen::VectorXs S_o = S ->getO()->getState(); - - // evaluate residuals - Vector3s res; - for (size_t i=0; i<cv123.size(); i++) - { - cv123.at(i)->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-10); - } - -} - -TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_scale) -{ - /* - * In this test we add 8 more points and perform optimization on the camera frames. - * - * C1 is not optimized as it acts as the reference - * S is not optimized either as observability is compromised - * C2.pos is fixed to set the unobservable scale - * C2.ori and all C3 are optimized - * - */ - - S ->getP()->fix(); // do not calibrate sensor pos - S ->getO()->fix(); // do not calibrate sensor ori - F1->getP()->fix(); // Cam 1 acts as reference - F1->getO()->fix(); // Cam 1 acts as reference - F2->getP()->fix(); // This fixes the scale - F2->getO()->unfix(); // Estimate Cam 2 ori - F3->getP()->unfix(); // Estimate Cam 3 pos - F3->getO()->unfix(); // Estimate Cam 3 ori - - // Perturbate states, change scale - F1->getP()->setState( 2 * pos1 ); - F1->getO()->setState( vquat1 ); - F2->getP()->setState( 2 * pos2 ); - F2->getO()->setState(( vquat2 + 0.2*Vector4s::Random()).normalized()); - F3->getP()->setState( 2 * pos3 + 0.2*Vector3s::Random()); - F3->getO()->setState(( vquat3 + 0.2*Vector4s::Random()).normalized()); - - std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); - - // Print results - WOLF_DEBUG("report: ", report); - problem->print(1,0,1,0); - - // Evaluate final states - ASSERT_MATRIX_APPROX(F2->getP()->getState(), 2 * pos2, 1e-8); - ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-8); - ASSERT_MATRIX_APPROX(F3->getP()->getState(), 2 * pos3, 1e-8); - ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-8); - - Eigen::VectorXs F1_p = F1->getP()->getState(); - Eigen::VectorXs F1_o = F1->getO()->getState(); - Eigen::VectorXs F2_p = F2->getP()->getState(); - Eigen::VectorXs F2_o = F2->getO()->getState(); - Eigen::VectorXs F3_p = F3->getP()->getState(); - Eigen::VectorXs F3_o = F3->getO()->getState(); - Eigen::VectorXs S_p = S ->getP()->getState(); - Eigen::VectorXs S_o = S ->getO()->getState(); - - // evaluate residuals - Vector3s res; - for (size_t i=0; i<cv123.size(); i++) - { - cv123.at(i)->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); - } -} - -#include "core/factor/factor_autodiff_distance_3D.h" - -TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_distance) -{ - /* - * In this test we add 8 more points and perform optimization on the camera frames. - * - * C1 is not optimized as it acts as the reference - * S is not optimized either as observability is compromised - * C2 and C3 are optimized - * The scale is observed through a distance factor - * - */ - - S ->getP()->fix(); // do not calibrate sensor pos - S ->getO()->fix(); // do not calibrate sensor ori - F1->getP()->fix(); // Cam 1 acts as reference - F1->getO()->fix(); // Cam 1 acts as reference - F2->getP()->unfix(); // Estimate Cam 2 pos - F2->getO()->unfix(); // Estimate Cam 2 ori - F3->getP()->unfix(); // Estimate Cam 3 pos - F3->getO()->unfix(); // Estimate Cam 3 ori - - // Perturbate states, change scale - F1->getP()->setState( pos1 ); - F1->getO()->setState( vquat1 ); - F2->getP()->setState( pos2 + 0.2*Vector3s::Random() ); - F2->getO()->setState((vquat2 + 0.2*Vector4s::Random()).normalized()); - F3->getP()->setState( pos3 + 0.2*Vector3s::Random()); - F3->getO()->setState((vquat3 + 0.2*Vector4s::Random()).normalized()); - - // Add a distance factor to fix the scale - Scalar distance = sqrt(2.0); - Scalar distance_std = 0.1; - - CaptureBasePtr Cd = std::make_shared<CaptureBase>("DISTANCE", F3->getTimeStamp()); - F3->addCapture(Cd); - FeatureBasePtr fd = std::make_shared<FeatureBase>("DISTANCE", Vector1s(distance), Matrix1s(distance_std * distance_std)); - Cd->addFeature(fd); - FactorAutodiffDistance3DPtr cd = std::make_shared<FactorAutodiffDistance3D>(fd, F1, nullptr, false, FAC_ACTIVE); - fd->addFactor(cd); - F1->addConstrainedBy(cd); - - cd->setStatus(FAC_INACTIVE); - std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); - WOLF_DEBUG("DISTANCE CONSTRAINT INACTIVE: \n", report); - - problem->print(1,0,1,0); - - cd->setStatus(FAC_ACTIVE); - report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); - - // Print results - WOLF_DEBUG("DISTANCE CONSTRAINT ACTIVE: \n", report); - problem->print(1,0,1,0); - - // Evaluate final states - ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2 , 1e-8); - ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-8); - ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3 , 1e-8); - ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-8); - - Eigen::VectorXs F1_p = F1->getP()->getState(); - Eigen::VectorXs F1_o = F1->getO()->getState(); - Eigen::VectorXs F2_p = F2->getP()->getState(); - Eigen::VectorXs F2_o = F2->getO()->getState(); - Eigen::VectorXs F3_p = F3->getP()->getState(); - Eigen::VectorXs F3_o = F3->getO()->getState(); - Eigen::VectorXs S_p = S ->getP()->getState(); - Eigen::VectorXs S_o = S ->getO()->getState(); - - // evaluate residuals - Vector3s res; - for (size_t i=0; i<cv123.size(); i++) - { - cv123.at(i)->operator ()(F1_p.data(), F1_o.data(), - F2_p.data(), F2_o.data(), - F3_p.data(), F3_o.data(), - S_p. data(), S_o. data(), - res.data()); - - ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); - } -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F1"; - // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F2"; - // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F3"; - // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_S"; - // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_multi_point"; - // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalMultiPointTest.solve_multi_point_distance"; - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp deleted file mode 100644 index bbebe93fb30256652e00168b0418dccedc2de7ad..0000000000000000000000000000000000000000 --- a/test/gtest_factor_odom_3D.cpp +++ /dev/null @@ -1,93 +0,0 @@ -/** - * \file gtest_factor_odom_3D.cpp - * - * Created on: Mar 30, 2017 - * \author: jsola - */ - -#include "utils_gtest.h" - -#include "core/factor/factor_odom_3D.h" -#include "core/capture/capture_motion.h" - -#include "core/ceres_wrapper/ceres_manager.h" - -using namespace Eigen; -using namespace wolf; -using std::cout; -using std::endl; - -Vector7s data2delta(Vector6s _data) -{ - return (Vector7s() << _data.head<3>() , v2q(_data.tail<3>()).coeffs()).finished(); -} - -// Input odometry data and covariance -Vector6s data(Vector6s::Random()); -Vector7s delta = data2delta(data); -Vector6s data_var((Vector6s() << 0.2,0.2,0.2,0.2,0.2,0.2).finished()); -Matrix6s data_cov = data_var.asDiagonal(); - -// perturbated priors -Vector7s x0 = data2delta(Vector6s::Random()*0.25); -Vector7s x1 = data2delta(data + Vector6s::Random()*0.25); - -// Problem and solver -ProblemPtr problem_ptr = Problem::create("PO 3D"); -CeresManager ceres_mgr(problem_ptr); - -// Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0)); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY_FRAME, delta, TimeStamp(1)); - -// Capture, feature and factor from frm1 to frm0 -CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr)); -FeatureBasePtr fea1 = cap1->addFeature(std::make_shared<FeatureBase>("ODOM 3D", delta, data_cov)); -FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(fea1->addFactor(std::make_shared<FactorOdom3D>(fea1, frm0, nullptr))); // create and add -FactorBasePtr dummy = frm0->addConstrainedBy(ctr1); - -//////////////////////////////////////////////////////// - -TEST(FactorOdom3D, check_tree) -{ - ASSERT_TRUE(problem_ptr->check(0)); -} - -TEST(FactorOdom3D, expectation) -{ - ASSERT_MATRIX_APPROX(ctr1->expectation() , delta, 1e-6); -} - -TEST(FactorOdom3D, fix_0_solve) -{ - - // Fix frame 0, perturb frm1 - frm0->fix(); - frm1->unfix(); - frm1->setState(x1); - - // solve for frm1 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); - - ASSERT_MATRIX_APPROX(frm1->getState(), delta, 1e-6); - -} - -TEST(FactorOdom3D, fix_1_solve) -{ - // Fix frame 1, perturb frm0 - frm0->unfix(); - frm1->fix(); - frm0->setState(x0); - - // solve for frm0 - std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); - - ASSERT_MATRIX_APPROX(frm0->getState(), problem_ptr->zeroState(), 1e-6); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp deleted file mode 100644 index 0bc4ced9efbcd410c7c8f52d931ce0b1c9d06aa2..0000000000000000000000000000000000000000 --- a/test/gtest_factor_pose_2D.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/** - * \file gtest_factor_pose_2D.cpp - * - * Created on: Mar 30, 2017 - * \author: jsola - */ - -#include "core/factor/factor_pose_2D.h" -#include "utils_gtest.h" - -#include "core/capture/capture_motion.h" - -#include "core/ceres_wrapper/ceres_manager.h" - -using namespace Eigen; -using namespace wolf; -using std::cout; -using std::endl; - -// Input data and covariance -Vector3s pose(Vector3s::Random()); -Vector3s data_var((Vector3s() << 0.2,0.2,0.2).finished()); -Matrix3s data_cov = data_var.asDiagonal(); - -// perturbated priors -Vector3s x0 = pose + Vector3s::Random()*0.25; - -// Problem and solver -ProblemPtr problem = Problem::create("PO 2D"); -CeresManager ceres_mgr(problem); - -// Two frames -FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0)); - -// Capture, feature and factor from frm1 to frm0 -CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 2D", 0, nullptr, pose, 3, 3, nullptr)); -FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov)); -FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(fea0->addFactor(std::make_shared<FactorPose2D>(fea0))); - -//////////////////////////////////////////////////////// - -TEST(FactorPose2D, check_tree) -{ - ASSERT_TRUE(problem->check(0)); -} - -//TEST(FactorFix, expectation) -//{ -// ASSERT_EIGEN_APPROX(ctr0->expectation() , delta); -//} - -TEST(FactorPose2D, solve) -{ - - // Fix frame 0, perturb frm1 - frm0->unfix(); - frm0->setState(x0); - - // solve for frm0 - std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - - ASSERT_MATRIX_APPROX(frm0->getState(), pose, 1e-6); - -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp deleted file mode 100644 index c0ff0c4276f94478e5aeda7272820d0abd94b22a..0000000000000000000000000000000000000000 --- a/test/gtest_factor_pose_3D.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/** - * \file gtest_factor_fix_3D.cpp - * - * Created on: Mar 30, 2017 - * \author: jsola - */ - -#include "core/factor/factor_pose_3D.h" -#include "utils_gtest.h" - -#include "core/capture/capture_motion.h" - -#include "core/ceres_wrapper/ceres_manager.h" - -using namespace Eigen; -using namespace wolf; -using std::cout; -using std::endl; - -Vector7s pose6toPose7(Vector6s _pose6) -{ - return (Vector7s() << _pose6.head<3>() , v2q(_pose6.tail<3>()).coeffs()).finished(); -} - -// Input pose6 and covariance -Vector6s pose(Vector6s::Random()); -Vector7s pose7 = pose6toPose7(pose); -Vector6s data_var((Vector6s() << 0.2,0.2,0.2,0.2,0.2,0.2).finished()); -Matrix6s data_cov = data_var.asDiagonal(); - -// perturbated priors -Vector7s x0 = pose6toPose7(pose + Vector6s::Random()*0.25); - -// Problem and solver -ProblemPtr problem = Problem::create("PO 3D"); -CeresManager ceres_mgr(problem); - -// Two frames -FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0)); - -// Capture, feature and factor -CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr)); -FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov)); -FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(fea0->addFactor(std::make_shared<FactorPose3D>(fea0))); - -//////////////////////////////////////////////////////// - -TEST(FactorPose3D, check_tree) -{ - ASSERT_TRUE(problem->check(0)); -} - -//TEST(FactorFix3D, expectation) -//{ -// ASSERT_EIGEN_APPROX(ctr0->expectation() , delta); -//} - -TEST(FactorPose3D, solve) -{ - - // Fix frame 0, perturb frm1 - frm0->unfix(); - frm0->setState(x0); - - // solve for frm0 - std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::FULL); - - ASSERT_MATRIX_APPROX(frm0->getState(), pose7, 1e-6); - -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_factor_sparse.cpp b/test/gtest_factor_sparse.cpp deleted file mode 100644 index 6c8f212be2030c5d04707eb5b5798848f36d8710..0000000000000000000000000000000000000000 --- a/test/gtest_factor_sparse.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/** - * \file gtest_factor_sparse.cpp - * - * Created on: Apr 25, 2017 - * \author: jsola - */ - -#include "utils_gtest.h" - -#include "factor_sparse.h" - -using namespace wolf; - -// Dummy class for avoiding the pure virtual compilation error -template <JacobianMethod J> -class FactorSparseObject : public FactorSparse<1, 2, 1> -{ - public: - FactorSparseObject(FactorType _tp, bool _apply_loss_function, FactorStatus _status, StateBlockPtr _xy, StateBlockPtr _theta) : - FactorSparse<1, 2, 1>(_tp, _apply_loss_function, _status, _xy, _theta) - { - // - } - virtual ~FactorSparseObject(){} - - virtual JacobianMethod getJacobianMethod() const - { - return J; - } -}; - -TEST(FactorSparseAnalytic, Constructor) -{ - FactorSparseObject<JAC_ANALYTIC> fac_analytic(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)); - ASSERT_EQ(fac_analytic.getJacobianMethod(), JAC_ANALYTIC); - ASSERT_EQ(fac_analytic.getApplyLossFunction(), false); - ASSERT_EQ(fac_analytic.getStatus(), FAC_ACTIVE); - ASSERT_EQ(fac_analytic.getSize(), 1); - - FactorSparseObject<JAC_AUTO> fac_auto(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)); - ASSERT_EQ(fac_auto.getJacobianMethod(), JAC_AUTO); - - FactorSparseObject<JAC_NUMERIC> fac_numeric(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)); - ASSERT_EQ(fac_numeric.getJacobianMethod(), JAC_NUMERIC); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp index 6f9248c6a9cd640afa4503801a42e96d308eb15b..15b39b3fc66d0aa4eb5df1a23c114ed7869145f9 100644 --- a/test/gtest_feature_IMU.cpp +++ b/test/gtest_feature_IMU.cpp @@ -34,7 +34,7 @@ class FeatureIMU_test : public testing::Test using std::static_pointer_cast; // Wolf problem - problem = Problem::create("POV 3D"); + problem = Problem::create("POV", 3); Eigen::VectorXs IMU_extrinsics(7); IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot IntrinsicsIMUPtr sen_imu_params = std::make_shared<IntrinsicsIMU>(); @@ -57,7 +57,7 @@ class FeatureIMU_test : public testing::Test x0 << 0,0,0, 0,0,0,1, 0,0,0; MatrixXs P0; P0.setIdentity(9,9); origin_frame = problem->setPrior(x0, P0, 0.0, 0.01); - + // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.) // give the capture a big covariance, otherwise it will be so small that it won't pass following assertions imu_ptr = std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero()); @@ -77,9 +77,8 @@ class FeatureIMU_test : public testing::Test //create Frame ts = problem->getProcessorMotion()->getBuffer().get().back().ts_; state_vec = problem->getProcessorMotion()->getCurrentState(); - last_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3))); - problem->getTrajectory()->addFrame(last_frame); - + last_frame = problem->emplaceFrame(KEY, state_vec, ts); + //create a feature delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_; delta_preint_cov = problem->getProcessorMotion()->getMotion().delta_integr_cov_ + MatrixXs::Identity(9,9)*1e-08; @@ -99,10 +98,10 @@ class FeatureIMU_test : public testing::Test // code here will be called just after the test completes // ok to through exceptions from here if need be /* - You can do deallocation of resources in TearDown or the destructor routine. - However, if you want exception handling you must do it only in the TearDown code because throwing an exception + You can do deallocation of resources in TearDown or the destructor routine. + However, if you want exception handling you must do it only in the TearDown code because throwing an exception from the destructor results in undefined behavior. - The Google assertion macros may throw exceptions in platforms where they are enabled in future releases. + The Google assertion macros may throw exceptions in platforms where they are enabled in future releases. Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance. */ } diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp deleted file mode 100644 index 3a93ef2cb46396defc8f3b3f541042ab85e4a1a8..0000000000000000000000000000000000000000 --- a/test/gtest_feature_base.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/* - * gtest_feature_base.cpp - * - * Created on: Apr 10, 2017 - * Author: jsola - */ - -#include "core/feature/feature_base.h" - -#include "utils_gtest.h" - -using namespace wolf; -using namespace Eigen; - -TEST(FeatureBase, Constructor) -{ - Vector3s m; m << 1,2,3; - Matrix3s M; M.setRandom(); M = M*M.transpose(); - Matrix3s I = M.inverse(); - Eigen::LLT<Eigen::MatrixXs> llt_of_info(I); // compute the Cholesky decomposition of A - Eigen::MatrixXs U = llt_of_info.matrixU(); - Eigen::MatrixXs L = llt_of_info.matrixL(); - - FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", m, M)); - - ASSERT_MATRIX_APPROX(f->getMeasurement(), m, 1e-6); - ASSERT_MATRIX_APPROX(f->getMeasurementCovariance(), M, 1e-6); - ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(), U.transpose()*U, 1e-6); - ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6); -} - -TEST(FeatureBase, SetMeasurement) -{ - Vector3s m; m << 1,2,3; - Matrix3s M; M.setRandom(); M = M*M.transpose(); - Matrix3s I = M.inverse(); - Eigen::LLT<Eigen::MatrixXs> llt_of_info(I); // compute the Cholesky decomposition of A - Eigen::MatrixXs U = llt_of_info.matrixU(); - Eigen::MatrixXs L = llt_of_info.matrixL(); - - FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3s::Zero(), Matrix3s::Identity())); - - f->setMeasurement(m); - - ASSERT_MATRIX_APPROX(f->getMeasurement(), m, 1e-6); -} - -TEST(FeatureBase, SetMeasurementCovariance) -{ - Vector3s m; m << 1,2,3; - Matrix3s M; M.setRandom(); M = M*M.transpose(); - Matrix3s I = M.inverse(); - Eigen::LLT<Eigen::MatrixXs> llt_of_info(I); // compute the Cholesky decomposition of A - Eigen::MatrixXs U = llt_of_info.matrixU(); - Eigen::MatrixXs L = llt_of_info.matrixL(); - - FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3s::Zero(), Matrix3s::Identity())); - - f->setMeasurementCovariance(M); - - ASSERT_MATRIX_APPROX(f->getMeasurementCovariance(), M, 1e-6); - ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(), U.transpose()*U, 1e-6); - ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp deleted file mode 100644 index 5d468d43b37063baf4d3a38455012192bb669c74..0000000000000000000000000000000000000000 --- a/test/gtest_frame_base.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/* - * gtest_frame_base.cpp - * - * Created on: Nov 15, 2016 - * Author: jsola - */ - -#include "utils_gtest.h" -#include "core/utils/logging.h" - -#include "core/frame/frame_base.h" -#include "core/sensor/sensor_odom_2D.h" -#include "core/processor/processor_odom_2D.h" -#include "core/factor/factor_odom_2D.h" -#include "core/capture/capture_motion.h" - -#include <iostream> - -using namespace Eigen; -using namespace std; -using namespace wolf; - -TEST(FrameBase, GettersAndSetters) -{ - FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - - // getters - ASSERT_EQ(F->id(), (unsigned int) 1); - ASSERT_EQ(F->getTimeStamp(), 1); - TimeStamp t; - F->getTimeStamp(t); - ASSERT_EQ(t, 1); - ASSERT_FALSE(F->isFixed()); - ASSERT_EQ(F->isKey(), false); -} - -TEST(FrameBase, StateBlocks) -{ - FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - - ASSERT_EQ(F->getStateBlockVec().size(), (unsigned int) 3); - ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2); - ASSERT_EQ(F->getO()->getState().size(),(unsigned int) 1); - ASSERT_EQ(F->getV(), nullptr); -} - -TEST(FrameBase, LinksBasic) -{ - FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - - ASSERT_FALSE(F->getTrajectory()); - ASSERT_FALSE(F->getProblem()); - // ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert() - // ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected - SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), IntrinsicsOdom2D()); - ASSERT_FALSE(F->getCaptureOf(S)); - ASSERT_TRUE(F->getCaptureList().empty()); - ASSERT_TRUE(F->getConstrainedByList().empty()); - ASSERT_EQ(F->getHits() , (unsigned int) 0); -} - -TEST(FrameBase, LinksToTree) -{ - // Problem with 2 frames and one motion factor between them - ProblemPtr P = Problem::create("PO 2D"); - TrajectoryBasePtr T = P->getTrajectory(); - IntrinsicsOdom2D intrinsics_odo; - intrinsics_odo.k_disp_to_disp = 1; - intrinsics_odo.k_rot_to_rot = 1; - SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); - P->getHardware()->addSensor(S); - FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - T->addFrame(F1); - FrameBasePtr F2 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - T->addFrame(F2); - CaptureMotionPtr C = make_shared<CaptureMotion>("MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr); - F1->addCapture(C); - /// @todo link sensor & proccessor - ProcessorBasePtr p = std::make_shared<ProcessorOdom2D>(make_shared<ProcessorParamsOdom2D>()); - FeatureBasePtr f = make_shared<FeatureBase>("f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01); - C->addFeature(f); - FactorOdom2DPtr c = make_shared<FactorOdom2D>(f, F2, p); - f->addFactor(c); - - // c-by link F2 -> c not yet established - ASSERT_TRUE(F2->getConstrainedByList().empty()); - - // tree is inconsistent since we are missing the constrained_by link - ASSERT_FALSE(P->check(0)); - - // establish constrained_by link F2 -> c - F2->addConstrainedBy(c); - - // tree is now consistent - ASSERT_TRUE(P->check(0)); - - // F1 has one capture and no factors-by - ASSERT_FALSE(F1->getCaptureList().empty()); - ASSERT_TRUE(F1->getConstrainedByList().empty()); - ASSERT_EQ(F1->getHits() , (unsigned int) 0); - - // F2 has no capture and one factor-by - ASSERT_TRUE(F2->getCaptureList().empty()); - ASSERT_FALSE(F2->getConstrainedByList().empty()); - ASSERT_EQ(F2->getHits() , (unsigned int) 1); - - // fix and unfix - F1->fix(); - ASSERT_TRUE(F1->isFixed()); - F1->unfix(); - ASSERT_FALSE(F1->isFixed()); - F1->fix(); - ASSERT_TRUE(F1->isFixed()); - F1->getP()->unfix(); - ASSERT_FALSE(F1->isFixed()); - F1->unfix(); - ASSERT_FALSE(F1->isFixed()); - F1->getP()->fix(); - ASSERT_FALSE(F1->isFixed()); - F1->getO()->fix(); - ASSERT_TRUE(F1->isFixed()); - - // set key - F1->setKey(); - ASSERT_TRUE(F1->isKey()); - - // Unlink - F1->unlinkCapture(C); - ASSERT_TRUE(F1->getCaptureList().empty()); -} - -#include "core/state_block/state_quaternion.h" -TEST(FrameBase, GetSetState) -{ - // Create PQV_3D state blocks - StateBlockPtr sbp = make_shared<StateBlock>(3); - StateBlockPtr sbv = make_shared<StateBlock>(3); - StateQuaternionPtr sbq = make_shared<StateQuaternion>(); - - // Create frame - FrameBase F(1, sbp, sbq, sbv); - - // Give values to vectors and vector blocks - VectorXs x(10), x1(10), x2(10); - VectorXs p(3), v(3), q(4); - p << 1,2,3; - v << 9,8,7; - q << .5, -.5, .5, -.5; - - x << p, q, v; - - // Set the state, check that state blocks hold the current states - F.setState(x); - ASSERT_TRUE((p - F.getP()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL)); - ASSERT_TRUE((q - F.getO()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL)); - ASSERT_TRUE((v - F.getV()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL)); - - // Get the state, form 1 by reference - F.getState(x1); - ASSERT_TRUE((x1 - x).isMuchSmallerThan(1, Constants::EPS_SMALL)); - - // get the state, form 2 by return value - x2 = F.getState(); - ASSERT_TRUE((x2 - x).isMuchSmallerThan(1, Constants::EPS_SMALL)); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp deleted file mode 100644 index 520a694f6622d4601bfa2689422dbed35a2ef413..0000000000000000000000000000000000000000 --- a/test/gtest_local_param.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/* - * \file test_local_param_quat.cpp - * - * Created on: Feb 19, 2016 - * author: jsola - */ - -#include "utils_gtest.h" -#include "core/utils/logging.h" - -#include "core/state_block/local_parametrization_quaternion.h" -#include "core/state_block/local_parametrization_homogeneous.h" -#include "core/math/rotations.h" - -#include "core/common/wolf.h" - -#include <iostream> - -#define JAC_NUMERIC(T, _x0, _J, dx) \ -{ \ - VectorXs dv(3); \ - Map<const VectorXs> _dv (dv.data(), 3); \ - VectorXs xo(4); \ - Map<VectorXs> _xo (xo.data(), 4); \ - for (int i = 0; i< 3; i++) \ - {\ - dv.setZero();\ - dv(i) = dx;\ - T.plus(_x0, _dv, _xo);\ - _J.col(i) = (_xo - _x0)/dx; \ - } \ -} - -using namespace Eigen; -using namespace std; -using namespace wolf; - -TEST(TestLocalParametrization, QuaternionLocal) -{ - - // Storage - VectorXs x_storage(22); - MatrixXs M_storage(1,12); // matrix dimensions do not matter, just storage size. - x_storage.setRandom(); - M_storage.setZero(); - - // QUATERNION ------------------------------------------ - Map<VectorXs> q(&x_storage(0),4); - q.normalize(); - - Map<VectorXs> da(&x_storage(4),3); - da /= 10.0; - Map<VectorXs> qo_m(&x_storage(7),4); - Map<MatrixXs> J(&M_storage(0,0),4,3); - MatrixXs J_num(4,3); - - LocalParametrizationQuaternion<DQ_LOCAL> Qpar_loc; - - Map<const VectorXs> q_m(q.data(),4); - Map<const VectorXs> da_m(da.data(),3); - - Qpar_loc.plus(q_m,da_m,qo_m); - - ASSERT_DOUBLE_EQ(qo_m.norm(), 1); - - Quaternions qref = Map<Quaternions>(q.data()) * wolf::v2q(da); - ASSERT_TRUE(qo_m.isApprox( qref.coeffs() ) ); - - Qpar_loc.computeJacobian(q_m,J); - - JAC_NUMERIC(Qpar_loc, q_m, J_num, 1e-9) - - ASSERT_NEAR((J-J_num).norm(), 0, 1e-6); - - Map<const VectorXs> qoc_m(qo_m.data(), 4); - Map<VectorXs> da2_m(x_storage.data() + 10, 3); - - Qpar_loc.minus(q_m, qoc_m, da2_m); - - ASSERT_MATRIX_APPROX(da_m, da2_m, 1e-10); - -} - -TEST(TestLocalParametrization, QuaternionGlobal) -{ - // Storage - VectorXs x_storage(22); - MatrixXs M_storage(1,12); // matrix dimensions do not matter, just storage size. - x_storage.setRandom(); - M_storage.setZero(); - - // QUATERNION ------------------------------------------ - Map<VectorXs> q(&x_storage(0),4); - q.normalize(); - - Map<VectorXs> da(&x_storage(4),3); - da /= 10.0; - Map<VectorXs> qo_m(&x_storage(7),4); - Map<MatrixXs> J(&M_storage(0,0),4,3); - MatrixXs J_num(4,3); - - LocalParametrizationQuaternion<DQ_GLOBAL> Qpar_glob; - - Map<const VectorXs> q_m(q.data(),4); - Map<const VectorXs> da_m(da.data(),3); - - Qpar_glob.plus(q_m,da_m,qo_m); - - ASSERT_DOUBLE_EQ(qo_m.norm(), 1); - - Quaternions qref = wolf::v2q(da) * Map<Quaternions>(q.data()); - ASSERT_TRUE(qo_m.isApprox( qref.coeffs() ) ); - - Qpar_glob.computeJacobian(q_m,J); - - JAC_NUMERIC(Qpar_glob, q_m, J_num, 1e-9) - - ASSERT_NEAR((J-J_num).norm(), 0, 1e-6); - - Map<const VectorXs> qoc_m(qo_m.data(), 4); - Map<VectorXs> da2_m(x_storage.data() + 10, 3); - - Qpar_glob.minus(q_m, qoc_m, da2_m); - - ASSERT_MATRIX_APPROX(da_m, da2_m, 1e-10); - -} - -TEST(TestLocalParametrization, Homogeneous) -{ - // Storage - VectorXs x_storage(22); - MatrixXs M_storage(1,12); // matrix dimensions do not matter, just storage size. - - // HOMOGENEOUS ---------------------------------------- - Map<VectorXs> h(&x_storage(11),4); - h.setRandom(); - Map<VectorXs> d(&x_storage(15),3); - d << .1,.2,.3; - Map<VectorXs> ho_m(&x_storage(18),4); - Map<MatrixXs> J(&M_storage(0,0),4,3); - MatrixXs J_num(4,3); - - LocalParametrizationHomogeneous Hpar; - Map<const VectorXs> h_m(h.data(),4); - Map<const VectorXs> d_m(d.data(),3); - - Hpar.plus(h_m,d_m,ho_m); - - ASSERT_DOUBLE_EQ(ho_m.norm(), h.norm()); - - Hpar.computeJacobian(h_m,J); - - JAC_NUMERIC(Hpar, h_m, J_num, 1e-9) - - ASSERT_NEAR((J-J_num).norm(), 0, 1e-6); - - Map<const VectorXs> hoc_m(ho_m.data(), 4); - Map<VectorXs> d2_m(x_storage.data() + 10, 3); - - Hpar.minus(h_m, hoc_m, d2_m); - - ASSERT_MATRIX_APPROX(d_m, d2_m, 1e-10); - -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_make_posdef.cpp b/test/gtest_make_posdef.cpp deleted file mode 100644 index 5a0b707a6487eda22e37f2601408cda64929015d..0000000000000000000000000000000000000000 --- a/test/gtest_make_posdef.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "utils_gtest.h" -#include "core/common/wolf.h" - -using namespace Eigen; -using namespace wolf; - -TEST(MakePosDefTest, OkTest) -{ - MatrixXs M = MatrixXs::Identity(3,3); - - EXPECT_TRUE(isCovariance(M)); - EXPECT_FALSE(makePosDef(M)); -} - -TEST(MakePosDefTest, FixTest) -{ - MatrixXs M = MatrixXs::Zero(3,3); - - EXPECT_FALSE(isCovariance(M)); - EXPECT_TRUE(makePosDef(M)); - EXPECT_TRUE(isCovariance(M)); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp deleted file mode 100644 index 3ef0bfaa050b6125427b2b16256c56719f54fbaf..0000000000000000000000000000000000000000 --- a/test/gtest_motion_buffer.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/* - * gtest_motion_buffer.cpp - * - * Created on: Nov 12, 2016 - * Author: jsola - */ - -#include "utils_gtest.h" -#include "core/utils/logging.h" - -#include "core/processor/motion_buffer.h" - -#include "core/common/wolf.h" - -#include <iostream> - -using namespace Eigen; -using namespace std; -using namespace wolf; - -Motion newMotion(TimeStamp t, Scalar d, Scalar D, Scalar C, Scalar J_d, Scalar J_D) -{ - Motion m(t, 1, 1, 1, 0); - m.delta_(0) = d; - m.delta_integr_(0) = D; - m.delta_cov_(0) = C; - m.jacobian_delta_(0,0) = J_d; - m.jacobian_delta_integr_(0,0) = J_D; - return m; -} - -namespace{ -TimeStamp t0(0), t1(1), t2(2), t3(3), t4(4); -Motion m0 = newMotion(t0, 0, 0 , 0, .1, 1); // ts, delta, Delta, delta_cov, J_delta, J_Delta -Motion m1 = newMotion(t1, 1, 1 , 1, .1, 1); -Motion m2 = newMotion(t2, 2, 3 , 1, .1, 1); -Motion m3 = newMotion(t3, 3, 6 , 1, .1, 1); -Motion m4 = newMotion(t4, 4, 10, 1, .1, 1); -} - -TEST(MotionBuffer, QueryTimeStamps) -{ - MotionBuffer MB(1,1,1,0); - - MB.get().push_back(m0); - MB.get().push_back(m1); - TimeStamp t; - - t.set(-1); // t is older than m0.ts_ -> return m0 - ASSERT_EQ(MB.getMotion(t).ts_ , m0.ts_); - - t.set( 0); // t is exactly m0.ts_ -> return m0 - ASSERT_EQ(MB.getMotion(t).ts_ , m0.ts_); - - t.set(0.5); // t is between m0.ts_ and m1.ts_ -> return m0 - ASSERT_EQ(MB.getMotion(t).ts_ , m0.ts_); - - t.set(+1); // t is exactly m1.ts_ -> return m1 - ASSERT_EQ(MB.getMotion(t).ts_ , m1.ts_); - - t.set(+2); // t is newer than m1.ts_ -> return m1 - ASSERT_EQ(MB.getMotion(t).ts_ , m1.ts_); -} - -TEST(MotionBuffer, getMotion) -{ - MotionBuffer MB(1,1,1,0); - - MB.get().push_back(m0); - ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_); - - MB.get().push_back(m1); - ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_); - ASSERT_EQ(MB.getMotion(t1).delta_, m1.delta_); - ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_); - ASSERT_EQ(MB.getMotion(t1).delta_integr_, m1.delta_integr_); -} - -TEST(MotionBuffer, getDelta) -{ - MotionBuffer MB(1,1,1,0); - - MB.get().push_back(m0); - - ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_); - - MB.get().push_back(m1); - - ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_); - ASSERT_EQ(MB.getMotion(t1).delta_integr_, m1.delta_integr_); -} - -TEST(MotionBuffer, Split) -{ - MotionBuffer MB(1,1,1,0); - - MB.get().push_back(m0); - MB.get().push_back(m1); - MB.get().push_back(m2); - MB.get().push_back(m3); - MB.get().push_back(m4); // put 5 motions - - MotionBuffer MB_old(1,1,1,0); - - TimeStamp t = 1.5; // between m1 and m2 - MB.split(t, MB_old); - - ASSERT_EQ(MB_old.get().size(), (unsigned int) 2); - ASSERT_EQ(MB .get().size(), (unsigned int) 3); - - ASSERT_EQ(MB_old.getMotion(t1).ts_, t1); - ASSERT_EQ(MB_old.getMotion(t2).ts_, t1); // last ts is t1 - ASSERT_EQ(MB .getMotion(t1).ts_, t2); // first ts is t2 - ASSERT_EQ(MB .getMotion(t2).ts_, t2); -} - -// TEST(MotionBuffer, integrateCovariance) -// { -// MotionBuffer MB(1,1,1,0); -// -// MB.get().push_back(m0); -// MB.get().push_back(m1); -// MB.get().push_back(m2); -// MB.get().push_back(m3); -// MB.get().push_back(m4); // put 5 motions -// -// Eigen::MatrixXs cov = MB.integrateCovariance(); -// ASSERT_NEAR(cov(0), 0.04, 1e-8); -// -// } -// -// TEST(MotionBuffer, integrateCovariance_ts) -// { -// MotionBuffer MB(1,1,1,0); -// -// MB.get().push_back(m0); -// MB.get().push_back(m1); -// MB.get().push_back(m2); -// MB.get().push_back(m3); -// MB.get().push_back(m4); // put 5 motions -// -// Eigen::MatrixXs cov = MB.integrateCovariance(t2); -// ASSERT_NEAR(cov(0), 0.02, 1e-8); -// } -// -// TEST(MotionBuffer, integrateCovariance_ti_tf) -// { -// MotionBuffer MB(1,1,1,0); -// -// MB.get().push_back(m0); -// MB.get().push_back(m1); -// MB.get().push_back(m2); -// MB.get().push_back(m3); -// MB.get().push_back(m4); // put 5 motions -// -// Eigen::MatrixXs cov = MB.integrateCovariance(t1, t3); -// ASSERT_NEAR(cov(0), 0.03, 1e-8); -// -// cov = MB.integrateCovariance(t0, t3); // first delta_cov is zero so it does not integate -// ASSERT_NEAR(cov(0), 0.03, 1e-8); -// } - -TEST(MotionBuffer, print) -{ - MotionBuffer MB(1,1,1,0); - - MB.get().push_back(m0); - MB.get().push_back(m1); - MB.get().push_back(m2); - - MB.print(); - MB.print(0,0,0,0); - MB.print(1,0,0,0); - MB.print(0,1,0,0); - MB.print(0,0,1,0); - MB.print(0,0,0,1); - MB.print(1,1,1,1); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp deleted file mode 100644 index 2ac4d8f321555efa2f73f5da5414c150232de8cd..0000000000000000000000000000000000000000 --- a/test/gtest_odom_2D.cpp +++ /dev/null @@ -1,491 +0,0 @@ -/** - * \file test_odom_2D.cpp - * - * Created on: Mar 15, 2016 - * \author: jsola - */ - -#include "utils_gtest.h" - -// Classes under test -#include "core/processor/processor_odom_2D.h" -#include "core/factor/factor_odom_2D.h" - -// Wolf includes -#include "core/sensor/sensor_odom_2D.h" -#include "core/state_block/state_block.h" -#include "core/common/wolf.h" -#include "core/ceres_wrapper/ceres_manager.h" - -// STL includes -#include <map> -#include <list> -#include <algorithm> -#include <iterator> - -// General includes -#include <iostream> -#include <iomanip> // std::setprecision -#include "core/capture/capture_pose.h" - -using namespace wolf; -using namespace Eigen; - -VectorXs plus(const VectorXs& _pose, const Vector2s& _data) -{ - VectorXs _pose_plus_data(3); - _pose_plus_data(0) = _pose(0) + cos(_pose(2) + _data(1) / 2) * _data(0); - _pose_plus_data(1) = _pose(1) + sin(_pose(2) + _data(1) / 2) * _data(0); - _pose_plus_data(2) = pi2pi(_pose(2) + _data(1)); - return _pose_plus_data; -} - -MatrixXs plus_jac_u(const VectorXs& _pose, const Vector2s& _data) -{ - MatrixXs Ju(3,2); - Ju(0, 0) = cos(_pose(2) + _data(1) / 2); - Ju(0, 1) = -sin(_pose(2) + _data(1) / 2) * _data(0) / 2 ; - Ju(1, 0) = sin(_pose(2) + _data(1) / 2); - Ju(1, 1) = cos(_pose(2) + _data(1) / 2) * _data(0) / 2 ; - Ju(2, 0) = 0.0; - Ju(2, 1) = 1.0; - return Ju; -} - -MatrixXs plus_jac_x(const VectorXs& _pose, const Vector2s& _data) -{ - Matrix3s Jx; - Jx(0, 0) = 1.0; - Jx(0, 1) = 0.0; - Jx(0, 2) = -sin(_pose(2) + _data(1) / 2) * _data(0); - Jx(1, 0) = 0.0; - Jx(1, 1) = 1.0; - Jx(1, 2) = cos(_pose(2) + _data(1) / 2) * _data(0); - Jx(2, 0) = 0.0; - Jx(2, 1) = 0.0; - Jx(2, 2) = 1.0; - return Jx; -} - -void show(const ProblemPtr& problem) -{ - using std::cout; - using std::endl; - cout << std::setprecision(4); - - for (FrameBasePtr F : problem->getTrajectory()->getFrameList()) - { - if (F->isKey()) - { - cout << "----- Key Frame " << F->id() << " -----" << endl; - if (!F->getCaptureList().empty()) - { - auto C = F->getCaptureList().front(); - if (!C->getFeatureList().empty()) - { - auto f = C->getFeatureList().front(); - cout << " feature measure: \n" - << F->getCaptureList().front()->getFeatureList().front()->getMeasurement().transpose() - << endl; - cout << " feature covariance: \n" - << F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl; - } - } - cout << " state: \n" << F->getState().transpose() << endl; - Eigen::MatrixXs cov; - problem->getFrameCovariance(F,cov); - cout << " covariance: \n" << cov << endl; - } - } -} - -TEST(Odom2D, FactorFix_and_FactorOdom2D) -{ - using std::cout; - using std::endl; - - // RATIONALE: - // We build this tree (a is `absolute`, m is `motion`): - // KF0 -- m -- KF1 -- m -- KF2 - // | - // a - // | - // GND - // `absolute` is made with FactorFix - // `motion` is made with FactorOdom2D - - std::cout << std::setprecision(4); - - TimeStamp t0(0.0), t = t0; - Scalar dt = .01; - Vector3s x0 (0,0,0); - Eigen::Matrix3s P0 = Eigen::Matrix3s::Identity() * 0.1; - Vector3s delta (2,0,0); - Matrix3s delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02; - - ProblemPtr Pr = Problem::create("PO 2D"); - CeresManager ceres_manager(Pr); - - // KF0 and absolute prior - FrameBasePtr F0 = Pr->setPrior(x0, P0,t0, dt/2); - - // KF1 and motion from KF0 - t += dt; - FrameBasePtr F1 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t); - CaptureBasePtr C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t)); - FeatureBasePtr f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov)); - FactorBasePtr c1 = f1->addFactor(std::make_shared<FactorOdom2D>(f1, F0, nullptr)); - F0->addConstrainedBy(c1); - - // KF2 and motion from KF1 - t += dt; - FrameBasePtr F2 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t); - CaptureBasePtr C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t)); - FeatureBasePtr f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov)); - FactorBasePtr c2 = f2->addFactor(std::make_shared<FactorOdom2D>(f2, F1, nullptr)); - F1->addConstrainedBy(c2); - - ASSERT_TRUE(Pr->check(0)); - -// cout << "===== full ====" << endl; - F0->setState(Vector3s(1,2,3)); - F1->setState(Vector3s(2,3,1)); - F2->setState(Vector3s(3,1,2)); - std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::FULL); -// std::cout << report << std::endl; - - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); -// show(Pr); - - Matrix3s P1, P2; - P1 << 0.12, 0, 0, - 0, 0.525, 0.22, - 0, 0.22, 0.12; - P2 << 0.14, 0, 0, - 0, 1.91, 0.48, - 0, 0.48, 0.14; - - // get covariances - MatrixXs P0_solver, P1_solver, P2_solver; - ASSERT_TRUE(Pr->getFrameCovariance(F0, P0_solver)); - ASSERT_TRUE(Pr->getFrameCovariance(F1, P1_solver)); - ASSERT_TRUE(Pr->getFrameCovariance(F2, P2_solver)); - - ASSERT_POSE2D_APPROX(F0->getState(), Vector3s(0,0,0), 1e-6); - ASSERT_MATRIX_APPROX(P0_solver, P0, 1e-6); - ASSERT_POSE2D_APPROX(F1->getState(), Vector3s(2,0,0), 1e-6); - ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6); - ASSERT_POSE2D_APPROX(F2->getState(), Vector3s(4,0,0), 1e-6); - ASSERT_MATRIX_APPROX(P2_solver, P2, 1e-6); -} - -TEST(Odom2D, VoteForKfAndSolve) -{ - std::cout << std::setprecision(4); - // time - TimeStamp t0(0.0), t = t0; - Scalar dt = .01; - // Origin frame: - Vector3s x0(0,0,0); - Eigen::Matrix3s P0 = Eigen::Matrix3s::Identity() * 0.1; - // motion data - VectorXs data(Vector2s(1, M_PI_4) ); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn - Eigen::MatrixXs data_cov = Eigen::MatrixXs::Identity(2, 2) * 0.01; - int N = 7; // number of process() steps - - // Create Wolf tree nodes - ProblemPtr problem = Problem::create("PO 2D"); - SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0)); - ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>()); - params->voting_active = true; - params->dist_traveled = 100; - params->angle_turned = 6.28; - params->max_time_span = 2.5*dt; // force KF at every third process() - params->cov_det = 100; - params->unmeasured_perturbation_std = 0.00; - Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity(); - ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params); - ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base); - - // NOTE: We integrate and create KFs as follows: - // i= 0 1 2 3 4 5 6 - // KF -- * -- * -- KF - * -- * -- KF - * - - // Ceres wrapper - CeresManager ceres_manager(problem); - - // Origin Key Frame - FrameBasePtr origin_frame = problem->setPrior(x0, P0, t0, dt/2); - ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); - - // std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl; - // std::cout << "Initial covariance : " << std::endl << problem->getLastKeyFrameCovariance() << std::endl; - // std::cout << "Motion data : " << data.transpose() << std::endl; - - // Check covariance values - Eigen::Vector3s integrated_pose = x0; - Eigen::Matrix3s integrated_cov = P0; - Eigen::Vector3s integrated_delta = Eigen::Vector3s::Zero(); - Eigen::Matrix3s integrated_delta_cov = Eigen::Matrix3s::Zero(); - Eigen::MatrixXs Ju(3, 2); - Eigen::Matrix3s Jx; - std::vector<Eigen::VectorXs> integrated_pose_vector; - std::vector<Eigen::MatrixXs> integrated_cov_vector; - -// std::cout << "\nIntegrating data..." << std::endl; - - t += dt; - // Capture to use as container for all incoming data - CaptureMotionPtr capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr); - - for (int n=1; n<=N; n++) - { - // std::cout << "-------------------\nStep " << i << " at time " << t << std::endl; - // re-use capture with updated timestamp - capture->setTimeStamp(t); - - // Processor - sensor_odom2d->process(capture); - ASSERT_TRUE(problem->check(0)); -// Matrix3s odom2d_delta_cov = processor_odom2d->integrateBufferCovariance(processor_odom2d->getBuffer()); - Matrix3s odom2d_delta_cov = processor_odom2d->getMotion().delta_integr_cov_; - // std::cout << "State(" << (t - t0) << ") : " << processor_odom2d->getCurrentState().transpose() << std::endl; - // std::cout << "PRC cov: \n" << odom2d_delta_cov << std::endl; - - // Integrate Delta - if (n==3 || n==6) // keyframes - { - integrated_delta.setZero(); - integrated_delta_cov.setZero(); - } - else - { - Ju = plus_jac_u(integrated_delta, data); - Jx = plus_jac_x(integrated_delta, data); - integrated_delta = plus(integrated_delta, data); - integrated_delta_cov = Jx * integrated_delta_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov; - } - - WOLF_DEBUG("n: ", n, " t:", t); - WOLF_DEBUG("wolf delta: ", processor_odom2d->getMotion().delta_integr_.transpose()); - WOLF_DEBUG("test delta: ", integrated_delta .transpose()); - - ASSERT_POSE2D_APPROX(processor_odom2d->getMotion().delta_integr_, integrated_delta, 1e-6); - ASSERT_MATRIX_APPROX(odom2d_delta_cov, integrated_delta_cov, 1e-6); - - // Integrate pose - Ju = plus_jac_u(integrated_pose, data); - Jx = plus_jac_x(integrated_pose, data); - integrated_pose = plus(integrated_pose, data); - integrated_cov = Jx * integrated_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov; - - ASSERT_POSE2D_APPROX(processor_odom2d->getCurrentState(), integrated_pose, 1e-6); - - integrated_pose_vector.push_back(integrated_pose); - integrated_cov_vector.push_back(integrated_cov); - - t += dt; - } - - // Solve - std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); -// std::cout << report << std::endl; - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); - - MatrixXs computed_cov; - ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov)); - ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[5], 1e-6); -} - -TEST(Odom2D, KF_callback) -{ - using std::cout; - using std::endl; - - std::cout << std::setprecision(4); - // time - TimeStamp t0(0.0), t = t0; - Scalar dt = .01; - Vector3s x0(0,0,0); - Eigen::Matrix3s x0_cov = Eigen::Matrix3s::Identity() * 0.1; - VectorXs data(Vector2s(1, M_PI/4) ); // advance 1m - Eigen::MatrixXs data_cov = Eigen::MatrixXs::Identity(2, 2) * 0.01; - int N = 16; // number of process() steps - - // NOTE: We integrate and create KFs as follows: - // n = 0 1 2 3 4 5 6 7 8 - // t = 0 dt 2dt 3dt 4dt 5dt 6dt 7dt 8dt - // KF8-- * -- * -- * -- * -- * -- * -- * -- * --> : no keyframes during integration - // And we create KFs as follows: - // KF10 - // KF11 - - // Create Wolf tree nodes - ProblemPtr problem = Problem::create("PO 2D"); - SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0)); - ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>()); - params->dist_traveled = 100; - params->angle_turned = 6.28; - params->max_time_span = 100; - params->cov_det = 100; - params->unmeasured_perturbation_std = 0.001; - Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity(); - ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params); - ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base); - processor_odom2d->setTimeTolerance(dt/2); - - // Ceres wrapper - CeresManager ceres_manager(problem); - - // Origin Key Frame - FrameBasePtr keyframe_0 = problem->setPrior(x0, x0_cov, t0, dt/2); - - // Check covariance values - Eigen::Vector3s integrated_pose = x0; - Eigen::Matrix3s integrated_cov = x0_cov; - Eigen::Vector3s integrated_delta = Eigen::Vector3s::Zero(); - Eigen::Matrix3s integrated_delta_cov = Eigen::Matrix3s::Zero(); - Eigen::MatrixXs Ju(3, 2); - Eigen::Matrix3s Jx; - std::vector<Eigen::VectorXs> integrated_pose_vector; - std::vector<Eigen::MatrixXs> integrated_cov_vector; - - // Store integrals - integrated_pose_vector.push_back(integrated_pose); - integrated_cov_vector.push_back(integrated_cov); - -// std::cout << "\nIntegrating data..." << std::endl; - - // Capture to use as container for all incoming data - CaptureMotionPtr capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr); - - std::cout << "t: " << t << std::endl; - for (int n=1; n<=N; n++) - { - t += dt; - - // re-use capture with updated timestamp - capture->setTimeStamp(t); - std::cout << "capture ts: " << capture->getTimeStamp() << " - " << capture->getTimeStamp().get(); - std::cout << "nsec: " << capture->getTimeStamp().getNanoSeconds() << std::endl; - std::cout << "filled nsec: " << std::setfill('0') << std::setw(9) << std::right << capture->getTimeStamp().getNanoSeconds() << std::endl; - std::cout << std::setfill(' '); - - // Processor - sensor_odom2d->process(capture); - ASSERT_TRUE(problem->check(0)); - - // Integrate Delta - Ju = plus_jac_u(integrated_delta, data); - Jx = plus_jac_x(integrated_delta, data); - integrated_delta = plus(integrated_delta, data); - integrated_delta_cov = Jx * integrated_delta_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov; - - // Integrate pose - Ju = plus_jac_u(integrated_pose, data); - Jx = plus_jac_x(integrated_pose, data); - integrated_pose = plus(integrated_pose, data); - integrated_cov = Jx * integrated_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov; - - // Store integrals - integrated_pose_vector.push_back(integrated_pose); - integrated_cov_vector.push_back(integrated_cov); - - } - -// std::cout << "=============================" << std::endl; - - //////////////////////////////////////////////////////////////// - // Split after the last keyframe, exact timestamp - int n_split = 8; - TimeStamp t_split = t0 + n_split*dt; - - std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl; - - Vector3s x_split = processor_odom2d->getState(t_split); - FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY_FRAME, x_split, t_split); - - ASSERT_TRUE(problem->check(0)); - processor_odom2d->keyFrameCallback(keyframe_2, dt/2); - ASSERT_TRUE(problem->check(0)); - t += dt; - capture->setTimeStamp(t); - processor_odom2d->process(capture); - ASSERT_TRUE(problem->check(0)); - - CaptureMotionPtr key_capture_n = std::static_pointer_cast<CaptureMotion>(keyframe_2->getCaptureList().front()); - - MotionBuffer key_buffer_n = key_capture_n->getBuffer(); - - std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); -// std::cout << report << std::endl; - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); - - ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6); - MatrixXs computed_cov; - ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov)); - ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6); - - //////////////////////////////////////////////////////////////// - // Split between keyframes, exact timestamp - int m_split = 4; - t_split = t0 + m_split*dt; - std::cout << "-----------------------------\nSplit between KFs; time: " << t_split << std::endl; - - problem->print(4,1,1,1); - - x_split = processor_odom2d->getState(t_split); - FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY_FRAME, x_split, t_split); - - ASSERT_TRUE(problem->check(0)); - processor_odom2d->keyFrameCallback(keyframe_1, dt/2); - ASSERT_TRUE(problem->check(0)); - t += dt; - capture->setTimeStamp(t); - processor_odom2d->process(capture); - ASSERT_TRUE(problem->check(0)); - - CaptureMotionPtr key_capture_m = std::static_pointer_cast<CaptureMotion>(keyframe_1->getCaptureList().front()); - MotionBuffer key_buffer_m = key_capture_m->getBuffer(); - - // solve -// cout << "===== full ====" << endl; - keyframe_0->setState(Vector3s(1,2,3)); - keyframe_1->setState(Vector3s(2,3,1)); - keyframe_2->setState(Vector3s(3,1,2)); - - report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); - ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); - - problem->print(4,1,1,1); - - // check the split KF - MatrixXs KF1_cov; - ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, KF1_cov)); - ASSERT_POSE2D_APPROX(keyframe_1->getState(), integrated_pose_vector[m_split], 1e-6); - ASSERT_MATRIX_APPROX(KF1_cov, integrated_cov_vector [m_split], 1e-6); - - // check other KF in the future of the split KF - MatrixXs KF2_cov; - ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov)); - ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState(), integrated_pose_vector[n_split], 1e-6); - ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6); - - // Check full trajectory - t = t0; - for (int n=1; n<=N; n++) - { - t += dt; - // WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).transpose()); - // WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose()); - EXPECT_POSE2D_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6); - } -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_odom_3D.cpp b/test/gtest_odom_3D.cpp deleted file mode 100644 index ce52ec3932b0162ca400cff88680ea27b7b4b196..0000000000000000000000000000000000000000 --- a/test/gtest_odom_3D.cpp +++ /dev/null @@ -1,580 +0,0 @@ -/** - * \file gtest_odom_3D.cpp - * - * Created on: Nov 11, 2016 - * \author: jsola - */ - -#include "utils_gtest.h" - -#include "core/common/wolf.h" -#include "core/utils/logging.h" - -#include "core/processor/processor_odom_3D.h" - -#include <iostream> - -#define JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, dx) \ -{ VectorXs Do(7); \ - prc_ptr->deltaPlusDelta(D, d, dt, Do); \ - VectorXs dd(7); \ - VectorXs DD(7); \ - VectorXs DDo(7); \ - for (int i = 0; i< 7; i++) {\ - dd = d;\ - DD = D; \ - dd(i) += dx;\ - prc_ptr->deltaPlusDelta(D, dd, dt, DDo);\ - J_d.col(i) = (DDo - Do)/dx; \ - dd = d;\ - DD = D; \ - DD(i) += dx; \ - prc_ptr->deltaPlusDelta(DD, d, dt, DDo); \ - J_D.col(i) = (DDo - Do)/dx; \ - }\ -} - -using namespace Eigen; -using namespace std; -using namespace wolf; - -/** Gain access to members of ProcessorOdom3D - */ -class ProcessorOdom3DTest : public ProcessorOdom3D -{ - public: - ProcessorOdom3DTest(); - - // getters :-D !! - VectorXs& delta() {return delta_;} - VectorXs& deltaInt() {return delta_integrated_;} - MatrixXs& deltaCov() {return delta_cov_;} - MatrixXs& deltaIntCov() {return delta_integrated_cov_;} - Scalar& kdd() {return k_disp_to_disp_;} - Scalar& kdr() {return k_disp_to_rot_;} - Scalar& krr() {return k_rot_to_rot_;} - Scalar& dvar_min() {return min_disp_var_;} - Scalar& rvar_min() {return min_rot_var_;} -}; -ProcessorOdom3DTest::ProcessorOdom3DTest() : ProcessorOdom3D(std::make_shared<ProcessorParamsOdom3D>()) -{ - dvar_min() = 0.5; - rvar_min() = 0.25; -} - -TEST(ProcessorOdom3D, computeCurrentDelta) -{ - // One instance of the processor to test - ProcessorOdom3DTest prc; - - // input data - Vector6s data; data.setRandom(); - Scalar dt = 1; // irrelevant, just for the API. - - // Build delta from Eigen tools - Vector3s data_dp = data.head<3>(); - Vector3s data_do = data.tail<3>(); - Vector3s delta_dp = data_dp; - Quaternions delta_dq = v2q(data_do); - Vector7s delta; - delta.head<3>() = delta_dp; - delta.tail<4>() = delta_dq.coeffs(); - - // construct covariance from processor parameters and motion magnitudes - Scalar disp = data_dp.norm(); - Scalar rot = data_do.norm(); - Scalar dvar = prc.dvar_min() + prc.kdd()*disp; - Scalar rvar = prc.rvar_min() + prc.kdr()*disp + prc.krr()*rot; - Vector6s diag; diag << dvar, dvar, dvar, rvar, rvar, rvar; - Matrix6s data_cov = diag.asDiagonal(); - Matrix6s delta_cov = data_cov; - - // return values for data2delta() - VectorXs delta_ret(7); - MatrixXs delta_cov_ret(6,6); - MatrixXs jac_delta_calib(6,0); - - // call the function under test - prc.computeCurrentDelta(data, data_cov, VectorXs::Zero(0), dt, delta_ret, delta_cov_ret, jac_delta_calib); - - ASSERT_MATRIX_APPROX(delta_ret , delta, Constants::EPS_SMALL); - ASSERT_MATRIX_APPROX(delta_cov_ret , delta_cov, Constants::EPS_SMALL); - -} - -TEST(ProcessorOdom3D, deltaPlusDelta) -{ - ProcessorOdom3DTest prc; - - VectorXs D(7); D.setRandom(); D.tail<4>().normalize(); - VectorXs d(7); d.setRandom(); d *= 1; d.tail<4>().normalize(); - - // Integrated delta value to check aginst - // Dp_int <-- Dp + Dq * dp - // Dq_int <-- Dq * dq - VectorXs D_int_check(7); - D_int_check.head<3>() = D.head<3>() + Quaternions(D.data()+3) * d.head<3>(); - D_int_check.tail<4>() = (Quaternions(D.data()+3) * Quaternions(d.data()+3)).coeffs(); - - Scalar dt = 1; // dummy, not used in Odom3D - - VectorXs D_int(7); - - prc.deltaPlusDelta(D, d, dt, D_int); - - ASSERT_MATRIX_APPROX(D_int , D_int_check, 1e-10); -// << "\nDpd : " << D_int.transpose() -// << "\ncheck: " << D_int_check.transpose(); -} - -TEST(ProcessorOdom3D, deltaPlusDelta_Jac) -{ - std::shared_ptr<ProcessorOdom3DTest> prc_ptr = std::make_shared<ProcessorOdom3DTest>(); - - VectorXs D(7); D.setRandom(); D.tail<4>().normalize(); - VectorXs d(7); d.setRandom(); d *= 1; d.tail<4>().normalize(); - Scalar dt = 1; - VectorXs Do(7); - MatrixXs D_D(6,6); - MatrixXs D_d(6,6); - - prc_ptr->deltaPlusDelta(D, d, dt, Do, D_D, D_d); - - WOLF_DEBUG("DD:\n ", D_D); - WOLF_DEBUG("Dd:\n ", D_d); - - MatrixXs J_D(7,7), J_d(7,7); - - JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, Constants::EPS); - WOLF_DEBUG("J_D:\n ", J_D); - WOLF_DEBUG("J_d:\n ", J_d); - -} - -TEST(ProcessorOdom3D, Interpolate0) // basic test -{ - /* Conditions: - * ref d = id - * ref D = id - * fin d = pos - * fin D = id - */ - - ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>()); - - Motion ref(0.0,6,7,6,0), final(0.0,6,7,6,0), interpolated(0.0,6,7,6,0); - - // set ref - ref.ts_ = 1; - ref.delta_ << 0,0,0, 0,0,0,1; - ref.delta_integr_ << 0,0,0, 0,0,0,1; - - WOLF_DEBUG("ref delta= ", ref.delta_.transpose()); - WOLF_DEBUG("ref Delta= ", ref.delta_integr_.transpose()); - - // set final - final.ts_ = 5; - final.delta_ << 1,0,0, 0,0,0,1; - final.delta_integr_ << 0,0,0, 0,0,0,1; - prc.deltaPlusDelta(ref.delta_integr_, final.delta_, (final.ts_ - ref.ts_), final.delta_integr_); - - WOLF_DEBUG("final delta= ", final.delta_.transpose()); - WOLF_DEBUG("final Delta= ", final.delta_integr_.transpose()); - - // interpolate! - Motion second = final; - TimeStamp t; t = 2; - // +--+--------+---> time(s) - // 1 2 3 4 5 // 2 = 25% into interpolated, 75% into second - interpolated = prc.interpolate(ref, second, t); - - WOLF_DEBUG("interpolated delta= ", interpolated.delta_.transpose()); - WOLF_DEBUG("interpolated Delta= ", interpolated.delta_integr_.transpose()); - - // delta - ASSERT_MATRIX_APPROX(interpolated.delta_.head<3>() , 0.25 * final.delta_.head<3>(), Constants::EPS); - ASSERT_MATRIX_APPROX(second.delta_.head<3>() , 0.75 * final.delta_.head<3>(), Constants::EPS); - -} - -TEST(ProcessorOdom3D, Interpolate1) // delta algebra test -{ - ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>()); - - /* - * We create several poses: origin, ref, int, second, final, as follows: - * - * +---+---+---+----> - * o r i s,f - * - * We compute all deltas between them: d_or, d_ri, d_is, d_rf - * We create the motions R, F - * We interpolate, and get I, S - */ - - // absolute poses: origin, ref, interp, second=final - Vector7s x_o, x_r, x_i, x_s, x_f; - Map<Vector3s> p_o(x_o.data(), 3); - Map<Quaternions> q_o(x_o.data() +3); - Map<Vector3s> p_r(x_r.data(), 3); - Map<Quaternions> q_r(x_r.data() +3); - Map<Vector3s> p_i(x_i.data(), 3); - Map<Quaternions> q_i(x_i.data() +3); - Map<Vector3s> p_s(x_s.data(), 3); - Map<Quaternions> q_s(x_s.data() +3); - Map<Vector3s> p_f(x_f.data(), 3); - Map<Quaternions> q_f(x_f.data() +3); - - // deltas -- referred to previous delta - // o-r r-i i-s s-f - Vector7s dx_or, dx_ri, dx_is, dx_rf; - Map<Vector3s> dp_or(dx_or.data(), 3); - Map<Quaternions> dq_or(dx_or.data() +3); - Map<Vector3s> dp_ri(dx_ri.data(), 3); - Map<Quaternions> dq_ri(dx_ri.data() +3); - Map<Vector3s> dp_is(dx_is.data(), 3); - Map<Quaternions> dq_is(dx_is.data() +3); - Map<Vector3s> dp_rf(dx_rf.data(), 3); - Map<Quaternions> dq_rf(dx_rf.data() +3); - Map<Vector7s> dx_rs(dx_rf.data(), 7); // this ensures dx_rs = dx_rf - - // Deltas -- always referred to origin - // o-r o-i o-s o-f - Vector7s Dx_or, Dx_oi, Dx_os, Dx_of; - Map<Vector3s> Dp_or(Dx_or.data(), 3); - Map<Quaternions> Dq_or(Dx_or.data() +3); - Map<Vector3s> Dp_oi(Dx_oi.data(), 3); - Map<Quaternions> Dq_oi(Dx_oi.data() +3); - Map<Vector3s> Dp_os(Dx_os.data(), 3); - Map<Quaternions> Dq_os(Dx_os.data() +3); - Map<Vector3s> Dp_of(Dx_of.data(), 3); - Map<Quaternions> Dq_of(Dx_of.data() +3); - - // time stamps and intervals - TimeStamp t_o(0), t_r(1), t_i(2.3), t_f(5); // t_i=2: 25% of motion; t_i=2.3: a general interpolation point - Scalar dt_ri = t_i - t_r; - Scalar dt_rf = t_f - t_r; - - WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_i: ", t_i.get(), "; t_f: ", t_f.get()); - WOLF_DEBUG("dt_ri: ", dt_ri, "; dt_rf ", dt_rf) - - // Constant velocity model - Vector3s v; - Vector3s w; - - // Motion structures - Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0); - - /////////// start experiment /////////////// - - // set origin and ref states - x_o << 1,2,3, 4,5,6,7; q_o.normalize(); - x_r << 7,6,5, 4,3,2,1; q_r.normalize(); - - // set constant velocity params - v << 3,2,1; // linear velocity - w << .1,.2,.3; // angular velocity - - // compute other poses from model - p_i = p_r + v * dt_ri; - q_i = q_r * v2q (w * dt_ri); - p_f = p_r + v * dt_rf; - q_f = q_r * v2q (w * dt_rf); - x_s = x_f; - - WOLF_DEBUG("o = ", x_o.transpose()); - WOLF_DEBUG("r = ", x_r.transpose()); - WOLF_DEBUG("i = ", x_i.transpose()); - WOLF_DEBUG("s = ", x_s.transpose()); - WOLF_DEBUG("f = ", x_f.transpose()); - - // deltas -- referred to previous delta - dp_or = q_o.conjugate() * (p_r - p_o); - dq_or = q_o.conjugate() * q_r; - dp_ri = q_r.conjugate() * (p_i - p_r); - dq_ri = q_r.conjugate() * q_i; - dp_is = q_i.conjugate() * (p_s - p_i); - dq_is = q_i.conjugate() * q_s; - dp_rf = q_r.conjugate() * (p_f - p_r); - dq_rf = q_r.conjugate() * q_f; - - // Deltas -- always referred to origin - Dp_or = q_o.conjugate() * (p_r - p_o); - Dq_or = q_o.conjugate() * q_r; - Dp_oi = q_o.conjugate() * (p_i - p_o); - Dq_oi = q_o.conjugate() * q_i; - Dp_os = q_o.conjugate() * (p_s - p_o); - Dq_os = q_o.conjugate() * q_s; - Dp_of = q_o.conjugate() * (p_f - p_o); - Dq_of = q_o.conjugate() * q_f; - - // set ref - R.ts_ = t_r; - R.delta_ = dx_or; // origin to ref - R.delta_integr_ = Dx_or; // origin to ref - - WOLF_DEBUG("* R.d = ", R.delta_.transpose()); - WOLF_DEBUG(" or = ", dx_or.transpose()); - ASSERT_MATRIX_APPROX(R.delta_ , dx_or, Constants::EPS); - - WOLF_DEBUG(" R.D = ", R.delta_integr_.transpose()); - WOLF_DEBUG(" or = ", Dx_or.transpose()); - ASSERT_MATRIX_APPROX(R.delta_integr_ , Dx_or, Constants::EPS); - - // set final - F.ts_ = t_f; - F.delta_ = dx_rf; // ref to final - F.delta_integr_ = Dx_of; // origin to final - - WOLF_DEBUG("* F.d = ", F.delta_.transpose()); - WOLF_DEBUG(" rf = ", dx_rf.transpose()); - ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS); - - WOLF_DEBUG(" F.D = ", F.delta_integr_.transpose()); - WOLF_DEBUG(" of = ", Dx_of.transpose()); - ASSERT_MATRIX_APPROX(F.delta_integr_ , Dx_of, Constants::EPS); - - S = F; // avoid overwriting final - WOLF_DEBUG("* S.d = ", S.delta_.transpose()); - WOLF_DEBUG(" rs = ", dx_rs.transpose()); - ASSERT_MATRIX_APPROX(S.delta_ , dx_rs, Constants::EPS); - - WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose()); - WOLF_DEBUG(" os = ", Dx_os.transpose()); - ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS); - - // interpolate! - WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed."); - I = prc.interpolate(R, S, t_i); - - WOLF_DEBUG("* I.d = ", I.delta_.transpose()); - WOLF_DEBUG(" ri = ", dx_ri.transpose()); - ASSERT_MATRIX_APPROX(I.delta_ , dx_ri, Constants::EPS); - - WOLF_DEBUG(" I.D = ", I.delta_integr_.transpose()); - WOLF_DEBUG(" oi = ", Dx_oi.transpose()); - ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_oi, Constants::EPS); - - WOLF_DEBUG("* S.d = ", S.delta_.transpose()); - WOLF_DEBUG(" is = ", dx_is.transpose()); - ASSERT_MATRIX_APPROX(S.delta_ , dx_is, Constants::EPS); - - WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose()); - WOLF_DEBUG(" os = ", Dx_os.transpose()); - ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS); - -} - -TEST(ProcessorOdom3D, Interpolate2) // timestamp out of bounds test -{ - ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>()); - - /* - * We create several poses: origin, ref, int, second, final, as follows: - * - * +---+---+---+----> - * o r i s,f - * - * We compute all deltas between them: d_or, d_ri, d_is, d_rf - * We create the motions R, F - * We interpolate, and get I, S - */ - - // deltas -- referred to previous delta - // o-r r-i i-s s-f - VectorXs dx_or(7), dx_ri(7), dx_is(7), dx_rf(7); - Map<VectorXs> dx_rs(dx_rf.data(), 7); // this ensures dx_rs = dx_rf - - // Deltas -- always referred to origin - // o-r o-i o-s o-f - VectorXs Dx_or(7), Dx_oi(7), Dx_os(7), Dx_of(7); - - // time stamps and intervals - TimeStamp t_o(0), t_r(1), t_i, t_f(5); // we'll set t_i later - - WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_f: ", t_f.get()); - - // Motion structures - Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0); - - /////////// start experiment /////////////// - - // set final and ref deltas - dx_or << 1,2,3, 4,5,6,7; dx_or.tail<4>().normalize(); - dx_rf << 7,6,5, 4,3,2,1; dx_rf.tail<4>().normalize(); - Dx_or = dx_or; - prc.deltaPlusDelta(Dx_or, dx_rf, t_f - t_r, Dx_of); - Dx_os = Dx_of; - - // set ref - R.ts_ = t_r; - R.delta_ = dx_or; // origin to ref - R.delta_integr_ = Dx_or; // origin to ref - - WOLF_DEBUG("* R.d = ", R.delta_.transpose()); - WOLF_DEBUG(" or = ", dx_or.transpose()); - ASSERT_MATRIX_APPROX(R.delta_ , dx_or, Constants::EPS); - - WOLF_DEBUG(" R.D = ", R.delta_integr_.transpose()); - WOLF_DEBUG(" or = ", Dx_or.transpose()); - ASSERT_MATRIX_APPROX(R.delta_integr_ , Dx_or, Constants::EPS); - - // set final - F.ts_ = t_f; - F.delta_ = dx_rf; // ref to final - F.delta_integr_ = Dx_of; // origin to final - - WOLF_DEBUG("* F.d = ", F.delta_.transpose()); - WOLF_DEBUG(" rf = ", dx_rf.transpose()); - ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS); - - WOLF_DEBUG(" F.D = ", F.delta_integr_.transpose()); - WOLF_DEBUG(" of = ", Dx_of.transpose()); - ASSERT_MATRIX_APPROX(F.delta_integr_ , Dx_of, Constants::EPS); - - S = F; // avoid overwriting final - WOLF_DEBUG("* S.d = ", S.delta_.transpose()); - WOLF_DEBUG(" rs = ", dx_rs.transpose()); - ASSERT_MATRIX_APPROX(S.delta_ , dx_rs, Constants::EPS); - - WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose()); - WOLF_DEBUG(" os = ", Dx_os.transpose()); - ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS); - - // interpolate! - t_i = 0.5; /// before ref! - WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed."); - I = prc.interpolate(R, S, t_i); - - WOLF_DEBUG("* I.d = ", I.delta_.transpose()); - WOLF_DEBUG(" ri = ", prc.deltaZero().transpose()); - ASSERT_MATRIX_APPROX(I.delta_ , prc.deltaZero(), Constants::EPS); - - WOLF_DEBUG(" I.D = ", I.delta_integr_.transpose()); - WOLF_DEBUG(" oi = ", Dx_or.transpose()); - ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_or, Constants::EPS); - - WOLF_DEBUG("* S.d = ", S.delta_.transpose()); - WOLF_DEBUG(" is = ", dx_rf.transpose()); - ASSERT_MATRIX_APPROX(S.delta_ , dx_rf, Constants::EPS); - - WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose()); - WOLF_DEBUG(" os = ", Dx_of.transpose()); - ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_of, Constants::EPS); - - // interpolate! - t_i = 5.5; /// after ref! - S = F; - WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed."); - I = prc.interpolate(R, S, t_i); - - WOLF_DEBUG("* I.d = ", I.delta_.transpose()); - WOLF_DEBUG(" ri = ", dx_rf.transpose()); - ASSERT_MATRIX_APPROX(I.delta_ , dx_rf, Constants::EPS); - - WOLF_DEBUG(" I.D = ", I.delta_integr_.transpose()); - WOLF_DEBUG(" oi = ", Dx_of.transpose()); - ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_of, Constants::EPS); - - WOLF_DEBUG("* S.d = ", S.delta_.transpose()); - WOLF_DEBUG(" is = ", prc.deltaZero().transpose()); - ASSERT_MATRIX_APPROX(S.delta_ , prc.deltaZero(), Constants::EPS); - - WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose()); - WOLF_DEBUG(" os = ", Dx_of.transpose()); - ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_of, Constants::EPS); - -} - - -TEST(ProcessorOdom3D, Interpolate3) // timestamp out of bounds test -{ - ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>()); - - /* - * We create several poses: origin, ref, int, second, final, as follows: - * - * +---+---+---+----> - * o r i s,f - * - * We compute all deltas between them: d_or, d_ri, d_is, d_rf - * We create the motions R, F - * We interpolate, and get I, S - */ - - // deltas -- referred to previous delta - // o-r r-i i-s s-f - VectorXs dx_or(7), dx_ri(7), dx_is(7), dx_if(7), dx_rf(7); - Map<VectorXs> dx_rs(dx_rf.data(), 7); // this ensures dx_rs = dx_rf - - // Deltas -- always referred to origin - // o-r o-i o-s o-f - VectorXs Dx_or(7), Dx_oi(7), Dx_os(7), Dx_of(7); - - // time stamps and intervals - TimeStamp t_o(0), t_r(1), t_i, t_f(5); // we'll set t_i later - - WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_f: ", t_f.get()); - - // Motion structures - Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0); - - - /////////// start experiment /////////////// - - // set ref and final deltas - dx_or << 1,2,3, 4,5,6,7; dx_or.tail<4>().normalize(); - dx_rf << 7,6,5, 4,3,2,1; dx_rf.tail<4>().normalize(); - Dx_or = dx_or; - prc.deltaPlusDelta(Dx_or, dx_rf, t_f - t_r, Dx_of); - Dx_os = Dx_of; - - // set ref - R.ts_ = t_r; - R.delta_ = dx_or; // origin to ref - R.delta_integr_ = Dx_or; // origin to ref - - WOLF_DEBUG("* R.d = ", R.delta_.transpose()); - WOLF_DEBUG(" or = ", dx_or.transpose()); - ASSERT_MATRIX_APPROX(R.delta_ , dx_or, Constants::EPS); - - WOLF_DEBUG(" R.D = ", R.delta_integr_.transpose()); - WOLF_DEBUG(" or = ", Dx_or.transpose()); - ASSERT_MATRIX_APPROX(R.delta_integr_ , Dx_or, Constants::EPS); - - // set final - F.ts_ = t_f; - F.delta_ = dx_rf; // ref to final - F.delta_integr_ = Dx_of; // origin to final - - WOLF_DEBUG("* F.d = ", F.delta_.transpose()); - WOLF_DEBUG(" rf = ", dx_rf.transpose()); - ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS); - - WOLF_DEBUG(" F.D = ", F.delta_integr_.transpose()); - WOLF_DEBUG(" of = ", Dx_of.transpose()); - ASSERT_MATRIX_APPROX(F.delta_integr_ , Dx_of, Constants::EPS); - - // interpolate! - t_i = 2; /// 25% between refs! - WOLF_DEBUG("*** INTERPOLATE *** I and S have been computed."); - I = prc.interpolate(R, F, t_i, S); - - - prc.deltaPlusDelta(R.delta_integr_, I.delta_, t_i-t_r, Dx_oi); - ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_oi, Constants::EPS); - - prc.deltaPlusDelta(I.delta_, S.delta_, t_f-t_i, dx_rf); - ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS); - -} - - - - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp deleted file mode 100644 index c2b8958d0ca9170f4ea317404b0c0cf2ccb9457f..0000000000000000000000000000000000000000 --- a/test/gtest_pack_KF_buffer.cpp +++ /dev/null @@ -1,244 +0,0 @@ -/* - * gtest_pack_KF_buffer.cpp - * - * Created on: Mar 5, 2018 - * Author: jsola - */ -//Wolf -#include "utils_gtest.h" - -#include "core/processor/processor_odom_2D.h" -#include "core/sensor/sensor_odom_2D.h" - -#include "core/processor/processor_tracker_feature_dummy.h" -#include "core/capture/capture_void.h" - -#include "core/problem/problem.h" - -// STL -#include <iterator> -#include <iostream> - -using namespace wolf; -using namespace Eigen; - -class BufferPackKeyFrameTest : public testing::Test -{ - public: - - PackKeyFrameBuffer pack_kf_buffer; - FrameBasePtr f10, f20, f21, f28; - Scalar tt10, tt20, tt21, tt28; - TimeStamp timestamp; - Scalar timetolerance; - - void SetUp(void) - { - f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr); - f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr); - f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr); - f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr); - - tt10 = 1.0; - tt20 = 1.0; - tt21 = 1.0; - tt28 = 1.0; - }; -}; - -TEST_F(BufferPackKeyFrameTest, empty) -{ - ASSERT_TRUE(pack_kf_buffer.empty()); -} - -TEST_F(BufferPackKeyFrameTest, add) -{ - pack_kf_buffer.add(f10, tt10); - ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1); - pack_kf_buffer.add(f20, tt20); - ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2); -} - -TEST_F(BufferPackKeyFrameTest, clear) -{ - pack_kf_buffer.add(f10, tt10); - pack_kf_buffer.add(f20, tt20); - ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2); - pack_kf_buffer.clear(); - ASSERT_TRUE(pack_kf_buffer.empty()); -} - -//TEST_F(BufferPackKeyFrameTest, print) -//{ -// kfpackbuffer.clear(); -// kfpackbuffer.add(f10, tt10); -// kfpackbuffer.add(f20, tt20); -// kfpackbuffer.print(); -//} - -TEST_F(BufferPackKeyFrameTest, checkTimeTolerance) -{ - pack_kf_buffer.clear(); - pack_kf_buffer.add(f10, tt10); - pack_kf_buffer.add(f20, tt20); - // min time tolerance > diff between time stamps. It should return true - ASSERT_TRUE(pack_kf_buffer.checkTimeTolerance(10, 20, 20, 20)); - // min time tolerance < diff between time stamps. It should return true - ASSERT_FALSE(pack_kf_buffer.checkTimeTolerance(10, 1, 20, 20)); -} - -TEST_F(BufferPackKeyFrameTest, selectPack) -{ - // Evaluation using two packs (p1,p2) - // with different time tolerances (tp1,tp2) - // using a query pack (q) with also different time tolerances - // depending on these tolerances we will get one (p1) or the other (p2) - // packages from the buffer (res). - // This can be summarized in the table hereafter: - // - // p1 p2 q | resulting pack time stamp - // -------------------------------- - // 2 2 2 | nullptr - // 2 2 5 | nullptr - // 2 2 7 | nullptr - // 2 7 2 | nullptr - // 2 7 5 | 20 - // 2 7 7 | 20 - // 7 2 2 | nullptr - // 7 2 5 | nullptr - // 7 2 7 | 10 - // 7 7 2 | nullptr - // 7 7 5 | 20 - // 7 7 7 | 20 - - pack_kf_buffer.clear(); - - // input packages - std::vector<int> p1 = {2, 7}; // Pack 1 time tolerances - std::vector<int> p2 = {2, 7}; // Pack 2 time tolerances - std::vector<int> q = {2, 5, 7}; // Query pack time tolerances - - // Solution matrix - Eigen::VectorXi res = Eigen::VectorXi::Zero(12); - res(4) = 20; - res(5) = 20; - res(8) = 10; - res(10) = 20; - res(11) = 20; - - // test - for (unsigned int ip1=0;ip1<p1.size();++ip1) - { - for (unsigned int ip2=0;ip2<p2.size();++ip2) - { - pack_kf_buffer.add(f10, p1[ip1]); - pack_kf_buffer.add(f20, p2[ip2]); - for (unsigned int iq=0;iq<q.size();++iq) - { - PackKeyFramePtr packQ = pack_kf_buffer.selectPack(16, q[iq]); - if (packQ!=nullptr) - ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq)); - } - pack_kf_buffer.clear(); - } - } -} - -TEST_F(BufferPackKeyFrameTest, selectFirstPackBefore) -{ - pack_kf_buffer.clear(); - - pack_kf_buffer.add(f10, tt10); - pack_kf_buffer.add(f20, tt20); - pack_kf_buffer.add(f21, tt21); - - // input time stamps - std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5}; - Scalar tt = 0.01; - - // Solution matrix - // q_ts | pack - //================= - // first time - //----------------- - // 9.5 nullptr - // 9.995 10 - // 10,005 10 - // 19.5 10 - // 20.5 10 - // 21.5 10 - // second time - //----------------- - // 9.5 nullptr - // 9.995 null - // 10,005 null - // 19.5 null - // 20.5 20 - // 21.5 20 - // third time - //----------------- - // 9.5 nullptr - // 9.995 null - // 10,005 null - // 19.5 null - // 20.5 null - // 21.5 21 - - Eigen::MatrixXs truth(3,6), res(3,6); - truth << 0,10,10,10,10,10, 0,0,0,0,20,20, 0,0,0,0,0,21; - res.setZero(); - - for (int i=0; i<3; i++) - { - PackKeyFramePtr packQ; - int j = 0; - for (auto ts : q_ts) - { - packQ = pack_kf_buffer.selectFirstPackBefore(ts, tt); - if (packQ) - res(i,j) = packQ->key_frame->getTimeStamp().get(); - - j++; - } - pack_kf_buffer.removeUpTo(packQ->key_frame->getTimeStamp()); - } - - ASSERT_MATRIX_APPROX(res, truth, 1e-6); -} - -TEST_F(BufferPackKeyFrameTest, removeUpTo) -{ - // Small time tolerance for all test asserts - Scalar tt = 0.1; - pack_kf_buffer.clear(); - pack_kf_buffer.add(f10, tt10); - pack_kf_buffer.add(f20, tt20); - pack_kf_buffer.add(f21, tt21); - - // it should remove f20 and f10, thus size should be 1 after removal - // Specifically, only f21 should remain - PackKeyFramePtr pack20 = std::make_shared<PackKeyFrame>(f20,tt20); - pack_kf_buffer.removeUpTo( pack20->key_frame->getTimeStamp() ); - ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1); - ASSERT_TRUE(pack_kf_buffer.selectPack(f10->getTimeStamp(),tt)==nullptr); - ASSERT_TRUE(pack_kf_buffer.selectPack(f20->getTimeStamp(),tt)==nullptr); - ASSERT_TRUE(pack_kf_buffer.selectPack(f21->getTimeStamp(),tt)!=nullptr); - - // Chech removal of an imprecise time stamp - // Specifically, only f28 should remain - pack_kf_buffer.add(f28, tt28); - ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2); - FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr); - PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5); - pack_kf_buffer.removeUpTo( pack22->key_frame->getTimeStamp() ); - ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1); - ASSERT_TRUE(pack_kf_buffer.selectPack(f21->getTimeStamp(),tt)==nullptr); - ASSERT_TRUE(pack_kf_buffer.selectPack(f28->getTimeStamp(),tt)!=nullptr); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp deleted file mode 100644 index 7af68fa025934e82be84349c5710f823736acdd7..0000000000000000000000000000000000000000 --- a/test/gtest_param_prior.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/* - * gtest_param_prior.cpp - * - * Created on: Feb 6, 2019 - * Author: jvallve - */ - -#include "utils_gtest.h" -#include "core/utils/logging.h" - -#include "core/problem/problem.h" -#include "core/ceres_wrapper/ceres_manager.h" -#include "core/sensor/sensor_odom_3D.h" - -#include <iostream> - -using namespace wolf; - -ProblemPtr problem_ptr = Problem::create("PO 3D"); -CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr); -Eigen::Vector7s initial_extrinsics((Eigen::Vector7s() << 1, 2, 3, 1, 0, 0, 0).finished()); -Eigen::Vector7s prior_extrinsics((Eigen::Vector7s() << 10, 20, 30, 0, 0, 0, 1).finished()); -Eigen::Vector7s prior2_extrinsics((Eigen::Vector7s() << 100, 200, 300, 0, 0, 0, 1).finished()); - -SensorOdom3DPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3D>(problem_ptr->installSensor("ODOM 3D", "odometer", initial_extrinsics, std::make_shared<IntrinsicsOdom3D>())); - -TEST(ParameterPrior, initial_extrinsics) -{ - ASSERT_TRUE(problem_ptr->check(0)); - ASSERT_TRUE(odom_sensor_ptr_->getP()); - ASSERT_TRUE(odom_sensor_ptr_->getO()); - ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),initial_extrinsics.head(3),1e-9); - ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),initial_extrinsics.tail(4),1e-9); -} - -TEST(ParameterPrior, prior_p) -{ - odom_sensor_ptr_->addPriorP(prior_extrinsics.head(3),Eigen::Matrix3s::Identity()); - - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); - - ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior_extrinsics.head(3),1e-6); -} - -TEST(ParameterPrior, prior_o) -{ - odom_sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Eigen::Matrix3s::Identity()); - - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); - - ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),prior_extrinsics.tail(4),1e-6); -} - -TEST(ParameterPrior, prior_p_overwrite) -{ - odom_sensor_ptr_->addPriorP(prior2_extrinsics.head(3),Eigen::Matrix3s::Identity()); - - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); - - ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior2_extrinsics.head(3),1e-6); -} - -TEST(ParameterPrior, prior_p_segment) -{ - odom_sensor_ptr_->addPriorP(prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2); - - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); - - ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6); -} - -TEST(ParameterPrior, prior_o_segment) -{ - // constraining segment of quaternion is not allowed - ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter(1, prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2),""); -} - - - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_pinhole.cpp b/test/gtest_pinhole.cpp deleted file mode 100644 index 376f15c10f3713b166e78e33feff6521c268480f..0000000000000000000000000000000000000000 --- a/test/gtest_pinhole.cpp +++ /dev/null @@ -1,201 +0,0 @@ -/* - * gtest_pinhole.cpp - * - * Created on: Nov 27, 2016 - * Author: jsola - */ - -#include "core/math/pinhole_tools.h" -#include "utils_gtest.h" - -using namespace Eigen; -using namespace wolf; -using namespace pinhole; -using Constants::EPS; -using Constants::EPS_SMALL; - -TEST(Pinhole, EigenTypes) -{ - Vector3s vs; vs << 1,2,4; - Vector2s ps; - Map<Vector3s> vm(vs.data()); - Map<Vector2s> pm(ps.data()); - Map<const Vector3s> cvm(vs.data()); - VectorXs vd(3); vd = vs; - VectorXs pd(2); - - Vector2s pe; pe << 0.25, 0.5; // expected result - - // static size - projectPointToNormalizedPlane(vs,ps); - ASSERT_TRUE((ps - pe).isMuchSmallerThan(1, EPS_SMALL)); - - // dynamic size - projectPointToNormalizedPlane(vd,pd); - ASSERT_TRUE((pd - pe).isMuchSmallerThan(1, EPS_SMALL)); - - // Map - projectPointToNormalizedPlane(vm,pm); - ASSERT_TRUE((pm - pe).isMuchSmallerThan(1, EPS_SMALL)); - - // Map const - projectPointToNormalizedPlane(cvm,pm); - ASSERT_TRUE((pm - pe).isMuchSmallerThan(1, EPS_SMALL)); -} - -TEST(Pinhole, pix_pnt_pix) -{ - Vector4s k; k << 516.686, 355.129, 991.852, 995.269; // From Joan Sola thesis example - Vector2s d; d << -0.301701, 0.0963189; - Vector2s u0; - Vector3s p; - Vector2s u1; - Vector2s c; // should be close to (0.297923, 0.216263) - Scalar depth = 10; - Scalar pix_error_allowed = 0.1; - - computeCorrectionModel(k, d, c); - - WOLF_DEBUG("correction: ", c.transpose(), " // should be close to (0.297923, 0.216263)"); - - // choose some random points - u0 << 123, 321; - p = backprojectPoint(k, c, u0, depth); - u1 = projectPoint(k, d, p); - WOLF_DEBUG("error: ", (u1-u0).transpose()); - ASSERT_DOUBLE_EQ(p(2), depth); - ASSERT_LT((u1 - u0).norm(), pix_error_allowed) << "u1: "<< u1.transpose() << "\nu0: " << u0.transpose(); - - u0 << 1, 1; - p = backprojectPoint(k, c, u0, depth); - u1 = projectPoint(k, d, p); - WOLF_DEBUG("error: ", (u1-u0).transpose()); - ASSERT_DOUBLE_EQ(p(2), depth); - ASSERT_LT((u1 - u0).norm(), pix_error_allowed) << "u1: "<< u1.transpose() << "\nu0: " << u0.transpose(); - - u0 << 320, 240; - p = backprojectPoint(k, c, u0, depth); - u1 = projectPoint(k, d, p); - WOLF_DEBUG("error: ", (u1-u0).transpose()); - ASSERT_DOUBLE_EQ(p(2), depth); - ASSERT_LT((u1 - u0).norm(), pix_error_allowed) << "u1: "<< u1.transpose() << "\nu0: " << u0.transpose(); - - u0 << 640, 480; - p = backprojectPoint(k, c, u0, depth); - u1 = projectPoint(k, d, p); - WOLF_DEBUG("error: ", (u1-u0).transpose()); - ASSERT_DOUBLE_EQ(p(2), depth); - ASSERT_LT((u1 - u0).norm(), pix_error_allowed) << "u1: "<< u1.transpose() << "\nu0: " << u0.transpose(); -} - -TEST(Pinhole, Jacobians) -{ - Vector4s k; k << 516.686, 355.129, 991.852, 995.269; // From Joan Sola thesis example - Vector2s d; d << -0.301701, 0.0963189; - Vector3s v; - Vector2s u; - MatrixXs U_v(2, 3); - - v << 1,2,4; - projectPoint(k, d, v, u, U_v); - - Vector2s c; - computeCorrectionModel(k, d, c); - - Vector3s p; - MatrixXs P_u(3,2), P_depth(3,1); - backprojectPoint(k, c, u, 4, p, P_u, P_depth); - - // check that reprojected point is close to original - ASSERT_LT((p-v).norm(), 1e-3) << "p: " << p.transpose() << "\nv: " << v.transpose(); - - // Check that both Jacobians are inverse one another (in the smallest dimension) - ASSERT_TRUE((U_v*P_u - Matrix2s::Identity()).isMuchSmallerThan(1, 1e-3)) << "U_v*P_u: " << U_v*P_u; - - WOLF_DEBUG("U_v*P_u: \n", U_v*P_u); - WOLF_DEBUG("P_u*U_v: \n", P_u*U_v); -} - -TEST(Pinhole, JacobiansDist) -{ - Vector4s k; k << 516.686, 355.129, 991.852, 995.269; // From Joan Sola thesis example - Vector2s d; d << -0.301701, 0.0963189; - Vector3s v; - Vector2s u; - MatrixXs U_v(2, 3); - Scalar dist; - - v << 1,2,4; - projectPoint(k, d, v, u, dist, U_v); - - // check that the recovered distance is indeed the distance to v - ASSERT_DOUBLE_EQ(dist, v.norm()); - - Vector2s c; - computeCorrectionModel(k, d, c); - - Vector3s p; - MatrixXs P_u(3,2), P_depth(3,1); - backprojectPoint(k, c, u, 4, p, P_u, P_depth); - - // check that reprojected point is close to original - ASSERT_LT((p-v).norm(), 1e-3) << "p: " << p.transpose() << "\nv: " << v.transpose(); - - // Check that both Jacobians are inverse one another (in the smallest dimension) - ASSERT_TRUE((U_v*P_u - Matrix2s::Identity()).isMuchSmallerThan(1, 1e-3)) << "U_v*P_u: " << U_v*P_u; -} - -TEST(Pinhole, isInRoi) -{ - Vector2s p; - p << 15, 15; - - ASSERT_TRUE (isInRoi(p, 10, 10, 10, 10)); - ASSERT_FALSE(isInRoi(p, 0, 0, 10, 10)); - ASSERT_FALSE(isInRoi(p, 0, 10, 10, 10)); - ASSERT_FALSE(isInRoi(p, 0, 20, 10, 10)); - ASSERT_FALSE(isInRoi(p, 10, 0, 10, 10)); - ASSERT_FALSE(isInRoi(p, 10, 20, 10, 10)); - ASSERT_FALSE(isInRoi(p, 20, 0, 10, 10)); - ASSERT_FALSE(isInRoi(p, 20, 10, 10, 10)); - ASSERT_FALSE(isInRoi(p, 20, 20, 10, 10)); -} - -TEST(Pinhole, isInImage) -{ - Vector2s p; - p << 15, 15; - ASSERT_TRUE (isInImage(p, 640, 480)); - - p << -15, -15; - ASSERT_FALSE (isInImage(p, 640, 480)); - - p << -15, 15; - ASSERT_FALSE (isInImage(p, 640, 480)); - - p << -15, 500; - ASSERT_FALSE (isInImage(p, 640, 480)); - - p << 15, -15; - ASSERT_FALSE (isInImage(p, 640, 480)); - - p << 15, 500; - ASSERT_FALSE (isInImage(p, 640, 480)); - - p << 700, -15; - ASSERT_FALSE (isInImage(p, 640, 480)); - - p << 700, 15; - ASSERT_FALSE (isInImage(p, 640, 480)); - - p << 700, 500; - ASSERT_FALSE (isInImage(p, 640, 480)); - -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp deleted file mode 100644 index 52383ad71c217a02afdc430fd8aa287f30d2c1b0..0000000000000000000000000000000000000000 --- a/test/gtest_problem.cpp +++ /dev/null @@ -1,287 +0,0 @@ -/* - * gtest_problem.cpp - * - * Created on: Nov 23, 2016 - * Author: jsola - */ - -#include "utils_gtest.h" -#include "core/utils/logging.h" - -#include "core/problem/problem.h" -#include "core/sensor/sensor_base.h" -#include "core/sensor/sensor_odom_3D.h" -#include "core/processor/processor_odom_3D.h" -#include "core/processor/processor_tracker_feature_dummy.h" - -#include <iostream> - -using namespace wolf; -using namespace Eigen; - -TEST(Problem, create) -{ - ProblemPtr P = Problem::create("POV 3D"); - - // check double ointers to branches - ASSERT_EQ(P, P->getHardware()->getProblem()); - ASSERT_EQ(P, P->getTrajectory()->getProblem()); - ASSERT_EQ(P, P->getMap()->getProblem()); - - // check frame structure through the state size - ASSERT_EQ(P->getFrameStructureSize(), 10); -} - -TEST(Problem, Sensors) -{ - ProblemPtr P = Problem::create("POV 3D"); - - // add a dummy sensor - SensorBasePtr S = std::make_shared<SensorBase>("Dummy", nullptr, nullptr, nullptr, 2, false); - P->addSensor(S); - - // check pointers - ASSERT_EQ(P, S->getProblem()); - ASSERT_EQ(P->getHardware(), S->getHardware()); - -} - -TEST(Problem, Processor) -{ - ProblemPtr P = Problem::create("PO 3D"); - - // check motion processor is null - ASSERT_FALSE(P->getProcessorMotion()); - - // add a motion sensor and processor - SensorBasePtr Sm = std::make_shared<SensorOdom3D>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); // with dummy intrinsics - P->addSensor(Sm); - - // add motion processor - ProcessorMotionPtr Pm = std::make_shared<ProcessorOdom3D>(std::make_shared<ProcessorParamsOdom3D>()); - Sm->addProcessor(Pm); - - // check motion processor IS NOT set by addSensor <-- using InstallProcessor it should, see test Installers - ASSERT_FALSE(P->getProcessorMotion()); - - // set processor motion - P->setProcessorMotion(Pm); - // re-check motion processor IS set by addSensor - ASSERT_EQ(P->getProcessorMotion(), Pm); -} - -TEST(Problem, Installers) -{ - std::string wolf_root = _WOLF_ROOT_DIR; - ProblemPtr P = Problem::create("PO 3D"); - Eigen::Vector7s xs; - - SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/src/examples/sensor_odom_3D.yaml"); - - // install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test) - ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>(); - params->time_tolerance = 0.1; - params->max_new_features = 5; - params->min_features_for_keyframe = 10; - ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params)); - S->addProcessor(pt); - - // check motion processor IS NOT set - ASSERT_FALSE(P->getProcessorMotion()); - - // install processor motion - ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml"); - - // check motion processor is set - ASSERT_TRUE(P->getProcessorMotion()); - - // check motion processor is correct - ASSERT_EQ(P->getProcessorMotion(), pm); -} - -TEST(Problem, SetOrigin_PO_2D) -{ - ProblemPtr P = Problem::create("PO 2D"); - TimeStamp t0(0); - Eigen::VectorXs x0(3); x0 << 1,2,3; - Eigen::MatrixXs P0(3,3); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id - - P->setPrior(x0, P0, t0, 1.0); - - // check that no sensor has been added - ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 0); - - // check that the state is correct - ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL)); - - // check that we have one frame, one capture, one feature, one factor - TrajectoryBasePtr T = P->getTrajectory(); - ASSERT_EQ(T->getFrameList().size(), (unsigned int) 1); - FrameBasePtr F = P->getLastFrame(); - ASSERT_EQ(F->getCaptureList().size(), (unsigned int) 1); - CaptureBasePtr C = F->getCaptureList().front(); - ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1); - FeatureBasePtr f = C->getFeatureList().front(); - ASSERT_EQ(f->getFactorList().size(), (unsigned int) 1); - - // check that the factor is absolute (no pointers to other F, f, or L) - FactorBasePtr c = f->getFactorList().front(); - ASSERT_FALSE(c->getFrameOther()); - ASSERT_FALSE(c->getLandmarkOther()); - - // check that the Feature measurement and covariance are the ones provided - ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL)); - ASSERT_TRUE((P0 - f->getMeasurementCovariance()).isMuchSmallerThan(1, Constants::EPS_SMALL)); - - // P->print(4,1,1,1); -} - -TEST(Problem, SetOrigin_PO_3D) -{ - ProblemPtr P = Problem::create("PO 3D"); - TimeStamp t0(0); - Eigen::VectorXs x0(7); x0 << 1,2,3,4,5,6,7; - Eigen::MatrixXs P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id - - P->setPrior(x0, P0, t0, 1.0); - - // check that no sensor has been added - ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 0); - - // check that the state is correct - ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL)); - - // check that we have one frame, one capture, one feature, one factor - TrajectoryBasePtr T = P->getTrajectory(); - ASSERT_EQ(T->getFrameList().size(), (unsigned int) 1); - FrameBasePtr F = P->getLastFrame(); - ASSERT_EQ(F->getCaptureList().size(), (unsigned int) 1); - CaptureBasePtr C = F->getCaptureList().front(); - ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1); - FeatureBasePtr f = C->getFeatureList().front(); - ASSERT_EQ(f->getFactorList().size(), (unsigned int) 1); - - // check that the factor is absolute (no pointers to other F, f, or L) - FactorBasePtr c = f->getFactorList().front(); - ASSERT_FALSE(c->getFrameOther()); - ASSERT_FALSE(c->getLandmarkOther()); - - // check that the Feature measurement and covariance are the ones provided - ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL)); - ASSERT_TRUE((P0 - f->getMeasurementCovariance()).isMuchSmallerThan(1, Constants::EPS_SMALL)); - - // P->print(4,1,1,1); -} - -TEST(Problem, emplaceFrame_factory) -{ - ProblemPtr P = Problem::create("PO 2D"); - - FrameBasePtr f0 = P->emplaceFrame("PO 2D", KEY_FRAME, VectorXs(3), TimeStamp(0.0)); - FrameBasePtr f1 = P->emplaceFrame("PO 3D", KEY_FRAME, VectorXs(7), TimeStamp(1.0)); - FrameBasePtr f2 = P->emplaceFrame("POV 3D", KEY_FRAME, VectorXs(10), TimeStamp(2.0)); - - // std::cout << "f0: type PO 2D? " << f0->getType() << std::endl; - // std::cout << "f1: type PO 3D? " << f1->getType() << std::endl; - // std::cout << "f2: type POV 3D? " << f2->getType() << std::endl; - - ASSERT_EQ(f0->getType().compare("PO 2D"), 0); - ASSERT_EQ(f1->getType().compare("PO 3D"), 0); - ASSERT_EQ(f2->getType().compare("POV 3D"), 0); - - // check that all frames are effectively in the trajectory - ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (unsigned int) 3); - - // check that all frames are linked to Problem - ASSERT_EQ(f0->getProblem(), P); - ASSERT_EQ(f1->getProblem(), P); - ASSERT_EQ(f2->getProblem(), P); -} - -TEST(Problem, StateBlocks) -{ - std::string wolf_root = _WOLF_ROOT_DIR; - - ProblemPtr P = Problem::create("PO 3D"); - Eigen::Vector7s xs; - - // 2 state blocks, fixed - SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml"); - ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) 2); - ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) 2); - - // 3 state blocks, fixed - SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml"); - ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) (2 + 3)); - ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) (2 + 3)); - - ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>(); - params->time_tolerance = 0.1; - params->max_new_features = 5; - params->min_features_for_keyframe = 10; - ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params)); - - St->addProcessor(pt); - ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml"); - - // 2 state blocks, estimated - P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0); - ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2)); - ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2)); - - // P->print(4,1,1,1); - - // change some SB properties - St->unfixExtrinsics(); - ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2)); - ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 3 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB! - - // P->print(4,1,1,1); -} - -TEST(Problem, Covariances) -{ - std::string wolf_root = _WOLF_ROOT_DIR; - - ProblemPtr P = Problem::create("PO 3D"); - Eigen::Vector7s xs; - - SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml"); - SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml"); - - ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>(); - params->time_tolerance = 0.1; - params->max_new_features = 5; - params->min_features_for_keyframe = 10; - ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params)); - - St->addProcessor(pt); - ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml"); - - // 4 state blocks, estimated - St->unfixExtrinsics(); - FrameBasePtr F = P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0); - - // set covariance (they are not computed without a solver) - P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity()); - P->addCovarianceBlock(F->getO(), Eigen::Matrix4s::Identity()); - P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix<Scalar,3,4>::Zero()); - - // get covariance - Eigen::MatrixXs Cov; - ASSERT_TRUE(P->getFrameCovariance(F, Cov)); - - // FIXME Frame covariance should be 6x6, but it is actually 7x7 (the state of the state blocks, not of the local parametrizations) - // JV: The local parameterization projects the covariance to the state size. - ASSERT_EQ(Cov.cols() , 7); - ASSERT_EQ(Cov.rows() , 7); - ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix7s::Identity(), 1e-12); - -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp index a76656dc84802e785af7aa87fef939c3bfacbba9..16bcaa48084f23a1167c664dfff2f1208137d411 100644 --- a/test/gtest_processor_IMU.cpp +++ b/test/gtest_processor_IMU.cpp @@ -48,7 +48,7 @@ class ProcessorIMUt : public testing::Test std::string wolf_root = _WOLF_ROOT_DIR; // Wolf problem - problem = Problem::create("POV 3D"); + problem = Problem::create("POV", 3); Vector7s extrinsics = (Vector7s() << 0,0,0, 0,0,0,1).finished(); sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml"); ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml"); @@ -97,7 +97,7 @@ TEST(ProcessorIMU_constructors, ALL) //Factory constructor without yaml std::string wolf_root = _WOLF_ROOT_DIR; - ProblemPtr problem = Problem::create("POV 3D"); + ProblemPtr problem = Problem::create("POV", 3); Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished(); SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml"); ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", ""); @@ -127,7 +127,7 @@ TEST(ProcessorIMU, voteForKeyFrame) std::string wolf_root = _WOLF_ROOT_DIR; // Wolf problem - ProblemPtr problem = Problem::create("POV 3D"); + ProblemPtr problem = Problem::create("POV", 3); Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished(); SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml"); ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>(); @@ -144,7 +144,6 @@ TEST(ProcessorIMU, voteForKeyFrame) x0 << 0,0,0, 0,0,0,1, 0,0,0; MatrixXs P0(9,9); P0.setIdentity(); problem->setPrior(x0, P0, t, 0.01); - //data variable and covariance matrix // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions Vector6s data; diff --git a/test/gtest_processor_IMU_jacobians.cpp b/test/gtest_processor_IMU_jacobians.cpp index 79edfc53f395bfabc34fcdf684088ae87a4e71f8..f3c0cda67a0a3934a42a664b4c5300f1581389f5 100644 --- a/test/gtest_processor_IMU_jacobians.cpp +++ b/test/gtest_processor_IMU_jacobians.cpp @@ -61,7 +61,7 @@ class ProcessorIMU_jacobians : public testing::Test data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad; // Wolf problem - ProblemPtr wolf_problem_ptr_ = Problem::create("POV 3D"); + ProblemPtr wolf_problem_ptr_ = Problem::create("POV", 3); Eigen::VectorXs IMU_extrinsics(7); IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot @@ -140,7 +140,7 @@ class ProcessorIMU_jacobians_Dq : public testing::Test data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad; // Wolf problem - ProblemPtr wolf_problem_ptr_ = Problem::create("POV 3D"); + ProblemPtr wolf_problem_ptr_ = Problem::create("POV", 3); Eigen::VectorXs IMU_extrinsics(7); IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp deleted file mode 100644 index 6f0f7fa127f3d4c78ae216c2c218dc246ac75212..0000000000000000000000000000000000000000 --- a/test/gtest_processor_base.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/* - * gtest_capture_base.cpp - * - * Created on: Feb 15, 2018 - * Author: asantamaria - */ - -//Wolf -#include "utils_gtest.h" - -#include "core/processor/processor_odom_2D.h" -#include "core/sensor/sensor_odom_2D.h" - -#include "core/processor/processor_tracker_feature_dummy.h" -#include "core/capture/capture_void.h" - -#include "core/problem/problem.h" - -// STL -#include <iterator> -#include <iostream> - -using namespace wolf; -using namespace Eigen; - -TEST(ProcessorBase, KeyFrameCallback) -{ - - using namespace wolf; - using std::shared_ptr; - using std::make_shared; - using std::static_pointer_cast; - using Eigen::Vector2s; - - Scalar dt = 0.01; - - // Wolf problem - ProblemPtr problem = Problem::create("PO 2D"); - - // Install tracker (sensor and processor) - SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), - std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), - std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); - - ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>(); - params->time_tolerance = dt/2; - params->max_new_features = 5; - params->min_features_for_keyframe = 5; - shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(params); - - problem->addSensor(sens_trk); - sens_trk->addProcessor(proc_trk); - - // Install odometer (sensor and processor) - SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), ""); - ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>(); - proc_odo_params->time_tolerance = dt/2; - ProcessorBasePtr proc_odo = problem->installProcessor("ODOM 2D", "odometer", sens_odo, proc_odo_params); - - std::cout << "sensor & processor created and added to wolf problem" << std::endl; - - // Sequence to test KeyFrame creations (callback calls) - - // initialize - TimeStamp t(0.0); - Vector3s x(0,0,0); - Matrix3s P = Matrix3s::Identity() * 0.1; - problem->setPrior(x, P, t, dt/2); // KF1 - - CaptureOdom2DPtr capt_odo = make_shared<CaptureOdom2D>(t, sens_odo, Vector2s(0.5,0)); - - // Track - CaptureVoidPtr capt_trk(make_shared<CaptureVoid>(t, sens_trk)); - proc_trk->process(capt_trk); - - for (size_t ii=0; ii<10; ii++ ) - { - // Move - t = t+dt; - WOLF_INFO("----------------------- ts: ", t , " --------------------------"); - - capt_odo->setTimeStamp(t); - proc_odo->process(capt_odo); - - // Track - capt_trk = make_shared<CaptureVoid>(t, sens_trk); - proc_trk->process(capt_trk); - -// problem->print(4,1,1,0); - - // Only odom creating KFs - ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2D")==0 ); - } -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp deleted file mode 100644 index 187fffeae8dfe4f6302fd99cb7029b003ddd8b8f..0000000000000000000000000000000000000000 --- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp +++ /dev/null @@ -1,235 +0,0 @@ - -/** - * \file gtest_processor_frame_nearest_neighbor_filter.cpp - * - * Created on: Aug 2, 2017 - * \author: tessajohanna - */ - -#include "utils_gtest.h" -#include "core/utils/logging.h" - -#include "core/sensor/sensor_odom_2D.h" -#include "core/processor/processor_frame_nearest_neighbor_filter.h" - -#include <iostream> - -// Dummy class so that it is no pur virtual -struct DummyLoopCloser : public wolf::ProcessorFrameNearestNeighborFilter -{ - DummyLoopCloser(ParamsPtr _params) : - wolf::ProcessorFrameNearestNeighborFilter(_params) { } - - bool confirmLoopClosure() override { return false; } - bool voteForKeyFrame() override { return false; } -}; - -// Declare Wolf problem -wolf::ProblemPtr problem = wolf::Problem::create("PO 2D"); - -// Declare Sensor -Eigen::Vector3s odom_extrinsics = Eigen::Vector3s(0,0,0); - -std::shared_ptr<wolf::IntrinsicsOdom2D> odom_intrinsics = - std::make_shared<wolf::IntrinsicsOdom2D>(wolf::IntrinsicsOdom2D()); - -wolf::SensorBasePtr sensor_ptr; - -// Declare Processors -wolf::ProcessorFrameNearestNeighborFilterPtr processor_ptr_point2d; -wolf::ProcessorParamsFrameNearestNeighborFilterPtr processor_params_point2d = - std::make_shared<wolf::ProcessorParamsFrameNearestNeighborFilter>(); - -wolf::ProcessorFrameNearestNeighborFilterPtr processor_ptr_ellipse2d; -wolf::ProcessorParamsFrameNearestNeighborFilterPtr processor_params_ellipse2d = - std::make_shared<wolf::ProcessorParamsFrameNearestNeighborFilter>(); - -// Declare Frame Pointer -wolf::StateBlockPtr stateblock_ptr1, stateblock_ptr2, stateblock_ptr3, stateblock_ptr4; -wolf::FrameBasePtr F1, F2, F3, F4; -wolf::CaptureBasePtr capture_dummy, incomming_dummy; - -//############################################################################## -TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated) -{ - // four different states [x, y] - Eigen::Vector3s state1, state2, state3, state4; - state1 << 3.0, 5.0, 0.0; - state2 << 3.0, 6.0, 0.0; - state3 << 3.0, 7.0, 0.0; - state4 << 100.0, 100.0, 0.0; - - auto stateblock_pptr1 = std::make_shared<wolf::StateBlock>(state1.head<2>()); - auto stateblock_optr1 = std::make_shared<wolf::StateBlock>(state1.tail<1>()); - - auto stateblock_pptr2 = std::make_shared<wolf::StateBlock>(state2.head<2>()); - auto stateblock_optr2 = std::make_shared<wolf::StateBlock>(state2.tail<1>()); - - auto stateblock_pptr3 = std::make_shared<wolf::StateBlock>(state3.head<2>()); - auto stateblock_optr3 = std::make_shared<wolf::StateBlock>(state3.tail<1>()); - - auto stateblock_pptr4 = std::make_shared<wolf::StateBlock>(state4.head<2>()); - auto stateblock_optr4 = std::make_shared<wolf::StateBlock>(state4.tail<1>()); - - // create Keyframes - F1 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, - 1, - stateblock_pptr1, - stateblock_optr1); - F2 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, - 1, - stateblock_pptr2, - stateblock_optr2); - F3 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, - 1, - stateblock_pptr3, - stateblock_optr3); - F4 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, - 1, - stateblock_pptr4, - stateblock_optr4); - - // add dummy capture - capture_dummy = std::make_shared<wolf::CaptureBase>("DUMMY", - 1.0, - sensor_ptr); - F1->addCapture(capture_dummy); - F2->addCapture(capture_dummy); - F3->addCapture(capture_dummy); - F4->addCapture(capture_dummy); - - // Add first three states to trajectory - problem->getTrajectory()->addFrame(F1); - problem->getTrajectory()->addFrame(F2); - problem->getTrajectory()->addFrame(F3); - problem->getTrajectory()->addFrame(F4); - - // Add same covariances for all states - Eigen::Matrix2s position_covariance_matrix; - position_covariance_matrix << 9.0, 0.0, - 0.0, 5.0; - - Eigen::Matrix1s orientation_covariance_matrix; - orientation_covariance_matrix << 0.01; - - Eigen::Vector2s tt_covariance_matrix; - tt_covariance_matrix << 0.0, 0.0; - - problem->addCovarianceBlock( stateblock_pptr1, stateblock_pptr1, - position_covariance_matrix); - problem->addCovarianceBlock( stateblock_optr1, stateblock_optr1, - orientation_covariance_matrix); - problem->addCovarianceBlock( stateblock_pptr1, stateblock_optr1, - tt_covariance_matrix); - - problem->addCovarianceBlock( stateblock_pptr2, stateblock_pptr2, - position_covariance_matrix); - problem->addCovarianceBlock( stateblock_optr2, stateblock_optr2, - orientation_covariance_matrix); - problem->addCovarianceBlock( stateblock_pptr2, stateblock_optr2, - tt_covariance_matrix); - - problem->addCovarianceBlock( stateblock_pptr3, stateblock_pptr3, - position_covariance_matrix); - problem->addCovarianceBlock( stateblock_optr3, stateblock_optr3, - orientation_covariance_matrix); - problem->addCovarianceBlock( stateblock_pptr3, stateblock_optr3, - tt_covariance_matrix); - - problem->addCovarianceBlock( stateblock_pptr4, stateblock_pptr4, - position_covariance_matrix); - problem->addCovarianceBlock( stateblock_optr4, stateblock_optr4, - orientation_covariance_matrix); - problem->addCovarianceBlock( stateblock_pptr4, stateblock_optr4, - tt_covariance_matrix); - - // create dummy capture - incomming_dummy = std::make_shared<wolf::CaptureBase>("DUMMY", - 1.2, - sensor_ptr); - // Make 3rd frame last Keyframe - problem->getTrajectory()->setLastKeyFrame(F3); - - // trigger search for loopclosure - processor_ptr_point2d->process(incomming_dummy); - - const std::vector<wolf::FrameBasePtr> &testloops = - processor_ptr_point2d->getCandidates(); - - ASSERT_EQ(testloops.size(), (unsigned int) 1); - ASSERT_EQ(testloops[0]->id(), (unsigned int) 2); -} - -//############################################################################## -TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance) -{ - // set covariance to a -90 degree turned ellipse - Eigen::Matrix2s position_covariance_matrix; -// position_covariance_matrix << 9.0, 1.8, -// 1.8, 5.0; - - position_covariance_matrix << 5.0, 0.0, - 0.0, 9.0; - - problem->addCovarianceBlock( F1->getP(), F1->getP(), - position_covariance_matrix); - problem->addCovarianceBlock( F2->getP(), F2->getP(), - position_covariance_matrix); - problem->addCovarianceBlock( F3->getP(), F3->getP(), - position_covariance_matrix); - problem->addCovarianceBlock( F4->getP(), F4->getP(), - position_covariance_matrix); - - // Make 3rd frame last Keyframe - problem->getTrajectory()->setLastKeyFrame(F3); - - // trigger search for loopclosure - processor_ptr_ellipse2d->process(incomming_dummy); - const std::vector<wolf::FrameBasePtr> &testloops = - processor_ptr_ellipse2d->getCandidates(); - - ASSERT_EQ(testloops.size(), (unsigned int) 2); - ASSERT_EQ(testloops[0]->id(), (unsigned int) 1); - ASSERT_EQ(testloops[1]->id(), (unsigned int) 2); - - // Make 4th frame last Keyframe - problem->getTrajectory()->setLastKeyFrame(F4); - - // trigger search for loopclosure again - processor_ptr_ellipse2d->process(incomming_dummy); - ASSERT_EQ(testloops.size(), (unsigned int) 0); -} - -//############################################################################## -int main(int argc, char **argv) -{ - // SENSOR PARAMETERS - odom_intrinsics->k_disp_to_disp = 0.2; - odom_intrinsics->k_rot_to_rot = 0.2; - - // install sensor - sensor_ptr = problem->installSensor("ODOM 2D", "odom", - odom_extrinsics, odom_intrinsics); - - // install the processors - processor_params_point2d->probability_ = 0.95; - processor_params_point2d->buffer_size_ = 0; // just exclude identical frameIDs - processor_params_point2d->distance_type_ = wolf::LoopclosureDistanceType::LC_POINT_ELLIPSE; - - processor_ptr_point2d = std::make_shared<DummyLoopCloser>(processor_params_point2d); - processor_ptr_point2d->setName("processor2Dpoint"); - - sensor_ptr->addProcessor(processor_ptr_point2d); - - processor_params_ellipse2d->probability_ = 0.95; - processor_params_ellipse2d->buffer_size_ = 0; // just exclude identical frameIDs - processor_params_ellipse2d->distance_type_ = wolf::LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE; - - processor_ptr_ellipse2d = std::make_shared<DummyLoopCloser>(processor_params_ellipse2d); - processor_ptr_ellipse2d->setName("processor2Dellipse"); - - sensor_ptr->addProcessor(processor_ptr_ellipse2d); - - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp deleted file mode 100644 index f919e0dbc4c40fea2315b6ff144ebffb19884e16..0000000000000000000000000000000000000000 --- a/test/gtest_processor_motion.cpp +++ /dev/null @@ -1,201 +0,0 @@ -/* - * gtest_processor_motion.cpp - * - * Created on: Sep 27, 2017 - * Author: jsola - */ - -#include "utils_gtest.h" - -#include "core/common/wolf.h" -#include "core/utils/logging.h" - -#include "core/sensor/sensor_odom_2D.h" -#include "core/processor/processor_odom_2D.h" -#include "core/ceres_wrapper/ceres_manager.h" - -using namespace Eigen; -using namespace wolf; -using std::static_pointer_cast; - -class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{ - public: - Scalar dt; - ProblemPtr problem; - SensorOdom2DPtr sensor; - ProcessorOdom2DPtr processor; - CaptureMotionPtr capture; - Vector2s data; - Matrix2s data_cov; - - ProcessorMotion_test() : - ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()), - dt(0) - { } - - virtual void SetUp() - { - dt = 1.0; - problem = Problem::create("PO 2D"); - ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>()); - params->time_tolerance = 0.25; - params->dist_traveled = 100; - params->angle_turned = 6.28; - params->max_time_span = 2.5*dt; // force KF at every third process() - params->cov_det = 100; - params->unmeasured_perturbation_std = 0.001; - sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0))); - processor = static_pointer_cast<ProcessorOdom2D>(problem->installProcessor("ODOM 2D", "odom", sensor, params)); - capture = std::make_shared<CaptureMotion>("ODOM 2D", 0.0, sensor, data, data_cov, 3, 3, nullptr); - - Vector3s x0; x0 << 0, 0, 0; - Matrix3s P0; P0.setIdentity(); - problem->setPrior(x0, P0, 0.0, 0.01); - } - - Motion interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts) - { - return ProcessorMotion::interpolate(_ref, _second, _ts); - } - - Motion interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second) - { - return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second); - } - - - virtual void TearDown(){} - -}; - -TEST_F(ProcessorMotion_test, IntegrateStraight) -{ - data << 1, 0; // advance straight - data_cov.setIdentity(); - TimeStamp t(0.0); - - for (int i = 0; i<9; i++) - { - t += dt; - capture->setTimeStamp(t); - capture->setData(data); - capture->setDataCovariance(data_cov); - processor->process(capture); - WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose()); - } - - ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3s()<<9,0,0).finished(), 1e-8); -} - -TEST_F(ProcessorMotion_test, IntegrateCircle) -{ - data << 1, 2*M_PI/10; // advance in circle - data_cov.setIdentity(); - TimeStamp t(0.0); - - for (int i = 0; i<10; i++) // one full turn exactly - { - t += dt; - capture->setTimeStamp(t); - capture->setData(data); - capture->setDataCovariance(data_cov); - processor->process(capture); - WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose()); - } - - ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3s()<<0,0,0).finished(), 1e-8); -} - -TEST_F(ProcessorMotion_test, Interpolate) -{ - data << 1, 2*M_PI/10; // advance in turn - data_cov.setIdentity(); - TimeStamp t(0.0); - std::vector<Motion> motions; - motions.push_back(motionZero(t)); - - for (int i = 0; i<10; i++) // one full turn exactly - { - t += dt; - capture->setTimeStamp(t); - capture->setData(data); - capture->setDataCovariance(data_cov); - processor->process(capture); - motions.push_back(processor->getMotion(t)); - WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose()); - } - - TimeStamp tt = 2.2; - Motion ref = motions[2]; - Motion second = motions[3]; - Motion interp = interpolate(ref, second, tt); - - ASSERT_NEAR( interp.ts_.get() , 2.2 , 1e-8); - ASSERT_MATRIX_APPROX(interp.data_ , VectorXs::Zero(2) , 1e-8); - ASSERT_MATRIX_APPROX(interp.delta_ , VectorXs::Zero(3) , 1e-8); - ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[2].delta_integr_ , 1e-8); - - tt = 2.6; - interp = interpolate(ref, second, tt); - - ASSERT_NEAR( interp.ts_.get() , 2.6 , 1e-8); - ASSERT_MATRIX_APPROX(interp.data_ , motions[3].data_ , 1e-8); - ASSERT_MATRIX_APPROX(interp.delta_ , motions[3].delta_ , 1e-8); - ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[3].delta_integr_ , 1e-8); -} - -TEST_F(ProcessorMotion_test, Interpolate_alternative) -{ - data << 1, 2*M_PI/10; // advance in turn - data_cov.setIdentity(); - TimeStamp t(0.0); - std::vector<Motion> motions; - motions.push_back(motionZero(t)); - - for (int i = 0; i<10; i++) // one full turn exactly - { - t += dt; - capture->setTimeStamp(t); - capture->setData(data); - capture->setDataCovariance(data_cov); - processor->process(capture); - motions.push_back(processor->getMotion(t)); - WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose()); - } - - TimeStamp tt = 2.2; - Motion ref1 = motions[2]; - Motion ref2 = motions[3]; - Motion second(0.0, 2, 3, 3, 0); - Motion interp = interpolate(ref1, ref2, tt, second); - - ASSERT_NEAR( interp.ts_.get() , 2.2 , 1e-8); - ASSERT_MATRIX_APPROX(interp.data_ , VectorXs::Zero(2) , 1e-8); - ASSERT_MATRIX_APPROX(interp.delta_ , VectorXs::Zero(3) , 1e-8); - ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[2].delta_integr_ , 1e-8); - - ASSERT_NEAR( second.ts_.get() , 3.0 , 1e-8); - ASSERT_MATRIX_APPROX(second.data_ , motions[3].data_ , 1e-8); - ASSERT_MATRIX_APPROX(second.delta_ , motions[3].delta_ , 1e-8); - ASSERT_MATRIX_APPROX(second.delta_integr_ , motions[3].delta_integr_ , 1e-8); - - tt = 2.6; - interp = interpolate(ref1, ref2, tt, second); - - ASSERT_NEAR( interp.ts_.get() , 2.6 , 1e-8); - ASSERT_MATRIX_APPROX(interp.data_ , motions[3].data_ , 1e-8); - ASSERT_MATRIX_APPROX(interp.delta_ , motions[3].delta_ , 1e-8); - ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[3].delta_integr_ , 1e-8); - - ASSERT_NEAR( second.ts_.get() , 3.0 , 1e-8); - ASSERT_MATRIX_APPROX(second.data_ , VectorXs::Zero(2) , 1e-8); - ASSERT_MATRIX_APPROX(second.delta_ , VectorXs::Zero(3) , 1e-8); - ASSERT_MATRIX_APPROX(second.delta_integr_ , motions[3].delta_integr_ , 1e-8); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp deleted file mode 100644 index 3e779fc6bdc2bb9be08f37fb37217ddb8bd453a1..0000000000000000000000000000000000000000 --- a/test/gtest_processor_tracker_feature_trifocal.cpp +++ /dev/null @@ -1,153 +0,0 @@ -#include "utils_gtest.h" - -#include "core/common/wolf.h" -#include "core/utils/logging.h" - -#include "vision_utils.h" - -#include "core/processor/processor_tracker_feature_trifocal.h" -#include "core/processor/processor_odom_3D.h" -#include "core/capture/capture_image.h" -#include "core/sensor/sensor_camera.h" - -using namespace Eigen; -using namespace wolf; - -// Use the following in case you want to initialize tests with predefines variables or methods. -//class ProcessorTrackerFeatureTrifocal_class : public testing::Test{ -// public: -// virtual void SetUp() -// { -// } -//}; - -//TEST(ProcessorTrackerFeatureTrifocal, Constructor) -//{ -// std::cout << "\033[1;33m [WARN]:\033[0m gtest for ProcessorTrackerFeatureTrifocal Constructor is empty." << std::endl; -//} - -//TEST(ProcessorTrackerFeatureTrifocal, Destructor) -//{ -// std::cout << "\033[1;33m [WARN]:\033[0m gtest for ProcessorTrackerFeatureTrifocal Destructor is empty." << std::endl; -//} - -////[Class methods] -//TEST(ProcessorTrackerFeatureTrifocal, trackFeatures) -//{ -// std::cout << "033[1;33m [WARN]:033[0m gtest for ProcessorTrackerFeatureTrifocal trackFeatures is empty." << std::endl; -//} -// -//TEST(ProcessorTrackerFeatureTrifocal, correctFeatureDrift) -//{ -// std::cout << "033[1;33m [WARN]:033[0m gtest for ProcessorTrackerFeatureTrifocal correctFeatureDrift is empty." << std::endl; -//} -// -//TEST(ProcessorTrackerFeatureTrifocal, voteForKeyFrame) -//{ -// std::cout << "033[1;33m [WARN]:033[0m gtest for ProcessorTrackerFeatureTrifocal voteForKeyFrame is empty." << std::endl; -//} -// -//TEST(ProcessorTrackerFeatureTrifocal, detectNewFeatures) -//{ -// std::cout << "033[1;33m [WARN]:033[0m gtest for ProcessorTrackerFeatureTrifocal detectNewFeatures is empty." << std::endl; -//} -// -//TEST(ProcessorTrackerFeatureTrifocal, createFactor) -//{ -// std::cout << "033[1;33m [WARN]:033[0m gtest for ProcessorTrackerFeatureTrifocal createFactor is empty." << std::endl; -//} - -TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback) -{ - - using namespace wolf; - using std::shared_ptr; - using std::make_shared; - using std::static_pointer_cast; - using Eigen::Vector2s; - - std::string wolf_root = _WOLF_ROOT_DIR; - - Scalar dt = 0.01; - - // Wolf problem - ProblemPtr problem = Problem::create("PO 3D"); - - // Install tracker (sensor and processor) - IntrinsicsCameraPtr intr = make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML - intr->width = 640; - intr->height = 480; - SensorCameraPtr sens_trk = make_shared<SensorCamera>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), - intr); - - ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>(); -// params_tracker_feature_trifocal->name = "trifocal"; - params_tracker_feature_trifocal->pixel_noise_std = 1.0; - params_tracker_feature_trifocal->voting_active = true; - params_tracker_feature_trifocal->min_features_for_keyframe = 5; - params_tracker_feature_trifocal->time_tolerance = dt/2; - params_tracker_feature_trifocal->max_new_features = 5; - params_tracker_feature_trifocal->min_response_new_feature = 25; - params_tracker_feature_trifocal->n_cells_h = 10; - params_tracker_feature_trifocal->n_cells_v = 10; - params_tracker_feature_trifocal->max_euclidean_distance = 20; - params_tracker_feature_trifocal->yaml_file_params_vision_utils = wolf_root + "/src/examples/ACTIVESEARCH.yaml"; - - ProcessorTrackerFeatureTrifocalPtr proc_trk = make_shared<ProcessorTrackerFeatureTrifocal>(params_tracker_feature_trifocal); - proc_trk->configure(sens_trk); - - problem->addSensor(sens_trk); - sens_trk->addProcessor(proc_trk); - - // Install odometer (sensor and processor) - IntrinsicsOdom3DPtr params = std::make_shared<IntrinsicsOdom3D>(); - SensorBasePtr sens_odo = problem->installSensor("ODOM 3D", "odometer", (Vector7s() << 0,0,0, 0,0,0,1).finished(), params); - ProcessorParamsOdom3DPtr proc_odo_params = make_shared<ProcessorParamsOdom3D>(); - proc_odo_params->time_tolerance = dt/2; - ProcessorBasePtr proc_odo = problem->installProcessor("ODOM 3D", "odometer", sens_odo, proc_odo_params); - - std::cout << "sensor & processor created and added to wolf problem" << std::endl; - - // Sequence to test KeyFrame creations (callback calls) - - // initialize - TimeStamp t(0.0); - Vector7s x; x << 0,0,0, 0,0,0,1; - Matrix6s P = Matrix6s::Identity() * 0.1; - problem->setPrior(x, P, t, dt/2); // KF1 - - CaptureOdom3DPtr capt_odo = make_shared<CaptureOdom3D>(t, sens_odo, Vector6s::Zero(), P); - - // Track - cv::Mat image(intr->height, intr->width, CV_8UC3); // OpenCV cv::Mat(rows, cols) - image *= 0; // TODO see if we want to use a real image - CaptureImagePtr capt_trk = make_shared<CaptureImage>(t, sens_trk, image); - proc_trk->process(capt_trk); - - for (size_t ii=0; ii<32; ii++ ) - { - // Move - t = t+dt; - WOLF_INFO("----------------------- ts: ", t , " --------------------------"); - - capt_odo->setTimeStamp(t); - proc_odo->process(capt_odo); - - // Track - capt_trk = make_shared<CaptureImage>(t, sens_trk, image); - proc_trk->process(capt_trk); - - CaptureBasePtr prev = proc_trk->getPrevOrigin(); - problem->print(2,0,0,0); - - // Only odom creating KFs - ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 3D")==0 ); - } -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp deleted file mode 100644 index cb601a7468c3bc32e2eba1e458c6ffbe4cd7cb75..0000000000000000000000000000000000000000 --- a/test/gtest_rotation.cpp +++ /dev/null @@ -1,773 +0,0 @@ -/** - * \file gtest_rotation.cpp - * - * Created on: Oct 13, 2016 - * \author: AtDinesh - */ - -//Eigen -#include <Eigen/Geometry> - -//Wolf -#include "core/common/wolf.h" -#include "core/math/rotations.h" - -//std -#include <iostream> -#include <fstream> -#include <iomanip> -#include <ctime> -#include <cmath> -#include "utils_gtest.h" - -//#define write_results - -// THESE ARE UNITARY TESTS FOR METHODS IN ROTATION.H - -using namespace wolf; -using namespace Eigen; - -TEST(exp_q, unit_norm) -{ - Vector3s v0 = Vector3s::Random(); - Scalar scale = 1.0; - for (int i = 0; i < 20; i++) - { - Vector3s v = v0 * scale; - Quaternions q = exp_q(v); - ASSERT_NEAR(q.norm(), 1.0, 1e-10) << "Failed at scale 1e-" << i << " with angle = " << 2.0*q.vec().norm(); - scale /= 10; - } -} - -TEST(rotations, pi2pi) -{ - ASSERT_NEAR(M_PI_2, pi2pi((Scalar)M_PI_2), 1e-10); - ASSERT_NEAR(-M_PI_2, pi2pi(3.0*M_PI_2), 1e-10); - ASSERT_NEAR(-M_PI_2, pi2pi(-M_PI_2), 1e-10); - ASSERT_NEAR(M_PI_2, pi2pi(-3.0*M_PI_2), 1e-10); - // ASSERT_NEAR(M_PI, pi2pi(M_PI), 1e-10); // Exact PI is not safely testable because of numeric issues. - ASSERT_NEAR(M_PI-.01, pi2pi(M_PI-.01), 1e-10); - ASSERT_NEAR(-M_PI+.01, pi2pi(M_PI+.01), 1e-10); -} - -TEST(skew, Skew_vee) -{ - using namespace wolf; - Vector3s vec3 = Vector3s::Random(); - Matrix3s skew_mat; - skew_mat = skew(vec3); - - // vee - Vector3s vec3_bis; - vec3_bis = vee(skew_mat); - - ASSERT_TRUE(vec3_bis == vec3); -} - -TEST(exp_q, v2q_q2v) -{ - using namespace wolf; - //defines scalars - wolf::Scalar deg_to_rad = M_PI/180.0; - - Vector4s vec0, vec1; - - //v2q - Vector3s rot_vector0, rot_vector1; - rot_vector0 = Vector3s::Random(); - rot_vector1 = rot_vector0 * 100 *deg_to_rad; //far from origin - rot_vector0 = rot_vector0*deg_to_rad; - - Quaternions quat0, quat1; - quat0 = v2q(rot_vector0); - quat1 = v2q(rot_vector1); - - //q2v - Vector3s quat_to_v0, quat_to_v1; - VectorXs quat_to_v0x, quat_to_v1x; - - quat_to_v0 = q2v(quat0); - quat_to_v1 = q2v(quat1); - quat_to_v0x = q2v(quat0); - quat_to_v1x = q2v(quat1); - - ASSERT_MATRIX_APPROX(rot_vector0, quat_to_v0, wolf::Constants::EPS); - ASSERT_MATRIX_APPROX(rot_vector1, quat_to_v1, wolf::Constants::EPS); - ASSERT_MATRIX_APPROX(rot_vector0, quat_to_v0x, wolf::Constants::EPS); - ASSERT_MATRIX_APPROX(rot_vector1, quat_to_v1x, wolf::Constants::EPS); -} - -TEST(exp_R, v2R_R2v) -{ - using namespace wolf; - //First test is to verify we get the good result with v -> v2R -> R2v -> v - //test 2 : how small can angles in rotation vector be ? - - //definition - wolf::Scalar deg_to_rad = M_PI/180.0; - Vector3s rot_vector0, rot_vector1; - - rot_vector0 = Vector3s::Random(); - rot_vector1 = rot_vector0 * 100 *deg_to_rad; //far from origin - rot_vector0 = rot_vector0*deg_to_rad; - - Matrix3s rot0, rot1; - rot0 = v2R(rot_vector0); - rot1 = v2R(rot_vector1); - - //R2v - Vector3s rot0_vec, rot1_vec; - rot0_vec = R2v(rot0); - rot1_vec = R2v(rot1); - - //check now - ASSERT_MATRIX_APPROX(rot0_vec, rot_vector0, wolf::Constants::EPS); - ASSERT_MATRIX_APPROX(rot1_vec, rot_vector1, wolf::Constants::EPS); -} - -TEST(log_R, R2v_v2R_limits) -{ - using namespace wolf; - //test 2 : how small can angles in rotation vector be ? - wolf::Scalar scale = 1; - Matrix3s v_to_R, initial_matrix; - Vector3s R_to_v; - - //Vector3s rv; - for(int i = 0; i<8; i++){ - initial_matrix = v2R(Vector3s::Random().eval() * scale); - - R_to_v = R2v(initial_matrix); - v_to_R = v2R(R_to_v); - - ASSERT_MATRIX_APPROX(v_to_R, initial_matrix, wolf::Constants::EPS); - scale = scale*0.1; - } -} - -TEST(log_R, R2v_v2R_AAlimits) -{ - using namespace wolf; - //let's see how small the angles can be here : limit reached at scale/10 = 1e-16 - wolf::Scalar scale = 1; - Matrix3s rotation_mat; - Vector3s rv; - - for(int i = 0; i<8; i++){ - rotation_mat = v2R(Vector3s::Random().eval() * scale); - - //rv = R2v(rotation_mat); //decomposing R2v below - AngleAxis<wolf::Scalar> aa0 = AngleAxis<wolf::Scalar>(rotation_mat); - rv = aa0.axis() * aa0.angle(); - //std::cout << "aa0.axis : " << aa0.axis().transpose() << ",\t aa0.angles :" << aa0.angle() <<std::endl; - - ASSERT_FALSE(rv == Vector3s::Zero()); - scale = scale*0.1; - } -} - -TEST(exp_q, v2q2R2v) -{ - using namespace wolf; - wolf::Scalar scale = 1; - // testing new path : vector -> quaternion -> matrix -> vector - - for(int i = 0; i< 8; i++){ - Vector3s vector_ = Vector3s::Random()*scale; - Quaternions quat_ = v2q(vector_); - Matrix3s mat_ = quat_.matrix(); - Vector3s vector_bis = R2v(mat_); - - ASSERT_MATRIX_APPROX(vector_, vector_bis, wolf::Constants::EPS); - scale = scale*0.1; - } -} - -TEST(rotations, AngleAxis_limits) -{ - using namespace wolf; - //Hypothesis : problem with construction of AngleAxis objects. - // Example : if R = I + [delta t]_x (happenning in the IMU case with delta t = 0.001). Then angle mays be evaluated as 0 (due to cosinus definition ?) - // Here we try to get the AngleAxis object from a random rotation matrix, then get back to the rotation matrix using AngleAxis::toRotationMatrix() - - wolf::Scalar scale = 1; - Matrix3s res, res_i, rotation_mati, rotation_mat; - Vector3s rv; - - for(int i = 0; i<8; i++){ //FIX ME : Random() will not create a rotation matrix. Then, R2v(Random_matrix()) makes no sense at all. - rotation_mat = v2R(Vector3s::Random().eval() * scale); - rotation_mati = rotation_mat; - - //rv = R2v(rotation_mat); //decomposing R2v below - AngleAxis<wolf::Scalar> aa0 = AngleAxis<wolf::Scalar>(rotation_mat); - rv = aa0.axis() * aa0.angle(); - //std::cout << "aa0.axis : " << aa0.axis().transpose() << ",\t aa0.angles :" << aa0.angle() <<std::endl; - res = aa0.toRotationMatrix(); - - // now we set the diagonal to identity - AngleAxis<wolf::Scalar> aa1 = AngleAxis<wolf::Scalar>(rotation_mat); - rv = aa1.axis() * aa1.angle(); - //std::cout << "aa1.axis : " << aa0.axis().transpose() << ",\t aa1.angles :" << aa0.angle() <<std::endl; - res_i = aa1.toRotationMatrix(); - - ASSERT_MATRIX_APPROX(res, rotation_mat, wolf::Constants::EPS); - ASSERT_MATRIX_APPROX(res_i, rotation_mati, wolf::Constants::EPS); - scale = scale*0.1; - } -} - -TEST(compose, Quat_compos_const_rateOfTurn) -{ - using namespace wolf; - - // ********* constant rate of turn ********* - - /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)) and in R3 - (q2v(v2q(v0*n*dt))). with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step. - But this is not OK, we cannot expect those 2 rotation integration to be equal. - The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold of SO3 - - more specifically : - - At a constant velocity, because we keep a constant rotation axis, the integral is the same. - - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only good method is the SO3. - - We change the idea : - define orientation and derive ox, oy, oz so that we get the rate of turn wx, wy, wz. - Then compare the final orientation from rotation matrix composition and quaternion composition - */ - - wolf::Scalar deg_to_rad = M_PI/180.0; - Eigen::Matrix3s rot0(Eigen::Matrix3s::Identity()); - Eigen::Quaternions q0, qRot; - q0.setIdentity(); - Eigen::Vector3s tmp_vec; //will be used to store rate of turn data - - const unsigned int x_rot_vel = 5; // deg/s - const unsigned int y_rot_vel = 2; // deg/s - const unsigned int z_rot_vel = 10; // deg/s - - wolf::Scalar tmpx, tmpy, tmpz; - /* - ox oy oz evolution in degrees (for understanding) --> converted in rad - with * pi/180 - ox = x_rot_vel * t; %express angle in rad before using sinus - oy = y_rot_vel * t; - oz = z_rot_vel * t; - - corresponding rate of turn - %rate of turn expressed in radians/s - wx = x_rot_vel; - wy = y_rot_vel; - wz = z_rot_vel; - */ - - //there is no need to compute the rate of turn at each time because it is constant here : - tmpx = deg_to_rad * x_rot_vel; // rad/s - tmpy = deg_to_rad * y_rot_vel; - tmpz = deg_to_rad * z_rot_vel; - tmp_vec << tmpx, tmpy, tmpz; - const wolf::Scalar dt = 0.1; - - for(unsigned int data_iter = 0; data_iter < 100; data_iter ++) - { - rot0 = rot0 * v2R(tmp_vec*dt); - q0 = q0 * v2q(tmp_vec*dt); //succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically) - - } - - // Compare results from rotation matrix composition and quaternion composition - qRot = (v2q(R2v(rot0))); - - Eigen::Vector3s final_orientation(q2v(qRot)); - ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() << - "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; -} - -TEST(compose, Quat_compos_var_rateOfTurn) -{ - using namespace wolf; - - //********* changing rate of turn - same freq for all axis ********* - - /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)) and in R3 - (q2v(v2q(v0*n*dt))). with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step. - But this is not OK, we cannot expect those 2 rotation integration to be equal. - The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold of SO3 - - more specifically : - - At a constant velocity, because we keep a constant rotation axis, the integral is the same. - - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only good method is the SO3. - - We change the idea : - define orientation and derive ox, oy, oz so that we get the rate of turn wx, wy, wz. - Then compare the final orientation from ox, oy, oz and quaternion we get by data integration - - ******* RESULT : ******* - The error in this test is due to discretization. The smaller is dt and the better is the integration ! - with dt = 0.001, the error is in 1e-5 - */ - - wolf::Scalar deg_to_rad = M_PI/180.0; - Eigen::Matrix3s rot0(Eigen::Matrix3s::Identity()); - Eigen::Quaternions q0, qRot; - q0.setIdentity(); - - Eigen::Vector3s tmp_vec; //will be used to store rate of turn data - wolf::Scalar time = 0; - const unsigned int x_rot_vel = 15; // deg/s - const unsigned int y_rot_vel = 15; // deg/s - const unsigned int z_rot_vel = 15; // deg/s - - wolf::Scalar tmpx, tmpy, tmpz; - /* - ox oy oz evolution in degrees (for understanding) --> converted in rad - with * pi/180 - ox = pi*sin(x_rot_vel * t * pi/180); %express angle in rad before using sinus - oy = pi*sin(y_rot_vel * t * pi/180); - oz = pi*sin(z_rot_vel * t * pi/180); - - corresponding rate of turn - %rate of turn expressed in radians/s - wx = pi * x_rot_vel * cos(x_rot_vel * t * pi/180) * pi/180; - wy = pi * y_rot_vel * cos(y_rot_vel * t * pi/180) * pi/180; - wz = pi * z_rot_vel * cos(z_rot_vel * t * pi/180) * pi/180; - */ - - const wolf::Scalar dt = 0.001; - - for(unsigned int data_iter = 0; data_iter <= 10000; data_iter ++) - { - tmpx = M_PI*x_rot_vel*cos(wolf::toRad(x_rot_vel * time))*deg_to_rad; - tmpy = M_PI*y_rot_vel*cos(wolf::toRad(y_rot_vel * time))*deg_to_rad; - tmpz = M_PI*z_rot_vel*cos(wolf::toRad(z_rot_vel * time))*deg_to_rad; - tmp_vec << tmpx, tmpy, tmpz; - - rot0 = rot0 * v2R(tmp_vec*dt); - q0 = q0 * v2q(tmp_vec*dt); //succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically) - - time += dt; - } - - // Compare results from rotation matrix composition and quaternion composition - qRot = (v2q(R2v(rot0))); - - Eigen::Vector3s final_orientation(q2v(qRot)); - - EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() << - "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; - ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,0.0001)) << "final orientation expected : " << final_orientation.transpose() << - "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; - -} - -TEST(compose, Quat_compos_var_rateOfTurn_diff) -{ - using namespace wolf; - - // ********* changing rate of turn - different freq for 1 axis ********* - - /* First idea was to integrate data on SO3 (succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically)) and in R3 - (q2v(v2q(v0*n*dt))). with v0 << 30.0*deg_to_rad, 5.0*deg_to_rad, 10.0*deg_to_rad : constant rate-of-turn in rad/s and dt the time step. - But this is not OK, we cannot expect those 2 rotation integration to be equal. - The whole point of the SO3 thing is that we cannot integrate rotation in the R3 space and expect it to work. This is why we must integrate it in the manifold of SO3 - - more specifically : - - At a constant velocity, because we keep a constant rotation axis, the integral is the same. - - for non-contant velocities, especially if we change the axis of rotation, then it’s not the same, and the only good method is the SO3. - - We change the idea : - define orientation and derive ox, oy, oz so that we get the rate of turn wx, wy, wz. - Then compare the final orientation from ox, oy, oz and quaternion we get by data integration - - ******* RESULT : ******* - Things are more tricky here. The errors go growing with time. - with dt = 0.001, the error is in 1e-4 for 1 s integration ! But this may also depend on the frequency given to the rotation on each of the axis. - */ - - wolf::Scalar deg_to_rad = M_PI/180.0; - Eigen::Matrix3s rot0(Eigen::Matrix3s::Identity()); - Eigen::Quaternions q0, qRot; - q0.setIdentity(); - - Eigen::Vector3s tmp_vec; //will be used to store rate of turn data - wolf::Scalar time = 0; - const unsigned int x_rot_vel = 1; // deg/s - const unsigned int y_rot_vel = 3; // deg/s - const unsigned int z_rot_vel = 6; // deg/s - - wolf::Scalar tmpx, tmpy, tmpz; - /* - ox oy oz evolution in degrees (for understanding) --> converted in rad - with * pi/180 - ox = pi*sin(x_rot_vel * t * pi/180); %express angle in rad before using sinus - oy = pi*sin(y_rot_vel * t * pi/180); - oz = pi*sin(z_rot_vel * t * pi/180); - - corresponding rate of turn - %rate of turn expressed in radians/s - wx = pi * x_rot_vel * cos(x_rot_vel * t * pi/180) * pi/180; - wy = pi * y_rot_vel * cos(y_rot_vel * t * pi/180) * pi/180; - wz = pi * z_rot_vel * cos(z_rot_vel * t * pi/180) * pi/180; - */ - - const wolf::Scalar dt = 0.001; - - for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++) - { - tmpx = M_PI*x_rot_vel*cos(wolf::toRad(x_rot_vel * time))*deg_to_rad; - tmpy = M_PI*y_rot_vel*cos(wolf::toRad(y_rot_vel * time))*deg_to_rad; - tmpz = M_PI*z_rot_vel*cos(wolf::toRad(z_rot_vel * time))*deg_to_rad; - tmp_vec << tmpx, tmpy, tmpz; - - rot0 = rot0 * v2R(tmp_vec*dt); - q0 = q0 * v2q(tmp_vec*dt); //succesive composition of quaternions : q = q * dq(w*dt) <=> q = q * dq(w*dt) * q' (mathematically) - - time += dt; - } - - // Compare results from rotation matrix composition and quaternion composition - qRot = (v2q(R2v(rot0))); - - Eigen::Vector3s final_orientation(q2v(qRot)); - - EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() << - "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; - - ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,0.001)) << "final orientation expected : " << final_orientation.transpose() << - "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl; -} - -TEST(Plus, Random) -{ - Quaternions q; - q .coeffs().setRandom().normalize(); - - Vector3s v; - v .setRandom(); - - Quaternions q2 = q * exp_q(v); - Quaternions q3 = exp_q(v) * q; - - ASSERT_QUATERNION_APPROX(plus(q,v) , q2, 1e-12); - ASSERT_QUATERNION_APPROX(plus_right(q,v), q2, 1e-12); - ASSERT_QUATERNION_APPROX(plus_left(v,q) , q3, 1e-12); - -} - -TEST(Plus, Identity_plus_small) -{ - Quaternions q; - q .setIdentity(); - - Vector3s v; - v .setRandom(); - v *= 1e-6; - - Quaternions q2; - q2.w() = 1; - q2.vec() = 0.5*v; - - ASSERT_QUATERNION_APPROX(plus(q,v), q2, 1e-12); -} - -TEST(Minus_and_diff, Random) -{ - Quaternions q1, q2, qo; - q1 .coeffs().setRandom().normalize(); - q2 .coeffs().setRandom().normalize(); - - Vector3s vr = log_q(q1.conjugate() * q2); - Vector3s vl = log_q(q2 * q1.conjugate()); - - ASSERT_MATRIX_APPROX(minus(q1, q2), vr, 1e-12); - ASSERT_MATRIX_APPROX(diff(q1, q2), vr, 1e-12); - ASSERT_MATRIX_APPROX(minus_left(q1, q2), vl, 1e-12); - - qo = plus(q1, minus(q1, q2)); - if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q - ASSERT_QUATERNION_APPROX(qo, q2, 1e-12); - - qo = plus(q1, diff(q1, q2)); - if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q - ASSERT_QUATERNION_APPROX(qo, q2, 1e-12); - - qo = plus_left(minus_left(q1, q2), q1); - if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q - ASSERT_QUATERNION_APPROX(qo, q2, 1e-12); -} - -TEST(Jacobians, Jr) -{ - Vector3s theta; theta.setRandom(); - Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4; - - // Check the main Jr property for q and R - // exp( theta + d_theta ) \approx exp(theta) * exp(Jr * d_theta) - Matrix3s Jr = jac_SO3_right(theta); - ASSERT_QUATERNION_APPROX(exp_q(theta+dtheta), exp_q(theta) * exp_q(Jr*dtheta), 1e-8); - ASSERT_MATRIX_APPROX(exp_R(theta+dtheta), (exp_R(theta) * exp_R(Jr*dtheta)), 1e-8); -} - -TEST(Jacobians, Jl) -{ - Vector3s theta; theta.setRandom(); - Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4; - - // Check the main Jl property for q and R - // exp( theta + d_theta ) \approx exp(Jl * d_theta) * exp(theta) - Matrix3s Jl = jac_SO3_left(theta); - ASSERT_QUATERNION_APPROX(exp_q(theta+dtheta), exp_q(Jl*dtheta) * exp_q(theta), 1e-8); - ASSERT_MATRIX_APPROX(exp_R(theta+dtheta), (exp_R(Jl*dtheta) * exp_R(theta)), 1e-8); - - // Jl = Jr.tr - ASSERT_MATRIX_APPROX(Jl, jac_SO3_right(theta).transpose(), 1e-8); - - // Jl = R*Jr - ASSERT_MATRIX_APPROX(Jl, exp_R(theta)*jac_SO3_right(theta), 1e-8); -} - -TEST(Jacobians, Jr_inv) -{ - Vector3s theta; theta.setRandom(); - Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4; - Quaternions q = v2q(theta); - Matrix3s R = v2R(theta); - - // Check the main Jr_inv property for q and R - // log( R * exp(d_theta) ) \approx log( R ) + Jrinv * d_theta - Matrix3s Jr_inv = jac_SO3_right_inv(theta); - ASSERT_MATRIX_APPROX(log_q(q * exp_q(dtheta)), log_q(q) + Jr_inv*dtheta, 1e-8); - ASSERT_MATRIX_APPROX(log_R(R * exp_R(dtheta)), log_R(R) + Jr_inv*dtheta, 1e-8); -} - -TEST(Jacobians, Jl_inv) -{ - Vector3s theta; theta.setRandom(); - Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4; - Quaternions q = v2q(theta); - Matrix3s R = v2R(theta); - - // Check the main Jl_inv property for q and R - // log( exp(d_theta) * R ) \approx log( R ) + Jlinv * d_theta - Matrix3s Jl_inv = jac_SO3_left_inv(theta); - ASSERT_MATRIX_APPROX(log_q(exp_q(dtheta) * q), log_q(q) + Jl_inv*dtheta, 1e-8); - ASSERT_MATRIX_APPROX(log_R(exp_R(dtheta) * R), log_R(R) + Jl_inv*dtheta, 1e-8); -} - -TEST(Jacobians, compose) -{ - - Vector3s th1(.1,.2,.3), th2(.3,.1,.2); - Quaternions q1(exp_q(th1)); - Quaternions q2(exp_q(th2)); - Quaternions qc; - Matrix3s J1a, J2a, J1n, J2n; - - // composition and analytic Jacobians - wolf::compose(q1, q2, qc, J1a, J2a); - - // Numeric Jacobians - Scalar dx = 1e-6; - Vector3s pert; - Quaternions q1_pert, q2_pert, qc_pert; - for (int i = 0; i<3; i++) - { - pert.setZero(); - pert(i) = dx; - - // Jac wrt q1 - q1_pert = q1*exp_q(pert); - qc_pert = q1_pert * q2; - J1n.col(i) = log_q(qc.conjugate()*qc_pert) / dx; - - // Jac wrt q2 - q2_pert = q2*exp_q(pert); - qc_pert = q1 * q2_pert; - J2n.col(i) = log_q(qc.conjugate()*qc_pert) / dx; - } - - ASSERT_MATRIX_APPROX(J1a, J1n, 1e-5); - ASSERT_MATRIX_APPROX(J2a, J2n, 1e-5); -} - -TEST(Jacobians, between) -{ - - Vector3s th1(.1,.2,.3), th2(.3,.1,.2); - Quaternions q1(exp_q(th1)); - Quaternions q2(exp_q(th2)); - Quaternions qc; - Matrix3s J1a, J2a, J1n, J2n; - - // composition and analytic Jacobians - wolf::between(q1, q2, qc, J1a, J2a); - - // Numeric Jacobians - Scalar dx = 1e-6; - Vector3s pert; - Quaternions q1_pert, q2_pert, qc_pert; - for (int i = 0; i<3; i++) - { - pert.setZero(); - pert(i) = dx; - - // Jac wrt q1 - q1_pert = q1*exp_q(pert); - qc_pert = q1_pert.conjugate() * q2; - J1n.col(i) = log_q(qc.conjugate()*qc_pert) / dx; - - // Jac wrt q2 - q2_pert = q2*exp_q(pert); - qc_pert = q1.conjugate() * q2_pert; - J2n.col(i) = log_q(qc.conjugate()*qc_pert) / dx; - } - - ASSERT_MATRIX_APPROX(J1a, J1n, 1e-5); - ASSERT_MATRIX_APPROX(J2a, J2n, 1e-5); -} - -TEST(exp_q, small) -{ - Vector3s u; u.setRandom().normalize(); - Vector3s v; - Quaternions q; - Scalar scale = 1.0; - for (int i = 0; i<20; i++) - { - v = u*scale; - q = exp_q(v); - Vector3s ratio = q.vec().array() / v.array(); - - WOLF_TRACE("scale = ", scale, "; ratio = ", ratio.transpose()); - - scale /= 10; - } - ASSERT_MATRIX_APPROX(q.vec()/(10*scale), u/2, 1e-12); -} - -TEST(log_q, double_cover) -{ - Quaternions qp; qp.coeffs().setRandom().normalize(); - Quaternions qn; qn.coeffs() = - qp.coeffs(); - ASSERT_MATRIX_APPROX(log_q(qp), log_q(qn), 1e-16); -} - -TEST(log_q, small) -{ - Vector3s u; u.setRandom().normalize(); - Scalar scale = 1.0; - for (int i = 0; i<20; i++) - { - Vector3s v = u*scale; - Quaternions q = exp_q(v); - Vector3s l = log_q(q); - - ASSERT_MATRIX_APPROX(v, l, 1e-10); - - scale /= 10; - } -} - -TEST(Conversions, q2R_R2q) -{ - Vector3s v; v.setRandom(); - Quaternions q = v2q(v); - Matrix3s R = v2R(v); - - Quaternions q_R = R2q(R); - Quaternions qq_R(R); - - ASSERT_NEAR(q.norm(), 1, wolf::Constants::EPS); - ASSERT_NEAR(q_R.norm(), 1, wolf::Constants::EPS); - ASSERT_NEAR(qq_R.norm(), 1, wolf::Constants::EPS); - - ASSERT_MATRIX_APPROX(q.coeffs(), R2q(R).coeffs(), wolf::Constants::EPS); - ASSERT_MATRIX_APPROX(q.coeffs(), qq_R.coeffs(), wolf::Constants::EPS); - ASSERT_MATRIX_APPROX(R, q2R(q), wolf::Constants::EPS); - ASSERT_MATRIX_APPROX(R, qq_R.matrix(), wolf::Constants::EPS); -} - -TEST(Conversions, e2q_q2e) -{ - Vector3s e, eo; - Quaternions q; - - e << 0.1, .2, .3; - - q = e2q(e); - - eo = q2e(q); - ASSERT_MATRIX_APPROX(eo, e, 1e-10); - - eo = q2e(q.coeffs()); - ASSERT_MATRIX_APPROX(eo, e, 1e-10); - -} - -TEST(Conversions, e2q_q2R_R2e) -{ - Vector3s e, eo; - Quaternions q; - Matrix3s R; - - e << 0.1, .2, .3; - q = e2q(e); - R = q2R(q); - - eo = R2e(R); - - ASSERT_MATRIX_APPROX(eo, e, 1e-10); -} - -TEST(Conversions, e2R_R2e) -{ - Vector3s e, eo; - Matrix3s R; - - e << 0.1, 0.2, 0.3; - - R = e2R(e); - eo = R2e(R); - ASSERT_MATRIX_APPROX(eo, e, 1e-10); -} - -TEST(Conversions, e2R_R2q_q2e) -{ - Vector3s e, eo; - Quaternions q; - Matrix3s R; - - e << 0.1, 0.2, 0.3; - R = e2R(e); - q = R2q(R); - - eo = q2e(q.coeffs()); - - ASSERT_MATRIX_APPROX(eo, e, 1e-10); -} - -int main(int argc, char **argv) -{ - using namespace wolf; - - /* - LIST OF FUNCTIONS : - - pi2pi - - skew -> Skew_vee OK - - vee -> Skew_vee OK - - v2q v -> v2q -> q2v -> v OK (precision wolf::Constants::EPS) - - Matrix<T, 3, 1> q2v(const Quaternion<T>& _q) v -> v2q -> q2v -> v OK (precision wolf::Constants::EPS) - - VectorXs q2v(const Quaternions& _q) v -> v2q -> q2v -> v OK (precision wolf::Constants::EPS) - - v2R - - R2v - - jac_SO3_right - - jac_SO3_right_inv - - jac_SO3_left - - jac_SO3_left_inv - - quaternion composition - */ - - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp deleted file mode 100644 index d8420b2b3fa2d9635149c28b34ce3853bd63ca1a..0000000000000000000000000000000000000000 --- a/test/gtest_sensor_base.cpp +++ /dev/null @@ -1,32 +0,0 @@ -/** - * \file gtest_sensor_base.cpp - * - * Created on: Mar 27, 2018 - * \author: jsola - */ - -#include "core/sensor/sensor_base.h" - -#include "utils_gtest.h" - -using namespace wolf; - -TEST(SensorBase, setNoiseStd) -{ - SensorBasePtr S (std::make_shared<SensorBase>("BASE", nullptr, nullptr, nullptr, 2)); // noise size 2 - - Eigen::Vector2s noise_std = Eigen::Vector2s::Ones() * 0.1; - Eigen::Matrix2s noise_cov = Eigen::Matrix2s::Identity() * 0.01; - - S->setNoiseStd(noise_std); - - ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8); - ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_sensor_camera.cpp b/test/gtest_sensor_camera.cpp deleted file mode 100644 index 7c27882d3f7bb3ae5d3c420b2b09ab58260cf771..0000000000000000000000000000000000000000 --- a/test/gtest_sensor_camera.cpp +++ /dev/null @@ -1,123 +0,0 @@ -/** - * \file gtest_sensor_camera.cpp - * - * Created on: Feb 7, 2019 - * \author: jsola - */ - - -#include "utils_gtest.h" - -#include "src/sensor/sensor_camera.cpp" -#include "core/sensor/sensor_factory.h" - -using namespace wolf; - -TEST(SensorCamera, Img_size) -{ - Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1; - IntrinsicsCamera params; - params.width = 640; - params.height = 480; - SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params); - - ASSERT_EQ(cam->getImgWidth() , 640); - ASSERT_EQ(cam->getImgHeight(), 480); - - cam->setImgWidth(100); - ASSERT_EQ(cam->getImgWidth() , 100); - - cam->setImgHeight(100); - ASSERT_EQ(cam->getImgHeight(), 100); -} - -TEST(SensorCamera, Intrinsics_Raw_Rectified) -{ - Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1; - IntrinsicsCamera params; - params.pinhole_model_raw << 321, 241, 321, 321; - params.pinhole_model_rectified << 320, 240, 320, 320; - SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params); - - Eigen::Matrix3s K_raw, K_rectified; - K_raw << 321, 0, 321, 0, 321, 241, 0, 0, 1; - K_rectified << 320, 0, 320, 0, 320, 240, 0, 0, 1; - Eigen::Vector4s k_raw(321,241,321,321); - Eigen::Vector4s k_rectified(320,240,320,320); - - // default is raw - ASSERT_TRUE(cam->isUsingRawImages()); - ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8); - ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsic()->getState(), 1e-8); - - cam->useRectifiedImages(); - ASSERT_FALSE(cam->isUsingRawImages()); - ASSERT_MATRIX_APPROX(K_rectified, cam->getIntrinsicMatrix(), 1e-8); - ASSERT_MATRIX_APPROX(k_rectified, cam->getIntrinsic()->getState(), 1e-8); - - cam->useRawImages(); - ASSERT_TRUE(cam->isUsingRawImages()); - ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8); - ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsic()->getState(), 1e-8); -} - -TEST(SensorCamera, Distortion) -{ - Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1; - IntrinsicsCamera params; - params.width = 640; - params.height = 480; - params.pinhole_model_raw << 321, 241, 321, 321; - params.pinhole_model_rectified << 320, 240, 320, 320; - params.distortion = Eigen::Vector3s( -0.3, 0.096, 0 ); - SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params); - - Eigen::Vector3s d(-0.3, 0.096, 0); - - ASSERT_MATRIX_APPROX(d, cam->getDistortionVector(), 1e-8); -} - -TEST(SensorCamera, Correction_zero) -{ - Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1; - IntrinsicsCamera params; - params.width = 640; - params.height = 480; - params.pinhole_model_raw << 321, 241, 321, 321; - params.pinhole_model_rectified << 320, 240, 320, 320; - params.distortion = Eigen::Vector3s( 0, 0, 0 ); - SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params); - - Eigen::MatrixXs c(cam->getCorrectionVector().size(), 1); - c.setZero(); - - ASSERT_GE(cam->getCorrectionVector().size(), cam->getDistortionVector().size()); - ASSERT_MATRIX_APPROX(c, cam->getCorrectionVector(), 1e-8); -} - -TEST(SensorCamera, create) -{ - Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1; - IntrinsicsCameraPtr params = std::make_shared<IntrinsicsCamera>(); - params->width = 640; - params->height = 480; - params->pinhole_model_raw << 321, 241, 321, 321; - params->pinhole_model_rectified << 320, 240, 320, 320; - params->distortion = Eigen::Vector3s( 0, 0, 0 ); - - SensorBasePtr sen = SensorCamera::create("camera", extrinsics, params); - - ASSERT_NE(sen, nullptr); - - SensorCameraPtr cam = std::static_pointer_cast<SensorCamera>(sen); - - ASSERT_NE(cam, nullptr); - ASSERT_EQ(cam->getImgWidth(), 640); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_shared_from_this.cpp b/test/gtest_shared_from_this.cpp deleted file mode 100644 index 6b14fff0c5b9db4d16f8c0e9b22393157387fd9d..0000000000000000000000000000000000000000 --- a/test/gtest_shared_from_this.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include "utils_gtest.h" -#include "core/common/node_base.h" - -class CChildBase; - -class CParentBase : public wolf::NodeBase -{ - public: - - std::list<std::shared_ptr<CChildBase> > child_list_; - - CParentBase() : - NodeBase("") - {}; - - virtual ~CParentBase(){}; - - virtual void addChild(std::shared_ptr<CChildBase> _child_ptr) final - { - child_list_.push_back(_child_ptr); - } -}; - -class CParentDerived : public CParentBase -{ - public: - - CParentDerived(){}; -}; - -class CChildBase : public wolf::NodeBase, public std::enable_shared_from_this<CChildBase> -{ - public: - std::shared_ptr<CParentBase> parent_ptr_; - - CChildBase(std::shared_ptr<CParentBase> _parent_ptr) : - NodeBase(""), - parent_ptr_(_parent_ptr) - { - auto wptr = std::shared_ptr<CChildBase>( this, [](CChildBase*){} ); - - parent_ptr_->addChild(shared_from_this()); - }; -}; - -class CChildDerived : public CChildBase -{ - public: - - CChildDerived(std::shared_ptr<CParentBase> _parent_ptr) : CChildBase(_parent_ptr){}; -}; - -TEST(TestTest, SingleTest) -{ - std::shared_ptr<CParentDerived> parent_derived_ptr = std::make_shared<CParentDerived>(); - - std::shared_ptr<CChildDerived> child_derived_ptr = std::make_shared<CChildDerived>(parent_derived_ptr); - - ASSERT_EQ(child_derived_ptr, parent_derived_ptr->child_list_.front()); - ASSERT_EQ(parent_derived_ptr, child_derived_ptr->parent_ptr_); - - std::string parent_name("my booooring father..."); - std::string child_name("my intelligent son!"); - parent_derived_ptr->setName(parent_name); - child_derived_ptr->setName(child_name); - - ASSERT_EQ(child_derived_ptr->getName(), child_name); - ASSERT_EQ(parent_derived_ptr->getName(), parent_name); - ASSERT_EQ(child_derived_ptr->getName(), parent_derived_ptr->child_list_.front()->getName()); - ASSERT_EQ(parent_derived_ptr->getName(), child_derived_ptr->parent_ptr_->getName()); - - //std::cout << "parent_derived_ptr->getName() " << parent_derived_ptr->getName() << std::endl; - //std::cout << "child_derived_ptr->getName() " << child_derived_ptr->getName() << std::endl; - //std::cout << "child_derived_ptr->parent_ptr_->getName() " << child_derived_ptr->parent_ptr_->getName() << std::endl; - //std::cout << "parent_derived_ptr->child_list_.front()->getName() " << parent_derived_ptr->child_list_.front()->getName() << std::endl; - -// PRINTF("All good at TestTest::DummyTestExample !\n"); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp deleted file mode 100644 index 2a0729f9b5d977dfb3fcb9c44a856e4ae9c545f0..0000000000000000000000000000000000000000 --- a/test/gtest_solver_manager.cpp +++ /dev/null @@ -1,616 +0,0 @@ -/* - * gtest_solver_manager.cpp - * - * Created on: Jun, 2018 - * Author: jvallve - */ - -#include "utils_gtest.h" -#include "core/utils/logging.h" - -#include "core/problem/problem.h" -#include "core/sensor/sensor_base.h" -#include "core/state_block/state_block.h" -#include "core/capture/capture_void.h" -#include "core/factor/factor_pose_2D.h" -#include "core/solver/solver_manager.h" -#include "core/state_block/local_parametrization_base.h" -#include "core/state_block/local_parametrization_angle.h" - -#include <iostream> - -using namespace wolf; -using namespace Eigen; - -WOLF_PTR_TYPEDEFS(SolverManagerWrapper); -class SolverManagerWrapper : public SolverManager -{ - public: - std::list<FactorBasePtr> factors_; - std::map<StateBlockPtr,bool> state_block_fixed_; - std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_; - - SolverManagerWrapper(const ProblemPtr& wolf_problem) : - SolverManager(wolf_problem) - { - }; - - bool isStateBlockRegistered(const StateBlockPtr& st) const - { - return state_blocks_.find(st)!=state_blocks_.end(); - }; - - bool isStateBlockFixed(const StateBlockPtr& st) const - { - return state_block_fixed_.at(st); - }; - - bool isFactorRegistered(const FactorBasePtr& fac_ptr) const - { - return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end(); - }; - - bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const - { - return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param; - }; - - bool hasLocalParametrization(const StateBlockPtr& st) const - { - return state_block_local_param_.find(st) != state_block_local_param_.end(); - }; - - virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){}; - virtual void computeCovariances(const StateBlockPtrList& st_list){}; - - // The following are dummy implementations - bool hasConverged() { return true; } - SizeStd iterations() { return 1; } - Scalar initialCost() { return Scalar(1); } - Scalar finalCost() { return Scalar(0); } - - - - protected: - - virtual std::string solveImpl(const ReportVerbosity report_level){ return std::string("");}; - virtual void addFactor(const FactorBasePtr& fac_ptr) - { - factors_.push_back(fac_ptr); - }; - virtual void removeFactor(const FactorBasePtr& fac_ptr) - { - factors_.remove(fac_ptr); - }; - virtual void addStateBlock(const StateBlockPtr& state_ptr) - { - state_block_fixed_[state_ptr] = state_ptr->isFixed(); - state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization(); - }; - virtual void removeStateBlock(const StateBlockPtr& state_ptr) - { - state_block_fixed_.erase(state_ptr); - state_block_local_param_.erase(state_ptr); - }; - virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) - { - state_block_fixed_[state_ptr] = state_ptr->isFixed(); - }; - virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) - { - if (state_ptr->getLocalParametrization() == nullptr) - state_block_local_param_.erase(state_ptr); - else - state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization(); - }; -}; - -TEST(SolverManager, Create) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // check double pointers to branches - ASSERT_EQ(P, solver_manager_ptr->getProblem()); -} - -TEST(SolverManager, AddStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // update solver - solver_manager_ptr->update(); - - // check stateblock - ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); -} - -TEST(SolverManager, DoubleAddStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // update solver - solver_manager_ptr->update(); - - // add stateblock again - P->addStateBlock(sb_ptr); - - // update solver - solver_manager_ptr->update(); - - // check stateblock - ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); -} - -TEST(SolverManager, UpdateStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); - - // update solver - solver_manager_ptr->update(); - - // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); - - // check stateblock unfixed - ASSERT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); - - // Fix frame - sb_ptr->fix(); - - // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_TRUE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); - - // update solver manager - solver_manager_ptr->update(); - - // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); - - // check stateblock fixed - ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); -} - -TEST(SolverManager, AddUpdateStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); - - // Fix state block - sb_ptr->fix(); - - // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_TRUE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); - - // update solver manager - solver_manager_ptr->update(); - - // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); - - // check stateblock fixed - ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); - ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); -} - -TEST(SolverManager, AddUpdateLocalParamStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); - - // Local param - LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>(); - sb_ptr->setLocalParametrization(local_ptr); - - // Fix state block - sb_ptr->fix(); - - // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_TRUE(sb_ptr->fixUpdated()); - ASSERT_TRUE(sb_ptr->localParamUpdated()); - - // update solver manager - solver_manager_ptr->update(); - - // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); - - // check stateblock fixed - ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); - ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); - ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); -} - -TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // Local param - LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>(); - sb_ptr->setLocalParametrization(local_ptr); - - // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_TRUE(sb_ptr->localParamUpdated()); - - // add stateblock - P->addStateBlock(sb_ptr); - - // update solver manager - solver_manager_ptr->update(); - - // check stateblock localparam - ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); - - // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_FALSE(sb_ptr->localParamUpdated()); - - // Remove local param - sb_ptr->removeLocalParametrization(); - - // check flags - ASSERT_FALSE(sb_ptr->stateUpdated()); - ASSERT_FALSE(sb_ptr->fixUpdated()); - ASSERT_TRUE(sb_ptr->localParamUpdated()); - - // update solver manager - solver_manager_ptr->update(); - - // check stateblock localparam - ASSERT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr)); -} - -TEST(SolverManager, RemoveStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // update solver - solver_manager_ptr->update(); - - // remove state_block - P->removeStateBlock(sb_ptr); - - // update solver - solver_manager_ptr->update(); - - // check stateblock - ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); -} - -TEST(SolverManager, AddRemoveStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // remove state_block - P->removeStateBlock(sb_ptr); - - // update solver - solver_manager_ptr->update(); - - // check state block - ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); -} - -TEST(SolverManager, RemoveUpdateStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add state_block - P->addStateBlock(sb_ptr); - - // remove state_block - P->removeStateBlock(sb_ptr); - - // update solver - solver_manager_ptr->update(); -} - -TEST(SolverManager, DoubleRemoveStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // add stateblock - P->addStateBlock(sb_ptr); - - // remove state_block - P->removeStateBlock(sb_ptr); - - // update solver - solver_manager_ptr->update(); - - // remove state_block - P->removeStateBlock(sb_ptr); - - // update solver manager - solver_manager_ptr->update(); -} - -TEST(SolverManager, AddUpdatedStateBlock) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // Fix - sb_ptr->fix(); - - // Set State - Vector2s state_2 = 2*state; - sb_ptr->setState(state_2); - - // Check flags have been set true - ASSERT_TRUE(sb_ptr->fixUpdated()); - ASSERT_TRUE(sb_ptr->stateUpdated()); - - // == When an ADD is notified, all previous notifications should be cleared (if not consumption of notifs) == - - // add state_block - P->addStateBlock(sb_ptr); - - ASSERT_EQ(P->getStateBlockNotificationMap().size(),1); - ASSERT_EQ(P->getStateBlockNotificationMap().begin()->second,ADD); - - // == Insert OTHER notifications == - - // Set State --> FLAG - state_2 = 2*state; - sb_ptr->setState(state_2); - - // Fix --> FLAG - sb_ptr->unfix(); - - ASSERT_EQ(P->getStateBlockNotificationMap().size(),1); // only ADD notification (fix and state are flags in sb) - - // == REMOVE should clear the previous notification (ADD) in the stack == - - // remove state_block - P->removeStateBlock(sb_ptr); - - ASSERT_EQ(P->getStateBlockNotificationMap().size(),0);// ADD + REMOVE = EMPTY - - // == UPDATES + REMOVE should clear the list of notifications == - - // add state_block - P->addStateBlock(sb_ptr); - - // update solver - solver_manager_ptr->update(); - - ASSERT_EQ(P->getStateBlockNotificationMap().size(),0); // After solver_manager->update, notifications should be empty - - // Fix - sb_ptr->fix(); - - // Set State - state_2 = 2*state; - sb_ptr->setState(state_2); - - // remove state_block - P->removeStateBlock(sb_ptr); - - ASSERT_EQ(P->getStateBlockNotificationMap().size(),1); - ASSERT_EQ(P->getStateBlockNotificationMap().begin()->second, REMOVE); -} - -TEST(SolverManager, AddFactor) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); - - // update solver - solver_manager_ptr->update(); - - // check factor - ASSERT_TRUE(solver_manager_ptr->isFactorRegistered(c)); -} - -TEST(SolverManager, RemoveFactor) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); - - // update solver - solver_manager_ptr->update(); - - // add factor - P->removeFactor(c); - - // update solver - solver_manager_ptr->update(); - - // check factor - ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c)); -} - -TEST(SolverManager, AddRemoveFactor) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); - - ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c); - - // add factor - P->removeFactor(c); - - ASSERT_TRUE(P->getFactorNotificationMap().empty()); - - // update solver - solver_manager_ptr->update(); - - // check factor - ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c)); -} - -TEST(SolverManager, DoubleRemoveFactor) -{ - ProblemPtr P = Problem::create("PO 2D"); - SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - - // Create State block - Vector2s state; state << 1, 2; - StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - - // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); - - // update solver - solver_manager_ptr->update(); - - // remove factor - P->removeFactor(c); - - // update solver - solver_manager_ptr->update(); - - // remove factor - P->removeFactor(c); - - // update solver - solver_manager_ptr->update(); - - // check factor - ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c)); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp deleted file mode 100644 index 03b1d0582280f294d1fbe5bd217b8294f861ca73..0000000000000000000000000000000000000000 --- a/test/gtest_time_stamp.cpp +++ /dev/null @@ -1,219 +0,0 @@ -#include "utils_gtest.h" -#include "core/common/time_stamp.h" - -#include <thread> - -TEST(WolfTestTimeStamp, TimeStampInitNow) -{ - wolf::TimeStamp start; - - // If we don't sleep, start == time_stamp sometimes. - // And sometimes start <= time_stamp ... - std::this_thread::sleep_for(std::chrono::microseconds(1)); - - ASSERT_NE(start.get(), 0); - - wolf::TimeStamp time_stamp; - -// std::cout << std::fixed; -// std::cout << std::setprecision(15); -// std::cout << start.get() << " | " << time_stamp.get() << std::endl; - - ASSERT_NE(time_stamp.get(), start.get()); - - ASSERT_LT(start.get(), time_stamp.get()); - -// PRINTF("All good at WolfTestTimeStamp::TimeStampInitNow !\n"); -} - -TEST(WolfTestTimeStamp, TimeStampInitScalar) -{ - wolf::Scalar val(101010); - - wolf::TimeStamp start(val); - - ASSERT_EQ(start.get(), val); - ASSERT_EQ(start.getSeconds(), val); - ASSERT_EQ(start.getNanoSeconds(), (unsigned int) 0); - - std::stringstream ss; - start.print(ss); - - ASSERT_STREQ("101010.000000000", ss.str().c_str()); - -// PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalar !\n"); -} - -TEST(WolfTestTimeStamp, TimeStampInitScalarSecNano) -{ - wolf::Scalar sec(101010); - wolf::Scalar nano(202020); - wolf::Scalar val(101010.000202020); - - wolf::TimeStamp start(sec, nano); - - // start.get -> 101010.000202020004508 - - ASSERT_EQ(start.get(), val); - ASSERT_EQ(start.getSeconds(), sec); - ASSERT_EQ(start.getNanoSeconds(), nano); - - std::stringstream ss; - start.print(ss); - - ASSERT_STREQ("101010.000202020", ss.str().c_str()); - -// PRINTF("All good at WolfTestTimeStamp::TimeStampInitScalarSecNano !\n"); -} - -TEST(WolfTestTimeStamp, TimeStampSetNow) -{ - wolf::TimeStamp t1; - wolf::TimeStamp t2(t1); - - ASSERT_EQ(t1,t2); - - // If we don't sleep, start == time_stamp sometimes. - // And sometimes start <= time_stamp ... - std::this_thread::sleep_for(std::chrono::microseconds(1)); - - t2.setToNow(); - - ASSERT_LT(t1,t2); -} - -TEST(WolfTestTimeStamp, TimeStampSetScalar) -{ - wolf::Scalar val(101010.000202020); - - wolf::TimeStamp start; - start.set(val); - - ASSERT_EQ(start.get(), val); - ASSERT_EQ(start.getSeconds(), 101010); - ASSERT_EQ(start.getNanoSeconds(), 202020); - - std::stringstream ss; - start.print(ss); - - ASSERT_STREQ("101010.000202020", ss.str().c_str()); -} - -TEST(WolfTestTimeStamp, TimeStampSetSecNano) -{ - unsigned long int sec(101010); - unsigned long int nano(202020); - - wolf::TimeStamp start; - start.set(sec,nano); - - // start.get -> 101010.000202020004508 - - ASSERT_EQ(start.getSeconds(), sec); - ASSERT_EQ(start.getNanoSeconds(), nano); -} - -TEST(WolfTestTimeStamp, TimeStampEquality) -{ - wolf::TimeStamp start; - - wolf::TimeStamp time_stamp(start); - - ASSERT_EQ(time_stamp, start); - - ASSERT_EQ(time_stamp.get(), start.get()); - - std::this_thread::sleep_for(std::chrono::microseconds(1)); - time_stamp.setToNow(); - - ASSERT_NE(time_stamp.get(), start.get()); - - time_stamp = start; - - ASSERT_EQ(time_stamp.get(), start.get()); - -// PRINTF("All good at WolfTestTimeStamp::TimeStampEquality !\n"); -} - -TEST(WolfTestTimeStamp, TimeStampInequality) -{ - wolf::TimeStamp start; - - std::this_thread::sleep_for(std::chrono::microseconds(1)); - - wolf::TimeStamp time_stamp; - - // error: no match for ‘operator!=’ - //ASSERT_NE(time_stamp, start); - - ASSERT_LT(start, time_stamp); - ASSERT_LE(start, time_stamp); - ASSERT_LE(start, start); - - // error: no match for ‘operator>’ - ASSERT_GT(time_stamp, start); - ASSERT_GE(time_stamp, start); - ASSERT_GE(start, start); - -// PRINTF("All good at WolfTestTimeStamp::TimeStampInequality !\n"); -} - -TEST(WolfTestTimeStamp, TimeStampSubstraction) -{ - wolf::TimeStamp t1; - wolf::TimeStamp t2(t1); - wolf::Scalar dt(1e-5); - - t2+=dt; - - ASSERT_LT(t1, t2); - ASSERT_EQ(t2-t1, dt); - ASSERT_EQ(t1-t2, -dt); -} - -TEST(WolfTestTimeStamp, TimeStampAdding) -{ - wolf::TimeStamp t1,t3; - wolf::TimeStamp t2(t1); - wolf::Scalar dt(1e-5); - - t2 +=dt; - t3 = t1+dt; - - ASSERT_EQ(t2, t3); -} - -TEST(WolfTestTimeStamp, TimeStampOperatorOstream) -{ - wolf::TimeStamp t(5); - wolf::Scalar dt = 1e-4; - t+=dt; - - std::ostringstream ss1, ss2; - t.print(ss1); - - ss2 << t; - - ASSERT_EQ(ss1.str(), ss2.str()); - -// PRINTF("All good at WolfTestTimeStamp::TimeStampOperatorOstream !\n"); -} - -TEST(WolfTestTimeStamp, TimeStampSecNanoSec) -{ - unsigned long int sec = 5; - unsigned long int nano = 1e5; - wolf::TimeStamp t1(wolf::Scalar(sec)+wolf::Scalar(nano)*1e-9); - wolf::TimeStamp t2(sec,nano); - - ASSERT_EQ(t1.getSeconds(),sec); - ASSERT_EQ(t2.getSeconds(),sec); - ASSERT_EQ(t1.getNanoSeconds(),nano); - ASSERT_EQ(t2.getNanoSeconds(),nano); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp deleted file mode 100644 index 2f956b0846a505568c084147a1121904cc2a80da..0000000000000000000000000000000000000000 --- a/test/gtest_track_matrix.cpp +++ /dev/null @@ -1,329 +0,0 @@ -/** - * \file gtest_track_matrix.cpp - * - * Created on: Mar 24, 2018 - * \author: jsola - */ - -#include "utils_gtest.h" - -#include "core/processor/track_matrix.h" - -using namespace wolf; - -class TrackMatrixTest : public testing::Test -{ - public: - TrackMatrix track_matrix; - - Eigen::Vector2s m; - Eigen::Matrix2s m_cov = Eigen::Matrix2s::Identity()*0.01; - - CaptureBasePtr C0, C1, C2, C3, C4; - FeatureBasePtr f0, f1, f2, f3, f4; - - virtual void SetUp() - { - C0 = std::make_shared<CaptureBase>("BASE", 0.0); - C1 = std::make_shared<CaptureBase>("BASE", 1.0); - C2 = std::make_shared<CaptureBase>("BASE", 2.0); - C3 = std::make_shared<CaptureBase>("BASE", 3.0); - C4 = std::make_shared<CaptureBase>("BASE", 4.0); - - f0 = std::make_shared<FeatureBase>("BASE", m, m_cov); - f1 = std::make_shared<FeatureBase>("BASE", m, m_cov); - f2 = std::make_shared<FeatureBase>("BASE", m, m_cov); - f3 = std::make_shared<FeatureBase>("BASE", m, m_cov); - f4 = std::make_shared<FeatureBase>("BASE", m, m_cov); - } -}; - -TEST_F(TrackMatrixTest, newTrack) -{ - track_matrix.newTrack(C0, f0); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); - - track_matrix.newTrack(C0, f1); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2); -} - -TEST_F(TrackMatrixTest, add) -{ - track_matrix.newTrack(C0, f0); - - track_matrix.add(f0->trackId(), C1, f1); - /* C0 C1 C2 snapshots - * - * f0---f1 trk 0 - */ - ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int) 2); - ASSERT_EQ(f1->trackId(), f0->trackId()); - - track_matrix.add(f0->trackId(), C2, f2); - /* C0 C1 C2 snapshots - * - * f0---f1---f2 trk 0 - */ - ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 3); - ASSERT_EQ(f2->trackId(), f0->trackId()); -} - -TEST_F(TrackMatrixTest, numTracks) -{ - track_matrix.newTrack(C0, f0); - track_matrix.add(f0->trackId(), C1, f1); - - /* C0 C1 C2 snapshots - * - * f0---f1 trk 0 - */ - - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); - - track_matrix.add(f0->trackId(), C0, f2); - - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); -} - -TEST_F(TrackMatrixTest, trackSize) -{ - track_matrix.newTrack(C0, f0); - track_matrix.add(f0->trackId(), C1, f1); - track_matrix.newTrack(C0, f2); - - /* C0 C1 C2 snapshots - * - * f0---f1 trk 0 - * | - * f2 trk 1 - */ - - ASSERT_EQ(track_matrix.trackSize(f0->trackId()), (unsigned int) 2); - ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 1); -} - -TEST_F(TrackMatrixTest, first_last_Feature) -{ - /* C0 C1 C2 snapshots - * - * f0--f1 trk 0 - * | - * f2 trk 1 - */ - - track_matrix.newTrack(C0, f0); - track_matrix.add(f0->trackId(), C1, f1); - track_matrix.newTrack(C1, f2); - - ASSERT_EQ(track_matrix.firstFeature(f0->trackId()), f0); - ASSERT_EQ(track_matrix.lastFeature (f0->trackId()), f1); - ASSERT_EQ(track_matrix.feature (f0->trackId(), C0 ), f0); - ASSERT_EQ(track_matrix.feature (f0->trackId(), C1 ), f1); - ASSERT_EQ(track_matrix.feature (f2->trackId(), C1 ), f2); -} - -TEST_F(TrackMatrixTest, remove_ftr) -{ - track_matrix.newTrack(C0, f0); - track_matrix.add(f0->trackId(), C1, f1); - track_matrix.newTrack(C0, f2); - - /* C0 C1 C2 snapshots - * - * f0---f1 trk 0 - * | - * f2 trk 1 - */ - - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2); - - track_matrix.remove(f0); - /* C0 C1 C2 snapshots - * - * f1 trk 0 - * - * f2 trk 1 - */ - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2); - ASSERT_EQ(track_matrix.firstFeature(f0->trackId()), f1); - ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2); - ASSERT_EQ(track_matrix.snapshot(C0).size(), (unsigned int) 1); - ASSERT_EQ(track_matrix.snapshot(C0).at(f2->trackId()), f2); - ASSERT_EQ(track_matrix.snapshot(C1).size(), (unsigned int) 1); - ASSERT_EQ(track_matrix.snapshot(C1).at(f0->trackId()), f1); - - track_matrix.remove(f1); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); - ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2); -} - -TEST_F(TrackMatrixTest, remove_trk) -{ - track_matrix.newTrack(C0, f0); - track_matrix.add(f0->trackId(), C1, f1); - track_matrix.newTrack(C0, f2); - - /* C0 C1 C2 snapshots - * - * f0---f1 trk 0 - * | - * f2 trk 1 - */ - - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2); - - track_matrix.remove(f0->trackId()); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); - ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2); - - track_matrix.remove(f2->trackId()); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0); -} - -TEST_F(TrackMatrixTest, remove_snapshot) -{ - track_matrix.newTrack(C0, f0); - track_matrix.add(f0->trackId(), C1, f1); - track_matrix.newTrack(C0, f2); - - /* C0 C1 C2 snapshots - * - * f0---f1 trk 0 - * | - * f2 trk 1 - */ - - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2); - - track_matrix.remove(C0); - /* C1 C2 snapshots - * - * f1 trk 0 - */ - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1); - ASSERT_EQ(track_matrix.firstFeature(f1->trackId()), f1); - - track_matrix.remove(C1); - ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0); -} - -TEST_F(TrackMatrixTest, track) -{ - track_matrix.newTrack(C0, f0); - track_matrix.add(f0->trackId(), C1, f1); - track_matrix.newTrack(C0, f2); - - /* C0 C1 C2 snapshots - * - * f0---f1 trk 0 - * | - * f2 trk 1 - */ - - Track t0 = track_matrix.track(f0->trackId()); - Track t2 = track_matrix.track(f2->trackId()); - - ASSERT_EQ(t0.size(), (unsigned int) 2); - ASSERT_EQ(t0.at(C0->getTimeStamp()), f0); - ASSERT_EQ(t0.at(C1->getTimeStamp()), f1); - - ASSERT_EQ(t2.size(), (unsigned int) 1); - ASSERT_EQ(t2.at(C0->getTimeStamp()), f2); -} - -TEST_F(TrackMatrixTest, snapshot) -{ - track_matrix.newTrack(C0, f0); - track_matrix.add(f0->trackId(), C1, f1); - track_matrix.newTrack(C0, f2); - - /* C0 C1 C2 snapshots - * - * f0---f1 trk 0 - * | - * f2 trk 1 - */ - - Snapshot s0 = track_matrix.snapshot(C0); - Snapshot s1 = track_matrix.snapshot(C1); - - ASSERT_EQ(s0.size(), (unsigned int) 2); - ASSERT_EQ(s0.at(f0->trackId()), f0); - ASSERT_EQ(s0.at(f2->trackId()), f2); - - ASSERT_EQ(s1.size(), (unsigned int) 1); - ASSERT_EQ(s1.at(f1->trackId()), f1); -} - -TEST_F(TrackMatrixTest, trackAsVector) -{ - track_matrix.newTrack(C0, f0); - track_matrix.add(f0->trackId(), C1, f1); - track_matrix.newTrack(C0, f2); - - /* C0 C1 C2 snapshots - * - * f0---f1 trk 0 - * | - * f2 trk 1 - */ - - std::vector<FeatureBasePtr> vt0 = track_matrix.trackAsVector(f0->trackId()); // get track 0 as vector - - ASSERT_EQ(vt0.size(), (unsigned int) 2); - ASSERT_EQ(vt0[0], f0); - ASSERT_EQ(vt0.at(1), f1); -} - -TEST_F(TrackMatrixTest, snapshotAsList) -{ - track_matrix.newTrack(C0, f0); - track_matrix.add(f0->trackId(), C1, f1); - track_matrix.newTrack(C0, f2); - - /* C0 C1 C2 snapshots - * - * f0---f1 trk 0 - * | - * f2 trk 1 - */ - - std::list<FeatureBasePtr> lt0 = track_matrix.snapshotAsList(f0->getCapture()); // get track 0 as vector - - ASSERT_EQ(lt0.size() , (unsigned int) 2); - ASSERT_EQ(lt0.front(), f0); - ASSERT_EQ(lt0.back() , f2); -} - -TEST_F(TrackMatrixTest, matches) -{ - track_matrix.newTrack(C0, f0); - track_matrix.add(f0->trackId(), C1, f1); - track_matrix.add(f0->trackId(), C2, f2); - track_matrix.newTrack(C0, f3); - track_matrix.add(f3->trackId(), C1, f4); - - /* C0 C1 C2 C3 snapshots - * - * f0---f1---f2 trk 0 - * | | - * f3---f4 trk 1 - */ - - TrackMatches pairs = track_matrix.matches(C0, C2); - - ASSERT_EQ(pairs.size(), (unsigned int) 1); - ASSERT_EQ(pairs.at(f0->trackId()).first, f0); - ASSERT_EQ(pairs.at(f0->trackId()).second, f2); - - pairs = track_matrix.matches(C2, C3); - - ASSERT_EQ(pairs.size(), (unsigned int) 0); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp deleted file mode 100644 index 65512225c4b75340855cd26d139cc4cc640a0941..0000000000000000000000000000000000000000 --- a/test/gtest_trajectory.cpp +++ /dev/null @@ -1,260 +0,0 @@ -/* - * gtest_trajectory.cpp - * - * Created on: Nov 13, 2016 - * Author: jsola - */ - -#include "utils_gtest.h" -#include "core/utils/logging.h" - -#include "core/problem/problem.h" -#include "core/trajectory/trajectory_base.h" -#include "core/frame/frame_base.h" - -#include <iostream> - -using namespace wolf; - -struct DummyNotificationProcessor -{ - DummyNotificationProcessor(ProblemPtr _problem) - : problem_(_problem) - { - // - } - - void update() - { - if (problem_ == nullptr) - { - FAIL() << "problem_ is nullptr !"; - } - - auto sb_noti_pair = problem_->getStateBlockNotificationMap().begin(); - while (sb_noti_pair != problem_->getStateBlockNotificationMap().end()) - { - switch (sb_noti_pair->second) - { - case ADD: - { - break; - } - case REMOVE: - { - break; - } - default: - throw std::runtime_error("SolverManager::update: State Block notification must be ADD or REMOVE."); - } - sb_noti_pair = problem_->getStateBlockNotificationMap().erase(sb_noti_pair); - } - ASSERT_TRUE(problem_->getStateBlockNotificationMap().empty()); - } - - ProblemPtr problem_; -}; - -/// Set to true if you want debug info -bool debug = false; - -TEST(TrajectoryBase, ClosestKeyFrame) -{ - - ProblemPtr P = Problem::create("PO 2D"); - TrajectoryBasePtr T = P->getTrajectory(); - - // Trajectory status: - // kf1 kf2 f3 frames - // 1 2 3 time stamps - // --+-----+-----+---> time - - FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, nullptr, nullptr); - FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, nullptr, nullptr); - FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, nullptr, nullptr); - T->addFrame(f1); - T->addFrame(f2); - T->addFrame(f3); - - FrameBasePtr kf; // closest key-frame queried - - kf = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return f0 - ASSERT_EQ(kf->id(), f1->id()); // same id! - - kf = T->closestKeyFrameToTimeStamp(1.1); // between keyframes --> return f1 - ASSERT_EQ(kf->id(), f1->id()); // same id! - - kf = T->closestKeyFrameToTimeStamp(1.9); // between keyframes --> return f2 - ASSERT_EQ(kf->id(), f2->id()); // same id! - - kf = T->closestKeyFrameToTimeStamp(2.6); // between keyframe and frame, but closer to frame --> return f2 - ASSERT_EQ(kf->id(), f2->id()); // same id! - - kf = T->closestKeyFrameToTimeStamp(3.2); // after the last frame --> return f2 - ASSERT_EQ(kf->id(), f2->id()); // same id! -} - -TEST(TrajectoryBase, Add_Remove_Frame) -{ - using std::make_shared; - - ProblemPtr P = Problem::create("PO 2D"); - TrajectoryBasePtr T = P->getTrajectory(); - - DummyNotificationProcessor N(P); - - // Trajectory status: - // kf1 kf2 f3 frames - // 1 2 3 time stamps - // --+-----+-----+---> time - - FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed - FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not - FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame - - std::cout << __LINE__ << std::endl; - - // add frames and keyframes - T->addFrame(f1); // KF - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1); - ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2); - ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2); - std::cout << __LINE__ << std::endl; - - T->addFrame(f2); // KF - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2); - ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 4); - ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4); - std::cout << __LINE__ << std::endl; - - T->addFrame(f3); // F - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 3); - ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 4); - ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4); - std::cout << __LINE__ << std::endl; - - ASSERT_EQ(T->getLastFrame()->id(), f3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); - std::cout << __LINE__ << std::endl; - - N.update(); - std::cout << __LINE__ << std::endl; - - // remove frames and keyframes - f2->remove(); // KF - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2); - ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2); - ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2); - std::cout << __LINE__ << std::endl; - - ASSERT_EQ(T->getLastFrame()->id(), f3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id()); - std::cout << __LINE__ << std::endl; - - f3->remove(); // F - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1); - ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2); - ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2); - std::cout << __LINE__ << std::endl; - - ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id()); - - f1->remove(); // KF - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 0); - ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 0); - ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4); - std::cout << __LINE__ << std::endl; - - N.update(); - - ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 0); - std::cout << __LINE__ << std::endl; -} - -TEST(TrajectoryBase, KeyFramesAreSorted) -{ - using std::make_shared; - - ProblemPtr P = Problem::create("PO 2D"); - TrajectoryBasePtr T = P->getTrajectory(); - - // Trajectory status: - // kf1 kf2 f3 frames - // 1 2 3 time stamps - // --+-----+-----+---> time - - FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed - FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not - FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame - - // add frames and keyframes in random order --> keyframes must be sorted after that - T->addFrame(f2); // KF2 - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getLastFrame() ->id(), f2->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); - - T->addFrame(f3); // F3 - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getLastFrame() ->id(), f3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); - - T->addFrame(f1); // KF1 - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getLastFrame() ->id(), f3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); - - f3->setKey(); // set KF3 - if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getLastFrame() ->id(), f3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id()); - - FrameBasePtr f4 = std::make_shared<FrameBase>(NON_KEY_FRAME, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame - T->addFrame(f4); - // Trajectory status: - // kf1 kf2 kf3 f4 frames - // 1 2 3 1.5 time stamps - // --+-----+-----+------+---> time - if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getLastFrame() ->id(), f4->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id()); - - f4->setKey(); - // Trajectory status: - // kf1 kf4 kf2 kf3 frames - // 1 1.5 2 3 time stamps - // --+-----+-----+------+---> time - if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getLastFrame() ->id(), f3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id()); - - f2->setTimeStamp(4); - // Trajectory status: - // kf1 kf4 kf3 kf2 frames - // 1 1.5 3 4 time stamps - // --+-----+-----+------+---> time - if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getLastFrame() ->id(), f2->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); - - f4->setTimeStamp(0.5); - // Trajectory status: - // kf4 kf2 kf3 kf2 frames - // 0.5 1 3 4 time stamps - // --+-----+-----+------+---> time - if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getFrameList().front()->id(), f4->id()); - -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} -