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 fef41cbba74fb13c7d02841c2ddbb876a0c28959..e0927077eb2e92d0f7bb6ecffe38efe64ea7cb7e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -48,7 +48,7 @@ SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
if(UNIX)
# GCC is not strict enough by default, so enable most of the warnings.
set(CMAKE_CXX_FLAGS
- "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
+ "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers -Wno-unused-but-set-variable")
endif(UNIX)
#Set compiler according C++11 support
@@ -57,12 +57,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()
@@ -125,17 +125,17 @@ FIND_PACKAGE(faramotics QUIET) #faramotics is not required
IF(faramotics_FOUND)
FIND_PACKAGE(GLUT REQUIRED)
FIND_PACKAGE(pose_state_time REQUIRED)
- MESSAGE("Faramotics Library FOUND: Faramotics related sources will be built.")
+ MESSAGE("Faramotics Library FOUND: Faramotics related sources will be built.")
ENDIF(faramotics_FOUND)
FIND_PACKAGE(laser_scan_utils QUIET) #laser_scan_utils is not required
IF(laser_scan_utils_FOUND)
- MESSAGE("laser_scan_utils Library FOUND: laser_scan_utils related sources will be built.")
+ MESSAGE("laser_scan_utils Library FOUND: laser_scan_utils related sources will be built.")
ENDIF(laser_scan_utils_FOUND)
FIND_PACKAGE(raw_gps_utils QUIET) #raw_gps_utils is not required
IF(raw_gps_utils_FOUND)
- MESSAGE("raw_gps_utils Library FOUND: raw_gps_utils related sources will be built.")
+ MESSAGE("raw_gps_utils Library FOUND: raw_gps_utils related sources will be built.")
ENDIF(raw_gps_utils_FOUND)
# Vision Utils
@@ -149,9 +149,19 @@ ENDIF(vision_utils_FOUND)
# Cereal
FIND_PACKAGE(cereal QUIET)
IF(cereal_FOUND)
- MESSAGE("cereal Library FOUND: cereal related sources will be built.")
+ 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)
@@ -252,6 +262,10 @@ IF(GLOG_FOUND)
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
@@ -661,6 +675,33 @@ IF (vision_utils_FOUND)
src/processor/processor_tracker_landmark_image.cpp
)
ENDIF(vision_utils_FOUND)
+
+IF (OPENCV_FOUND AND Apriltag_FOUND)
+ SET(HDRS_FACTOR ${HDRS_FACTOR}
+ include/base/factor/factor_autodiff_apriltag.h
+ )
+ SET(HDRS_FEATURE ${HDRS_FEATURE}
+ include/base/feature/feature_apriltag.h
+ )
+ SET(HDRS_LANDMARK ${HDRS_LANDMARK}
+ include/base/landmark/landmark_apriltag.h
+ )
+ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+ include/base/processor/processor_tracker_landmark_apriltag.h
+ include/base/processor/ippe.h
+ )
+ SET(SRCS_FEATURE ${SRCS_FEATURE}
+ src/feature/feature_apriltag.cpp
+ )
+ SET(SRCS_LANDMARK ${SRCS_LANDMARK}
+ src/landmark/landmark_apriltag.cpp
+ )
+ SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+ src/processor/processor_tracker_landmark_apriltag.cpp
+ src/processor/ippe.cpp
+ )
+ENDIF(OPENCV_FOUND AND Apriltag_FOUND)
+
#SUBDIRECTORIES
add_subdirectory(hello_wolf)
IF (cereal_FOUND)
@@ -680,7 +721,7 @@ IF(YAMLCPP_FOUND)
SET(HDRS_YAML ${HDRS_YAML}
include/base/yaml/yaml_conversion.h
)
-
+
# sources
SET(SRCS ${SRCS}
src/yaml/processor_odom_3D_yaml.cpp
@@ -699,9 +740,14 @@ IF(YAMLCPP_FOUND)
src/yaml/processor_image_yaml.cpp
src/yaml/processor_tracker_feature_trifocal_yaml.cpp
)
+ IF(Apriltag_FOUND)
+ SET(SRCS ${SRCS}
+ src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
+ )
+ ENDIF(Apriltag_FOUND)
ENDIF(vision_utils_FOUND)
ENDIF(YAMLCPP_FOUND)
-
+
# create the shared library
ADD_LIBRARY(${PROJECT_NAME}
SHARED
@@ -748,6 +794,12 @@ ENDIF (YAMLCPP_FOUND)
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/base/factor/factor_GPS_pseudorange_3D.h b/include/base/factor/factor_GPS_pseudorange_3D.h
index 72e44d4f62f55358e33cf2ebb14254ca532f1bd3..a28c069ec14fc2e38692cc7ab9ba60a210f0a310 100644
--- a/include/base/factor/factor_GPS_pseudorange_3D.h
+++ b/include/base/factor/factor_GPS_pseudorange_3D.h
@@ -26,6 +26,16 @@ class FactorGPSPseudorange3D: public FactorAutodiff<FactorGPSPseudorange3D, 1, 3
public:
+<<<<<<< HEAD:include/base/constraint/constraint_GPS_pseudorange_3D.h
+ ConstraintGPSPseudorange3D(FeatureBasePtr _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
+ bool _apply_loss_function = false,
+ ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintAutodiff<ConstraintGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>("GPS PR 3D",
+ nullptr, nullptr, nullptr, nullptr, _pr_ptr, _apply_loss_function, _status,
+ _ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to map frame
+ _ftr_ptr->getFramePtr()->getOPtr(), // orientation of the vehicle's frame wrt map frame
+ _ftr_ptr->getCapturePtr()->getSensorPPtr(), // position of the sensor (gps antenna) with respect to base frame
+=======
FactorGPSPseudorange3D(FeatureBasePtr _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
@@ -34,6 +44,7 @@ class FactorGPSPseudorange3D: public FactorAutodiff<FactorGPSPseudorange3D, 1, 3
_ftr_ptr->getFrame()->getP(), // position of the vehicle's frame with respect to map frame
_ftr_ptr->getFrame()->getO(), // orientation of the vehicle's frame wrt map frame
_ftr_ptr->getCapture()->getSensorP(), // position of the sensor (gps antenna) with respect to base frame
+>>>>>>> devel:include/base/factor/factor_GPS_pseudorange_3D.h
// orientation of antenna is not needed, because omnidirectional
_ftr_ptr->getCapture()->getSensor()->getIntrinsic(), //intrinsic parameter = receiver time bias
(std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor()))->getMapP(), // initial vehicle position wrt ecef frame
diff --git a/include/base/factor/factor_autodiff_apriltag.h b/include/base/factor/factor_autodiff_apriltag.h
new file mode 100644
index 0000000000000000000000000000000000000000..3402cc497ec9c71f293dff2038be6a7c1d985b50
--- /dev/null
+++ b/include/base/factor/factor_autodiff_apriltag.h
@@ -0,0 +1,182 @@
+#ifndef _FACTOR_AUTODIFF_APRILTAG_H_
+#define _FACTOR_AUTODIFF_APRILTAG_H_
+
+//Wolf includes
+#include "base/wolf.h"
+#include "base/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
+{
+ //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_measured.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/factor/factor_autodiff_distance_3D.h b/include/base/factor/factor_autodiff_distance_3D.h
index 77eb08e2ae049978c7f2978eee3106fcec66ba9c..aaf3872c3d879ad5cb628f1d455331d28e301202 100644
--- a/include/base/factor/factor_autodiff_distance_3D.h
+++ b/include/base/factor/factor_autodiff_distance_3D.h
@@ -50,7 +50,12 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
Map<const Matrix<T,3,1>> pos2(_pos2);
Map<Matrix<T,1,1>> res(_residual);
- Matrix<T,1,1> dist_exp ( sqrt( ( pos2 - pos1 ).squaredNorm() ) );
+ // If pos2 and pos1 are the same, undefined behavior when computing the jacobian
+ T norm_squared = ( pos2 - pos1 ).squaredNorm();
+ if (norm_squared < (T)1e-8){
+ norm_squared += (T)1e-8;
+ }
+ Matrix<T,1,1> dist_exp ( sqrt(norm_squared) );
Matrix<T,1,1> dist_meas (getMeasurement().cast<T>() );
Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper().cast<T>();
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/feature/feature_base.h b/include/base/feature/feature_base.h
index 3732bc55247d9d39b90992a07afe7bb04d7f7681..87d6ceeedd22a7904e8ee1151975141fbcad76b3 100644
--- a/include/base/feature/feature_base.h
+++ b/include/base/feature/feature_base.h
@@ -36,12 +36,19 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
public:
+ typedef enum
+ {
+ UNCERTAINTY_IS_COVARIANCE,
+ UNCERTAINTY_IS_INFO,
+ UNCERTAINTY_IS_STDDEV
+ } UncertaintyType;
+
/** \brief Constructor from capture pointer and measure
* \param _tp type of feature -- see wolf.h
* \param _measurement the measurement
* \param _meas_covariance the noise of the measurement
*/
- FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
+ FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_uncertainty, UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
virtual ~FeatureBase();
virtual void remove();
diff --git a/include/base/landmark/landmark_apriltag.h b/include/base/landmark/landmark_apriltag.h
new file mode 100644
index 0000000000000000000000000000000000000000..c5ab9e8398a8fa9d5c044de9e66eaf2d75ad7fa9
--- /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_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/landmark/landmark_match.h b/include/base/landmark/landmark_match.h
index 0c5d3a4915892a3680d634f24a6a40a73df6e26c..e63a569f6d8ee2c0d08f6a722affea064553f891 100644
--- a/include/base/landmark/landmark_match.h
+++ b/include/base/landmark/landmark_match.h
@@ -18,8 +18,8 @@ typedef std::map<FeatureBasePtr, LandmarkMatchPtr> LandmarkMatchMap;
**/
struct LandmarkMatch
{
- LandmarkBasePtr landmark_ptr_;
- Scalar normalized_score_;
+ LandmarkBasePtr landmark_ptr_; ///< Pointer to the matched landmark
+ Scalar normalized_score_; ///< Similarity measure: 0: no match -- 1: perfect match
LandmarkMatch() :
landmark_ptr_(nullptr), normalized_score_(0)
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.h b/include/base/processor/processor_tracker_landmark.h
index 23e8132e308138b41b9c2e79553c82e4cb4b03ba..5ca0c7f702b04dc13697840b09f808faa3cd388a 100644
--- a/include/base/processor/processor_tracker_landmark.h
+++ b/include/base/processor/processor_tracker_landmark.h
@@ -107,6 +107,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker
* \param _landmarks_in input list of landmarks to be found in incoming
* \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_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& _landmarks_in,
FeatureBasePtrList& _features_incoming_out,
@@ -130,14 +131,14 @@ class ProcessorTrackerLandmark : public ProcessorTracker
*/
unsigned int processNew(const unsigned int& _max_features);
- /** \brief Detect new Features
+ /** \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 Features/Landmarks in the system.
+ * 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,
- * to be used for landmark initialization.
+ * 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 unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out) = 0;
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..abe2cb0f61dfd153cc161d42e8a786fcfcc725a8
--- /dev/null
+++ b/include/base/processor/processor_tracker_landmark_apriltag.h
@@ -0,0 +1,186 @@
+#ifndef _PROCESSOR_TRACKER_LANDMARK_APRILTAG_H_
+#define _PROCESSOR_TRACKER_LANDMARK_APRILTAG_H_
+
+//Wolf includes
+#include "base/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_;
+ int tag_black_border_;
+
+ // 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_;
+ bool refine_decode_;
+ bool refine_pose_;
+
+ 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 unsigned 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/include/base/wolf.h b/include/base/wolf.h
index 74b7a0bbb3f0f161c7279f1f873a55b1af478935..f04337a925f070e3bbc5fe04296a1be91fd53eb7 100644
--- a/include/base/wolf.h
+++ b/include/base/wolf.h
@@ -117,7 +117,13 @@ typedef Matrix<wolf::Scalar, 1, Dynamic> RowVectorXs; ///< variable size r
typedef Quaternion<wolf::Scalar> Quaternions; ///< Quaternion of real Scalar type
typedef AngleAxis<wolf::Scalar> AngleAxiss; ///< Angle-Axis of real Scalar type
typedef Rotation2D<wolf::Scalar> Rotation2Ds; ///< Rotation2D of real Scalar type
+typedef RotationBase<wolf::Scalar, 3> Rotation3Ds; ///< Rotation3D of real Scalar type
+// 3. Translations
+typedef Translation<wolf::Scalar, 2> Translation2ds;
+typedef Translation<wolf::Scalar, 3> Translation3ds;
+
+// 4. Transformations
typedef Transform<wolf::Scalar,2,Affine> Affine2ds; ///< Affine2d of real Scalar type
typedef Transform<wolf::Scalar,3,Affine> Affine3ds; ///< Affine3d of real Scalar type
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/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 1d2aa58dbd1b0a5b1767def1eb55d8978f729ac4..6249c6216f27ae5bad5717d14ea6918a2c409e60 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -111,8 +111,9 @@ void CaptureBase::addFeatureList(FeatureBasePtrList& _new_ft_list)
feature_ptr->setCapture(shared_from_this());
if (getProblem() != nullptr)
feature_ptr->setProblem(getProblem());
+ feature_list_.push_back(feature_ptr);
}
- feature_list_.splice(feature_list_.end(), _new_ft_list);
+// feature_list_.splice(feature_list_.end(), _new_ft_list);
}
void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
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_tracker_landmark_apriltag.yaml b/src/examples/processor_tracker_landmark_apriltag.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..fc78c0d2b7c8feaa92f85600e68f3cc8cb565e39
--- /dev/null
+++ b/src/examples/processor_tracker_landmark_apriltag.yaml
@@ -0,0 +1,59 @@
+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)
+ refine_decode: false # improve detection probability for small tags (quite expensive (~*3)
+ refine_pose: false # improves detection by maximizing local contrast so that future pose extraction works better (VERY expensive ~*10-20)
+ 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/factor/CMakeLists.txt b/src/factor/CMakeLists.txt
index 9c575b51f3e266d23e243f6713356516d70901ab..d910e757557c66ff81a85c5c8397c1aff9bb1fff 100644
--- a/src/factor/CMakeLists.txt
+++ b/src/factor/CMakeLists.txt
@@ -5,7 +5,11 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
# Add in this section the CONDITIONAL CLUES [IF/ELSE]
# for external libraries and move inside them the respective lines from above.
-
+IF (OPENCV_FOUND AND Apriltag_FOUND)
+ SET(HDRS_CONSTRAINT ${HDRS_CONSTRAINT}
+ ${CMAKE_CURRENT_SOURCE_DIR}/constraint_autodiff_apriltag.h
+ )
+ENDIF(OPENCV_FOUND AND Apriltag_FOUND)
#=========================================
#=========================================
@@ -22,6 +26,6 @@ SET(SRCS_CONSTRAINT ${SRCS_CONSTRAINT}
# Forward var to parent scope
# These lines always at the end
SET(HDRS_CONSTRAINT ${HDRS_CONSTRAINT} PARENT_SCOPE)
-SET(SRCS_CONSTRAINT ${SRCS_CONSTRAINT} PARENT_SCOPE)
+SET(SRCS_CONSTRAINT ${SRCS_CONSTRAINT} PARENT_SCOPE)
\ No newline at end of file
diff --git a/src/feature/CMakeLists.txt b/src/feature/CMakeLists.txt
index 316731a39f5dd8e3721e70d6b23ceec4186e523e..ca674167dd7c8d7a4bd36bf0e9af40f24232f926 100644
--- a/src/feature/CMakeLists.txt
+++ b/src/feature/CMakeLists.txt
@@ -5,8 +5,15 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
# Add in this section the CONDITIONAL CLUES [IF/ELSE]
# for external libraries and move inside them the respective lines from above.
-
-
+IF (OPENCV_FOUND AND Apriltag_FOUND)
+ SET(HDRS_FEATURE ${HDRS_FEATURE}
+ ${CMAKE_CURRENT_SOURCE_DIR}/feature_apriltag.h
+ )
+ SET(SRCS_FEATURE ${SRCS_FEATURE}
+ ${CMAKE_CURRENT_SOURCE_DIR}/feature_apriltag.cpp
+ )
+ENDIF(OPENCV_FOUND AND Apriltag_FOUND)
+
#=========================================
#=========================================
@@ -17,7 +24,7 @@ SET(HDRS_FEATURE ${HDRS_FEATURE}
SET(SRCS_FEATURE ${SRCS_FEATURE}
${CMAKE_CURRENT_SOURCE_DIR}/feature_diff_drive.cpp
)
-
+
# Forward var to parent scope
# These lines always at the end
SET(HDRS_FEATURE ${HDRS_FEATURE} PARENT_SCOPE)
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/feature/feature_base.cpp b/src/feature/feature_base.cpp
index b47c956b9f39e574f8a8c019788669bc024efbe7..ca47cb78f30cea23a2711394b2c909cf61dd10ef 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -6,7 +6,7 @@ namespace wolf {
unsigned int FeatureBase::feature_id_count_ = 0;
-FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
+FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_uncertainty, UncertaintyType _uncertainty_type) :
NodeBase("FEATURE", _type),
capture_ptr_(),
feature_id_(++feature_id_count_),
@@ -14,8 +14,21 @@ FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measu
landmark_id_(0),
measurement_(_measurement)
{
- setMeasurementCovariance(_meas_covariance);
-// std::cout << "constructed +f" << id() << std::endl;
+ switch (_uncertainty_type)
+ {
+ case UNCERTAINTY_IS_INFO :
+ setMeasurementInformation(_meas_uncertainty);
+ break;
+ case UNCERTAINTY_IS_COVARIANCE :
+ setMeasurementCovariance(_meas_uncertainty);
+ break;
+ case UNCERTAINTY_IS_STDDEV :
+ WOLF_ERROR("STDEV case Not implemented yet");
+ break;
+ default :
+ break;
+ }
+ // std::cout << "constructed +f" << id() << std::endl;
}
FeatureBase::~FeatureBase()
diff --git a/src/landmark/landmark_apriltag.cpp b/src/landmark/landmark_apriltag.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bca929367583b25607e0239eaa1b608176d97ff4
--- /dev/null
+++ b/src/landmark/landmark_apriltag.cpp
@@ -0,0 +1,93 @@
+
+#include "base/landmark/landmark_apriltag.h"
+#include "base/factory.h"
+#include "base/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/map_base.cpp b/src/map_base.cpp
index d1ad03121691b64052bfab01372010f68de863f5..1330da1e31d685cb735a4b8b8a09ebc6f268f104 100644
--- a/src/map_base.cpp
+++ b/src/map_base.cpp
@@ -38,14 +38,16 @@ LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
void MapBase::addLandmarkList(LandmarkBasePtrList& _landmark_list)
{
- LandmarkBasePtrList lmk_list_copy = _landmark_list; //since _landmark_list will be empty after addDownNodeList()
- for (LandmarkBasePtr landmark_ptr : lmk_list_copy)
- {
- landmark_ptr->setMap(shared_from_this());
- landmark_ptr->setProblem(getProblem());
- landmark_ptr->registerNewStateBlocks();
- }
- landmark_list_.splice(landmark_list_.end(), _landmark_list);
+ for (auto lmk : _landmark_list)
+ addLandmark(lmk);
+// LandmarkBasePtrList lmk_list_copy = _landmark_list; //since _landmark_list will be empty after addDownNodeList()
+// for (LandmarkBasePtr landmark_ptr : lmk_list_copy)
+// {
+// landmark_ptr->setMapPtr(shared_from_this());
+// landmark_ptr->setProblem(getProblem());
+// landmark_ptr->registerNewStateBlocks();
+// }
+// landmark_list_.splice(landmark_list_.end(), _landmark_list);
}
void MapBase::load(const std::string& _map_file_dot_yaml)
diff --git a/src/problem.cpp b/src/problem.cpp
index b9cd0a1e6bd63bfb40a3eb67bda35e1b5c6096b2..3c731d6233eba0d8cde8f276d9768b5970232a45 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -691,7 +691,9 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen:
// emplace feature and factor
init_capture->emplaceFeatureAndFactor();
- // Notify all processors about the prior KF
+ WOLF_DEBUG("Set prior callback: KF", origin_keyframe->id(), " Callback emitted with ts = ", origin_keyframe->getTimeStamp());
+
+ // Notify all motion processors about the prior KF
for (auto sensor : hardware_ptr_->getSensorList())
for (auto processor : sensor->getProcessorList())
if (processor->isMotion())
diff --git a/src/processor/CMakeLists.txt b/src/processor/CMakeLists.txt
index 5c223e4f28ff54f4c62267f980124d62f3d6ccda..202687990e02889c621563a4706de0e56647e2b6 100644
--- a/src/processor/CMakeLists.txt
+++ b/src/processor/CMakeLists.txt
@@ -15,6 +15,16 @@ SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
)
ENDIF (vision_utils_FOUND)
+IF (OPENCV_FOUND AND Apriltag_FOUND)
+ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+ ${CMAKE_CURRENT_SOURCE_DIR}/processor_tracker_landmark_apriltag.h
+ ${CMAKE_CURRENT_SOURCE_DIR}/ippe.h
+ )
+ SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+ ${CMAKE_CURRENT_SOURCE_DIR}/processor_tracker_landmark_apriltag.cpp
+ ${CMAKE_CURRENT_SOURCE_DIR}/ippe.cpp
+ )
+ENDIF(OPENCV_FOUND AND Apriltag_FOUND)
#=========================================
#=========================================
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_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 670f3aaf776d371bc1db000d84184f5643cc8e2e..f2eaa97e0e4c149334984aabac275d729a135516 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -30,6 +30,11 @@ unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_fe
* the last and incoming Captures.
*/
+ // clear lists of new features
+ new_features_last_.clear();
+ new_features_incoming_.clear();
+ matches_last_from_incoming_.clear();
+
// Populate the last Capture with new Features. The result is in new_features_last_.
unsigned int n = detectNewFeatures(_max_new_features, new_features_last_);
for (auto ftr : new_features_last_)
@@ -61,6 +66,9 @@ unsigned int ProcessorTrackerFeature::processKnown()
assert(matches_last_from_incoming_.size() == 0
&& "In ProcessorTrackerFeature::processKnown(): match list from last to incoming must be empty before processKnown()");
+ // clear list of known features
+ known_features_incoming_.clear();
+
if (!last_ptr_ || last_ptr_->getFeatureList().empty())
{
return 0;
@@ -97,13 +105,8 @@ unsigned int ProcessorTrackerFeature::processKnown()
}
}
- // Add to wolf tree and clear
+ // Add to wolf tree
incoming_ptr_->addFeatureList(known_features_incoming_);
- known_features_incoming_.clear();
-
- // Print resulting list of matches
-// for (auto match : matches_last_from_incoming_)
-// WOLF_DEBUG("Known track: ", match.first->trackId(), ", last: ", match.second->feature_ptr_->id(), ", inc: ", match.first->id());
return matches_last_from_incoming_.size();
}
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 3d5b75c8fcc6e9ef70ef396c31a6960a5aa3a016..1aa241a7fbd92314c9cc0767f5c47d9886c18038 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -43,37 +43,55 @@ void ProcessorTrackerLandmark::advanceDerived()
}
matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
new_features_last_ = std::move(new_features_incoming_);
- // for (auto match : matches_landmark_from_last_)
- // std::cout << "\t" << match.first->id() << " to " << match.second->landmark_ptr_->id() << std::endl;
}
void ProcessorTrackerLandmark::resetDerived()
{
- //std::cout << "ProcessorTrackerLandmark::reset" << std::endl;
for (auto match : matches_landmark_from_last_)
{
match.second.reset(); // TODO: Should we just remove the entries? What about match.first?
}
matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
new_features_last_ = std::move(new_features_incoming_);
- // for (auto match : matches_landmark_from_last_)
- // std::cout << "\t" << match.first->id() << " to " << match.second.landmark_ptr_->id() << std::endl;
+}
+
+unsigned int ProcessorTrackerLandmark::processKnown()
+{
+ // clear matches list
+ matches_landmark_from_incoming_.clear();
+
+ // Find landmarks in incoming_ptr_
+ FeatureBasePtrList known_features_list_incoming;
+ unsigned int n = findLandmarks(getProblem()->getMap()->getLandmarkList(),
+ known_features_list_incoming, matches_landmark_from_incoming_);
+ // Append found incoming features
+ incoming_ptr_->addFeatureList(known_features_list_incoming);
+
+ return n;
}
unsigned int ProcessorTrackerLandmark::processNew(const unsigned int& _max_features)
{
/* Rationale: A keyFrame will be created using the last Capture.
+ *
* First, we work on this Capture to detect new Features,
* eventually create Landmarks with them,
* and in such case create the new Factors feature-landmark.
* When done, we need to track these new Features to the incoming Capture.
+ *
* At the end, all new Features are appended to the lists of known Features in
* the last and incoming Captures.
*/
- // We first need to populate the \b incoming Capture with new Features
+ // clear new lists
+ new_features_last_.clear();
+ new_features_incoming_.clear();
+ new_landmarks_.clear();
+
+ // We first need to populate the \b last Capture with new Features
unsigned int n = detectNewFeatures(_max_features, new_features_last_);
+ // create new landmarks with the new features discovered
createNewLandmarks();
// Find the new landmarks in incoming_ptr_ (if it's not nullptr)
@@ -81,11 +99,11 @@ unsigned int ProcessorTrackerLandmark::processNew(const unsigned int& _max_featu
{
findLandmarks(new_landmarks_, new_features_incoming_, matches_landmark_from_incoming_);
- // Append all new Features to the Capture's list of Features
+ // Append all new Features to the incoming Capture's list of Features
incoming_ptr_->addFeatureList(new_features_incoming_);
}
- // Append all new Features to the Capture's list of Features
+ // Append all new Features to the last Capture's list of Features
last_ptr_->addFeatureList(new_features_last_);
// Append new landmarks to the map
@@ -113,17 +131,17 @@ void ProcessorTrackerLandmark::createNewLandmarks()
}
}
-unsigned int ProcessorTrackerLandmark::processKnown()
-{
- // Find landmarks in incoming_ptr_
- FeatureBasePtrList known_features_list_incoming;
- unsigned int n = findLandmarks(getProblem()->getMap()->getLandmarkList(),
- known_features_list_incoming, matches_landmark_from_incoming_);
- // Append found incoming features
- incoming_ptr_->addFeatureList(known_features_list_incoming);
-
- return n;
-}
+// unsigned int ProcessorTrackerLandmark::processKnown()
+// {
+// // Find landmarks in incoming_ptr_
+// FeatureBasePtrList known_features_list_incoming;
+// unsigned int n = findLandmarks(getProblem()->getMap()->getLandmarkList(),
+// known_features_list_incoming, matches_landmark_from_incoming_);
+// // Append found incoming features
+// incoming_ptr_->addFeatureList(known_features_list_incoming);
+
+// return n;
+// }
void ProcessorTrackerLandmark::establishFactors()
{
diff --git a/src/processor/processor_tracker_landmark_apriltag.cpp b/src/processor/processor_tracker_landmark_apriltag.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..27f6be72295c60e01ccce2dbacdaaeae5f215068
--- /dev/null
+++ b/src/processor/processor_tracker_landmark_apriltag.cpp
@@ -0,0 +1,867 @@
+#include "base/processor/processor_tracker_landmark_apriltag.h"
+
+#include "base/capture/capture_image.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/rotations.h"
+#include "base/feature/feature_apriltag.h"
+#include "base/factor/factor_autodiff_apriltag.h"
+#include "base/landmark/landmark_apriltag.h"
+#include "base/state_quaternion.h"
+#include "base/pinhole_tools.h"
+
+// April tags
+#include "common/homography.h"
+#include "common/zarray.h"
+
+#include <tag36h11.h>
+#include <tag36h10.h>
+#include <tag36artoolkit.h>
+#include <tag25h9.h>
+#include <tag25h7.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)
+
+{
+ // Constant transformation from apriltag camera frame (RUB: Z axis looking away from the tag)
+ // to wolf camera frame (RDF: Z axis looking at the tag)
+// c_M_ac_.matrix() = (Eigen::Vector4s() << 1, -1, -1, 1).finished().asDiagonal(); // Not used anymore with solvePnP
+
+ // configure apriltag detector
+ std::string famname(_params_tracker_landmark_apriltag->tag_family_);
+ if (famname == "tag36h11")
+ tag_family_ = *tag36h11_create();
+ else if (famname == "tag36h10")
+ tag_family_ = *tag36h10_create();
+ else if (famname == "tag36artoolkit")
+ tag_family_ = *tag36artoolkit_create();
+ else if (famname == "tag25h9")
+ tag_family_ = *tag25h9_create();
+ else if (famname == "tag25h7")
+ tag_family_ = *tag25h7_create();
+ else {
+ WOLF_ERROR("Unrecognized tag family name. Use e.g. \"tag36h11\".");
+ exit(-1);
+ }
+
+ tag_family_.black_border = _params_tracker_landmark_apriltag->tag_black_border_;
+
+ 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_;
+ detector_.refine_decode = _params_tracker_landmark_apriltag->refine_decode_;
+ detector_.refine_pose = _params_tracker_landmark_apriltag->refine_pose_;
+}
+
+// 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
+// const clock_t begin_time_detection = clock();
+ // std::cout << "BEfore detect" << std::endl;
+ zarray_t *detections = apriltag_detector_detect(&detector_, &im);
+ // std::cout << "After detect" << std::endl;
+// WOLF_DEBUG("tag detection: ", (double)(clock() - begin_time_detection) / CLOCKS_PER_SEC);
+
+ // loop all detections
+ for (int i = 0; i < zarray_size(detections); i++) {
+
+ // get raw Apriltag pose from homography
+ 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; // only redefined if using IPPE
+ //////////////////
+ // 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_);
+ //std::cout << " Tag id: " << tag_id << " ippe_ratio: " << rep_error2 / rep_error1 << " rep error " << rep_error1 << std::endl;
+ //////////////////
+
+ //////////////////
+ // OPENCV
+ // Slower than UMICH (iterative algorithm LM) but yield more precise results
+ // Does not solve the ambiguity on the rotation however
+ //////////////////
+ // M_PnP = opencvPoseEstimation(det, cv_K_, tag_width);
+ //////////////////
+
+ //////////////////
+ // UMICH
+ // Implementation found in the original Apriltag c implementation.
+ // Analytical formula but high reprojection error for large angles
+ //////////////////
+ // M_april = umichPoseEstimation(det, cv_K_, tag_width);
+ //////////////////
+
+// WOLF_DEBUG("ippe1\n", M_ippe1 .matrix());
+// WOLF_DEBUG("ippe2\n", M_ippe2 .matrix());
+// WOLF_DEBUG("M_PnP\n", M_PnP .matrix());
+// WOLF_DEBUG("M_april\n", M_april .matrix());
+
+ c_M_t = M_ippe1;
+
+// if (tag_id == 1){
+// WOLF_INFO("TEST1: change solution of tag 1");
+// c_M_t = M_ippe2;
+// }
+
+ // 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){
+// WOLF_INFO("Ambiguity on estimated rotation is likely");
+ // 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();
+ }
+
+// FOR TEST ONLY
+// if (tag_id == 1){
+// // Put a very high covariance on angles measurements
+// WOLF_INFO("TEST2: Increase meas cov on tag 1");
+// cov.bottomRightCorner(3, 3) = 1000000*Eigen::Matrix3s::Identity();
+// }
+
+// WOLF_TRACE("tag ", tag_id, " cov diagonal: [", cov.diagonal().transpose(), "]");
+ // 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));
+ // std::cout << "Meas Covariance tag " << tag_id << "\n" << info.inverse() << std::endl;
+ // WOLF_TRACE("Meas Covariance tag ", tag_id, "\n", info.inverse());
+// WOLF_TRACE("---------------------\n");
+ }
+
+ apriltag_detections_destroy(detections);
+}
+
+// To compare with apriltag implementation
+// Returned translation is in tag units: needs to be multiplied by tag_width/2
+Eigen::Affine3ds ProcessorTrackerLandmarkApriltag::opencvPoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar> _K, double _tag_width){
+ // 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
+
+ // Solve for pose
+ // The estimated r and t brings points from tag frame to camera frame
+ cv::Mat rvec, tvec;
+// cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<Scalar>::type); // Assuming corrected images
+
+ cv::solvePnP(obj_pts, corners_pix, _K, cv::Mat(), rvec, tvec);
+
+ // Puts the result in a Eigen affine Transform
+ cv::Matx33d rmat;
+ cv::Rodrigues(rvec, rmat);
+ Eigen::Matrix3s R_eigen; cv2eigen(rmat, R_eigen);
+ Eigen::Vector3s t_eigen; cv2eigen(tvec, t_eigen);
+ Eigen::Affine3ds M;
+ M.matrix().block(0,0,3,3) = R_eigen;
+ M.matrix().block(0,3,3,1) = t_eigen;
+
+ return M;
+}
+
+Eigen::Affine3d ProcessorTrackerLandmarkApriltag::umichPoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar> _K, double _tag_width){
+ // To put in the usual camera frame with Z looking in front (RDF)
+ Eigen::Affine3d c_M_ac;
+ c_M_ac.matrix() = (Eigen::Vector4d() << 1, -1, -1, 1).finished().asDiagonal();
+
+ Eigen::Affine3d M_april_raw;
+ matd_t *pose_matrix = homography_to_pose(_det->H, -_K(0,0), _K(1,1), _K(0,2), _K(1,2)); // !! fx Negative sign advised by apriltag library commentary
+ // write it in Eigen form
+ Eigen::Affine3d ac_M_t;
+ for(int r=0; r<4; r++)
+ for(int c=0; c<4; c++)
+ ac_M_t.matrix()(r,c) = matd_get(pose_matrix, r, c);
+
+ Eigen::Affine3d c_M_t = c_M_ac * ac_M_t;
+
+ // Normalize transform
+ c_M_t.matrix().block(0,3,3,1) *= _tag_width/2;
+
+ return c_M_t;
+}
+
+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){
+
+ // camera coefficients
+// cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<Scalar>::type); // Assuming corrected images
+
+ // 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;
+
+ /**
+ * @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
+ */
+// pose_solver.solveGeneric(obj_pts, corners_pix, _K, cv::Mat(),
+// rvec1, tvec1, err1,
+// rvec2, tvec2, err2);
+
+ /** @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
+ */
+// pose_solver.solveSquare(float squareLength, InputArray _imagePoints, InputArray _cameraMatrix, InputArray _distCoeffs,
+// OutputArray _rvec1, OutputArray _tvec1, float& err1, OutputArray _rvec2, OutputArray _tvec2, float& err2)
+
+ 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,
+ CTR_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 unsigned 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);
+ // features and landmarks must be tested with their ID !!
+ // 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)
+ {
+ // Loop over the
+ 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);
+ break; //it should not be possible two detection with the same id before getting there so we can stop here.
+ }
+
+ _new_features_last.push_back(feature_in_image); // If the feature is not in the map and not in the list of newly detected features yet then we add it.
+ } //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);
+ // std::cout << "nb_userot " << nb_userot << std::endl;
+ // std::cout << "nb_not_userot " << nb_not_userot << std::endl;
+ }
+ 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);
+ // std::cout << "vote: " << vote << std::endl;
+
+ 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;
+ Eigen::Matrix3s 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;
+ }
+
+
+ // COMPARISON WITH OPENCV IMPLEMENTATION
+ 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
+ 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 J_opencv; // 2N x (10 + nb_dist_coeffs): img point derivates with regard to rvec, tvec, focal length, principal point coordinates (+ dist_coeffs)
+ cv::Mat rvec, tvec;
+ Eigen::AngleAxis<Scalar> rvec_eigen(R);
+ Eigen::Vector3s toto = rvec_eigen.angle()*rvec_eigen.axis();
+ eigen2cv(toto, rvec);
+ eigen2cv(t, tvec);
+ std::vector<cv::Point2d> p_proj;
+ cv::projectPoints(obj_pts, rvec, tvec, cv_K_, cv::Mat(), p_proj, J_opencv);
+
+
+ // Build Eigen jacobian with same convention as J_u_TR from opencv result
+ Eigen::Matrix<Scalar, 8, 6> J_u_TR_opencv;
+ Eigen::Matrix<Scalar, 8, 3> J_T_opencv;
+ Eigen::Matrix<Scalar, 8, 3> J_R_opencv;
+ // !! Rect(startX, startY, ncols, nrows)
+ cv2eigen(J_opencv(cv::Rect(3,0,3,8)), J_T_opencv);
+ cv2eigen(J_opencv(cv::Rect(0,0,3,8)), J_R_opencv);
+ J_u_TR_opencv.topLeftCorner(8,3) = J_T_opencv;
+ J_u_TR_opencv.topRightCorner(8,3) = J_R_opencv;
+
+
+ // PRINT COMPARISON --> NO diff for J_T but some difference for J_R
+// std::cout << "pvec maison:" << std::endl;
+// for (int i=0; i < 4; i++){
+// std::cout << proj_pix_vec[i] << std::endl;
+// }
+// std::cout << "Jac maison:\n" << J_u_TR << std::endl;
+// std::cout << "pvec opencv:" << std::endl;
+// for (int i=0; i < 4; i++){
+// std::cout << p_proj[i] << std::endl;
+// }
+// std::cout << "Jac cv:\n" << J_u_TR_opencv << std::endl;
+ /////////////////////////////////////
+
+ // 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 covariance computed with covariance propagation formula (inverted)
+// Eigen::Matrix6s transformation_cov = (J_u_TR.transpose() * pixel_cov.inverse() * J_u_TR).inverse();
+
+ // 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
+// Eigen::Matrix6s transformation_info = (J_u_TR_opencv.transpose() * pixel_cov.inverse() * J_u_TR_opencv); // OpencvJac
+
+ 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
+ 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, shared_from_this, false, CTR_ACTIVE);
+ FactorAutodiffDistance3DPtr cstr = std::make_shared<FactorAutodiffDistance3D>(feat_dist, ori_frame, nullptr, false, CTR_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_tracker_landmark_apriltag_yaml.cpp b/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4cd2d67e87e8995b30af459ebef87172c2402fd9
--- /dev/null
+++ b/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
@@ -0,0 +1,94 @@
+/**
+ * \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/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->refine_decode_ = detector_parameters["refine_decode"] .as<bool>();
+ params->refine_pose_ = detector_parameters["refine_pose"] .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/src/yaml/sensor_camera_yaml.cpp b/src/yaml/sensor_camera_yaml.cpp
index 5e5e3df19cd91a8735f6625ecd2d551deb7e7b77..8813dfcbff79576775467d15a7ea8d6d7f30e95d 100644
--- a/src/yaml/sensor_camera_yaml.cpp
+++ b/src/yaml/sensor_camera_yaml.cpp
@@ -32,12 +32,29 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
using namespace Eigen;
unsigned int width = camera_config["image_width"] .as<unsigned int>();
unsigned int height = camera_config["image_height"] .as<unsigned int>();
+//<<<<<<< HEAD
+// VectorXd intrinsic = camera_config["camera_matrix"]["data"] .as<VectorXd>();
+// VectorXd projection = camera_config["projection_matrix"]["data"] .as<VectorXd>();
+//=======
+//>>>>>>> devel
VectorXd distortion = camera_config["distortion_coefficients"]["data"] .as<VectorXd>();
VectorXd intrinsic = camera_config["camera_matrix"]["data"] .as<VectorXd>();
VectorXd projection = camera_config["projection_matrix"]["data"] .as<VectorXd>();
// Eigen:: to wolf::
std::shared_ptr<IntrinsicsCamera> intrinsics_cam = std::make_shared<IntrinsicsCamera>();
+//<<<<<<< HEAD
+// intrinsics_cam->type = sensor_type;
+// intrinsics_cam->name = sensor_name;
+//// intrinsics_cam->pinhole_model[0] = intrinsic[2];
+//// intrinsics_cam->pinhole_model[1] = intrinsic[5];
+//// intrinsics_cam->pinhole_model[2] = intrinsic[0];
+//// intrinsics_cam->pinhole_model[3] = intrinsic[4];
+// intrinsics_cam->pinhole_model[0] = projection[2]; // u0
+// intrinsics_cam->pinhole_model[1] = projection[6]; // v0
+// intrinsics_cam->pinhole_model[2] = projection[0]; // au
+// intrinsics_cam->pinhole_model[3] = projection[5]; // av
+//=======
intrinsics_cam->width = width;
intrinsics_cam->height = height;
@@ -52,6 +69,7 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
intrinsics_cam->pinhole_model_rectified[2] = projection[0];
intrinsics_cam->pinhole_model_rectified[3] = projection[5];
+//>>>>>>> devel
assert (distortion.size() == 5 && "Distortion size must be size 5!");
WOLF_WARN_COND( distortion(2) != 0 || distortion(3) != 0 , "Wolf does not handle tangential distortion. Please consider re-calibrating without tangential distortion!");
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 9c46f4d94400f5e8651eb73b53f4e5261cac8564..23278f0629752f09b5450bf70e25e6fdcf926b02 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -134,6 +134,12 @@ ENDIF(Ceres_FOUND)
wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp)
target_link_libraries(gtest_factor_absolute ${PROJECT_NAME})
+IF (OPENCV_FOUND AND APRILTAG_LIBRARY)
+ # FactorAutodiffApriltag test
+ wolf_add_gtest(gtest_factor_autodiff_apriltag gtest_factor_autodiff_apriltag.cpp)
+ target_link_libraries(gtest_factor_autodiff_apriltag ${PROJECT_NAME})
+ENDIF(OPENCV_FOUND AND APRILTAG_LIBRARY)
+
# FactorAutodiffDistance3D test
wolf_add_gtest(gtest_factor_autodiff_distance_3D gtest_factor_autodiff_distance_3D.cpp)
target_link_libraries(gtest_factor_autodiff_distance_3D ${PROJECT_NAME})
@@ -156,6 +162,12 @@ target_link_libraries(gtest_factor_pose_2D ${PROJECT_NAME})
wolf_add_gtest(gtest_factor_pose_3D gtest_factor_pose_3D.cpp)
target_link_libraries(gtest_factor_pose_3D ${PROJECT_NAME})
+IF (OPENCV_FOUND AND APRILTAG_LIBRARY)
+ # FeatureApriltag test
+ wolf_add_gtest(gtest_feature_apriltag gtest_feature_apriltag.cpp)
+ target_link_libraries(gtest_feature_apriltag ${PROJECT_NAME})
+ENDIF(OPENCV_FOUND AND APRILTAG_LIBRARY)
+
# FeatureIMU test
wolf_add_gtest(gtest_feature_IMU gtest_feature_IMU.cpp)
target_link_libraries(gtest_feature_IMU ${PROJECT_NAME})
@@ -164,6 +176,12 @@ target_link_libraries(gtest_feature_IMU ${PROJECT_NAME})
wolf_add_gtest(gtest_IMU gtest_IMU.cpp)
target_link_libraries(gtest_IMU ${PROJECT_NAME})
+IF (OPENCV_FOUND AND APRILTAG_LIBRARY)
+ # gtest_landmark_apriltag test
+ wolf_add_gtest(gtest_landmark_apriltag gtest_landmark_apriltag.cpp)
+ target_link_libraries(gtest_landmark_apriltag ${PROJECT_NAME})
+ENDIF(OPENCV_FOUND AND APRILTAG_LIBRARY)
+
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
@@ -207,6 +225,14 @@ wolf_add_gtest(gtest_sensor_camera gtest_sensor_camera.cpp)
target_link_libraries(gtest_sensor_camera ${PROJECT_NAME})
+IF (OPENCV_FOUND AND APRILTAG_LIBRARY)
+ # ProcessorTrackerLandmarkApriltag test
+ wolf_add_gtest(gtest_processor_tracker_landmark_apriltag gtest_processor_tracker_landmark_apriltag.cpp)
+ target_link_libraries(gtest_processor_tracker_landmark_apriltag ${PROJECT_NAME})
+ENDIF(OPENCV_FOUND AND APRILTAG_LIBRARY)
+
+
+
# ------- Now Core classes Serialization ----------
add_subdirectory(serialization)
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 71217c8e6df3dc2cd0a9ce5bc2e73c97ec19c72a..d354b9814ff1c7f493332b50cc5ca6bd74fc1785 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -97,7 +97,7 @@ TEST(CaptureBase, addFeatureList)
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(fl.size(), (unsigned int) 0); // features have been moved, not copied // EDIT 02-2019: features have been copied
ASSERT_EQ(C->getFeatureList().front(), f_first);
ASSERT_EQ(C->getFeatureList().back(), f_last);
}
diff --git a/test/gtest_factor_autodiff_apriltag.cpp b/test/gtest_factor_autodiff_apriltag.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f88c461d526664424f0b18fad74451fde28da812
--- /dev/null
+++ b/test/gtest_factor_autodiff_apriltag.cpp
@@ -0,0 +1,413 @@
+#include "utils_gtest.h"
+
+#include "base/wolf.h"
+#include "base/logging.h"
+
+#include "base/ceres_wrapper/ceres_manager.h"
+#include "base/processor/processor_tracker_landmark_apriltag.h"
+#include "base/capture/capture_image.h"
+#include "base/factor/factor_autodiff_apriltag.h"
+#include "base/processor/processor_factory.h"
+
+#include <apriltag.h>
+
+using namespace Eigen;
+using namespace wolf;
+using std::static_pointer_cast;
+
+////////////////////////////////////////////////////////////////
+/*
+ * Wrapper class to be able to have setOrigin() and setLast() in ProcessorTrackerLandmarkApriltag
+ */
+WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkApriltag_Wrapper);
+class ProcessorTrackerLandmarkApriltag_Wrapper : public ProcessorTrackerLandmarkApriltag
+{
+ public:
+ ProcessorTrackerLandmarkApriltag_Wrapper(ProcessorParamsTrackerLandmarkApriltagPtr _params_tracker_landmark_apriltag) :
+ ProcessorTrackerLandmarkApriltag(_params_tracker_landmark_apriltag)
+ {
+ setType("TRACKER LANDMARK APRILTAG WRAPPER");
+ };
+ ~ProcessorTrackerLandmarkApriltag_Wrapper(){}
+ void setOriginPtr(const CaptureBasePtr _origin_ptr) { origin_ptr_ = _origin_ptr; }
+ void setLastPtr (const CaptureBasePtr _last_ptr) { last_ptr_ = _last_ptr; }
+ void setIncomingPtr (const CaptureBasePtr _incoming_ptr) { incoming_ptr_ = _incoming_ptr; }
+
+ // for factory
+ static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr)
+ {
+ 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>();
+
+ ProcessorTrackerLandmarkApriltag_WrapperPtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkApriltag_Wrapper>(prc_apriltag_params_);
+ prc_ptr->setName(_unique_name);
+ return prc_ptr;
+ }
+
+};
+namespace wolf{
+// Register in the Factories
+WOLF_REGISTER_PROCESSOR("TRACKER LANDMARK APRILTAG WRAPPER", ProcessorTrackerLandmarkApriltag_Wrapper);
+}
+////////////////////////////////////////////////////////////////
+
+// Use the following in case you want to initialize tests with predefines variables or methods.
+class FactorAutodiffApriltag_class : public testing::Test{
+ public:
+ Vector3s pos_camera, pos_robot, pos_landmark;
+ Vector3s euler_camera, euler_robot, euler_landmark;
+ Quaternions quat_camera, quat_robot, quat_landmark;
+ Vector4s vquat_camera, vquat_robot, vquat_landmark; // quaternions as vectors
+ Vector7s pose_camera, pose_robot, pose_landmark;
+
+ ProblemPtr problem;
+ CeresManagerPtr ceres_manager;
+
+ SensorCameraPtr camera;
+ ProcessorTrackerLandmarkApriltag_WrapperPtr proc_apriltag;
+
+ SensorBasePtr S;
+ FrameBasePtr F1;
+ CaptureImagePtr C1;
+ FeatureApriltagPtr f1;
+ LandmarkApriltagPtr lmk1;
+ FactorAutodiffApriltagPtr c_tag;
+ apriltag_detection_t det;
+
+ virtual void SetUp()
+ {
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ // configuration
+
+ /* We have three poses to take into account:
+ * - pose of the camera (extrinsincs)
+ * - pose of the landmark
+ * - pose of the robot (Keyframe)
+ *
+ * The measurement provides the pose of the landmark wrt camera's current pose in the world.
+ * Camera's current pose in World is the composition of the robot pose with the camera extrinsics.
+ *
+ * The robot has a camera looking forward
+ * S: pos = (0,0,0), ori = (0, 0, 0)
+ *
+ * There is a point at the origin
+ * P: pos = (0,0,0)
+ *
+ * The camera is canonical
+ * K = Id.
+ *
+ * Therefore, P projects exactly at the origin of the camera,
+ * f: p = (0,0)
+ *
+ * We form a Wolf tree with 1 frames F1, 1 capture C1,
+ * 1 feature f1 (measurement), 1 landmark l and 1 apriltag constraint c1:
+ *
+ * Frame F1, Capture C1, feature f1, landmark l1, constraint c1
+ *
+ * The frame pose F1, and the camera pose S
+ * in the robot frame are variables subject to optimization
+ *
+ * We perform a number of tests based on this configuration.*/
+
+
+ // camera is at origin of robot and looking forward
+ // robot is at (0,0,0)
+ // landmark is right in front of camera. Its orientation wrt camera is identity.
+ pos_camera << 0,0,0;
+ pos_robot << 0,0,0; //robot is at origin
+ pos_landmark << 0,1,0;
+ euler_camera << 0,0,0;
+ //euler_camera << -M_PI_2, 0, -M_PI_2;
+ euler_robot << 0,0,0;
+ euler_landmark << 0,0,0;
+ quat_camera = e2q(euler_camera);
+ quat_robot = e2q(euler_robot);
+ quat_landmark = e2q(euler_landmark);
+ vquat_camera = quat_camera.coeffs();
+ vquat_robot = quat_robot.coeffs();
+ vquat_landmark = quat_landmark.coeffs();
+ pose_camera << pos_camera, vquat_camera;
+ pose_robot << pos_robot, vquat_robot;
+ pose_landmark << pos_landmark, vquat_landmark;
+
+ // Build problem
+ problem = Problem::create("PO 3D");
+ ceres::Solver::Options options;
+ ceres_manager = std::make_shared<CeresManager>(problem, options);
+
+ // Install sensor and processor
+ S = problem->installSensor("CAMERA", "canonical", pose_camera, wolf_root + "/src/examples/camera_params_canonical.yaml");
+ camera = std::static_pointer_cast<SensorCamera>(S);
+
+ ProcessorParamsTrackerLandmarkApriltagPtr params = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
+ // Need to set some parameters ? do it now !
+ params->tag_family_ = "tag36h11";
+ //params->name = params->tag_family_;
+
+ ProcessorBasePtr proc = problem->installProcessor("TRACKER LANDMARK APRILTAG WRAPPER", "apriltags_wrapper", camera, params);
+ proc_apriltag = std::static_pointer_cast<ProcessorTrackerLandmarkApriltag_Wrapper>(proc);
+
+ // F1 is be origin KF
+ F1 = problem->setPrior(pose_robot, Matrix6s::Identity(), 0.0, 0.1);
+
+ //create feature and landmark
+ C1 = std::make_shared<CaptureImage>(1.0, camera, cv::Mat(2,2,CV_8UC1));
+ F1-> addCapture(C1);
+ proc_apriltag->setOriginPtr(C1);
+ proc_apriltag->setLastPtr(C1);
+
+ // the sensor is at origin as well as the robot. The measurement matches with the pose of the tag wrt camera (and also wrt robot and world)
+ Eigen::Matrix6s meas_cov(Eigen::Matrix6s::Identity());
+ meas_cov.topLeftCorner(3,3) *= 1e-2;
+ meas_cov.bottomRightCorner(3,3) *= 1e-3;
+ int tag_id = 1;
+
+ det.id = tag_id;
+ det.p[0][0] = 1.0;
+ det.p[0][1] = -1.0;
+ det.p[1][0] = 1.0;
+ det.p[1][1] = 1.0;
+ det.p[2][0] = -1.0;
+ det.p[2][1] = 1.0;
+ det.p[3][0] = -1.0;
+ det.p[3][1] = -1.0;
+
+ Scalar rep_error1 = 0.01;
+ Scalar rep_error2 = 0.1;
+ bool use_rotation = true;
+
+ f1 = std::make_shared<FeatureApriltag>(pose_landmark, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
+ lmk1 = std::static_pointer_cast<LandmarkApriltag>(proc_apriltag->createLandmark(f1));
+
+ // Add the feature and the landmark in the graph as needed
+ C1->addFeature(f1); // add feature to capture
+ problem->addLandmark(lmk1); // add landmark to map
+ }
+};
+
+
+TEST_F(FactorAutodiffApriltag_class, Constructor)
+{
+ FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
+ S,
+ F1,
+ lmk1,
+ f1,
+ false,
+ CTR_ACTIVE
+ );
+
+ ASSERT_TRUE(constraint->getType() == "AUTODIFF APRILTAG");
+}
+
+TEST_F(FactorAutodiffApriltag_class, Check_tree)
+{
+ FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
+ S,
+ F1,
+ lmk1,
+ f1,
+ false,
+ CTR_ACTIVE
+ );
+
+ FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint));
+ lmk1->addConstrainedBy(constraint);
+ //check is returning true even without the lines below....
+ WOLF_WARN("I think the lines below are needed... to be checked !")
+ F1->addConstrainedBy(constraint);
+ f1->addConstrainedBy(constraint);
+
+ ASSERT_TRUE(problem->check(0));
+}
+
+TEST_F(FactorAutodiffApriltag_class, solve_F1_P_perturbated)
+{
+ FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
+ S,
+ F1,
+ lmk1,
+ f1,
+ false,
+ CTR_ACTIVE
+ );
+
+ FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint));
+ lmk1->addConstrainedBy(constraint);
+ F1->addConstrainedBy(constraint);
+ f1->addConstrainedBy(constraint);
+
+ // unfix F1, perturbate state
+ F1->unfix();
+ Vector3s p0 = Vector3s::Random() * 0.25;
+// WOLF_DEBUG("Perturbation: ")
+// WOLF_DEBUG(p0.transpose());
+ Vector7s x0(pose_robot);
+
+ x0.head<3>() += p0;
+ WOLF_DEBUG("State before perturbation: ");
+ WOLF_DEBUG(F1->getState().transpose());
+ F1->setState(x0);
+// WOLF_DEBUG("State after perturbation: ");
+// WOLF_DEBUG(F1->getState().transpose());
+
+// solve
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
+// WOLF_DEBUG("State after solve: ");
+// WOLF_DEBUG(F1->getState().transpose());
+ ASSERT_MATRIX_APPROX(F1->getState(), pose_robot, 1e-6);
+
+}
+
+TEST_F(FactorAutodiffApriltag_class, solve_F1_O_perturbated)
+{
+ FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
+ S,
+ F1,
+ lmk1,
+ f1,
+ false,
+ CTR_ACTIVE
+ );
+
+ FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint));
+ lmk1->addConstrainedBy(constraint);
+ F1->addConstrainedBy(constraint);
+ f1->addConstrainedBy(constraint);
+
+ // unfix F1, perturbate state
+ F1->unfix();
+ Vector3s e0 = euler_robot + Vector3s::Random() * 0.25;
+ Quaternions e0_quat = e2q(e0);
+ Vector4s e0_vquat = e0_quat.coeffs();
+// WOLF_DEBUG("Perturbation: ")
+// WOLF_DEBUG(e0.transpose());
+ Vector7s x0(pose_robot);
+
+ x0.tail<4>() = e0_vquat;
+ WOLF_DEBUG("State before perturbation: ");
+ WOLF_DEBUG(F1->getState().transpose());
+ F1->setState(x0);
+// WOLF_DEBUG("State after perturbation: ");
+// WOLF_DEBUG(F1->getState().transpose());
+
+// solve
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
+// WOLF_DEBUG("State after solve: ");
+// WOLF_DEBUG(F1->getState().transpose());
+ ASSERT_MATRIX_APPROX(F1->getState(), pose_robot, 1e-6);
+
+}
+
+TEST_F(FactorAutodiffApriltag_class, Check_initialization)
+{
+ FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
+ S,
+ F1,
+ lmk1,
+ f1,
+ false,
+ CTR_ACTIVE
+ );
+
+ FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint));
+ lmk1->addConstrainedBy(constraint);
+ F1->addConstrainedBy(constraint);
+ f1->addConstrainedBy(constraint);
+
+ ASSERT_MATRIX_APPROX(F1->getState(), pose_robot, 1e-6);
+ ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6);
+ ASSERT_MATRIX_APPROX(lmk1->getState(), pose_landmark, 1e-6);
+
+}
+
+TEST_F(FactorAutodiffApriltag_class, solve_L1_P_perturbated)
+{
+ FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
+ S,
+ F1,
+ lmk1,
+ f1,
+ false,
+ CTR_ACTIVE
+ );
+
+ FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint));
+ lmk1->addConstrainedBy(constraint);
+ F1->addConstrainedBy(constraint);
+ f1->addConstrainedBy(constraint);
+
+
+ // unfix lmk1, perturbate state
+ lmk1->unfix();
+ Vector3s p0 = Vector3s::Random() * 0.25;
+// WOLF_DEBUG("Perturbation: ")
+// WOLF_DEBUG(p0.transpose());
+ Vector7s x0(pose_landmark);
+
+ x0.head<3>() += p0;
+ //WOLF_DEBUG("Landmark state before perturbation: ");
+ //WOLF_DEBUG(lmk1->getState().transpose());
+ lmk1->getP()->setState(x0.head<3>());
+ //WOLF_DEBUG("Landmark state after perturbation: ");
+ //WOLF_DEBUG(lmk1->getState().transpose());
+
+// solve
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
+ //WOLF_DEBUG("Landmark state after solve: ");
+ //WOLF_DEBUG(lmk1->getState().transpose());
+ ASSERT_MATRIX_APPROX(F1->getState(), pose_robot, 1e-6);
+ ASSERT_MATRIX_APPROX(lmk1->getState(), pose_landmark, 1e-6);
+}
+
+TEST_F(FactorAutodiffApriltag_class, solve_L1_O_perturbated)
+{
+ FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
+ S,
+ F1,
+ lmk1,
+ f1,
+ false,
+ CTR_ACTIVE
+ );
+
+ FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint));
+ lmk1->addConstrainedBy(constraint);
+ F1->addConstrainedBy(constraint);
+ f1->addConstrainedBy(constraint);
+
+ // unfix F1, perturbate state
+ lmk1->unfix();
+ Vector3s e0 = euler_landmark + Vector3s::Random() * 0.25;
+ Quaternions e0_quat = e2q(e0);
+ Vector4s e0_vquat = e0_quat.coeffs();
+// WOLF_DEBUG("Perturbation: ")
+// WOLF_DEBUG(e0.transpose());
+
+ //WOLF_DEBUG("Landmark state before perturbation: ");
+ //WOLF_DEBUG(lmk1->getState().transpose());
+ lmk1->getO()->setState(e0_vquat);
+ //WOLF_DEBUG("Landmark state after perturbation: ");
+ //WOLF_DEBUG(lmk1->getState().transpose());
+
+// solve
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
+ //WOLF_DEBUG("Landmark state after solve: ");
+ //WOLF_DEBUG(lmk1->getState().transpose());
+ ASSERT_MATRIX_APPROX(F1->getState(), pose_robot, 1e-6);
+ ASSERT_MATRIX_APPROX(lmk1->getState(), pose_landmark, 1e-6);
+
+}
+
+//[Class methods]
+
+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
index 7559bfa7582aa1449232391806e498fb294d03be..5a54bdf73a17215dbda0d9c00598fecfcc8a81a0 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -76,7 +76,7 @@ TEST_F(FactorAutodiffDistance3D_Test, ground_truth)
c2->operator ()(pos1.data(), pos2.data(), &res);
- ASSERT_NEAR(res, 0.0, 1e-8);
+ ASSERT_NEAR(res, 0.0, 1e-5);
}
TEST_F(FactorAutodiffDistance3D_Test, expected_residual)
@@ -90,7 +90,7 @@ TEST_F(FactorAutodiffDistance3D_Test, expected_residual)
Scalar res;
c2->operator ()(pos1.data(), pos2.data(), &res);
- ASSERT_NEAR(res, res_expected, 1e-8);
+ ASSERT_NEAR(res, res_expected, 1e-5);
}
TEST_F(FactorAutodiffDistance3D_Test, solve)
diff --git a/test/gtest_feature_apriltag.cpp b/test/gtest_feature_apriltag.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e2610d5beb025f642e8246e4452910c2b079b83b
--- /dev/null
+++ b/test/gtest_feature_apriltag.cpp
@@ -0,0 +1,119 @@
+/**
+ * \file gtest_feature_apriltag.cpp
+ *
+ * Created on: Dec 22, 2018
+ * \author: jsola
+ */
+
+
+#include "utils_gtest.h"
+
+#include "base/feature/feature_apriltag.h"
+
+using namespace wolf;
+
+class FeatureApriltag_test : public testing::Test
+{
+ public:
+ Eigen::Vector7s pose;
+ Eigen::Matrix6s cov;
+ int tag_id;
+ apriltag_detection_t det;
+ Scalar rep_error1;
+ Scalar rep_error2;
+ bool use_rotation;
+
+ virtual void SetUp()
+ {
+ pose << 1,2,3,4,5,6,7;
+ cov.setIdentity() * 2.0;
+
+ det.id = 1;
+ tag_id = det.id;
+ det.p[0][0] = 1.0;
+ det.p[0][1] = -1.0;
+ det.p[1][0] = 1.0;
+ det.p[1][1] = 1.0;
+ det.p[2][0] = -1.0;
+ det.p[2][1] = 1.0;
+ det.p[3][0] = -1.0;
+ det.p[3][1] = -1.0;
+
+ rep_error1 = 0.01;
+ rep_error2 = 0.1;
+ use_rotation = true;
+ }
+};
+
+TEST_F(FeatureApriltag_test, type)
+{
+ FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
+
+ ASSERT_EQ(f.getType(), "APRILTAG");
+}
+
+TEST_F(FeatureApriltag_test, getTagId)
+{
+ FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
+
+ ASSERT_EQ(f.getTagId(), 1);
+}
+
+TEST_F(FeatureApriltag_test, getCorners)
+{
+ FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
+
+ ASSERT_EQ(f.getTagCorners().size(), 4);
+
+ ASSERT_EQ(f.getTagCorners()[0].x, 1.0);
+ ASSERT_EQ(f.getTagCorners()[0].y, -1.0);
+ ASSERT_EQ(f.getTagCorners()[1].x, 1.0);
+ ASSERT_EQ(f.getTagCorners()[1].y, 1.0);
+ ASSERT_EQ(f.getTagCorners()[2].x, -1.0);
+ ASSERT_EQ(f.getTagCorners()[2].y, 1.0);
+ ASSERT_EQ(f.getTagCorners()[3].x, -1.0);
+ ASSERT_EQ(f.getTagCorners()[3].y, -1.0);
+}
+
+TEST_F(FeatureApriltag_test, getDetection)
+{
+ FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
+
+ ASSERT_EQ(f.getDetection().id, 1);
+}
+
+TEST_F(FeatureApriltag_test, tagid_detid_equality)
+{
+ FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
+
+ ASSERT_EQ(f.getDetection().id, f.getTagId());
+}
+
+TEST_F(FeatureApriltag_test, tagCorners_detection_equality)
+{
+ FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
+
+ for (int i = 0; i<f.getTagCorners().size(); i++)
+ {
+ ASSERT_EQ(f.getTagCorners()[i].x, f.getDetection().p[i][0]);
+ ASSERT_EQ(f.getTagCorners()[i].y, f.getDetection().p[i][1]);
+ }
+}
+
+TEST_F(FeatureApriltag_test, getRepErrors)
+{
+ FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
+
+ Scalar err1 = f.getRepError1();
+ Scalar err2 = f.getRepError2();
+
+ ASSERT_EQ(err1, rep_error1);
+ ASSERT_EQ(err2, rep_error2);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_landmark_apriltag.cpp b/test/gtest_landmark_apriltag.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9c9e889907b3957254a4a9d53c37f0a83bee2c0b
--- /dev/null
+++ b/test/gtest_landmark_apriltag.cpp
@@ -0,0 +1,87 @@
+/**
+ * \file gtest_landmark_apriltag.cpp
+ *
+ * Created on: Dec 6, 2018
+ * \author: jsola
+ */
+
+
+#include "utils_gtest.h"
+
+
+#include "base/wolf.h"
+#include "base/logging.h"
+
+#include "base/landmark/landmark_apriltag.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::static_pointer_cast;
+
+class LandmarkApriltag_class : public testing::Test{
+ public:
+ virtual void SetUp()
+ {
+ wolf_root = _WOLF_ROOT_DIR;
+ problem = Problem::create("PO 3D");
+ }
+ public:
+ std::string wolf_root;
+ ProblemPtr problem;
+};
+
+TEST(LandmarkApriltag, getTagId)
+{
+ Vector7s p;
+ LandmarkApriltagPtr l = std::make_shared<LandmarkApriltag>(p, 5, 0.2); // pose, tag_id, tag_width
+ ASSERT_EQ(l->getTagId(), 5);
+}
+
+TEST(LandmarkApriltag, getTagWidth)
+{
+ Vector7s p;
+ LandmarkApriltagPtr l = std::make_shared<LandmarkApriltag>(p, 5, 0.2); // pose, tag_id, tag_width
+ ASSERT_EQ(l->getTagWidth(), 0.2);
+}
+
+TEST(LandmarkApriltag, getPose)
+{
+ Vector7s p;
+ p << 0,0,0, 0,0,0,1;
+ LandmarkApriltagPtr l = std::make_shared<LandmarkApriltag>(p, 5, 0.2); // pose, tag_id, tag_width
+ ASSERT_MATRIX_APPROX(l->getState(), p, 1e-6);
+}
+
+TEST_F(LandmarkApriltag_class, create)
+{
+ // load original hand-written map
+ problem->loadMap(wolf_root + "/src/examples/map_apriltag_1.yaml"); // this will call create()
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 4);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().front()->getType(), "APRILTAG");
+}
+
+TEST_F(LandmarkApriltag_class, saveToYaml)
+{
+ // load original hand-written map
+ problem->loadMap(wolf_root + "/src/examples/map_apriltag_1.yaml");
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 4);
+
+ // write map on new file
+ problem->saveMap(wolf_root + "/src/examples/map_apriltag_save.yaml"); // this will call saveToYaml()
+
+ // delete existing map
+ problem->getMap()->getLandmarkList().clear();
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 0);
+
+ // reload the saved map
+ problem->loadMap(wolf_root + "/src/examples/map_apriltag_save.yaml");
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 4);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().front()->getType(), "APRILTAG");
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_processor_tracker_landmark_apriltag.cpp b/test/gtest_processor_tracker_landmark_apriltag.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6cb146f61846d58a80e789e0728f6f968ec7e620
--- /dev/null
+++ b/test/gtest_processor_tracker_landmark_apriltag.cpp
@@ -0,0 +1,413 @@
+#include "utils_gtest.h"
+
+#include "base/wolf.h"
+#include "base/logging.h"
+
+#include "base/processor/processor_tracker_landmark_apriltag.h"
+#include "base/feature/feature_apriltag.h"
+#include "base/landmark/landmark_apriltag.h"
+#include "base/capture/capture_pose.h"
+#include "base/processor/processor_factory.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::static_pointer_cast;
+
+
+////////////////////////////////////////////////////////////////
+/*
+ * Wrapper class to be able to have setOriginPtr() and setLastPtr() in ProcessorTrackerLandmarkApriltag
+ */
+WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkApriltag_Wrapper);
+class ProcessorTrackerLandmarkApriltag_Wrapper : public ProcessorTrackerLandmarkApriltag
+{
+ public:
+ ProcessorTrackerLandmarkApriltag_Wrapper(ProcessorParamsTrackerLandmarkApriltagPtr _params_tracker_landmark_apriltag) :
+ ProcessorTrackerLandmarkApriltag(_params_tracker_landmark_apriltag)
+ {
+ setType("TRACKER LANDMARK APRILTAG WRAPPER");
+ };
+ ~ProcessorTrackerLandmarkApriltag_Wrapper(){}
+ void setOriginPtr(const CaptureBasePtr _origin_ptr) { origin_ptr_ = _origin_ptr; }
+ void setLastPtr (const CaptureBasePtr _last_ptr) { last_ptr_ = _last_ptr; }
+ void setIncomingPtr (const CaptureBasePtr _incoming_ptr) { incoming_ptr_ = _incoming_ptr; }
+ unsigned int getMinFeaturesForKeyframe (){return min_features_for_keyframe_;}
+ Scalar getMinTimeVote (){return min_time_vote_;}
+ void setIncomingDetections(const FeatureBasePtrList _incoming_detections) { detections_incoming_ = _incoming_detections; }
+ void setLastDetections(const FeatureBasePtrList _last_detections) { detections_last_ = _last_detections; }
+
+ // for factory
+ static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr)
+ {
+ 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>();
+
+ ProcessorTrackerLandmarkApriltag_WrapperPtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkApriltag_Wrapper>(prc_apriltag_params_);
+ prc_ptr->setName(_unique_name);
+ return prc_ptr;
+ }
+
+};
+namespace wolf{
+// Register in the Factories
+WOLF_REGISTER_PROCESSOR("TRACKER LANDMARK APRILTAG WRAPPER", ProcessorTrackerLandmarkApriltag_Wrapper);
+}
+////////////////////////////////////////////////////////////////
+
+
+
+////////////////////////////////////////////////////////////////
+/*
+ * Test class to prepare a little wolf problem to test the class ProcessorTrackerLandmarkApriltag
+ *
+ * The class ProcessorTrackerLandmarkApriltag is sometimes tested via the wrapper ProcessorTrackerLandmarkApriltag_Wrapper
+ */
+// Use the following in case you want to initialize tests with predefined variables or methods.
+class ProcessorTrackerLandmarkApriltag_class : public testing::Test{
+ public:
+ virtual void SetUp()
+ {
+ wolf_root = _WOLF_ROOT_DIR;
+
+ // configure wolf problem
+ problem = Problem::create("PO 3D");
+ sen = problem->installSensor("CAMERA", "camera", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/camera_params_canonical.yaml");
+ prc = problem->installProcessor("TRACKER LANDMARK APRILTAG WRAPPER", "apriltags_wrapper", "camera", wolf_root + "/src/examples/processor_tracker_landmark_apriltag.yaml");
+ prc_apr = std::static_pointer_cast<ProcessorTrackerLandmarkApriltag_Wrapper>(prc);
+
+ // set prior
+ F1 = problem->setPrior((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity(), 0.0, 0.1);
+
+ // minimal config for the processor to be operative
+ C1 = std::make_shared<CapturePose>(1.0, sen, Vector7s(), Matrix6s());
+ F1->addCapture(C1);
+ prc_apr->setOriginPtr(C1);
+ prc_apr->setLastPtr(C1);
+
+ det.p[0][0] = 1.0;
+ det.p[0][1] = -1.0;
+ det.p[1][0] = 1.0;
+ det.p[1][1] = 1.0;
+ det.p[2][0] = -1.0;
+ det.p[2][1] = 1.0;
+ det.p[3][0] = -1.0;
+ det.p[3][1] = -1.0;
+
+ rep_error1 = 0.01;
+ rep_error2 = 0.1;
+ use_rotation = true;
+ }
+
+ public:
+ ProcessorTrackerLandmarkApriltag_WrapperPtr prc_apr;
+ std::string wolf_root;
+ ProblemPtr problem;
+ SensorBasePtr sen;
+ ProcessorBasePtr prc;
+ FrameBasePtr F1;
+ CaptureBasePtr C1;
+ apriltag_detection_t det;
+ Scalar rep_error1;
+ Scalar rep_error2;
+ bool use_rotation;
+};
+////////////////////////////////////////////////////////////////
+
+
+
+/////////////////// TESTS START HERE ///////////////////////////
+// //
+TEST(ProcessorTrackerLandmarkApriltag, Constructor)
+{
+ std::string s1;
+ std::string s2;
+
+ ProcessorParamsTrackerLandmarkApriltagPtr params = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
+ params->tag_family_ = "tag36h11";
+ ProcessorTrackerLandmarkApriltagPtr p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
+ ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
+
+ params->tag_family_ = "tag36h10";
+ p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
+ ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
+
+ params->tag_family_ = "tag36artoolkit";
+ p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
+ ASSERT_TRUE(p->getTagFamily() == "artoolkit"); // This tagfamily is stored differently by apriltag library
+
+ params->tag_family_ = "tag25h9";
+ p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
+ ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
+
+ params->tag_family_ = "tag25h7";
+ p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
+ ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
+
+ params->tag_family_ = "wrong_family";
+ WOLF_INFO("The following runtime error \"Unrecognized tag family name. Use e.g. \"tag36h11\".\" is expected and does not imply a failed test:");
+ ASSERT_DEATH( { std::make_shared<ProcessorTrackerLandmarkApriltag>(params); }, "" );
+}
+
+TEST_F(ProcessorTrackerLandmarkApriltag_class, voteForKeyFrame)
+{
+ Scalar min_time_vote = prc_apr->getMinTimeVote();
+ unsigned int min_features_for_keyframe = prc_apr->getMinFeaturesForKeyframe();
+ Scalar start_ts = 2.0;
+
+ CaptureBasePtr Ca = std::make_shared<CapturePose>(start_ts, sen, Vector7s(), Matrix6s());
+ CaptureBasePtr Cb = std::make_shared<CapturePose>(start_ts + min_time_vote/2, sen, Vector7s(), Matrix6s());
+ CaptureBasePtr Cc = std::make_shared<CapturePose>(start_ts + 2*min_time_vote, sen, Vector7s(), Matrix6s());
+ CaptureBasePtr Cd = std::make_shared<CapturePose>(start_ts + 2.5*min_time_vote, sen, Vector7s(), Matrix6s());
+ CaptureBasePtr Ce = std::make_shared<CapturePose>(start_ts + 3*min_time_vote, sen, Vector7s(), Matrix6s());
+
+ for (int i=0; i < min_features_for_keyframe; i++){
+ det.id = i;
+ FeatureApriltagPtr f = std::make_shared<FeatureApriltag>((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity(), i, det, rep_error1, rep_error2, use_rotation);
+ Ca->addFeature(f);
+ Ca->addFeature(f);
+ Cc->addFeature(f);
+ if (i != min_features_for_keyframe-1){
+ Cd->addFeature(f);
+ Ce->addFeature(f);
+ }
+ }
+ F1->addCapture(Ca);
+ F1->addCapture(Cb);
+ F1->addCapture(Cc);
+ F1->addCapture(Cd);
+ F1->addCapture(Ce);
+
+ // CASE 1: Not enough time between origin and incoming
+ prc_apr->setOriginPtr(Ca);
+ prc_apr->setIncomingPtr(Cb);
+ ASSERT_FALSE(prc_apr->voteForKeyFrame());
+
+ // CASE 2: Enough time but still too many features in image to trigger a KF
+ prc_apr->setOriginPtr(Ca);
+ prc_apr->setLastPtr(Cb);
+ prc_apr->setIncomingPtr(Cc);
+ ASSERT_FALSE(prc_apr->voteForKeyFrame());
+
+ // CASE 3: Enough time, enough features in last, not enough features in incoming
+ prc_apr->setOriginPtr(Ca);
+ prc_apr->setLastPtr(Cc);
+ prc_apr->setIncomingPtr(Cd);
+ ASSERT_TRUE(prc_apr->voteForKeyFrame());
+
+ // CASE 4: Enough time, not enough features in last, not enough features in incoming
+ prc_apr->setOriginPtr(Ca);
+ prc_apr->setLastPtr(Cd);
+ prc_apr->setIncomingPtr(Ce);
+ ASSERT_FALSE(prc_apr->voteForKeyFrame());
+
+}
+
+TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeatures)
+{
+ // No detected features
+ FeatureBasePtrList features_out;
+ prc_apr->detectNewFeatures(1, features_out);
+ ASSERT_EQ(features_out.size(), 0);
+
+ // Some detected features TODO
+ FeatureBasePtrList features_in;
+ Eigen::Vector3s pos;
+ Eigen::Vector3s ori; //Euler angles in rad
+ Eigen::Quaternions quat;
+ Eigen::Vector7s pose;
+ Eigen::Matrix6s meas_cov( (prc_apr->getVarVec()).asDiagonal() );
+ int tag_id;
+
+ // feature 0
+ pos << 0,2,0;
+ ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
+ quat = e2q(ori);
+ pose << pos, quat.coeffs();
+ tag_id = 0;
+ det.id = tag_id;
+ FeatureBasePtr f0 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
+
+ // feature 1
+ pos << 1,2,0;
+ ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
+ quat = e2q(ori);
+ pose << pos, quat.coeffs();
+ tag_id = 1;
+ det.id = tag_id;
+ FeatureBasePtr f1 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
+
+ // feature 2
+ pos << 0,2,1;
+ ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
+ quat = e2q(ori);
+ pose << pos, quat.coeffs();
+ tag_id = 2;
+ det.id = tag_id;
+ FeatureBasePtr f2 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
+
+ features_in.push_back(f0);
+ features_in.push_back(f0);
+
+ // We just added twice the same feature in the list.
+ prc_apr->setLastDetections(features_in);
+ // at this point we have 0 detections in last, 2 detections in incoming with same id. We should keep only one in the final list of new detected features
+ prc_apr->detectNewFeatures(2, features_out);
+ ASSERT_EQ(features_out.size(), 1);
+
+ //we add new different features in the list
+ features_in.clear();
+ features_in.push_back(f0);
+ features_in.push_back(f1);
+ //these features are set as the incoming detections due to processing an image
+ prc_apr->setLastDetections(features_in);
+ // at this point we have 0 detections in last, 2 detections in incoming with different ids, thus we should have 2 new detected features (if max_features set to >= 2)
+ prc_apr->detectNewFeatures(2, features_out);
+ ASSERT_EQ(features_out.size(), 2);
+
+ // Put some of the features in the graph with createLandmark() and detect some of them as well as others with detectNewFeatures() running again.
+ WOLF_WARN("call to function createLandmark() in unit test for detectNewFeatures().")
+ C1->addFeature(f0);
+ LandmarkBasePtr lmk0 = prc_apr->createLandmark(f0);
+ C1->addFeature(f1);
+ LandmarkBasePtr lmk1 = prc_apr->createLandmark(f1);
+
+ // Add landmarks to the map
+ LandmarkBasePtrList landmark_list;
+ landmark_list.push_back(lmk0);
+ landmark_list.push_back(lmk1);
+ problem->addLandmarkList(landmark_list);
+ //problem->print(4,1,1,1);
+
+ // Add 1 one more new feature to the detection list
+ features_in.push_back(f2);
+ prc_apr->setLastDetections(features_in);
+ // At this point we have 2 landmarks (for f0 and f1), and 3 detections (f0, f1 and f2).
+ // Hence we should 1 new detected feature : f2
+ features_out.clear();
+ prc_apr->detectNewFeatures(2, features_out);
+ ASSERT_EQ(features_out.size(), 1);
+ ASSERT_EQ(std::static_pointer_cast<FeatureApriltag>(features_out.front())->getTagId(), 2);
+}
+
+TEST_F(ProcessorTrackerLandmarkApriltag_class, createLandmark)
+{
+ Vector7s pose_landmark((Vector7s()<<0,0,0,0,0,0,1).finished());
+ det.id = 1;
+ FeatureApriltagPtr f1 = std::make_shared<FeatureApriltag>(pose_landmark, Matrix6s::Identity(), 1, det, rep_error1, rep_error2, use_rotation);
+
+ C1->addFeature(f1);
+ LandmarkBasePtr lmk = prc_apr->createLandmark(f1);
+ LandmarkApriltagPtr lmk_april = std::static_pointer_cast<LandmarkApriltag>(lmk);
+ ASSERT_TRUE(lmk_april->getType() == "APRILTAG");
+ ASSERT_MATRIX_APPROX(lmk_april->getState(), pose_landmark, 1e-6);
+}
+
+TEST_F(ProcessorTrackerLandmarkApriltag_class, createFactor)
+{
+ det.id = 1;
+ FeatureApriltagPtr f1 = std::make_shared<FeatureApriltag>((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity(), 1, det, rep_error1, rep_error2, use_rotation);
+
+ C1->addFeature(f1);
+ LandmarkBasePtr lmk = prc_apr->createLandmark(f1);
+ LandmarkApriltagPtr lmk_april = std::static_pointer_cast<LandmarkApriltag>(lmk);
+
+ FactorBasePtr ctr = prc_apr->createFactor(f1, lmk);
+
+ ASSERT_TRUE(ctr->getType() == "AUTODIFF APRILTAG");
+}
+
+TEST_F(ProcessorTrackerLandmarkApriltag_class, computeInformation)
+{
+ Scalar cx = 320;
+ Scalar cy = 240;
+ Scalar fx = 320;
+ Scalar fy = 320;
+ Eigen::Matrix3s K;
+ K << fx, 0, cx,
+ 0, fy, cy,
+ 0, 0, 1;
+ Eigen::Vector3s t; t << 0.0, 0.0, 0.4;
+ Eigen::Vector3s v; v << 0.2, 0.0, 0.0;
+ Scalar tag_width = 0.05;
+ Scalar s = tag_width/2;
+ Eigen::Vector3s p1; p1 << s, s, 0; // bottom right
+ Eigen::Vector3s p2; p2 << -s, s, 0; // bottom left
+
+ // Got from Matlab code:
+ // Top left corner
+ Eigen::Vector3s h1_matlab; h1_matlab << 137.5894, 105.0325, 0.4050;
+ Eigen::Matrix3s J_h_T1_matlab;
+ J_h_T1_matlab << 320, 0, 320,
+ 0, 320, 240,
+ 0, 0, 1;
+ Eigen::Matrix3s J_h_R1_matlab;
+ J_h_R1_matlab << 7.8405, -7.8405, -6.4106,
+ 4.2910, -4.2910, 9.0325,
+ 0.0245, -0.0245, 0.0050;
+ // Top right corner
+ Eigen::Vector3s h2_matlab; h2_matlab << 121.5894, 105.0325, 0.4050;
+ Eigen::Matrix3s J_h_T2_matlab;
+ J_h_T2_matlab << 320, 0, 320,
+ 0, 320, 240,
+ 0, 0, 1;
+ Eigen::Matrix3s J_h_R2_matlab;
+ J_h_R2_matlab << 7.8405, 7.8405, -9.5894,
+ 4.2910, 4.2910, -9.0325,
+ 0.0245, 0.0245, -0.0050;
+
+ Eigen::Vector3s h1;
+ Eigen::Matrix3s J_h_T1;
+ Eigen::Matrix3s J_h_R1;
+ Eigen::Vector3s h2;
+ Eigen::Matrix3s J_h_T2;
+ Eigen::Matrix3s J_h_R2;
+
+ prc_apr->pinholeHomogeneous(K, t, v2R(v), p1, h1, J_h_T1, J_h_R1);
+ prc_apr->pinholeHomogeneous(K, t, v2R(v), p2, h2, J_h_T2, J_h_R2);
+
+ ASSERT_MATRIX_APPROX(h1, h1_matlab, 1e-3);
+ ASSERT_MATRIX_APPROX(J_h_T1, J_h_T1_matlab, 1e-3);
+ ASSERT_MATRIX_APPROX(J_h_R1, J_h_R1_matlab, 1e-3);
+ ASSERT_MATRIX_APPROX(h2, h2_matlab, 1e-3);
+ ASSERT_MATRIX_APPROX(J_h_T2, J_h_T2_matlab, 1e-3);
+ ASSERT_MATRIX_APPROX(J_h_R2, J_h_R2_matlab, 1e-3);
+
+ Scalar sig_q = 2;
+ Eigen::Matrix6s transformation_info = prc_apr->computeInformation(t, v2R(v), K, tag_width, sig_q);
+
+ // From Matlab
+// Eigen::Matrix6s transformation_cov_matlab;
+// transformation_cov_matlab <<
+// 0.0000, 0.0000, -0.0000, 0.0000, -0.0002, 0.0000,
+// 0.0000, 0.0000, -0.0000, 0.0002, 0.0000, 0.0000,
+// -0.0000, -0.0000, 0.0004, -0.0040, -0.0000, 0.0000,
+// 0.0000, 0.0002, -0.0040, 0.1027, 0.0000, 0.0000,
+// -0.0002, 0.0000, -0.0000, 0.0000, 0.1074, -0.0106,
+// 0.0000, 0.0000, 0.0000, 0.0000, -0.0106, 0.0023;
+
+ Eigen::Matrix6s transformation_info_matlab;
+ transformation_info_matlab <<
+ 6.402960973553990, 0, 0.000000000000000, -0.000000000000000, 0.009809735541319, 0.001986080274985,
+ 0, 6.402960973553990, 0.014610695222409, -0.008824560412472, 0.000000000000000, 0.000000000000000,
+ 0.000000000000000, 0.014610695222409, 0.049088870761338, 0.001889201771982, 0.000000000000000, 0.000000000000000,
+ -0.000000000000000, -0.008824560412472, 0.001889201771982, 0.000183864607538, -0.000000000000000, 0.000000000000000,
+ 0.009809735541319, 0.000000000000000, 0.000000000000000, -0.000000000000000, 0.000183864607538, 0.000773944077821,
+ 0.001986080274985, 0.000000000000000, 0.000000000000000, -0.000000000000000, 0.000773944077821, 0.007846814985446;
+
+ transformation_info_matlab = transformation_info_matlab*100000.0;
+
+
+ ASSERT_MATRIX_APPROX(transformation_info, transformation_info_matlab, 1e-3);
+
+
+}
+
+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
index 5d656eda2fe23cde71e16b1ca15c389e11d65940..eb3d894da1622d80b1cfed0e6a813c631466c91f 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -669,6 +669,8 @@ TEST(log_q, small)
}
}
+//<<<<<<< HEAD
+//=======
TEST(Conversions, q2R_R2q)
{
Vector3s v; v.setRandom();
@@ -705,18 +707,34 @@ TEST(Conversions, e2q_q2e)
}
+//>>>>>>> master
TEST(Conversions, e2q_q2R_R2e)
{
Vector3s e, eo;
Quaternions q;
Matrix3s R;
+//<<<<<<< HEAD
+// e.setRandom();
+//=======
+//>>>>>>> master
e << 0.1, .2, .3;
q = e2q(e);
R = q2R(q);
eo = R2e(R);
+//<<<<<<< HEAD
+// WOLF_TRACE("euler ", e.transpose());
+// WOLF_TRACE("quat ", q.coeffs().transpose());
+// WOLF_TRACE("R \n", R);
+//
+// WOLF_TRACE("euler o ", eo.transpose());
+//
+//
+// ASSERT_MATRIX_APPROX(eo, e, 1e-10);
+//
+//=======
ASSERT_MATRIX_APPROX(eo, e, 1e-10);
}
@@ -730,6 +748,7 @@ TEST(Conversions, e2R_R2e)
R = e2R(e);
eo = R2e(R);
ASSERT_MATRIX_APPROX(eo, e, 1e-10);
+//>>>>>>> master
}
TEST(Conversions, e2R_R2q_q2e)
@@ -738,13 +757,31 @@ TEST(Conversions, e2R_R2q_q2e)
Quaternions q;
Matrix3s R;
+//<<<<<<< HEAD
+// e.setRandom();
+// e << 0.1, 0.2, 0.3;
+// R = e2R(e(0), e(1), e(2));
+//=======
e << 0.1, 0.2, 0.3;
R = e2R(e);
+//>>>>>>> master
q = R2q(R);
eo = q2e(q.coeffs());
+//<<<<<<< HEAD
+// WOLF_TRACE("euler ", e.transpose());
+// WOLF_TRACE("R \n", R);
+// WOLF_TRACE("quat ", q.coeffs().transpose());
+//
+// WOLF_TRACE("euler o ", eo.transpose());
+//
+//
+// ASSERT_MATRIX_APPROX(eo, e, 1e-10);
+//
+//=======
ASSERT_MATRIX_APPROX(eo, e, 1e-10);
+//>>>>>>> master
}
int main(int argc, char **argv)