diff --git a/include/IMU/factor/factor_fix_bias.h b/include/IMU/factor/factor_fix_bias.h
new file mode 100644
index 0000000000000000000000000000000000000000..6fb889b55ddda62ad17b0d2644d3cf0ac7044d33
--- /dev/null
+++ b/include/IMU/factor/factor_fix_bias.h
@@ -0,0 +1,85 @@
+
+#ifndef FACTOR_FIX_BIAS_H_
+#define FACTOR_FIX_BIAS_H_
+
+//Wolf includes
+#include "core/capture/capture_IMU.h"
+#include "core/feature/feature_IMU.h"
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorFixBias);
+
+//class
+class FactorFixBias: public FactorAutodiff<FactorFixBias,6,3,3>
+{
+ public:
+ FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorFixBias, 6, 3, 3>("FIX BIAS",
+ nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getAccBias(),
+ std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getGyroBias())
+ {
+ // std::cout << "created FactorFixBias " << std::endl;
+ }
+
+ virtual ~FactorFixBias() = default;
+
+ template<typename T>
+ bool operator ()(const T* const _ab, const T* const _wb, T* _residuals) const;
+
+};
+
+template<typename T>
+inline bool FactorFixBias::operator ()(const T* const _ab, const T* const _wb, T* _residuals) const
+{
+ // measurement
+ Eigen::Matrix<T,6,1> meas = getMeasurement().cast<T>();
+ Eigen::Matrix<T,3,1> ab(_ab);
+ Eigen::Matrix<T,3,1> wb(_wb);
+
+ // error
+ Eigen::Matrix<T,6,1> er;
+ er.head(3) = meas.head(3) - ab;
+ er.tail(3) = meas.tail(3) - wb;
+
+ // residual
+ Eigen::Map<Eigen::Matrix<T,6,1>> res(_residuals);
+ res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+
+ ////////////////////////////////////////////////////////
+ // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
+// using ceres::Jet;
+// Eigen::MatrixXs J(6,6);
+// J.row(0) = ((Jet<Scalar, 3>)(er(0))).v;
+// J.row(1) = ((Jet<Scalar, 3>)(er(1))).v;
+// J.row(2) = ((Jet<Scalar, 3>)(er(2))).v;
+// J.row(3) = ((Jet<Scalar, 3>)(er(3))).v;
+// J.row(4) = ((Jet<Scalar, 3>)(er(4))).v;
+// J.row(5) = ((Jet<Scalar, 3>)(er(5))).v;
+
+// J.row(0) = ((Jet<Scalar, 3>)(res(0))).v;
+// J.row(1) = ((Jet<Scalar, 3>)(res(1))).v;
+// J.row(2) = ((Jet<Scalar, 3>)(res(2))).v;
+// J.row(3) = ((Jet<Scalar, 3>)(res(3))).v;
+// J.row(4) = ((Jet<Scalar, 3>)(res(4))).v;
+// J.row(5) = ((Jet<Scalar, 3>)(res(5))).v;
+
+// if (sizeof(er(0)) != sizeof(double))
+// {
+// std::cout << "FactorFixBias::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorFixBias::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "Sqrt Info(c" << id() << ") = \n " << getMeasurementSquareRootInformationUpper() << std::endl;
+// }
+ ////////////////////////////////////////////////////////
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/base/factor/factor_autodiff_apriltag.h b/include/base/factor/factor_autodiff_apriltag.h
deleted file mode 100644
index b23f30e6f5d0bf97eb184412c79d817c66169d68..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_autodiff_apriltag.h
+++ /dev/null
@@ -1,211 +0,0 @@
-#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
deleted file mode 100644
index f2fbb23756ea252365f1ba366d6a16119f2336e6..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_apriltag.h
+++ /dev/null
@@ -1,59 +0,0 @@
-#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
deleted file mode 100644
index 8a586d735a8d86d34f91f142e0cc06e6c2c59150..0000000000000000000000000000000000000000
--- a/include/base/landmark/landmark_apriltag.h
+++ /dev/null
@@ -1,63 +0,0 @@
-
-#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
deleted file mode 100644
index d445b223f3e4afd4449da4cfe630cda87c329239..0000000000000000000000000000000000000000
--- a/include/base/processor/ippe.h
+++ /dev/null
@@ -1,222 +0,0 @@
-#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
deleted file mode 100644
index 9e0532cf5f869e2b4eedb750f90b996020203bfe..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_landmark_apriltag.h
+++ /dev/null
@@ -1,183 +0,0 @@
-#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_ */