diff --git a/CMakeLists.txt b/CMakeLists.txt
index e0927077eb2e92d0f7bb6ecffe38efe64ea7cb7e..fee4c0eb11094b6c4611f12c251211fb472d172b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -128,39 +128,12 @@ IF(faramotics_FOUND)
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.")
-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.")
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision Utils
-FIND_PACKAGE(vision_utils QUIET)
-IF (vision_utils_FOUND)
- MESSAGE("vision_utils Library FOUND: vision related sources will be built.")
- SET(PRINT_INFO_VU false)
- FIND_PACKAGE(OpenCV QUIET)
-ENDIF(vision_utils_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)
@@ -232,19 +205,6 @@ IF(faramotics_FOUND)
INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
ENDIF(faramotics_FOUND)
-IF(laser_scan_utils_FOUND)
- INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
-ENDIF(laser_scan_utils_FOUND)
-
-IF(raw_gps_utils_FOUND)
- INCLUDE_DIRECTORIES(${raw_gps_utils_INCLUDE_DIRS})
-ENDIF(raw_gps_utils_FOUND)
-
-IF(vision_utils_FOUND)
- INCLUDE_DIRECTORIES(${vision_utils_INCLUDE_DIR})
- INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
-ENDIF(vision_utils_FOUND)
-
# cereal
IF(cereal_FOUND)
INCLUDE_DIRECTORIES(${cereal_INCLUDE_DIRS})
@@ -262,9 +222,6 @@ 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
@@ -276,11 +233,9 @@ SET(HDRS_BASE
include/base/factory.h
include/base/frame_base.h
include/base/hardware_base.h
- include/base/IMU_tools.h
include/base/local_parametrization_angle.h
include/base/local_parametrization_base.h
include/base/local_parametrization_homogeneous.h
- include/base/local_parametrization_polyline_extreme.h
include/base/local_parametrization_quaternion.h
include/base/logging.h
include/base/make_unique.h
@@ -300,8 +255,6 @@ SET(HDRS_BASE
include/base/track_matrix.h
include/base/trajectory_base.h
include/base/wolf.h
- include/base/IMU_tools.h
- include/base/local_parametrization_polyline_extreme.h
)
SET(HDRS_CAPTURE
include/base/capture/capture_base.h
@@ -310,11 +263,9 @@ SET(HDRS_CAPTURE
include/base/capture/capture_void.h
include/base/capture/capture_motion.h
include/base/capture/capture_GPS_fix.h
- include/base/capture/capture_IMU.h
include/base/capture/capture_odom_2D.h
include/base/capture/capture_odom_3D.h
include/base/capture/capture_GPS_fix.h
- include/base/capture/capture_IMU.h
include/base/capture/capture_odom_2D.h
include/base/capture/capture_odom_3D.h
include/base/capture/capture_velocity.h
@@ -324,9 +275,7 @@ SET(HDRS_FACTOR
include/base/factor/factor_block_absolute.h
include/base/factor/factor_container.h
include/base/factor/factor_corner_2D.h
- include/base/factor/factor_AHP.h
include/base/factor/factor_epipolar.h
- include/base/factor/factor_IMU.h
include/base/factor/factor_fix_bias.h
include/base/factor/factor_GPS_2D.h
include/base/factor/factor_GPS_pseudorange_3D.h
@@ -334,21 +283,16 @@ SET(HDRS_FACTOR
include/base/factor/factor_odom_2D.h
include/base/factor/factor_odom_2D_analytic.h
include/base/factor/factor_odom_3D.h
- include/base/factor/factor_point_2D.h
- include/base/factor/factor_point_to_line_2D.h
include/base/factor/factor_pose_2D.h
include/base/factor/factor_pose_3D.h
include/base/factor/factor_quaternion_absolute.h
include/base/factor/factor_relative_2D_analytic.h
- include/base/factor/factor_autodiff_trifocal.h
include/base/factor/factor_autodiff_distance_3D.h
- include/base/factor/factor_AHP.h
include/base/factor/factor_block_absolute.h
include/base/factor/factor_container.h
include/base/factor/factor_corner_2D.h
include/base/factor/factor_diff_drive.h
include/base/factor/factor_epipolar.h
- include/base/factor/factor_IMU.h
include/base/factor/factor_fix_bias.h
include/base/factor/factor_GPS_2D.h
include/base/factor/factor_GPS_pseudorange_3D.h
@@ -356,8 +300,6 @@ SET(HDRS_FACTOR
include/base/factor/factor_odom_2D.h
include/base/factor/factor_odom_2D_analytic.h
include/base/factor/factor_odom_3D.h
- include/base/factor/factor_point_2D.h
- include/base/factor/factor_point_to_line_2D.h
include/base/factor/factor_pose_2D.h
include/base/factor/factor_pose_3D.h
include/base/factor/factor_quaternion_absolute.h
@@ -370,16 +312,12 @@ SET(HDRS_FEATURE
include/base/feature/feature_corner_2D.h
include/base/feature/feature_GPS_fix.h
include/base/feature/feature_GPS_pseudorange.h
- include/base/feature/feature_IMU.h
include/base/feature/feature_odom_2D.h
- include/base/feature/feature_polyline_2D.h
include/base/feature/feature_corner_2D.h
include/base/feature/feature_diff_drive.h
include/base/feature/feature_GPS_fix.h
include/base/feature/feature_GPS_pseudorange.h
- include/base/feature/feature_IMU.h
include/base/feature/feature_odom_2D.h
- include/base/feature/feature_polyline_2D.h
include/base/feature/feature_base.h
include/base/feature/feature_match.h
include/base/feature/feature_pose.h
@@ -390,24 +328,20 @@ SET(HDRS_LANDMARK
include/base/landmark/landmark_corner_2D.h
include/base/landmark/landmark_container.h
include/base/landmark/landmark_line_2D.h
- include/base/landmark/landmark_polyline_2D.h
include/base/landmark/landmark_corner_2D.h
include/base/landmark/landmark_container.h
include/base/landmark/landmark_line_2D.h
- include/base/landmark/landmark_polyline_2D.h
)
SET(HDRS_PROCESSOR
include/base/processor/processor_capture_holder.h
include/base/processor/processor_diff_drive.h
include/base/processor/processor_frame_nearest_neighbor_filter.h
- include/base/processor/processor_IMU.h
include/base/processor/processor_odom_2D.h
include/base/processor/processor_odom_3D.h
include/base/processor/processor_tracker_feature_dummy.h
include/base/processor/processor_tracker_landmark.h
include/base/processor/processor_tracker_landmark_dummy.h
include/base/processor/processor_frame_nearest_neighbor_filter.h
- include/base/processor/processor_IMU.h
include/base/processor/processor_odom_2D.h
include/base/processor/processor_odom_3D.h
include/base/processor/processor_tracker_feature_dummy.h
@@ -423,17 +357,14 @@ SET(HDRS_PROCESSOR
)
SET(HDRS_SENSOR
include/base/sensor/sensor_base.h
- include/base/sensor/sensor_camera.h
include/base/sensor/sensor_diff_drive.h
include/base/sensor/sensor_GPS.h
include/base/sensor/sensor_GPS_fix.h
- include/base/sensor/sensor_IMU.h
include/base/sensor/sensor_odom_2D.h
include/base/sensor/sensor_odom_3D.h
include/base/sensor/sensor_camera.h
include/base/sensor/sensor_GPS.h
include/base/sensor/sensor_GPS_fix.h
- include/base/sensor/sensor_IMU.h
include/base/sensor/sensor_odom_2D.h
include/base/sensor/sensor_odom_3D.h
include/base/sensor/sensor_factory.h
@@ -532,12 +463,10 @@ SET(SRCS_BASE
)
SET(SRCS
- src/local_parametrization_polyline_extreme.cpp
test/processor_IMU_UnitTester.cpp
)
SET(SRCS_CAPTURE
src/capture/capture_GPS_fix.cpp
- src/capture/capture_IMU.cpp
src/capture/capture_odom_2D.cpp
src/capture/capture_odom_3D.cpp
src/capture/capture_velocity.cpp
@@ -548,20 +477,16 @@ SET(SRCS_FEATURE
src/feature/feature_diff_drive.cpp
src/feature/feature_GPS_fix.cpp
src/feature/feature_GPS_pseudorange.cpp
- src/feature/feature_IMU.cpp
src/feature/feature_odom_2D.cpp
- src/feature/feature_polyline_2D.cpp
)
SET(SRCS_LANDMARK
src/landmark/landmark_corner_2D.cpp
src/landmark/landmark_container.cpp
src/landmark/landmark_line_2D.cpp
- src/landmark/landmark_polyline_2D.cpp
)
SET(SRCS_PROCESSOR
src/processor/processor_frame_nearest_neighbor_filter.cpp
src/processor/processor_diff_drive.cpp
- src/processor/processor_IMU.cpp
src/processor/processor_odom_2D.cpp
src/processor/processor_odom_3D.cpp
src/processor/processor_tracker_feature.cpp
@@ -570,11 +495,9 @@ SET(SRCS_PROCESSOR
src/processor/processor_tracker_landmark.cpp
)
SET(SRCS_SENSOR
- src/sensor/sensor_camera.cpp
src/sensor/sensor_diff_drive.cpp
src/sensor/sensor_GPS.cpp
src/sensor/sensor_GPS_fix.cpp
- src/sensor/sensor_IMU.cpp
src/sensor/sensor_odom_2D.cpp
src/sensor/sensor_odom_3D.cpp
)
@@ -610,98 +533,6 @@ ELSE(Ceres_FOUND)
SET(SRCS_WRAPPER)
ENDIF(Ceres_FOUND)
-IF (laser_scan_utils_FOUND)
- SET(HDRS_CAPTURE ${HDRS_CAPTURE}
- include/base/capture/capture_laser_2D.h
- )
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/base/processor/processor_tracker_feature_corner.h
- include/base/processor/processor_tracker_landmark_corner.h
- include/base/processor/processor_tracker_landmark_polyline.h
- )
- SET(HDRS_SENSOR ${HDRS_SENSOR}
- include/base/sensor/sensor_laser_2D.h
- )
- SET(SRCS ${SRCS}
- src/sensor/sensor_laser_2D.cpp
- src/processor/processor_tracker_feature_corner.cpp
- src/processor/processor_tracker_landmark_corner.cpp
- src/processor/processor_tracker_landmark_polyline.cpp
- )
-ENDIF(laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- SET(HDRS_CAPTURE ${HDRS_CAPTURE}
- include/base/capture/capture_GPS.h
- )
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/base/processor/processor_GPS.h
- )
- SET(SRCS ${SRCS}
- src/capture/capture_GPS.cpp
- src/processor/processor_GPS.cpp
- )
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision
-IF (vision_utils_FOUND)
- SET(HDRS_CAPTURE ${HDRS_CAPTURE}
- include/base/capture/capture_image.h
- )
- SET(HDRS_FEATURE ${HDRS_FEATURE}
- include/base/feature/feature_point_image.h
- )
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/base/processor/processor_tracker_feature_trifocal.h
- include/base/processor/processor_params_image.h
- include/base/processor/processor_tracker_feature_image.h
- include/base/processor/processor_tracker_landmark_image.h
- )
- SET(HDRS_LANDMARK ${HDRS_LANDMARK}
- include/base/landmark/landmark_point_3D.h
- include/base/landmark/landmark_AHP.h
- )
- SET(SRCS ${SRCS}
- src/capture/capture_image.cpp
- src/feature/feature_point_image.cpp
- )
- SET(SRCS_LANDMARK ${SRCS_LANDMARK}
- src/landmark/landmark_point_3D.cpp
- src/landmark/landmark_AHP.cpp
- )
- SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
- src/processor/processor_tracker_feature_trifocal.cpp
- src/processor/processor_tracker_feature_image.cpp
- 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)
@@ -725,27 +556,8 @@ IF(YAMLCPP_FOUND)
# sources
SET(SRCS ${SRCS}
src/yaml/processor_odom_3D_yaml.cpp
- src/yaml/processor_IMU_yaml.cpp
- src/yaml/sensor_camera_yaml.cpp
src/yaml/sensor_odom_3D_yaml.cpp
- src/yaml/sensor_IMU_yaml.cpp
)
- IF(laser_scan_utils_FOUND)
- SET(SRCS ${SRCS}
- src/yaml/sensor_laser_2D_yaml.cpp
- )
- ENDIF(laser_scan_utils_FOUND)
- IF(vision_utils_FOUND)
- SET(SRCS ${SRCS}
- 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
@@ -772,20 +584,7 @@ IF (Ceres_FOUND)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
ENDIF(Ceres_FOUND)
-IF (laser_scan_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${laser_scan_utils_LIBRARY})
-ENDIF (laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${raw_gps_utils_LIBRARY})
-ENDIF (raw_gps_utils_FOUND)
-IF (OPENCV_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS})
- IF (vision_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${vision_utils_LIBRARY})
- ENDIF (vision_utils_FOUND)
-ENDIF (OPENCV_FOUND)
IF (YAMLCPP_FOUND)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY})
@@ -795,10 +594,6 @@ 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)
diff --git a/include/base/capture/capture_GPS.h b/include/base/capture/capture_GPS.h
deleted file mode 100644
index 5f5088bd6dbf5e9674bcd12c3c13caa6a213c8e5..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_GPS.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef CAPTURE_GPS_H_
-#define CAPTURE_GPS_H_
-
-// Wolf includes
-#include "raw_gps_utils/satellites_obs.h"
-#include "base/capture/capture_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureGPS);
-
-class CaptureGPS : public CaptureBase
-{
- protected:
- rawgpsutils::SatellitesObs obs_;
-
- public:
- CaptureGPS(const TimeStamp &_ts, SensorBasePtr _sensor_ptr, rawgpsutils::SatellitesObs &_obs);
- virtual ~CaptureGPS();
-
- rawgpsutils::SatellitesObs &getData();
-};
-
-} // namespace wolf
-
-#endif //CAPTURE_GPS_H_
diff --git a/include/base/capture/capture_image.h b/include/base/capture/capture_image.h
deleted file mode 100644
index 4e9771cf08808342e9339dfe9c3f108e49aae542..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_image.h
+++ /dev/null
@@ -1,50 +0,0 @@
-#ifndef CAPTURE_IMAGE_H
-#define CAPTURE_IMAGE_H
-
-//Wolf includes
-#include "base/capture/capture_base.h"
-#include "base/feature/feature_point_image.h"
-#include "base/sensor/sensor_camera.h"
-
-// Vision Utils includes
-#include "vision_utils/vision_utils.h"
-#include "vision_utils/common_class/frame.h"
-
-namespace wolf {
-
-// Set ClassPtr, ClassConstPtr and ClassWPtr typedefs;
-WOLF_PTR_TYPEDEFS(CaptureImage);
-
-/**
- * \brief class CaptureImage
- *
- * This class stores a cv::Mat image, with keypoints and descriptors defined in the OpenCV format.
- * This encapsulation allows this Capture to be used in OpenCV with ease.
- */
-class CaptureImage : public CaptureBase
-{
- protected:
- vision_utils::Frame frame_;
-
- public:
- vision_utils::FeatureIdxGridPtr grid_features_;
- KeyPointVector keypoints_;
- cv::Mat descriptors_;
- DMatchVector matches_from_precedent_;
- std::vector<Scalar> matches_normalized_scores_;
- std::map<int, int> map_index_to_next_;
-
- public:
- CaptureImage(const TimeStamp& _ts, SensorCameraPtr _camera_ptr, cv::Mat _data_cv);
- virtual ~CaptureImage();
-
- const cv::Mat& getImage() const;
- void setDescriptors(const cv::Mat &_descriptors);
- void setKeypoints(const std::vector<cv::KeyPoint>& _keypoints);
- cv::Mat& getDescriptors();
- std::vector<cv::KeyPoint>& getKeypoints();
-};
-
-} // namespace wolf
-
-#endif // CAPTURE_IMAGE_H
diff --git a/include/base/capture/capture_laser_2D.h b/include/base/capture/capture_laser_2D.h
deleted file mode 100644
index 96a434547b5e802376472f472be0aeb0e27af940..0000000000000000000000000000000000000000
--- a/include/base/capture/capture_laser_2D.h
+++ /dev/null
@@ -1,52 +0,0 @@
-
-#ifndef CAPTURE_LASER_2D_H_
-#define CAPTURE_LASER_2D_H_
-
-//wolf forward declarations
-namespace wolf{
-class SensorLaser2D;
-}
-
-//wolf includes
-#include "base/capture/capture_base.h"
-#include "base/sensor/sensor_laser_2D.h"
-
-//laserscanutils includes
-#include "laser_scan_utils/laser_scan.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureLaser2D);
-
-class CaptureLaser2D : public CaptureBase
-{
- public:
- /** \brief Constructor with ranges
- **/
- CaptureLaser2D(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges);
- virtual ~CaptureLaser2D() = default;
-
- laserscanutils::LaserScan& getScan();
-
- void setSensor(const SensorBasePtr sensor_ptr);
-
- private:
- SensorLaser2DPtr laser_ptr_; //specific pointer to sensor laser 2D object
- laserscanutils::LaserScan scan_;
-
-};
-
-inline laserscanutils::LaserScan& CaptureLaser2D::getScan()
-{
- return scan_;
-}
-
-inline void CaptureLaser2D::setSensor(const SensorBasePtr sensor_ptr)
-{
- CaptureBase::setSensor(sensor_ptr);
- laser_ptr_ = std::static_pointer_cast<SensorLaser2D>(sensor_ptr);
-}
-
-} // namespace wolf
-
-#endif /* CAPTURE_LASER_2D_H_ */
diff --git a/include/base/factor/factor_autodiff_apriltag.h b/include/base/factor/factor_autodiff_apriltag.h
deleted file mode 100644
index 3402cc497ec9c71f293dff2038be6a7c1d985b50..0000000000000000000000000000000000000000
--- a/include/base/factor/factor_autodiff_apriltag.h
+++ /dev/null
@@ -1,182 +0,0 @@
-#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/feature/feature_apriltag.h b/include/base/feature/feature_apriltag.h
deleted file mode 100644
index f2fbb23756ea252365f1ba366d6a16119f2336e6..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_apriltag.h
+++ /dev/null
@@ -1,59 +0,0 @@
-#ifndef FEATURE_APRILTAG_H_
-#define FEATURE_APRILTAG_H_
-
-//Wolf includes
-#include "base/feature/feature_base.h"
-
-//std includes
-
-//external library incudes
-#include "apriltag.h"
-#include "common/zarray.h"
-
-// opencv
-#include <opencv2/features2d.hpp>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeatureApriltag);
-
-//class FeatureApriltag
-class FeatureApriltag : public FeatureBase
-{
- public:
-
- FeatureApriltag(const Eigen::Vector7s & _measurement, const Eigen::Matrix6s & _meas_covariance,
- const int _tag_id, const apriltag_detection_t & _det,
- Scalar _rep_error1, Scalar _rep_error2, bool _use_rotation,
- UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO);
- virtual ~FeatureApriltag();
-
- /** \brief Returns tag id
- *
- * Returns tag id
- *
- **/
- Scalar getTagId() const;
-
- const apriltag_detection_t& getDetection() const;
-
- const std::vector<cv::Point2d>& getTagCorners() const;
-
- Scalar getRepError1() const;
- Scalar getRepError2() const;
- bool getUserotation() const;
-
-
- private:
- int tag_id_;
- std::vector<cv::Point2d> tag_corners_;
- apriltag_detection_t detection_;
- Scalar rep_error1_;
- Scalar rep_error2_;
- bool use_rotation_;
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/feature/feature_point_image.h b/include/base/feature/feature_point_image.h
deleted file mode 100644
index aa07d80dfd836d51440383bea1acf7ab5cea0451..0000000000000000000000000000000000000000
--- a/include/base/feature/feature_point_image.h
+++ /dev/null
@@ -1,137 +0,0 @@
-#ifndef FEATURE_IMAGE_H
-#define FEATURE_IMAGE_H
-
-//Wolf includes
-#include "base/feature/feature_base.h"
-
-// Vision Utils includes
-#include <vision_utils.h>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeaturePointImage);
-
-//class FeaturePointImage
-class FeaturePointImage : public FeatureBase
-{
- private:
- cv::KeyPoint keypoint_; ///< Warning: every write operation to this member needs to write measurement_. See setKeypoint() as an example.
- unsigned int index_keypoint_;
- cv::Mat descriptor_;
- bool is_known_;
- Scalar score_;
- cv::Rect tracker_roi_;
-
- public:
-
- /// Constructor from Eigen measured pixel
- FeaturePointImage(const Eigen::Vector2s & _measured_pixel,
- const unsigned int& _index_keypoint,
- const cv::Mat& _descriptor,
- const Eigen::Matrix2s& _meas_covariance) :
- FeatureBase("POINT IMAGE", _measured_pixel, _meas_covariance),
- keypoint_(_measured_pixel(0), _measured_pixel(1), 1), // Size 1 is a dummy value
- index_keypoint_(_index_keypoint),
- descriptor_( _descriptor),
- is_known_(false),
- score_(0)
- {
- keypoint_.pt.x = measurement_(0);
- keypoint_.pt.y = measurement_(1);
- }
-
- /// Constructor from OpenCV measured keypoint
- FeaturePointImage(const cv::KeyPoint& _keypoint,
- const unsigned int& _index_keypoint,
- const cv::Mat& _descriptor,
- const Eigen::Matrix2s& _meas_covariance) :
- FeatureBase("POINT IMAGE", Eigen::Vector2s::Zero(), _meas_covariance),
- keypoint_(_keypoint),
- index_keypoint_(_index_keypoint),
- descriptor_(_descriptor),
- is_known_(false),
- score_(0)
- {
- measurement_(0) = Scalar(_keypoint.pt.x);
- measurement_(1) = Scalar(_keypoint.pt.y);
- }
-
- virtual ~FeaturePointImage();
-
- const cv::KeyPoint& getKeypoint() const;
- void setKeypoint(const cv::KeyPoint& _kp);
-
- const cv::Mat& getDescriptor() const;
- void setDescriptor(const cv::Mat& _descriptor);
-
- SizeStd getIndexKeyPoint() const
- { return index_keypoint_; }
-
- bool isKnown();
- void setIsKnown(bool _is_known);
-
- /*Eigen::VectorXs & getMeasurement(){
- measurement_(0) = Scalar(keypoint_.pt.x);
- measurement_(1) = Scalar(keypoint_.pt.y);
- return measurement_;
- }*/
-
- const cv::Rect& getTrackerRoi() const;
- void setTrackerRoi(const cv::Rect& tracker_roi);
-
- Scalar getScore() const
- {
- return score_;
- }
-
- void setScore(Scalar score)
- {
- score_ = score;
- }
-};
-
-inline const cv::KeyPoint& FeaturePointImage::getKeypoint() const
-{
- return keypoint_;
-}
-
-inline void FeaturePointImage::setKeypoint(const cv::KeyPoint& _kp)
-{
- keypoint_ = _kp;
- measurement_(0) = _kp.pt.x;
- measurement_(1) = _kp.pt.y;
-}
-
-inline const cv::Mat& FeaturePointImage::getDescriptor() const
-{
- return descriptor_;
-}
-
-inline void FeaturePointImage::setDescriptor(const cv::Mat& _descriptor)
-{
- descriptor_ = _descriptor;
-}
-
-inline bool FeaturePointImage::isKnown()
-{
- return is_known_;
-}
-
-inline void FeaturePointImage::setIsKnown(bool _is_known)
-{
- is_known_ = _is_known;
-}
-
-inline const cv::Rect& FeaturePointImage::getTrackerRoi() const
-{
- return tracker_roi_;
-}
-
-inline void FeaturePointImage::setTrackerRoi(const cv::Rect& tracker_roi)
-{
- tracker_roi_ = tracker_roi;
-}
-
-} // namespace wolf
-
-#endif // FEATURE_IMAGE_H
diff --git a/include/base/landmark/landmark_AHP.h b/include/base/landmark/landmark_AHP.h
deleted file mode 100644
index a706f87dadcfa938d4ec0666a2d1a241ff40c253..0000000000000000000000000000000000000000
--- a/include/base/landmark/landmark_AHP.h
+++ /dev/null
@@ -1,89 +0,0 @@
-#ifndef LANDMARK_AHP_H
-#define LANDMARK_AHP_H
-
-//Wolf includes
-#include "base/landmark/landmark_base.h"
-
-// yaml
-#include <yaml-cpp/yaml.h>
-
-// Vision utils
-#include <vision_utils/vision_utils.h>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(LandmarkAHP);
-
-/* Landmark - Anchored Homogeneous Point*/
-class LandmarkAHP : public LandmarkBase
-{
- protected:
- cv::Mat cv_descriptor_;
- FrameBasePtr anchor_frame_;
- SensorBasePtr anchor_sensor_;
-
- public:
- LandmarkAHP(Eigen::Vector4s _position_homogeneous, FrameBasePtr _anchor_frame, SensorBasePtr _anchor_sensor, cv::Mat _2D_descriptor);
-
- virtual ~LandmarkAHP();
-
- const cv::Mat& getCvDescriptor() const;
- void setCvDescriptor(const cv::Mat& _descriptor);
-
- const FrameBasePtr getAnchorFrame () const;
- const SensorBasePtr getAnchorSensor() const;
-
- void setAnchorFrame (FrameBasePtr _anchor_frame );
- void setAnchorSensor (SensorBasePtr _anchor_sensor);
- void setAnchor (FrameBasePtr _anchor_frame , SensorBasePtr _anchor_sensor);
- Eigen::Vector3s getPointInAnchorSensor() const;
- Eigen::Vector3s point() const;
-
- YAML::Node saveToYaml() const;
-
- /** \brief Creator for Factory<LandmarkBase, YAML::Node>
- * Caution: This creator does not set the landmark's anchor frame and sensor.
- * These need to be set afterwards.
- */
- static LandmarkBasePtr create(const YAML::Node& _node);
-};
-
-inline const cv::Mat& LandmarkAHP::getCvDescriptor() const
-{
- return cv_descriptor_;
-}
-
-inline void LandmarkAHP::setCvDescriptor(const cv::Mat& _descriptor)
-{
- cv_descriptor_ = _descriptor;
-}
-
-inline const FrameBasePtr LandmarkAHP::getAnchorFrame() const
-{
- return anchor_frame_;
-}
-
-inline void LandmarkAHP::setAnchorFrame(FrameBasePtr _anchor_frame)
-{
- anchor_frame_ = _anchor_frame;
-}
-
-inline const SensorBasePtr LandmarkAHP::getAnchorSensor() const
-{
- return anchor_sensor_;
-}
-
-inline void LandmarkAHP::setAnchorSensor(SensorBasePtr _anchor_sensor)
-{
- anchor_sensor_ = _anchor_sensor;
-}
-
-inline void LandmarkAHP::setAnchor(FrameBasePtr _anchor_frame, SensorBasePtr _anchor_sensor)
-{
- anchor_frame_ = _anchor_frame;
- anchor_sensor_ = _anchor_sensor;
-}
-
-} // namespace wolf
-
-#endif // LANDMARK_AHP_H
diff --git a/include/base/landmark/landmark_apriltag.h b/include/base/landmark/landmark_apriltag.h
deleted file mode 100644
index c5ab9e8398a8fa9d5c044de9e66eaf2d75ad7fa9..0000000000000000000000000000000000000000
--- a/include/base/landmark/landmark_apriltag.h
+++ /dev/null
@@ -1,63 +0,0 @@
-
-#ifndef LANDMARK_APRILTAG_H_
-#define LANDMARK_APRILTAG_H_
-
-//Wolf includes
-#include "base/landmark/landmark_base.h"
-
-#include "base/state_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_point_3D.h b/include/base/landmark/landmark_point_3D.h
deleted file mode 100644
index 7ee1c0507b3f3f63d0015bf509fb7eb629db309c..0000000000000000000000000000000000000000
--- a/include/base/landmark/landmark_point_3D.h
+++ /dev/null
@@ -1,50 +0,0 @@
-#ifndef LANDMARK_POINT_3D_H
-#define LANDMARK_POINT_3D_H
-
-//Wolf includes
-#include "base/landmark/landmark_base.h"
-
-// Vision Utils includes
-#include <vision_utils/vision_utils.h>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(LandmarkPoint3D);
-
-//class
-class LandmarkPoint3D : public LandmarkBase
-{
- protected:
- cv::Mat descriptor_;
- Eigen::Vector3s position_;
- public:
- LandmarkPoint3D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, Eigen::Vector3s _position, cv::Mat _2D_descriptor);
-
- virtual ~LandmarkPoint3D();
-
- const Eigen::Vector3s& getPosition() const;
- void setPosition(const Eigen::Vector3s & _pos)
- {
- position_ = _pos;
- }
-
- const cv::Mat& getDescriptor() const;
- void setDescriptor(const cv::Mat& _descriptor)
- {
- descriptor_ = _descriptor;
- }
-};
-
-inline const Eigen::Vector3s& LandmarkPoint3D::getPosition() const
-{
- return position_;
-}
-
-inline const cv::Mat& LandmarkPoint3D::getDescriptor() const
-{
- return descriptor_;
-}
-
-} // namespace wolf
-
-#endif // LANDMARK_POINT_3D_H
diff --git a/include/base/processor/ippe.h b/include/base/processor/ippe.h
deleted file mode 100644
index d445b223f3e4afd4449da4cfe630cda87c329239..0000000000000000000000000000000000000000
--- a/include/base/processor/ippe.h
+++ /dev/null
@@ -1,222 +0,0 @@
-#ifndef _IPPE_H_
-#define _IPPE_H_
-
-#include <opencv2/core.hpp>
-#include <opencv2/calib3d.hpp>
-
-#include <limits>
-
-#define IPPE_SMALL 1e-7 //a small constant used to test 'small' values close to zero.
-
-namespace IPPE {
-
-class PoseSolver {
-
-public:
-
- /**
- * @brief PoseSolver constructor
- */
- PoseSolver();
-
- /**
- * @brief PoseSolver destructor
- */
- ~PoseSolver();
-
- /**
- * @brief Finds the two possible poses of a planar object given a set of correspondences and their respective reprojection errors. The poses are sorted with the first having the lowest reprojection error.
- * @param _objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points
- * @param _imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates.
- * @param _cameraMatrix Intrinsic camera matrix (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray()
- * @param _distCoeffs Intrinsic camera distortion vector (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray()
- * @param _rvec1 First rotation solution (3x1 rotation vector)
- * @param _tvec1 First translation solution (3x1 vector)
- * @param reprojErr1 Reprojection error of first solution
- * @param _rvec2 Second rotation solution (3x1 rotation vector)
- * @param _tvec2 Second translation solution (3x1 vector)
- * @param reprojErr2 Reprojection error of second solution
- */
- void solveGeneric(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
- cv::OutputArray _rvec1, cv::OutputArray _tvec1, float& reprojErr1, cv::OutputArray _rvec2, cv::OutputArray _tvec2, float& reprojErr2);
-
- /** @brief Finds the two possible poses of a square planar object and their respective reprojection errors using IPPE. These poses are sorted so that the first one is the one with the lowest reprojection error.
- *
- * @param _squareLength The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.)
- * @param _imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates.
- * @param _cameraMatrix Intrinsic camera matrix (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray()
- * @param _distCoeffs Intrinsic camera distortion vector (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray()
- * @param _rvec1 First rotation solution (3x1 rotation vector)
- * @param _tvec1 First translation solution (3x1 vector)
- * @param reprojErr1 Reprojection error of first solution
- * @param _rvec2 Second rotation solution (3x1 rotation vector)
- * @param _tvec2 Second translation solution (3x1 vector)
- * @param reprojErr2 Reprojection error of second solution
- */
- void solveSquare(float squareLength, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
- cv::OutputArray _rvec1, cv::OutputArray _tvec1, float& reprojErr1, cv::OutputArray _rvec2, cv::OutputArray _tvec2, float& reprojErr2);
-
- /**
- * @brief Generates the 4 object points of a square planar object
- * @param squareLength The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.)
- * @param _objectPoints Set of 4 object points (1x4 3-channel double)
- */
- void generateSquareObjectCorners3D(double squareLength, cv::OutputArray _objectPoints);
-
- /**
- * @brief Generates the 4 object points of a square planar object, without including the z-component (which is z=0 for all points).
- * @param squareLength The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.)
- * @param _objectPoints Set of 4 object points (1x4 2-channel double)
- */
- void generateSquareObjectCorners2D(double squareLength, cv::OutputArray _objectPoints);
-
- /**
- * @brief Computes the average depth of an object given its pose in camera coordinates
- * @param objectPoints: Object points defined in 3D object space
- * @param rvec: Rotation component of pose
- * @param tvec: Translation component of pose
- * @return: average depth of the object
- */
- double meanSceneDepth(cv::InputArray objectPoints, cv::InputArray rvec, cv::InputArray tvec);
-
-private:
- /**
- * @brief Finds the two possible poses of a planar object given a set of correspondences in normalized pixel coordinates. These poses are **NOT** sorted on reprojection error. Note that the returned poses are object-to-camera transforms, and not camera-to-object transforms.
- * @param _objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double).
- * @param _normalizedImagePoints Array of corresponding image points in normalized pixel coordinates, 1xN/Nx1 2-channel (float or double).
- * @param _Ma First pose solution (unsorted)
- * @param _Mb Second pose solution (unsorted)
- */
- void solveGeneric(cv::InputArray _objectPoints, cv::InputArray _normalizedImagePoints, cv::OutputArray _Ma, cv::OutputArray _Mb);
-
- /**
- * @brief Finds the two possible poses of a planar object in its canonical position, given a set of correspondences in normalized pixel coordinates. These poses are **NOT** sorted on reprojection error. Note that the returned poses are object-to-camera transforms, and not camera-to-object transforms.
- * @param _canonicalObjPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (double) where N is the number of points
- * @param _normalizedInputPoints Array of corresponding image points in normalized pixel coordinates, 1xN/Nx1 2-channel (double) where N is the number of points
- * @param _H Homography mapping canonicalObjPoints to normalizedInputPoints.
- * @param _Ma
- * @param _Mb
- */
- void solveCanonicalForm(cv::InputArray _canonicalObjPoints, cv::InputArray _normalizedInputPoints, cv::InputArray _H,
- cv::OutputArray _Ma, cv::OutputArray _Mb);
-
- /** @brief Computes the translation solution for a given rotation solution
- * @param _objectPoints Array of corresponding object points, 1xN/Nx1 3-channel where N is the number of points
- * @param _normalizedImagePoints Array of corresponding image points (undistorted), 1xN/Nx1 2-channel where N is the number of points
- * @param _R Rotation solution (3x1 rotation vector)
- * @param _t Translation solution Translation solution (3x1 rotation vector)
- */
- void computeTranslation(cv::InputArray _objectPoints, cv::InputArray _normalizedImgPoints, cv::InputArray _R, cv::OutputArray _t);
-
- /** @brief Computes the two rotation solutions from the Jacobian of a homography matrix H at a point (ux,uy) on the object plane. For highest accuracy the Jacobian should be computed at the centroid of the point correspondences (see the IPPE paper for the explanation of this). For a point (ux,uy) on the object plane, suppose the homography H maps (ux,uy) to a point (p,q) in the image (in normalized pixel coordinates). The Jacobian matrix [J00, J01; J10,J11] is the Jacobian of the mapping evaluated at (ux,uy).
- * @param j00 Homography jacobian coefficent at (ux,uy)
- * @param j01 Homography jacobian coefficent at (ux,uy)
- * @param j10 Homography jacobian coefficent at (ux,uy)
- * @param j11 Homography jacobian coefficent at (ux,uy)
- * @param p the x coordinate of point (ux,uy) mapped into the image (undistorted and normalized position)
- * @param q the y coordinate of point (ux,uy) mapped into the image (undistorted and normalized position)
- */
- void computeRotations(double j00, double j01, double j10, double j11, double p, double q, cv::OutputArray _R1, cv::OutputArray _R2);
-
- /** @brief Closed-form solution for the homography mapping with four corner correspondences of a square (it maps source points to target points). The source points are the four corners of a zero-centred squared defined by:
- * point 0: [-squareLength / 2.0, squareLength / 2.0]
- * point 1: [squareLength / 2.0, squareLength / 2.0]
- * point 2: [squareLength / 2.0, -squareLength / 2.0]
- * point 3: [-squareLength / 2.0, -squareLength / 2.0]
- *
- * @param _targetPoints Array of four corresponding target points, 1x4/4x1 2-channel. Note that the points should be ordered to correspond with points 0, 1, 2 and 3.
- * @param halfLength the square's half length (i.e. squareLength/2.0)
- * @param _H Homograhy mapping the source points to the target points, 3x3 single channel
- */
- void homographyFromSquarePoints(cv::InputArray _targetPoints, double halfLength, cv::OutputArray _H);
-
- /** @brief Fast conversion from a rotation matrix to a rotation vector using Rodrigues' formula
- * @param _R Input rotation matrix, 3x3 1-channel (double)
- * @param _r Output rotation vector, 3x1/1x3 1-channel (double)
- */
- void rot2vec(cv::InputArray _R, cv::OutputArray _r);
-
- /**
- * @brief Takes a set of planar object points and transforms them to 'canonical' object coordinates This is when they have zero mean and are on the plane z=0
- * @param _objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points
- * @param _canonicalObjectPoints Object points in canonical coordinates 1xN/Nx1 2-channel (double)
- * @param _MobjectPoints2Canonical Transform matrix mapping _objectPoints to _canonicalObjectPoints: 4x4 1-channel (double)
- */
- void makeCanonicalObjectPoints(cv::InputArray _objectPoints, cv::OutputArray _canonicalObjectPoints, cv::OutputArray _MobjectPoints2Canonical);
-
- /**
- * @brief Evaluates the Root Mean Squared (RMS) reprojection error of a pose solution.
- * @param _objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points
- * @param _imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates.
- * @param _cameraMatrix Intrinsic camera matrix (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray().
- * @param _distCoeffs Intrinsic camera distortion vector (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray().
- * @param _M Pose matrix from 3D object to camera coordinates: 4x4 1-channel (double)
- * @param err RMS reprojection error
- */
- void evalReprojError(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, cv::InputArray _M, float& err);
-
- /**
- * @brief Sorts two pose solutions according to their RMS reprojection error (lowest first).
- * @param _objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points
- * @param _imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates.
- * @param _cameraMatrix Intrinsic camera matrix (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray().
- * @param _distCoeffs Intrinsic camera distortion vector (same definition as OpenCV). If _imagePoints is in normalized pixel coordinates you must set _cameraMatrix = cv::noArray().
- * @param _Ma Pose matrix 1: 4x4 1-channel
- * @param _Mb Pose matrix 2: 4x4 1-channel
- * @param _M1 Member of (Ma,Mb} with lowest RMS reprojection error. Performs deep copy.
- * @param _M2 Member of (Ma,Mb} with highest RMS reprojection error. Performs deep copy.
- * @param err1 RMS reprojection error of _M1
- * @param err2 RMS reprojection error of _M2
- */
- void sortPosesByReprojError(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, cv::InputArray _Ma, cv::InputArray _Mb, cv::OutputArray _M1, cv::OutputArray _M2, float& err1, float& err2);
-
- /**
- * @brief Finds the rotation _Ra that rotates a vector _a to the z axis (0,0,1)
- * @param _a vector: 3x1 mat (double)
- * @param _Ra Rotation: 3x3 mat (double)
- */
- void rotateVec2ZAxis(cv::InputArray _a, cv::OutputArray _Ra);
-
-
- /**
- * @brief Computes the rotation _R that rotates the object points to the plane z=0. This uses the cross-product method with the first three object points.
- * @param _objectPoints Array of N>=3 coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points
- * @param _R Rotation Mat: 3x3 (double)
- * @return success (true) or failure (false)
- */
- bool computeObjextSpaceR3Pts(cv::InputArray _objectPoints, cv::OutputArray _R);
-
- /**
- * @brief computeObjextSpaceRSvD Computes the rotation _R that rotates the object points to the plane z=0. This uses the cross-product method with the first three object points.
- * @param _objectPointsZeroMean zero-meaned co=planar object points: 3xN matrix (double) where N>=3
- * @param _R Rotation Mat: 3x3 (double)
- * @return success (true) or failure (false)
- */
- bool computeObjextSpaceRSvD(cv::InputArray _objectPointsZeroMean, cv::OutputArray _R);
-};
-}
-
-namespace HomographyHO {
-
-/**
- * @brief Computes the best-fitting homography matrix from source to target points using Harker and O'Leary's method:
- * Harker, M., O'Leary, P., Computation of Homographies, Proceedings of the British Machine Vision Conference 2005, Oxford, England.
- * This is not the author's implementation.
- * @param srcPoints Array of source points: 1xN/Nx1 2-channel (float or double) where N is the number of points
- * @param targPoints Array of target points: 1xN/Nx1 2-channel (float or double)
- * @param H Homography from source to target: 3x3 1-channel (double)
- */
-void homographyHO(cv::InputArray srcPoints, cv::InputArray targPoints, cv::OutputArray H);
-
-/**
- * @brief Performs data normalization before homography estimation. For details see Hartley, R., Zisserman, A., Multiple View Geometry in Computer Vision,
- * Cambridge University Press, Cambridge, 2001
- * @param Data Array of source data points: 1xN/Nx1 2-channel (float or double) where N is the number of points
- * @param DataN Normalized data points: 1xN/Nx1 2-channel (float or double) where N is the number of points
- * @param T Homogeneous transform from source to normalized: 3x3 1-channel (double)
- * @param Ti Homogeneous transform from normalized to source: 3x3 1-channel (double)
- */
-void normalizeDataIsotropic(cv::InputArray Data, cv::OutputArray DataN, cv::OutputArray T, cv::OutputArray Ti);
-}
-
-#endif
diff --git a/include/base/processor/processor_GPS.h b/include/base/processor/processor_GPS.h
deleted file mode 100644
index 35658410c4a0565c7da75976d72766b377e1712e..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_GPS.h
+++ /dev/null
@@ -1,41 +0,0 @@
-//
-// Created by ptirindelli on 16/12/15.
-//
-
-#ifndef WOLF_PROCESSOR_GPS_H
-#define WOLF_PROCESSOR_GPS_H
-
-// Wolf includes
-#include "base/processor/processor_base.h"
-#include "base/capture/capture_GPS.h"
-
-// Std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(ProcessorGPS);
-
-//class
-class ProcessorGPS : public ProcessorBase
-{
- protected:
- CaptureGPSPtr capture_gps_ptr_;
- Scalar gps_covariance_;
-
- public:
- ProcessorGPS(ProcessorParamsBasePtr _params);
- virtual ~ProcessorGPS();
- virtual void configure(SensorBasePtr _sensor) { };
- virtual void init(CaptureBasePtr _capture_ptr);
- virtual void process(CaptureBasePtr _capture_ptr);
- virtual bool voteForKeyFrame();
- virtual void keyFrameCallback(FrameBasePtr, const Scalar& _time_tol);
-
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-
-};
-
-} // namespace wolf
-
-#endif //WOLF_PROCESSOR_GPS_H
diff --git a/include/base/processor/processor_params_image.h b/include/base/processor/processor_params_image.h
deleted file mode 100644
index 6ce3eaace3119ed1e68725bb33d2ca0df4684bbb..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_params_image.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef PROCESSOR_IMAGE_PARAMS_H
-#define PROCESSOR_IMAGE_PARAMS_H
-
-// wolf
-#include "base/processor/processor_tracker_feature.h"
-#include "base/processor/processor_tracker_landmark.h"
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureImage);
-
-struct ProcessorParamsTrackerFeatureImage : public ProcessorParamsTrackerFeature
-{
- std::string yaml_file_params_vision_utils;
-
- Scalar min_response_for_new_features; ///< minimum value of the response to create a new feature
- Scalar distance;
-
- Scalar pixel_noise_std; ///< std noise of the pixel
- Scalar pixel_noise_var; ///< var noise of the pixel
-};
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmarkImage);
-
-struct ProcessorParamsTrackerLandmarkImage : public ProcessorParamsTrackerLandmark
-{
- std::string yaml_file_params_vision_utils;
-
- Scalar min_response_for_new_features; ///< minimum value of the response to create a new feature
- Scalar distance;
-
- Scalar pixel_noise_std; ///< std noise of the pixel
- Scalar pixel_noise_var; ///< var noise of the pixel
-};
-}
-
-#endif // PROCESSOR_IMAGE_PARAMS_H
diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h
deleted file mode 100644
index e0c2ecbad2f1b89d8e11b15ea174714066ea4f58..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_feature_corner.h
+++ /dev/null
@@ -1,136 +0,0 @@
-/**
- * \file processor_tracker_feature_corner.h
- *
- * Created on: Apr 11, 2016
- * \author: jvallve
- */
-
-#ifndef PROCESSOR_TRACKER_FEATURE_CORNER_H_
-#define PROCESSOR_TRACKER_FEATURE_CORNER_H_
-
-// Wolf includes
-#include "base/sensor/sensor_laser_2D.h"
-#include "base/capture/capture_laser_2D.h"
-#include "base/feature/feature_corner_2D.h"
-#include "base/landmark/landmark_corner_2D.h"
-#include "base/factor/factor_corner_2D.h"
-#include "base/state_block.h"
-#include "base/association/association_tree.h"
-#include "base/processor/processor_tracker_feature.h"
-
-//laser_scan_utils
-//#include "laser_scan_utils/scan_basics.h"
-//#include "laser_scan_utils/corner_detector.h"
-#include "laser_scan_utils/laser_scan.h"
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/corner_finder.h"
-
-namespace wolf
-{
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureCorner);
-
-struct ProcessorParamsTrackerFeatureCorner : public ProcessorParamsTrackerFeature
-{
- laserscanutils::LineFinderIterativeParams line_finder_params;
- unsigned int n_corners_th;
- const Scalar position_error_th = 1;
-};
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureCorner);
-
-
-//some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level
-//const Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
-//const Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
-//const Scalar position_error_th_ = 1;
-//const Scalar min_features_ratio_th_ = 0.5;
-
-class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
-{
- private:
- ProcessorParamsTrackerFeatureCornerPtr params_tracker_feature_corner_;
- //laserscanutils::ScanParams scan_params_;
- //laserscanutils::ExtractCornerParams corner_alg_params_;
- //laserscanutils::LaserScan laser_data_;
- laserscanutils::LineFinderIterative line_finder_;
- laserscanutils::CornerFinder corner_finder_;
- //TODO: add corner_finder_params
-
- FeatureBasePtrList corners_incoming_;
- FeatureBasePtrList corners_last_;
-
- Eigen::Matrix3s R_world_sensor_, R_world_sensor_prev_;
- Eigen::Matrix3s R_robot_sensor_;
- Eigen::Matrix3s R_current_prev_;
- Eigen::Vector3s t_world_sensor_, t_world_sensor_prev_;
- Eigen::Vector3s t_robot_sensor_;
- Eigen::Vector3s t_current_prev_;
- Eigen::Vector3s t_world_robot_;
- bool extrinsics_transformation_computed_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- ProcessorTrackerFeatureCorner(ProcessorParamsTrackerFeatureCornerPtr _params_tracker_feature_corner);
- virtual ~ProcessorTrackerFeatureCorner();
- virtual void configure(SensorBasePtr _sensor) { };
-
- protected:
-
- virtual void preProcess();
-
- void advanceDerived();
-
- /** \brief Track provided features from \b last to \b incoming
- * \param _features_last_in input list of features in \b last to track
- * \param _features_incoming_out returned list of features found in \b incoming
- * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
- */
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences);
-
- /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
- * \param _last_feature input feature in last capture tracked
- * \param _incoming_feature input/output feature in incoming capture to be corrected
- * \return false if the the process discards the correspondence with origin's feature
- */
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature);
-
- /** \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
- * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
- * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function sets the member new_features_list_, the list of newly detected features,
- * to be used for landmark initialization.
- */
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
-
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
-
- private:
-
- void extractCorners(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _corner_list);
-
-};
-
-inline bool ProcessorTrackerFeatureCorner::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature,
- FeatureBasePtr _incoming_feature)
-{
- return true;
-}
-
-} // namespace wolf
-
-#endif /* PROCESSOR_TRACKER_FEATURE_CORNER_H_ */
diff --git a/include/base/processor/processor_tracker_feature_image.h b/include/base/processor/processor_tracker_feature_image.h
deleted file mode 100644
index 800ca1211fbdcdd3d8f99781be90d4ddfdbfad11..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_feature_image.h
+++ /dev/null
@@ -1,169 +0,0 @@
-#ifndef PROCESSOR_TRACKER_FEATURE_IMAGE_H
-#define PROCESSOR_TRACKER_FEATURE_IMAGE_H
-
-// Wolf includes
-#include "base/sensor/sensor_camera.h"
-#include "base/capture/capture_image.h"
-#include "base/feature/feature_point_image.h"
-#include "base/state_block.h"
-#include "base/state_quaternion.h"
-#include "base/processor/processor_tracker_feature.h"
-#include "base/factor/factor_epipolar.h"
-#include "base/processor/processor_params_image.h"
-
-// vision_utils
-#include <detectors/detector_base.h>
-#include <descriptors/descriptor_base.h>
-#include <matchers/matcher_base.h>
-#include <algorithms/activesearch/alg_activesearch.h>
-
-// General includes
-#include <cmath>
-#include <complex> // std::complex, std::norm
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureImage);
-
-//class
-class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
-{
- // vision utils params
- protected:
-
- vision_utils::DetectorBasePtr det_ptr_;
- vision_utils::DescriptorBasePtr des_ptr_;
- vision_utils::MatcherBasePtr mat_ptr_;
- vision_utils::AlgorithmACTIVESEARCHPtr active_search_ptr_; // Active Search
-
- int cell_width_; ///< Active search cell width
- int cell_height_; ///< Active search cell height
- vision_utils::AlgorithmParamsACTIVESEARCHPtr params_tracker_feature_image_activesearch_ptr_; ///< Active search parameters
-
- protected:
-
- ProcessorParamsTrackerFeatureImagePtr params_tracker_feature_image_; ///< Struct with parameters of the processors
- cv::Mat image_last_, image_incoming_; ///< Images of the "last" and "incoming" Captures
-
- struct
- {
- unsigned int width_; ///< width of the image
- unsigned int height_; ///< height of the image
- } image_;
-
- public:
-
- // Lists to store values to debug
- std::list<cv::Rect> tracker_roi_;
- std::list<cv::Rect> tracker_roi_inflated_;
- std::list<cv::Rect> detector_roi_;
- std::list<cv::Point> tracker_target_;
-
- ProcessorTrackerFeatureImage(ProcessorParamsTrackerFeatureImagePtr _params_image);
- virtual ~ProcessorTrackerFeatureImage();
-
- virtual void configure(SensorBasePtr _sensor) ;
-
- protected:
-
- /**
- * \brief Does cast of the images and renews the active grid.
- */
- void preProcess();
-
- /**
- * \brief Does the drawing of the features.
- *
- * Used for debugging
- */
- void postProcess();
-
- void advanceDerived()
- {
- ProcessorTrackerFeature::advanceDerived();
- image_last_ = image_incoming_;
- }
-
- void resetDerived()
- {
- ProcessorTrackerFeature::resetDerived();
- image_last_ = image_incoming_;
- }
-
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences);
-
- /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
- * \param _last_feature input feature in last capture tracked
- * \param _incoming_feature input/output feature in incoming capture to be corrected
- * \return false if the the process discards the correspondence with origin's feature
- */
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature,
- FeatureBasePtr _incoming_feature);
-
- /** \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
- *
- * This is intended to create Features that are not among the Features already known in the Map.
- *
- * This function sets new_features_last_, the list of newly detected features.
- *
- * \return The number of detected Features.
- */
- virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
-
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
-
- private:
-
- /**
- * \brief Detects keypoints its descriptors in a specific roi of the image
- * \param _image input image in which the algorithm will search
- * \param _roi input roi used to define the area of search within the image
- * \param _new_keypoints output keypoints obtained in the function
- * \param new_descriptors output descriptors obtained in the function
- * \return the number of detected features
- */
- virtual unsigned int detect(cv::Mat _image, cv::Rect& _roi, std::vector<cv::KeyPoint>& _new_keypoints,
- cv::Mat& _new_descriptors);
-
- /**
- * \brief Does the match between a target descriptor and (potentially) multiple candidate descriptors of a Feature.
- * \param _target_descriptor descriptor of the target
- * \param _candidate_descriptors descriptors of the candidates
- * \param _cv_matches output variable in which the best result will be stored (in the position [0])
- * \return normalized score of similarity (1 - exact match; 0 - complete mismatch)
- */
- virtual Scalar match(cv::Mat _target_descriptor, cv::Mat _candidate_descriptors,
- std::vector<cv::DMatch>& _cv_matches);
-
- // These only to debug, will disappear one day
- public:
- virtual void drawFeatures(cv::Mat _image);
- virtual void drawTarget(cv::Mat _image, std::list<cv::Point> _target_list);
- virtual void drawRoi(cv::Mat _image, std::list<cv::Rect> _roi_list, cv::Scalar _color);
- virtual void resetVisualizationFlag(FeatureBasePtrList& _feature_list_last);
-
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params,
- const SensorBasePtr sensor_ptr = nullptr);
-
-};
-
-inline bool ProcessorTrackerFeatureImage::voteForKeyFrame()
-{
- return (incoming_ptr_->getFeatureList().size() < params_tracker_feature_image_->min_features_for_keyframe);
-}
-
-} // namespace wolf
-
-#endif // PROCESSOR_TRACKER_FEATURE_IMAGE_H
diff --git a/include/base/processor/processor_tracker_feature_trifocal.h b/include/base/processor/processor_tracker_feature_trifocal.h
deleted file mode 100644
index dba9e91714a2736b7bae8a970dc5f8cde9cc05d6..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_feature_trifocal.h
+++ /dev/null
@@ -1,154 +0,0 @@
-#ifndef _PROCESSOR_TRACKER_FEATURE_TRIFOCAL_H_
-#define _PROCESSOR_TRACKER_FEATURE_TRIFOCAL_H_
-
-//Wolf includes
-#include "base/processor/processor_tracker_feature.h"
-#include "base/capture/capture_image.h"
-
-// Vision utils
-#include <vision_utils.h>
-#include <detectors/detector_base.h>
-#include <descriptors/descriptor_base.h>
-#include <matchers/matcher_base.h>
-#include <algorithms/activesearch/alg_activesearch.h>
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureTrifocal);
-
-struct ProcessorParamsTrackerFeatureTrifocal : public ProcessorParamsTrackerFeature
-{
- std::string yaml_file_params_vision_utils;
-
- int n_cells_h;
- int n_cells_v;
- int min_response_new_feature;
- Scalar max_euclidean_distance;
- Scalar pixel_noise_std; ///< std noise of the pixel
- int min_track_length_for_factor; ///< Minimum track length of a matched feature to create a factor
-};
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureTrifocal);
-
-class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
-{
- // Parameters for vision_utils
- protected:
- vision_utils::DetectorBasePtr det_ptr_;
- vision_utils::DescriptorBasePtr des_ptr_;
- vision_utils::MatcherBasePtr mat_ptr_;
-
- protected:
-
- ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal_; ///< Configuration parameters
-
- CaptureImagePtr capture_last_;
- CaptureImagePtr capture_incoming_;
-
- private:
- CaptureBasePtr prev_origin_ptr_; ///< Capture previous to origin_ptr_ for the third focus of the trifocal.
- bool initialized_; ///< Flags the situation where three focus are available: prev_origin, origin, and last.
-
- public:
-
- /** \brief Class constructor
- */
- ProcessorTrackerFeatureTrifocal( ProcessorParamsTrackerFeatureTrifocalPtr _params_tracker_feature_trifocal );
-
- /** \brief Class Destructor
- */
- virtual ~ProcessorTrackerFeatureTrifocal();
- virtual void configure(SensorBasePtr _sensor) override;
-
- /** \brief Track provided features from \b last to \b incoming
- * \param _features_last_in input list of features in \b last to track
- * \param _features_incoming_out returned list of features found in \b incoming
- * \param _feature_matches returned map of correspondences: _feature_matches[feature_out_ptr] = feature_in_ptr
- */
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_matches) override;
-
- /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
- * \param _origin_feature input feature in origin capture tracked
- * \param _incoming_feature input/output feature in incoming capture to be corrected
- * \return false if the the process discards the correspondence with origin's feature
- */
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature) override;
-
- /** \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() override;
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function sets the member new_features_last_, the list of newly detected features.
- */
- virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out) override;
-
- /** \brief Create a new factor and link it to the wolf tree
- * \param _feature_ptr pointer to the parent Feature
- * \param _feature_other_ptr pointer to the other feature constrained.
- *
- * Implement this method in derived classes.
- *
- * This function creates a factor of the appropriate type for the derived processor.
- */
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override;
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief advance pointers
- *
- */
- virtual void advanceDerived() override;
-
- /** \brief reset pointers and match lists at KF creation
- *
- */
- virtual void resetDerived() override;
-
- /** \brief Pre-process: check if all captures (prev-origin, origin, last) are initialized to allow factors creation
- *
- */
- virtual void preProcess() override;
-
- /** \brief Post-process
- *
- */
- virtual void postProcess() override;
-
- /** \brief Establish factors between features in Captures \b last and \b origin
- */
- virtual void establishFactors() override;
-
- CaptureBasePtr getPrevOrigin();
-
- bool isInlier(const cv::KeyPoint& _kp_incoming, const cv::KeyPoint& _kp_last);
-
- void setParams(const ProcessorParamsTrackerFeatureTrifocalPtr _params);
-
- public:
-
- /// @brief Factory method
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr _sensor_ptr);
-};
-
-inline CaptureBasePtr ProcessorTrackerFeatureTrifocal::getPrevOrigin()
-{
- return prev_origin_ptr_;
-}
-
-} // namespace wolf
-
-#endif /* _PROCESSOR_TRACKER_FEATURE_TRIFOCAL_H_ */
diff --git a/include/base/processor/processor_tracker_landmark_apriltag.h b/include/base/processor/processor_tracker_landmark_apriltag.h
deleted file mode 100644
index abe2cb0f61dfd153cc161d42e8a786fcfcc725a8..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_landmark_apriltag.h
+++ /dev/null
@@ -1,186 +0,0 @@
-#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/processor/processor_tracker_landmark_corner.h b/include/base/processor/processor_tracker_landmark_corner.h
deleted file mode 100644
index a25a2646f21efd437201b8cac0c0899164a07597..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_landmark_corner.h
+++ /dev/null
@@ -1,181 +0,0 @@
-/*
- * processor_tracker_landmark_corner.h
- *
- * Created on: Apr 4, 2016
- * Author: jvallve
- */
-
-#ifndef SRC_PROCESSOR_TRACKER_LANDMARK_CORNER_H_
-#define SRC_PROCESSOR_TRACKER_LANDMARK_CORNER_H_
-
-// Wolf includes
-#include "base/sensor/sensor_laser_2D.h"
-#include "base/capture/capture_laser_2D.h"
-#include "base/feature/feature_corner_2D.h"
-#include "base/landmark/landmark_corner_2D.h"
-#include "base/factor/factor_corner_2D.h"
-#include "base/state_block.h"
-#include "base/association/association_tree.h"
-#include "base/processor/processor_tracker_landmark.h"
-
-//laser_scan_utils
-//#include "laser_scan_utils/scan_basics.h"
-//#include "laser_scan_utils/corner_detector.h"
-#include "laser_scan_utils/laser_scan.h"
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/corner_finder.h"
-
-namespace wolf
-{
-
-//some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level
-const Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
-const Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
-const Scalar position_error_th_ = 1;
-const Scalar min_features_ratio_th_ = 0.5;
-
-//forward declaration to typedef class pointers
-struct ProcessorParamsLaser;
-typedef std::shared_ptr<ProcessorParamsLaser> ProcessorParamsLaserPtr;
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkCorner);
-
-struct ProcessorParamsLaser : public ProcessorParamsTrackerLandmark
-{
- laserscanutils::LineFinderIterativeParams line_finder_params_;
- //TODO: add corner_finder_params
- unsigned int new_corners_th;
- unsigned int loop_frames_th;
-
- // These values below are constant and defined within the class -- provide a setter or accept them at construction time if you need to configure them
- // Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
- // Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
- // Scalar position_error_th_ = 1;
- // Scalar min_features_ratio_th_ = 0.5;
-};
-
-class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
-{
- private:
- laserscanutils::LineFinderIterative line_finder_;
- laserscanutils::CornerFinder corner_finder_;
- //TODO: add corner_finder_params
-
- FeatureBasePtrList corners_incoming_;
- FeatureBasePtrList corners_last_;
- unsigned int new_corners_th_;
- unsigned int loop_frames_th_;
-
- // These values are constant -- provide a setter or accept them at construction time if you need to configure them
- Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
-// Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
-// Scalar position_error_th_ = 1;
-// Scalar min_features_ratio_th_ = 0.5;
-
- Eigen::Matrix3s R_sensor_world_, R_world_sensor_;
- Eigen::Matrix3s R_robot_sensor_;
- Eigen::Matrix3s R_current_prev_;
- Eigen::Vector3s t_sensor_world_, t_world_sensor_, t_world_sensor_prev_, t_sensor_world_prev_;
- Eigen::Vector3s t_robot_sensor_;
- Eigen::Vector3s t_current_prev_;
- Eigen::Vector3s t_world_robot_;
- bool extrinsics_transformation_computed_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- ProcessorTrackerLandmarkCorner(ProcessorParamsLaserPtr _params_laser);
-
- virtual ~ProcessorTrackerLandmarkCorner();
- virtual void configure(SensorBasePtr _sensor) { };
-
- protected:
-
- virtual void preProcess();
-// virtual void postProcess() { }
-
- void advanceDerived()
- {
- //std::cout << "\tProcessorTrackerLandmarkCorner::advance:" << std::endl;
- //std::cout << "\t\tcorners_last: " << corners_last_.size() << std::endl;
- //std::cout << "\t\tcorners_incoming_: " << corners_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::advanceDerived();
- while (!corners_last_.empty())
- {
- corners_last_.front()->remove();
- corners_last_.pop_front(); // TODO check if this is needed
- }
- corners_last_ = std::move(corners_incoming_);
- }
-
- void resetDerived()
- {
- //std::cout << "\tProcessorTrackerLandmarkCorner::reset:" << std::endl;
- //std::cout << "\t\tcorners_last: " << corners_last_.size() << std::endl;
- //std::cout << "\t\tcorners_incoming_: " << corners_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::resetDerived();
- corners_last_ = std::move(corners_incoming_);
- }
-
- /** \brief Find provided landmarks in the incoming capture
- * \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
- */
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_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
- * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
- * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function sets the member new_features_list_, the list of newly detected features,
- * to be used for landmark initialization.
- */
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_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 factor
- * \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);
-
- private:
-
- void extractCorners(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _corner_list);
-
- void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::Vector4s& expected_feature_,
- Eigen::Matrix3s& expected_feature_cov_);
-
- Eigen::VectorXs computeSquaredMahalanobisDistances(const FeatureBasePtr _feature_ptr,
- const Eigen::Vector4s& _expected_feature,
- const Eigen::Matrix3s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu);
- // Factory method
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-};
-
-} // namespace wolf
-
-#endif /* SRC_PROCESSOR_TRACKER_LASER_H_ */
diff --git a/include/base/processor/processor_tracker_landmark_image.h b/include/base/processor/processor_tracker_landmark_image.h
deleted file mode 100644
index d58920cc10b943c74a27dd1c3f61946624747133..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_landmark_image.h
+++ /dev/null
@@ -1,192 +0,0 @@
-#ifndef PROCESSOR_TRACKER_LANDMARK_IMAGE_H
-#define PROCESSOR_TRACKER_LANDMARK_IMAGE_H
-
-// Wolf includes
-
-#include "base/landmark/landmark_AHP.h"
-#include "base/landmark/landmark_match.h"
-#include "base/processor/processor_params_image.h"
-#include "base/processor/processor_tracker_landmark.h"
-#include "base/wolf.h"
-
-#include <algorithms/activesearch/alg_activesearch.h>
-#include <descriptors/descriptor_base.h>
-#include <detectors/detector_base.h>
-#include <matchers/matcher_base.h>
-
-#include <opencv2/core/mat.hpp>
-#include <opencv2/core/mat.inl.hpp>
-#include <opencv2/core/types.hpp>
-
-#include <list>
-#include <memory>
-#include <string>
-#include <vector>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkImage);
-
-//Class
-class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
-{
- protected:
-
- vision_utils::DetectorBasePtr det_ptr_;
- vision_utils::DescriptorBasePtr des_ptr_;
- vision_utils::MatcherBasePtr mat_ptr_;
- vision_utils::AlgorithmACTIVESEARCHPtr active_search_ptr_; // Active Search
-
- int cell_width_; ///< Active search cell width
- int cell_height_; ///< Active search cell height
- vision_utils::AlgorithmParamsACTIVESEARCHPtr params_tracker_landmark_image_activesearch_ptr_; ///< Active search parameters
-
- protected:
- ProcessorParamsTrackerLandmarkImagePtr params_tracker_landmark_image_; // Struct with parameters of the processors
-
- cv::Mat image_last_, image_incoming_; // Images of the "last" and "incoming" Captures
-
- struct
- {
- unsigned int width_; ///< width of the image
- unsigned int height_; ///< height of the image
- }image_;
-
- unsigned int landmarks_tracked_ = 0;
-
- /* pinhole params */
- Eigen::Vector2s distortion_;
- Eigen::Vector2s correction_;
-
- /* transformations */
- Eigen::Vector3s world2cam_translation_;
- Eigen::Vector4s world2cam_orientation_;
-
- Eigen::Vector3s cam2world_translation_;
- Eigen::Vector4s cam2world_orientation_;
-
- unsigned int n_feature_;
- unsigned int landmark_idx_non_visible_;
-
- std::list<float> list_response_;
-
- public:
-
- // Lists to store values to debug
- std::list<cv::Rect> tracker_roi_;
- std::list<cv::Rect> detector_roi_;
- std::list<cv::Point> tracker_target_;
- FeatureBasePtrList feat_lmk_found_;
-
- ProcessorTrackerLandmarkImage(ProcessorParamsTrackerLandmarkImagePtr _params_tracker_landmark_image);
- virtual ~ProcessorTrackerLandmarkImage();
-
- virtual void configure(SensorBasePtr _sensor) ;
-
- protected:
-
- /**
- * \brief Does cast of the images and renews the active grid.
- */
- void preProcess();
-
- void advanceDerived()
- {
- ProcessorTrackerLandmark::advanceDerived();
- image_last_ = image_incoming_;
- }
-
- void resetDerived()
- {
- ProcessorTrackerLandmark::resetDerived();
- image_last_ = image_incoming_;
- }
-
- /**
- * \brief Does the drawing of the features.
- *
- * Used for debugging
- */
- void postProcess();
-
- //Pure virtual
- /** \brief Find provided landmarks in the incoming capture
- * \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
- */
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_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
- *
- * This is intended to create Features that are not among the Features already known in the Map.
- *
- * This function sets new_features_last_, the list of newly detected features.
- *
- * \return The number of detected Features.
- */
- virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_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);
-
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-
- /** \brief Create a new factor
- * \param _feature_ptr pointer to the Feature to constrain
- * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
- */
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
-
- //Other functions
- private:
-
- /**
- * \brief Detects keypoints its descriptors in a specific roi of the image
- * \param _image input image in which the algorithm will search
- * \param _roi input roi used to define the area of search within the image
- * \param _new_keypoints output keypoints obtained in the function
- * \param new_descriptors output descriptors obtained in the function
- * \return the number of detected features
- */
- unsigned int detect(const cv::Mat _image, cv::Rect& _roi, std::vector<cv::KeyPoint>& _new_keypoints,cv::Mat& new_descriptors);
-
- /**
- * \brief Does the match between a target descriptor and (potentially) multiple candidate descriptors of a Feature.
- * \param _target_descriptor descriptor of the target
- * \param _candidate_descriptors descriptors of the candidates
- * \param _cv_matches output variable in which the best result will be stored (in the position [0])
- * \return normalized score of similarity (1 - exact match; 0 - complete mismatch)
- */
- Scalar match(const cv::Mat _target_descriptor, const cv::Mat _candidate_descriptors, std::vector<cv::DMatch>& _cv_matches);
-
- void landmarkInCurrentCamera(const Eigen::VectorXs& _frame_state,
- const LandmarkAHPPtr _landmark,
- Eigen::Vector4s& _point3D_hmg);
-
- // These only to debug, will disappear one day soon
- public:
- void drawLandmarks(cv::Mat _image);
- void drawFeaturesFromLandmarks(cv::Mat _image);
- void drawRoi(cv::Mat _image, std::list<cv::Rect> _roi_list, cv::Scalar _color);
- void drawTrackerRoi(cv::Mat _image, cv::Scalar _color);
-
-};
-
-} // namespace wolf
-
-#endif // PROCESSOR_TRACKER_LANDMARK_IMAGE_H
diff --git a/include/base/processor/processor_tracker_landmark_polyline.h b/include/base/processor/processor_tracker_landmark_polyline.h
deleted file mode 100644
index a6f902c930d696dc595ae19f133a0df4fd0e00c2..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_landmark_polyline.h
+++ /dev/null
@@ -1,248 +0,0 @@
-/*
- * processor_tracker_landmark_polyline.h
- *
- * Created on: May 26, 2016
- * Author: jvallve
- */
-
-#ifndef SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_
-#define SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_
-
-// Wolf includes
-#include "base/sensor/sensor_laser_2D.h"
-#include "base/capture/capture_laser_2D.h"
-#include "base/feature/feature_polyline_2D.h"
-#include "base/landmark/landmark_polyline_2D.h"
-#include "base/factor/factor_point_2D.h"
-#include "base/factor/factor_point_to_line_2D.h"
-#include "base/state_block.h"
-#include "base/association/association_tree.h"
-#include "base/processor/processor_tracker_landmark.h"
-
-//laser_scan_utils
-#include "laser_scan_utils/laser_scan.h"
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/polyline.h"
-
-namespace wolf
-{
-
-//some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level
-const Scalar position_error_th_ = 1;
-const Scalar min_features_ratio_th_ = 0.5;
-
-//forward declaration to typedef class pointers
-struct LandmarkPolylineMatch;
-typedef std::shared_ptr<LandmarkPolylineMatch> LandmarkPolylineMatchPtr;
-
-//forward declaration to typedef class pointers
-struct ProcessorParamsPolyline;
-typedef std::shared_ptr<ProcessorParamsPolyline> ProcessorParamsPolylinePtr;
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkPolyline);
-
-// Match Feature - Landmark
-struct LandmarkPolylineMatch : public LandmarkMatch
-{
- int landmark_match_from_id_;
- int feature_match_from_id_;
- int landmark_match_to_id_;
- int feature_match_to_id_;
-
-// std::vector<unsigned int> landmark_points_match_;
-// std::vector<unsigned int> feature_points_match_;
-// std::vector<unsigned int> feature_points_add_front_;
-// std::vector<unsigned int> feature_points_add_back_;
-
- LandmarkPolylineMatch() :
- landmark_match_from_id_(0),
- feature_match_from_id_(0),
- landmark_match_to_id_(0),
- feature_match_to_id_(0)
- {
- //
- }
- LandmarkPolylineMatch(int _landmark_match_from_id,
- int _feature_match_from_id,
- int _landmark_match_to_id,
- int _feature_match_to_id) :
- landmark_match_from_id_(_landmark_match_from_id),
- feature_match_from_id_(_feature_match_from_id),
- landmark_match_to_id_(_landmark_match_to_id),
- feature_match_to_id_(_landmark_match_to_id)
- {
- //
- }
-};
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsPolyline);
-struct ProcessorParamsPolyline : public ProcessorParamsTrackerLandmark
-{
- laserscanutils::LineFinderIterativeParams line_finder_params;
- Scalar position_error_th;
- unsigned int new_features_th;
- unsigned int loop_frames_th;
-
- // These values below are constant and defined within the class -- provide a setter or accept them at construction time if you need to configure them
- // Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
- // Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
- // Scalar position_error_th_ = 1;
- // Scalar min_features_ratio_th_ = 0.5;
-};
-
-class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
-{
- private:
- laserscanutils::LineFinderIterative line_finder_;
- ProcessorParamsPolylinePtr params_;
-
- FeatureBasePtrList polylines_incoming_;
- FeatureBasePtrList polylines_last_;
-
- Eigen::Matrix2s R_sensor_world_, R_world_sensor_;
- Eigen::Matrix2s R_robot_sensor_;
- Eigen::Matrix2s R_current_prev_;
- Eigen::Vector2s t_sensor_world_, t_world_sensor_, t_world_sensor_prev_, t_sensor_world_prev_;
- Eigen::Vector2s t_robot_sensor_;
- Eigen::Vector2s t_current_prev_;
- Eigen::Vector2s t_world_robot_;
- bool extrinsics_transformation_computed_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params);
-
- virtual ~ProcessorTrackerLandmarkPolyline();
- virtual void configure(SensorBasePtr _sensor) { };
-
- const FeatureBasePtrList& getLastPolylines() const;
-
- protected:
-
- virtual void preProcess();
- void computeTransformations(const TimeStamp& _ts);
- virtual void postProcess();
-
- void advanceDerived();
-
- void resetDerived();
-
- /** \brief Find provided landmarks in the incoming capture
- * \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
- */
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_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
- * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
- * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function sets the member new_features_list_, the list of newly detected features,
- * to be used for landmark initialization.
- */
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
-
- /** \brief Creates a landmark for each of new_features_last_
- **/
- virtual void createNewLandmarks();
-
- /** \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 Establish factors between features in Captures \b last and \b origin
- */
- virtual void establishFactors();
-
- /** \brief look for known objects in the list of unclassified polylines
- */
- void classifyPolilines(LandmarkBasePtrList& _lmk_list);
-
- /** \brief Create a new factor
- * \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);
-
- private:
-
- void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list);
-
- void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
- Eigen::MatrixXs& expected_feature_cov_);
-
- Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
- const Eigen::Matrix2s& _feature_cov,
- const Eigen::Vector2s& _expected_feature,
- const Eigen::Matrix2s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu);
- Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B,
- bool _A_extreme, bool _B_extreme);
- // Factory method
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-};
-
-inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params) :
- ProcessorTrackerLandmark("TRACKER LANDMARK POLYLINE", _params),
- line_finder_(_params->line_finder_params),
- params_(_params),
- extrinsics_transformation_computed_(false)
-{
-}
-
-inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
-{
- // already computed since each scan is computed in preprocess()
- _features_incoming_out = std::move(polylines_last_);
- return _features_incoming_out.size();
-}
-
-inline void ProcessorTrackerLandmarkPolyline::advanceDerived()
-{
- //std::cout << "\tProcessorTrackerLandmarkPolyline::advance:" << std::endl;
- //std::cout << "\t\tcorners_last: " << polylines_last_.size() << std::endl;
- //std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::advanceDerived();
- for (auto polyline : polylines_last_)
- polyline->remove();
- polylines_last_ = std::move(polylines_incoming_);
- //std::cout << "advanced" << std::endl;
-}
-
-inline void ProcessorTrackerLandmarkPolyline::resetDerived()
-{
- //std::cout << "\tProcessorTrackerLandmarkPolyline::reset:" << std::endl;
- //std::cout << "\t\tcorners_last: " << corners_last_.size() << std::endl;
- //std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::resetDerived();
- polylines_last_ = std::move(polylines_incoming_);
-}
-
-inline const FeatureBasePtrList& ProcessorTrackerLandmarkPolyline::getLastPolylines() const
-{
- return polylines_last_;
-}
-
-} // namespace wolf
-
-#endif /* SRC_PROCESSOR_TRACKER_LASER_H_ */
diff --git a/include/base/sensor/sensor_laser_2D.h b/include/base/sensor/sensor_laser_2D.h
deleted file mode 100644
index 6b927c5b4bbbdc23a05cad52cc9f1be4a8590733..0000000000000000000000000000000000000000
--- a/include/base/sensor/sensor_laser_2D.h
+++ /dev/null
@@ -1,86 +0,0 @@
-// sensor_laser_2D.h
-
-#ifndef SENSOR_LASER_2D_H_
-#define SENSOR_LASER_2D_H_
-
-//wolf
-#include "base/sensor/sensor_base.h"
-
-//laser_scan_utils
-#include "laser_scan_utils/laser_scan.h"
-
-// std
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsLaser2D);
-
-struct IntrinsicsLaser2D : public IntrinsicsBase
-{
- virtual ~IntrinsicsLaser2D() = default;
-
- laserscanutils::LaserScanParams scan_params;
-};
-
-WOLF_PTR_TYPEDEFS(SensorLaser2D);
-
-class SensorLaser2D : public SensorBase
-{
- protected:
- laserscanutils::LaserScanParams scan_params_;
-
- public:
- /** \brief Constructor with extrinsics
- *
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- *
- **/
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr);
-
- /** \brief Constructor with extrinsics and scan parameters
- *
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- *
- **/
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _angle_min, const double& _angle_max, const double& _angle_step, const double& _scan_time, const double& _range_min, const double& _range_max, const double& _range_std_dev, const double& _angle_std_dev);
-
- /** \brief Constructor with extrinsics and scan parameters
- *
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- * \param _params Scan parameters
- *
- **/
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params);
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params);
- SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params);
-
- virtual ~SensorLaser2D();
-
- void setDefaultScanParams();
-
- /** \brief Set scanner intrinsic parameters
- *
- * \param _params struct with scanner intrinsic parameters. See laser_scan_utils library API for reference.
- *
- **/
- void setScanParams(const laserscanutils::LaserScanParams & _params);
-
- /** \brief Get scanner intrinsic parameters
- *
- * Get scanner intrinsic parameters
- *
- **/
- const laserscanutils::LaserScanParams & getScanParams() const;
-
- public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po, const IntrinsicsBasePtr _intrinsics);
- static IntrinsicsBasePtr createParams(const std::string& _filename_dot_yaml);
-
-};
-
-} // namespace wolf
-
-#endif /*SENSOR_LASER_2D_H_*/
diff --git a/src/capture/capture_GPS.cpp b/src/capture/capture_GPS.cpp
deleted file mode 100644
index 5c6520810890c564502bbb297705e96413289fb4..0000000000000000000000000000000000000000
--- a/src/capture/capture_GPS.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-#include "base/capture/capture_GPS.h"
-
-namespace wolf {
-
-CaptureGPS::CaptureGPS(const TimeStamp &_ts, SensorBasePtr _sensor_ptr, rawgpsutils::SatellitesObs &_obs) :
- CaptureBase("GPS", _ts, _sensor_ptr),
- obs_(_obs)
-{
- //
-// std::cout << "CaptureGPS constructor.\t\tReceived " << obs_.measurements_.size() << " meas" << std::endl;
-// std::cout << obs_.toString() << std::endl;
-}
-
-CaptureGPS::~CaptureGPS()
-{
- //std::cout << "deleting CaptureGPS " << id() << std::endl;
-}
-
-rawgpsutils::SatellitesObs &CaptureGPS::getData()
-{
- return obs_;
-}
-
-} //namespace wolf
diff --git a/src/capture/capture_image.cpp b/src/capture/capture_image.cpp
deleted file mode 100644
index 1d8ac8b8d7b5959173948948e89d403470d34d45..0000000000000000000000000000000000000000
--- a/src/capture/capture_image.cpp
+++ /dev/null
@@ -1,48 +0,0 @@
-#include "base/capture/capture_image.h"
-
-namespace wolf {
-
-CaptureImage::CaptureImage(const TimeStamp& _ts, SensorCameraPtr _camera_ptr, cv::Mat _data_cv) :
- CaptureBase("IMAGE", _ts, _camera_ptr),
- frame_(_data_cv),
- grid_features_(nullptr),
- keypoints_(KeyPointVector()),
- descriptors_(cv::Mat()),
- matches_from_precedent_(DMatchVector()),
- matches_normalized_scores_(std::vector<Scalar>()),
- map_index_to_next_(std::map<int, int>())
-{
- //
-}
-
-CaptureImage::~CaptureImage()
-{
- //
-}
-
-const cv::Mat& CaptureImage::getImage() const
-{
- return frame_.getImage();
-}
-
-void CaptureImage::setDescriptors(const cv::Mat& _descriptors)
-{
- frame_.setDescriptors(_descriptors);
-}
-
-void CaptureImage::setKeypoints(const std::vector<cv::KeyPoint> &_keypoints)
-{
- frame_.setKeyPoints(_keypoints);
-}
-
-cv::Mat& CaptureImage::getDescriptors()
-{
- return frame_.getDescriptors();
-}
-
-std::vector<cv::KeyPoint>& CaptureImage::getKeypoints()
-{
- return frame_.getKeyPoints();
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_apriltag.cpp b/src/feature/feature_apriltag.cpp
deleted file mode 100644
index 18cb89cdf9813a5112b0aa3b7769834f1318fdaf..0000000000000000000000000000000000000000
--- a/src/feature/feature_apriltag.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-
-#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_point_image.cpp b/src/feature/feature_point_image.cpp
deleted file mode 100644
index e7d3781b6935b78cff79a0b25ee8998e3a7c6d44..0000000000000000000000000000000000000000
--- a/src/feature/feature_point_image.cpp
+++ /dev/null
@@ -1,11 +0,0 @@
-
-#include "base/feature/feature_point_image.h"
-
-namespace wolf {
-
-FeaturePointImage::~FeaturePointImage()
-{
- //
-}
-
-} // namespace wolf
diff --git a/src/landmark/landmark_AHP.cpp b/src/landmark/landmark_AHP.cpp
deleted file mode 100644
index 1daa37470b0c1ce87d47e4d174344fd09e471d9d..0000000000000000000000000000000000000000
--- a/src/landmark/landmark_AHP.cpp
+++ /dev/null
@@ -1,76 +0,0 @@
-#include "base/landmark/landmark_AHP.h"
-
-#include "base/state_homogeneous_3D.h"
-#include "base/factory.h"
-#include "base/yaml/yaml_conversion.h"
-
-namespace wolf {
-
-/* Landmark - Anchored Homogeneous Point*/
-LandmarkAHP::LandmarkAHP(Eigen::Vector4s _position_homogeneous,
- FrameBasePtr _anchor_frame,
- SensorBasePtr _anchor_sensor,
- cv::Mat _2D_descriptor) :
- LandmarkBase("AHP", std::make_shared<StateHomogeneous3D>(_position_homogeneous)),
- cv_descriptor_(_2D_descriptor.clone()),
- anchor_frame_(_anchor_frame),
- anchor_sensor_(_anchor_sensor)
-{
-}
-
-LandmarkAHP::~LandmarkAHP()
-{
- //
-}
-
-YAML::Node LandmarkAHP::saveToYaml() const
-{
- // First base things
- YAML::Node node = LandmarkBase::saveToYaml();
-
- // Then add specific things
- std::vector<int> v;
- LandmarkAHP::cv_descriptor_.copyTo(v);
- node["descriptor"] = v;
- return node;
-}
-
-Eigen::Vector3s LandmarkAHP::getPointInAnchorSensor() const
-{
- Eigen::Map<const Eigen::Vector4s> hmg_point(getP()->getState().data());
- return hmg_point.head<3>()/hmg_point(3);
-}
-
-Eigen::Vector3s LandmarkAHP::point() const
-{
- using namespace Eigen;
- Transform<Scalar,3,Affine> T_w_r
- = Translation<Scalar,3>(getAnchorFrame()->getP()->getState())
- * Quaternions(getAnchorFrame()->getO()->getState().data());
- Transform<Scalar,3,Affine> T_r_c
- = Translation<Scalar,3>(getAnchorSensor()->getP()->getState())
- * Quaternions(getAnchorSensor()->getO()->getState().data());
- Vector4s point_hmg_c = getP()->getState();
- Vector4s point_hmg = T_w_r * T_r_c * point_hmg_c;
- return point_hmg.head<3>()/point_hmg(3);
-}
-
-LandmarkBasePtr LandmarkAHP::create(const YAML::Node& _node)
-{
- unsigned int id = _node["id"] .as< unsigned int >();
- Eigen::VectorXs pos_homog = _node["position"] .as< Eigen::VectorXs >();
- std::vector<int> v = _node["descriptor"] .as< std::vector<int> >();
- cv::Mat desc(v);
-
- LandmarkBasePtr lmk = std::make_shared<LandmarkAHP>(pos_homog, nullptr, nullptr, desc);
- lmk->setId(id);
- return lmk;
-}
-
-// Register landmark creator
-namespace
-{
-const bool WOLF_UNUSED registered_lmk_ahp = LandmarkFactory::get().registerCreator("AHP", LandmarkAHP::create);
-}
-
-} // namespace wolf
diff --git a/src/landmark/landmark_apriltag.cpp b/src/landmark/landmark_apriltag.cpp
deleted file mode 100644
index bca929367583b25607e0239eaa1b608176d97ff4..0000000000000000000000000000000000000000
--- a/src/landmark/landmark_apriltag.cpp
+++ /dev/null
@@ -1,93 +0,0 @@
-
-#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/landmark/landmark_point_3D.cpp b/src/landmark/landmark_point_3D.cpp
deleted file mode 100644
index dda3984e904ecb1221074df749cdcc671dfed3ff..0000000000000000000000000000000000000000
--- a/src/landmark/landmark_point_3D.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-#include "base/landmark/landmark_point_3D.h"
-
-namespace wolf {
-
-LandmarkPoint3D::LandmarkPoint3D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, Eigen::Vector3s _position, cv::Mat _2D_descriptor) :
- LandmarkBase("POINT 3D", _p_ptr, _o_ptr),
- descriptor_(_2D_descriptor),
- position_(_position)
-{
- //LandmarkPoint3D* landmark_ptr = (LandmarkPoint3D*)_p_ptr;
-// position_ =
-// descriptor_ = _2D_descriptor;
-}
-
-LandmarkPoint3D::~LandmarkPoint3D()
-{
- //
-}
-
-}
diff --git a/src/processor/ippe.cpp b/src/processor/ippe.cpp
deleted file mode 100644
index abbd4c4f8653ffbdde69192094151ef6afb53adc..0000000000000000000000000000000000000000
--- a/src/processor/ippe.cpp
+++ /dev/null
@@ -1,1050 +0,0 @@
-#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_GPS.cpp b/src/processor/processor_GPS.cpp
deleted file mode 100644
index 8409f78c2572489083d36a6c9deb31c45ed53ee1..0000000000000000000000000000000000000000
--- a/src/processor/processor_GPS.cpp
+++ /dev/null
@@ -1,75 +0,0 @@
-//
-// Created by ptirindelli on 16/12/15.
-//
-
-#include "base/factor/factor_GPS_pseudorange_2D.h"
-#include "base/feature/feature_GPS_pseudorange.h"
-#include "base/processor/processor_GPS.h"
-
-#include "base/capture/capture_GPS.h"
-
-namespace wolf
-{
-
-ProcessorGPS::ProcessorGPS(ProcessorParamsBasePtr _params) :
- ProcessorBase("GPS", _params),
- capture_gps_ptr_(nullptr)
-{
- gps_covariance_ = 10;
-}
-
-ProcessorGPS::~ProcessorGPS()
-{
-}
-
-void ProcessorGPS::init(CaptureBasePtr _capture_ptr)
-{
-}
-
-void ProcessorGPS::process(CaptureBasePtr _capture_ptr)
-{
- std::cout << "ProcessorGPS::process(GPScapture)" << std::endl;
- capture_gps_ptr_ = std::static_pointer_cast<CaptureGPS>(_capture_ptr);
-
- //std::cout << "Extracting gps features..." << std::endl;
- rawgpsutils::SatellitesObs obs = capture_gps_ptr_->getData();
- for (unsigned int i = 0; i < obs.measurements_.size(); ++i)
- {
- Eigen::Vector3s sat_pos = obs.measurements_[i].sat_position_;
- Scalar pr = obs.measurements_[i].pseudorange_;
- capture_gps_ptr_->addFeature(std::make_shared<FeatureGPSPseudorange>(sat_pos, pr, gps_covariance_));
- }
- //std::cout << "gps features extracted" << std::endl;
- //std::cout << "Establishing factors to gps features..." << std::endl;
- for (auto i_it = capture_gps_ptr_->getFeatureList().begin();
- i_it != capture_gps_ptr_->getFeatureList().end(); i_it++)
- {
- capture_gps_ptr_->getFeatureList().front()->addFactor(std::make_shared<FactorGPSPseudorange2D>((*i_it), shared_from_this()));
- }
- //std::cout << "Factors established" << std::endl;
-}
-
-bool ProcessorGPS::voteForKeyFrame()
-{
- return false;
-}
-
-void ProcessorGPS::keyFrameCallback(FrameBasePtr, const Scalar& _time_tol)
-{
- //
-}
-
-ProcessorBasePtr ProcessorGPS::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
-{
- ProcessorGPSPtr prc_ptr = std::make_shared<ProcessorGPS>(_params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("GPS",ProcessorGPS)
-} // namespace wolf
diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp
deleted file mode 100644
index 4414d8caf499e187e28961b54f8a65e80455716a..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_feature_corner.cpp
+++ /dev/null
@@ -1,201 +0,0 @@
-/**
- * \file processor_tracker_feature_corner.cpp
- *
- * Created on: Apr 11, 2016
- * \author: jvallve
- */
-
-#include "base/processor/processor_tracker_feature_corner.h"
-#include "base/feature/feature_corner_2D.h"
-
-namespace wolf
-{
-
-ProcessorTrackerFeatureCorner::ProcessorTrackerFeatureCorner(ProcessorParamsTrackerFeatureCornerPtr _params_tracker_feature_corner) :
- ProcessorTrackerFeature("TRACKER FEATURE CORNER", _params_tracker_feature_corner),
- params_tracker_feature_corner_(_params_tracker_feature_corner),
- R_world_sensor_(Eigen::Matrix3s::Identity()),
- R_robot_sensor_(Eigen::Matrix3s::Identity()),
- extrinsics_transformation_computed_(false)
-{
- //
-}
-
-ProcessorTrackerFeatureCorner::~ProcessorTrackerFeatureCorner()
-{
- for (auto corner : corners_last_)
- corner->remove();
- for (auto corner : corners_incoming_)
- corner->remove();
-}
-
-void ProcessorTrackerFeatureCorner::preProcess()
-{
- // extract corners of incoming
- extractCorners(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_), corners_incoming_);
-
- // store previous transformations
- R_world_sensor_prev_ = R_world_sensor_;
- t_world_sensor_prev_ = t_world_sensor_;
-
- // compute transformations
- t_world_robot_ = getProblem()->getState(incoming_ptr_->getTimeStamp());
-
- // world_robot
- Eigen::Matrix3s R_world_robot = Eigen::Matrix3s::Identity();
- R_world_robot.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_world_robot_(2)).matrix();
-
- // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
- || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
- {
- t_robot_sensor_.head<2>() = getSensor()->getP()->getState();
- t_robot_sensor_(2) = getSensor()->getO()->getState()(0);
- R_robot_sensor_.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_robot_sensor_(2)).matrix();
- extrinsics_transformation_computed_ = true;
- }
-
- // global_sensor
- R_world_sensor_.topLeftCorner<2, 2>() = R_world_robot.topLeftCorner<2, 2>() * R_robot_sensor_.topLeftCorner<2, 2>();
- t_world_sensor_ = t_world_robot_ + R_robot_sensor_ * t_robot_sensor_;
-
- // current_prev
- R_current_prev_ = R_world_sensor_.transpose() * R_world_sensor_prev_;
- t_current_prev_ = R_world_sensor_.transpose() * (t_world_sensor_prev_ - t_world_sensor_);
-}
-
-void ProcessorTrackerFeatureCorner::advanceDerived()
-{
- ProcessorTrackerFeature::advanceDerived();
- corners_last_ = std::move(corners_incoming_);
-}
-
-unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBasePtrList& _features_last_in,
- FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences)
-{
- std::cout << "tracking " << _features_last_in.size() << " features..." << std::endl;
-
- Eigen::Vector3s expected_feature_pose;
- for (auto feat_in_ptr : _features_last_in)
- {
- expected_feature_pose = R_current_prev_ * feat_in_ptr->getMeasurement().head<3>() + t_current_prev_;
-
- auto feat_out_next = corners_incoming_.begin();
- auto feat_out_it = feat_out_next++; // next is used to obtain the next iterator after splice
- while (feat_out_it != corners_incoming_.end()) //runs over extracted feature
- {
- if (((*feat_out_it)->getMeasurement().head<3>() - expected_feature_pose).squaredNorm() > params_tracker_feature_corner_->position_error_th*params_tracker_feature_corner_->position_error_th)
- {
- // match
- _feature_correspondences[*feat_out_it] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
-
- // move matched feature to list
- _features_incoming_out.splice(_features_incoming_out.end(), corners_incoming_, feat_out_it);
-
- std::cout << "feature " << feat_in_ptr->id() << " tracked!" << std::endl;
- }
- feat_out_it = feat_out_next++;
- }
- }
-
- return _features_incoming_out.size();
-}
-
-bool ProcessorTrackerFeatureCorner::voteForKeyFrame()
-{
- return incoming_ptr_->getFeatureList().size() < params_tracker_feature_corner_->n_corners_th;
-}
-
-unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
-{
- // in corners_last_ remain all not tracked corners
- _features_incoming_out = std::move(corners_last_);
- return _features_incoming_out.size();
-}
-
-FactorBasePtr ProcessorTrackerFeatureCorner::createFactor(FeatureBasePtr _feature_ptr,
- FeatureBasePtr _feature_other_ptr)
-{
- // Getting landmark ptr
- LandmarkCorner2DPtr landmark_ptr = nullptr;
- for (auto factor : _feature_other_ptr->getFactorList())
- if (factor->getLandmarkOther() != nullptr && factor->getLandmarkOther()->getType() == "CORNER 2D")
- landmark_ptr = std::static_pointer_cast<LandmarkCorner2D>(factor->getLandmarkOther());
-
- if (landmark_ptr == nullptr)
- {
- // Create new landmark
- Eigen::Vector3s feature_global_pose = R_world_sensor_ * _feature_ptr->getMeasurement() + t_world_sensor_;
- landmark_ptr = std::make_shared<LandmarkCorner2D>(std::make_shared<StateBlock>(feature_global_pose.head(2)),
- std::make_shared<StateBlock>(feature_global_pose.tail(1)),
- _feature_ptr->getMeasurement()(3));
-
- // Add landmark factor to the other feature
- _feature_other_ptr->addFactor(std::make_shared<FactorCorner2D>(_feature_other_ptr, landmark_ptr, shared_from_this()));
- }
-
-// std::cout << "creating factor: last feature " << _feature_ptr->getMeasurement()
-// << " with origin feature " << _feature_other_ptr->getMeasurement() << std::endl
-// << " corresponding to landmark " << landmark_ptr->id() << std::endl;
- return std::make_shared<FactorCorner2D>(_feature_ptr, landmark_ptr, shared_from_this());
-}
-
-void ProcessorTrackerFeatureCorner::extractCorners(CaptureLaser2DPtr _capture_laser_ptr,
- FeatureBasePtrList& _corner_list)
-{
- // TODO: sort corners by bearing
- std::list<laserscanutils::CornerPoint> corners;
-
- std::cout << "Extracting corners..." << std::endl;
- corner_finder_.findCorners(_capture_laser_ptr->getScan(),
- (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(),
- line_finder_,
- corners);
-
- Eigen::Vector4s measurement;
- for (auto corner : corners)
- {
- measurement.head<2>() = corner.point_.head<2>();
- measurement(2)=corner.orientation_;
- measurement(3)=corner.aperture_;
-
- _corner_list.push_back(std::make_shared<FeatureCorner2D>(measurement, corner.covariance_));
- }
-
-/* //variables
- std::list<laserscanutils::Corner> corners;
- //extract corners from range data
- laserscanutils::extractCorners(scan_params_, corner_alg_params_, _capture_laser_ptr->getRanges(), corners);
- //std::cout << corners.size() << " corners extracted" << std::endl;
- Eigen::Matrix4s measurement_cov;
- Eigen::Matrix3s R = Eigen::Matrix3s::Identity();
- Eigen::Vector4s measurement;
- for (auto corner : corners)
- {
- measurement.head(2) = corner.pt_.head(2);
- measurement(2) = corner.orientation_;
- measurement(3) = corner.aperture_;
- Scalar L1 = corner.line_1_.length();
- Scalar L2 = corner.line_2_.length();
- Scalar cov_angle_line1 = 12 * corner.line_1_.error_
- / (pow(L1, 2) * (pow(corner.line_1_.np_, 3) - pow(corner.line_1_.np_, 2)));
- Scalar cov_angle_line2 = 12 * corner.line_2_.error_
- / (pow(L2, 2) * (pow(corner.line_2_.np_, 3) - pow(corner.line_2_.np_, 2)));
- //init cov in corner coordinates
- measurement_cov << corner.line_1_.error_ + cov_angle_line1 * L1 * L1 / 4, 0, 0, 0, 0, corner.line_2_.error_
- + cov_angle_line2 * L2 * L2 / 4, 0, 0, 0, 0, cov_angle_line1 + cov_angle_line2, 0, 0, 0, 0, cov_angle_line1
- + cov_angle_line2;
- measurement_cov = 10 * measurement_cov;
- //std::cout << "New feature: " << meas.transpose() << std::endl;
- //std::cout << measurement_cov << std::endl;
- // Rotate covariance
- R.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(corner.orientation_).matrix();
- measurement_cov.topLeftCorner<3, 3>() = R.transpose() * measurement_cov.topLeftCorner<3, 3>() * R;
- //std::cout << "rotated covariance: " << std::endl;
- //std::cout << measurement_cov << std::endl;
- _corner_list.push_back(new FeatureCorner2D(measurement, measurement_cov));
- }*/
-}
-
-} // namespace wolf
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
deleted file mode 100644
index 3f6bec8fe828580d8b04e5b0c390a8441cc24a9e..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_feature_image.cpp
+++ /dev/null
@@ -1,399 +0,0 @@
-// Wolf includes
-#include "base/processor/processor_tracker_feature_image.h"
-
-// Vision utils
-#include <detectors.h>
-#include <descriptors.h>
-#include <matchers.h>
-#include <algorithms.h>
-
-// standard libs
-#include <bitset>
-#include <algorithm>
-
-namespace wolf
-{
-
-ProcessorTrackerFeatureImage::ProcessorTrackerFeatureImage(ProcessorParamsTrackerFeatureImagePtr _params_tracker_feature_image) :
- ProcessorTrackerFeature("IMAGE", _params_tracker_feature_image),
- cell_width_(0), cell_height_(0), // These will be initialized to proper values taken from the sensor via function configure()
- params_tracker_feature_image_(_params_tracker_feature_image)
-{
- // Detector
- std::string det_name = vision_utils::readYamlType(params_tracker_feature_image_->yaml_file_params_vision_utils, "detector");
- det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_tracker_feature_image_->yaml_file_params_vision_utils);
-
- if (det_name.compare("ORB") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorORB>(det_ptr_);
- else if (det_name.compare("FAST") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorFAST>(det_ptr_);
- else if (det_name.compare("SIFT") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSIFT>(det_ptr_);
- else if (det_name.compare("SURF") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSURF>(det_ptr_);
- else if (det_name.compare("BRISK") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorBRISK>(det_ptr_);
- else if (det_name.compare("MSER") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorMSER>(det_ptr_);
- else if (det_name.compare("GFTT") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorGFTT>(det_ptr_);
- else if (det_name.compare("HARRIS") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorHARRIS>(det_ptr_);
- else if (det_name.compare("SBD") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSBD>(det_ptr_);
- else if (det_name.compare("KAZE") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorKAZE>(det_ptr_);
- else if (det_name.compare("AKAZE") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorAKAZE>(det_ptr_);
- else if (det_name.compare("AGAST") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorAGAST>(det_ptr_);
-
- // Descriptor
- std::string des_name = vision_utils::readYamlType(params_tracker_feature_image_->yaml_file_params_vision_utils, "descriptor");
- des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_tracker_feature_image_->yaml_file_params_vision_utils);
-
- if (des_name.compare("ORB") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorORB>(des_ptr_);
- else if (des_name.compare("SIFT") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorSIFT>(des_ptr_);
- else if (des_name.compare("SURF") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorSURF>(des_ptr_);
- else if (des_name.compare("BRISK") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorBRISK>(des_ptr_);
- else if (des_name.compare("KAZE") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorKAZE>(des_ptr_);
- else if (des_name.compare("AKAZE") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorAKAZE>(des_ptr_);
- else if (des_name.compare("LATCH") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorLATCH>(des_ptr_);
- else if (des_name.compare("FREAK") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorFREAK>(des_ptr_);
- else if (des_name.compare("BRIEF") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorBRIEF>(des_ptr_);
- else if (des_name.compare("DAISY") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorDAISY>(des_ptr_);
- else if (des_name.compare("LUCID") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorLUCID>(des_ptr_);
-
- // Matcher
- std::string mat_name = vision_utils::readYamlType(params_tracker_feature_image_->yaml_file_params_vision_utils, "matcher");
- mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_tracker_feature_image_->yaml_file_params_vision_utils);
-
- if (mat_name.compare("FLANNBASED") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherFLANNBASED>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_L1") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_L1>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_HAMMING") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_HAMMING_2") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING_2>(mat_ptr_);
-
- // Active search grid
- vision_utils::AlgorithmBasePtr alg_ptr = vision_utils::setupAlgorithm("ACTIVESEARCH", "ACTIVESEARCH algorithm", params_tracker_feature_image_->yaml_file_params_vision_utils);
- active_search_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmACTIVESEARCH>(alg_ptr);
-}
-
-//Destructor
-ProcessorTrackerFeatureImage::~ProcessorTrackerFeatureImage()
-{
- //
-}
-
-void ProcessorTrackerFeatureImage::configure(SensorBasePtr _sensor)
-{
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(_sensor);
-
- image_.width_ = camera->getImgWidth();
- image_.height_ = camera->getImgHeight();
-
- active_search_ptr_->initAlg(camera->getImgWidth(), camera->getImgHeight() , det_ptr_->getPatternRadius());
-
- params_tracker_feature_image_activesearch_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmParamsACTIVESEARCH>( active_search_ptr_->getParams() );
-
- cell_width_ = image_.width_ / params_tracker_feature_image_activesearch_ptr_->n_cells_h;
- cell_height_ = image_.height_ / params_tracker_feature_image_activesearch_ptr_->n_cells_v;
-}
-
-void ProcessorTrackerFeatureImage::preProcess()
-{
- image_incoming_ = std::static_pointer_cast<CaptureImage>(incoming_ptr_)->getImage();
-
- active_search_ptr_->renew();
-
- //The visualization functions and variables
- if(last_ptr_ != nullptr)
- resetVisualizationFlag(last_ptr_->getFeatureList());
-
- tracker_roi_.clear();
- detector_roi_.clear();
- tracker_target_.clear();
-}
-
-void ProcessorTrackerFeatureImage::postProcess()
-{
-}
-
-unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_matches)
-{
- KeyPointVector candidate_keypoints;
- cv::Mat candidate_descriptors;
- DMatchVector cv_matches;
-
- for (auto feature_base_ptr : _features_last_in)
- {
- FeaturePointImagePtr feature_ptr = std::static_pointer_cast<FeaturePointImage>(feature_base_ptr);
-
- cv::Rect roi = vision_utils::setRoi(feature_ptr->getKeypoint().pt.x, feature_ptr->getKeypoint().pt.y, cell_width_, cell_height_);
-
- active_search_ptr_->hitCell(feature_ptr->getKeypoint());
-
- cv::Mat target_descriptor = feature_ptr->getDescriptor();
-
- //lists used to debug
- tracker_target_.push_back(feature_ptr->getKeypoint().pt);
- tracker_roi_.push_back(roi);
-
- if (detect(image_incoming_, roi, candidate_keypoints, candidate_descriptors))
- {
- Scalar normalized_score = match(target_descriptor,candidate_descriptors,cv_matches);
- if ( normalized_score > mat_ptr_->getParams()->min_norm_score )
- {
- FeaturePointImagePtr incoming_point_ptr = std::make_shared<FeaturePointImage>(
- candidate_keypoints[cv_matches[0].trainIdx],
- cv_matches[0].trainIdx,
- (candidate_descriptors.row(cv_matches[0].trainIdx)),
- Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
- incoming_point_ptr->setIsKnown(feature_ptr->isKnown());
- _features_incoming_out.push_back(incoming_point_ptr);
-
- _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_base_ptr, normalized_score}));
- }
- else
- {
- //lists used to debug
- tracker_target_.pop_back();
- tracker_roi_.pop_back();
- }
- }
- else
- {
- //lists used to debug
- tracker_target_.pop_back();
- tracker_roi_.pop_back();
- }
- }
-// std::cout << "TrackFeatures - Number of Features tracked: " << _features_incoming_out.size() << std::endl;
- return _features_incoming_out.size();
-}
-
-bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
-{
- DMatchVector matches_mat;
- FeaturePointImagePtr feat_incoming_ptr = std::static_pointer_cast<FeaturePointImage>(_incoming_feature);
- FeaturePointImagePtr feat_origin_ptr = std::static_pointer_cast<FeaturePointImage>(_origin_feature);
-
- cv::Mat origin_descriptor = feat_origin_ptr->getDescriptor();
- cv::Mat incoming_descriptor = feat_incoming_ptr->getDescriptor();
-
- KeyPointVector origin_keypoint;
- origin_keypoint.push_back(feat_origin_ptr->getKeypoint());
-
- Scalar normalized_score = match(origin_descriptor,incoming_descriptor,matches_mat);
-
- if(normalized_score > mat_ptr_->getParams()->min_norm_score)
- return true;
- else
- {
- /* CORRECT */
-
- unsigned int roi_width = cell_width_;
- unsigned int roi_heigth = cell_height_;
- unsigned int roi_x;
- unsigned int roi_y;
-
- KeyPointVector correction_keypoints;
- cv::Mat correction_descriptors;
- DMatchVector correction_matches;
-
- FeaturePointImagePtr feat_last_ptr = std::static_pointer_cast<FeaturePointImage>(_last_feature);
-
- active_search_ptr_->hitCell(feat_last_ptr->getKeypoint());
-
- roi_x = (feat_last_ptr->getKeypoint().pt.x) - (roi_heigth / 2);
- roi_y = (feat_last_ptr->getKeypoint().pt.y) - (roi_width / 2);
- cv::Rect roi(roi_x, roi_y, roi_width, roi_heigth);
-
- if (detect(image_incoming_, roi, correction_keypoints, correction_descriptors))
- {
- Scalar normalized_score_correction = match(origin_descriptor,correction_descriptors,correction_matches);
- if(normalized_score_correction > mat_ptr_->getParams()->min_norm_score )
- {
- feat_incoming_ptr->setKeypoint(correction_keypoints[correction_matches[0].trainIdx]);
- feat_incoming_ptr->setDescriptor(correction_descriptors.row(correction_matches[0].trainIdx));
- return true;
- }
- else
- {
- return false;
- }
- }
- return false;
- }
-}
-
-unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
-{
- cv::Rect roi;
- KeyPointVector new_keypoints;
- cv::Mat new_descriptors;
- cv::KeyPointsFilter keypoint_filter;
- unsigned int n_new_features = 0;
-
- for (unsigned int n_iterations = 0; _max_new_features == 0 || n_iterations < _max_new_features; n_iterations++)
- {
-
- if (active_search_ptr_->pickEmptyRoi(roi))
- {
- detector_roi_.push_back(roi);
- if (detect(image_last_, roi, new_keypoints, new_descriptors))
- {
- KeyPointVector list_keypoints = new_keypoints;
- unsigned int index = 0;
- keypoint_filter.retainBest(new_keypoints,1);
- for(unsigned int i = 0; i < list_keypoints.size(); i++)
- {
- if(list_keypoints[i].pt == new_keypoints[0].pt)
- index = i;
- }
- if(new_keypoints[0].response > params_tracker_feature_image_activesearch_ptr_->min_response_new_feature)
- {
- std::cout << "response: " << new_keypoints[0].response << std::endl;
- FeaturePointImagePtr point_ptr = std::make_shared<FeaturePointImage>(
- new_keypoints[0],
- 0,
- new_descriptors.row(index),
- Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
- point_ptr->setIsKnown(false);
- _features_incoming_out.push_back(point_ptr);
-
- active_search_ptr_->hitCell(new_keypoints[0]);
-
- n_new_features++;
- }
- }
- else
- {
- active_search_ptr_->blockCell(roi);
- }
- }
- else
- break;
- }
-
- WOLF_DEBUG( "DetectNewFeatures - Number of new features detected: " , n_new_features );
- return n_new_features;
-}
-
-//============================================================
-
-Scalar ProcessorTrackerFeatureImage::match(cv::Mat _target_descriptor, cv::Mat _candidate_descriptors, DMatchVector& _cv_matches)
-{
- mat_ptr_->match(_target_descriptor, _candidate_descriptors, des_ptr_->getSize(), _cv_matches);
- Scalar normalized_score = 1 - (Scalar)(_cv_matches[0].distance)/(des_ptr_->getSize()*8);
- return normalized_score;
-}
-
-FactorBasePtr ProcessorTrackerFeatureImage::createFactor(FeatureBasePtr _feature_ptr,
- FeatureBasePtr _feature_other_ptr)
-{
- FactorEpipolarPtr const_epipolar_ptr = std::make_shared<FactorEpipolar>(_feature_ptr, _feature_other_ptr,
- shared_from_this());
- // _feature_ptr->addFactor(const_epipolar_ptr);
- // _feature_other_ptr->addConstrainedBy(const_epipolar_ptr);
- return const_epipolar_ptr;
-}
-
-unsigned int ProcessorTrackerFeatureImage::detect(cv::Mat _image, cv::Rect& _roi, std::vector<cv::KeyPoint>& _new_keypoints,
- cv::Mat& _new_descriptors)
-{
- _new_keypoints = det_ptr_->detect(_image, _roi);
- _new_descriptors = des_ptr_->getDescriptor(_image, _new_keypoints);
- return _new_keypoints.size();
-}
-
-void ProcessorTrackerFeatureImage::resetVisualizationFlag(FeatureBasePtrList& _feature_list_last)
-{
- for (auto feature_base_last_ptr : _feature_list_last)
- {
- FeaturePointImagePtr feature_last_ptr = std::static_pointer_cast<FeaturePointImage>(feature_base_last_ptr);
- feature_last_ptr->setIsKnown(true);
- }
-}
-
-// draw functions ===================================================================
-
-void ProcessorTrackerFeatureImage::drawTarget(cv::Mat _image, std::list<cv::Point> _target_list)
-{
- if (last_ptr_!=nullptr)
- {
- // draw the target of the tracking
- for(auto target_point : _target_list)
- cv::circle(_image, target_point, 7, cv::Scalar(255.0, 0.0, 255.0), 1, 3, 0);
- }
-}
-
-void ProcessorTrackerFeatureImage::drawRoi(cv::Mat _image, std::list<cv::Rect> _roi_list, cv::Scalar _color)
-{
- if (last_ptr_!=nullptr)
- {
- for (auto roi : _roi_list)
- cv::rectangle(_image, roi, _color, 1, 8, 0);
- }
-}
-
-void ProcessorTrackerFeatureImage::drawFeatures(cv::Mat _image)
-{
- if (last_ptr_!=nullptr)
- {
- unsigned int known_feature_counter = 0;
- unsigned int new_feature_counter = 0;
-
- for (auto feature_ptr : last_ptr_->getFeatureList())
- {
- FeaturePointImagePtr point_ptr = std::static_pointer_cast<FeaturePointImage>(feature_ptr);
- if (point_ptr->isKnown())
- {
- cv::circle(_image, point_ptr->getKeypoint().pt, 4, cv::Scalar(51.0, 255.0, 51.0), -1, 3, 0);
- known_feature_counter++;
- }
- else
- {
- cv::circle(_image, point_ptr->getKeypoint().pt, 4, cv::Scalar(0.0, 0.0, 255.0), -1, 3, 0);
- new_feature_counter++;
- }
-
- cv::putText(_image, std::to_string(feature_ptr->trackId()), point_ptr->getKeypoint().pt,
- cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255.0, 255.0, 0.0));
- }
- std::cout << "\nKnown: " << known_feature_counter << "\tNew: " << new_feature_counter << std::endl;
- }
-}
-
-ProcessorBasePtr ProcessorTrackerFeatureImage::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sensor_ptr)
-{
- ProcessorTrackerFeatureImagePtr prc_ptr = std::make_shared<ProcessorTrackerFeatureImage>(std::static_pointer_cast<ProcessorParamsTrackerFeatureImage>(_params));
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("IMAGE FEATURE", ProcessorTrackerFeatureImage)
-} // namespace wolf
-
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
deleted file mode 100644
index 980c652af00ecb490250ac0baabb375ddf5ba98e..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ /dev/null
@@ -1,458 +0,0 @@
-
-// wolf
-#include "base/processor/processor_tracker_feature_trifocal.h"
-
-#include "base/sensor/sensor_camera.h"
-#include "base/feature/feature_point_image.h"
-#include "base/factor/factor_autodiff_trifocal.h"
-#include "base/capture/capture_image.h"
-
-// vision_utils
-#include <vision_utils.h>
-#include <detectors.h>
-#include <descriptors.h>
-#include <matchers.h>
-#include <algorithms.h>
-
-#include <memory>
-
-namespace wolf {
-
-//// DEBUG =====================================
-//debug_tTmp = clock();
-//WOLF_TRACE("======== DetectNewFeatures =========");
-//// ===========================================
-//
-//// DEBUG =====================================
-//debug_comp_time_ = (double)(clock() - debug_tTmp) / CLOCKS_PER_SEC;
-//WOLF_TRACE("--> TIME: Detect New Features: detect ",debug_comp_time_);
-//// ===========================================
-
-// Constructor
-ProcessorTrackerFeatureTrifocal::ProcessorTrackerFeatureTrifocal(ProcessorParamsTrackerFeatureTrifocalPtr _params_tracker_feature_trifocal) :
- ProcessorTrackerFeature("TRACKER FEATURE TRIFOCAL", _params_tracker_feature_trifocal ),
- params_tracker_feature_trifocal_(_params_tracker_feature_trifocal),
- capture_last_(nullptr),
- capture_incoming_(nullptr),
- prev_origin_ptr_(nullptr),
- initialized_(false)
-{
- assert(!(params_tracker_feature_trifocal_->yaml_file_params_vision_utils.empty()) && "Missing YAML file with vision_utils parameters!");
- assert(file_exists(params_tracker_feature_trifocal_->yaml_file_params_vision_utils) && "Cannot setup processor: vision_utils' YAML file does not exist.");
-
- // Detector
- std::string det_name = vision_utils::readYamlType(params_tracker_feature_trifocal_->yaml_file_params_vision_utils, "detector");
- det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_tracker_feature_trifocal_->yaml_file_params_vision_utils);
-
- if (det_name.compare("ORB") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorORB>(det_ptr_);
- else if (det_name.compare("FAST") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorFAST>(det_ptr_);
- else if (det_name.compare("SIFT") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSIFT>(det_ptr_);
- else if (det_name.compare("SURF") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSURF>(det_ptr_);
- else if (det_name.compare("BRISK") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorBRISK>(det_ptr_);
- else if (det_name.compare("MSER") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorMSER>(det_ptr_);
- else if (det_name.compare("GFTT") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorGFTT>(det_ptr_);
- else if (det_name.compare("HARRIS") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorHARRIS>(det_ptr_);
- else if (det_name.compare("SBD") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSBD>(det_ptr_);
- else if (det_name.compare("KAZE") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorKAZE>(det_ptr_);
- else if (det_name.compare("AKAZE") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorAKAZE>(det_ptr_);
- else if (det_name.compare("AGAST") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorAGAST>(det_ptr_);
-
- // Descriptor
- std::string des_name = vision_utils::readYamlType(params_tracker_feature_trifocal_->yaml_file_params_vision_utils, "descriptor");
- des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_tracker_feature_trifocal_->yaml_file_params_vision_utils);
-
- if (des_name.compare("ORB") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorORB>(des_ptr_);
- else if (des_name.compare("SIFT") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorSIFT>(des_ptr_);
- else if (des_name.compare("SURF") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorSURF>(des_ptr_);
- else if (des_name.compare("BRISK") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorBRISK>(des_ptr_);
- else if (des_name.compare("KAZE") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorKAZE>(des_ptr_);
- else if (des_name.compare("AKAZE") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorAKAZE>(des_ptr_);
- else if (des_name.compare("LATCH") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorLATCH>(des_ptr_);
- else if (des_name.compare("FREAK") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorFREAK>(des_ptr_);
- else if (des_name.compare("BRIEF") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorBRIEF>(des_ptr_);
- else if (des_name.compare("DAISY") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorDAISY>(des_ptr_);
- else if (des_name.compare("LUCID") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorLUCID>(des_ptr_);
-
- // Matcher
- std::string mat_name = vision_utils::readYamlType(params_tracker_feature_trifocal_->yaml_file_params_vision_utils, "matcher");
- mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_tracker_feature_trifocal_->yaml_file_params_vision_utils);
-
- if (mat_name.compare("FLANNBASED") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherFLANNBASED>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_L1") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_L1>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_HAMMING") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_HAMMING_2") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING_2>(mat_ptr_);
-
- // DEBUG VIEW
- cv::startWindowThread();
- cv::namedWindow("DEBUG VIEW", cv::WINDOW_NORMAL);
-}
-
-// Destructor
-ProcessorTrackerFeatureTrifocal::~ProcessorTrackerFeatureTrifocal()
-{
-// cv::destroyAllWindows();
-}
-
-bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, const cv::KeyPoint& _kp_incoming)
-{
- // List of conditions
- bool inlier = true;
-
- // A. Check euclidean norm
- inlier = inlier && (cv::norm(_kp_incoming.pt-_kp_last.pt) < params_tracker_feature_trifocal_->max_euclidean_distance);
-
- return inlier;
-}
-
-
-unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
-{
-// // DEBUG =====================================
-// clock_t debug_tStart;
-// double debug_comp_time_;
-// debug_tStart = clock();
-// WOLF_TRACE("======== DetectNewFeatures =========");
-// // ===========================================
-
- for (unsigned int n_iterations = 0; n_iterations < _max_new_features; ++n_iterations)
- {
- Eigen::Vector2i cell_last;
- if (capture_last_->grid_features_->pickEmptyTrackingCell(cell_last))
- {
- // Get best keypoint in cell
- vision_utils::FeatureIdxMap cell_feat_map = capture_last_->grid_features_->getFeatureIdxMap(cell_last(0), cell_last(1));
- bool found_feature_in_cell = false;
-
- for (auto target_last_pair_response_idx : cell_feat_map)
- {
- // Get last KeyPoint
- unsigned int index_last = target_last_pair_response_idx.second;
- cv::KeyPoint kp_last = capture_last_->keypoints_.at(index_last);
- assert(target_last_pair_response_idx.first == kp_last.response && "[ProcessorTrackerFeatureTrifocal::detectNewFeatures]: response mismatch.");
-
- // Check if there is match with incoming, if not we do not want it
- if (capture_last_->map_index_to_next_.count(index_last))
- {
- unsigned int index_incoming = capture_last_->map_index_to_next_[index_last];
- cv::KeyPoint kp_incoming = capture_incoming_->keypoints_.at(index_incoming);
-
- if (isInlier( kp_incoming, kp_last))
- {
- // Create WOLF feature
- FeaturePointImagePtr last_point_ptr = std::make_shared<FeaturePointImage>(
- kp_last,
- index_last,
- capture_last_->descriptors_.row(index_last),
- Eigen::Matrix2s::Identity() * params_tracker_feature_trifocal_->pixel_noise_std * params_tracker_feature_trifocal_->pixel_noise_std);
-
- _features_incoming_out.push_back(last_point_ptr);
-
- // hit cell to acknowledge there's a tracked point in that cell
- capture_last_->grid_features_->hitTrackingCell(kp_last);
-
- found_feature_in_cell = true;
-
- break; // Good kp found for this grid.
- }
- }
- }
- if (!found_feature_in_cell)
- capture_last_->grid_features_->blockTrackingCell(cell_last);
- }
- else
- break; // There are no empty cells
- }
-
- WOLF_TRACE("DetectNewFeatures - Number of new features detected: " , _features_incoming_out.size() );
-
-// // DEBUG
-// debug_comp_time_ = (double)(clock() - debug_tStart) / CLOCKS_PER_SEC;
-// WOLF_TRACE("--> TIME: detect new features: TOTAL ",debug_comp_time_);
-// WOLF_TRACE("======== ========= =========");
-
- return _features_incoming_out.size();
-}
-
-unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_matches)
-{
-// // DEBUG =====================================
-// clock_t debug_tStart;
-// double debug_comp_time_;
-// debug_tStart = clock();
-// WOLF_TRACE("======== TrackFeatures =========");
-// // ===========================================
-
- for (auto feature_base_last_ : _features_last_in)
- {
- FeaturePointImagePtr feature_last_ = std::static_pointer_cast<FeaturePointImage>(feature_base_last_);
-
- if ( capture_last_->map_index_to_next_.count(feature_last_->getIndexKeyPoint()) )
- {
- int index_kp_incoming = capture_last_->map_index_to_next_[feature_last_->getIndexKeyPoint()];
-
- if (capture_incoming_->matches_normalized_scores_.at(index_kp_incoming) > mat_ptr_->getParams()->min_norm_score )
- {
- // Check Euclidean distance between keypoints
- cv::KeyPoint kp_incoming = capture_incoming_->keypoints_.at(index_kp_incoming);
- cv::KeyPoint kp_last = capture_last_->keypoints_.at(feature_last_->getIndexKeyPoint());
-
- if (isInlier(kp_last, kp_incoming))
- {
- FeaturePointImagePtr incoming_point_ptr = std::make_shared<FeaturePointImage>(
- kp_incoming,
- index_kp_incoming,
- capture_incoming_->descriptors_.row(index_kp_incoming),
- Eigen::Matrix2s::Identity() * params_tracker_feature_trifocal_->pixel_noise_std * params_tracker_feature_trifocal_->pixel_noise_std);
-
- _features_incoming_out.push_back(incoming_point_ptr);
-
- _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_last_, capture_incoming_->matches_normalized_scores_.at(index_kp_incoming)}));
-
- // hit cell to acknowledge there's a tracked point in that cell
- capture_incoming_->grid_features_->hitTrackingCell(kp_incoming);
- }
- }
- }
- }
-
-// // DEBUG
-// debug_comp_time_ = (double)(clock() - debug_tStart) / CLOCKS_PER_SEC;
-// WOLF_TRACE("--> TIME: track: ",debug_comp_time_);
-// WOLF_TRACE("======== ========= =========");
-
- WOLF_TRACE("TrAckFeatures - Number of features tracked: " , _features_incoming_out.size() );
-
- return _features_incoming_out.size();
-}
-
-bool ProcessorTrackerFeatureTrifocal::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
-{
- return true;
-}
-
-bool ProcessorTrackerFeatureTrifocal::voteForKeyFrame()
-{
- // A. crossing voting threshold with ascending number of features
- bool vote_up = true;
- // 1. vote if we did not have enough features before
- vote_up = vote_up && (previousNumberOfTracks() < params_tracker_feature_trifocal_->min_features_for_keyframe);
- // 2. vote if we have enough features now
- vote_up = vote_up && (incoming_ptr_->getFeatureList().size() >= params_tracker_feature_trifocal_->min_features_for_keyframe);
-
- // B. crossing voting threshold with descending number of features
- bool vote_down = true;
- // 1. vote if we had enough features before
- vote_down = vote_down && (last_ptr_->getFeatureList().size() >= params_tracker_feature_trifocal_->min_features_for_keyframe);
- // 2. vote if we have not enough features now
- vote_down = vote_down && (incoming_ptr_->getFeatureList().size() < params_tracker_feature_trifocal_->min_features_for_keyframe);
-
- if (vote_up)
- WOLF_TRACE("VOTE UP");
- if (vote_down)
- WOLF_TRACE("VOTE DOWN");
-
- return vote_up || vote_down;
-}
-
-void ProcessorTrackerFeatureTrifocal::advanceDerived()
-{
- ProcessorTrackerFeature::advanceDerived();
-}
-
-void ProcessorTrackerFeatureTrifocal::resetDerived()
-{
- // Call parent class method
- ProcessorTrackerFeature::resetDerived();
-
- // Conditionally assign the prev_origin pointer
- if (initialized_)
- prev_origin_ptr_ = origin_ptr_;
-
-// // Print resulting list
-// TrackMatches matches_prevorig_origin = track_matrix_.matches(prev_origin_ptr_, origin_ptr_);
-// for (auto const & pair_trkid_pair : matches_prevorig_origin)
-// {
-// FeatureBasePtr feature_in_prev = pair_trkid_pair.second.first;
-// FeatureBasePtr feature_in_origin = pair_trkid_pair.second.second;
-//
-// WOLF_DEBUG("Matches reset prev <-- orig: track: ", pair_trkid_pair.first,
-// " prev: ", feature_in_prev->id(),
-// " origin: ", feature_in_origin->id());
-// }
-}
-
-void ProcessorTrackerFeatureTrifocal::preProcess()
-{
- WOLF_TRACE("-------- Image ", getIncoming()->id(), " -- t = ", getIncoming()->getTimeStamp(), " s ----------");
-
- if (!initialized_)
- {
- if (origin_ptr_ && last_ptr_ && (last_ptr_ != origin_ptr_) && prev_origin_ptr_ == nullptr)
- prev_origin_ptr_ = origin_ptr_;
-
- if (prev_origin_ptr_ && origin_ptr_ && last_ptr_ && prev_origin_ptr_ != origin_ptr_)
- initialized_ = true;
- }
-
- // Get capture
- capture_incoming_ = std::static_pointer_cast<CaptureImage>(incoming_ptr_);
-
- // Detect INC KeyPoints
- capture_incoming_->keypoints_ = det_ptr_->detect(capture_incoming_->getImage());
-
- // Get INC descriptors
- capture_incoming_->descriptors_ = des_ptr_->getDescriptor(capture_incoming_->getImage(), capture_incoming_->keypoints_);
-
- // Create and fill incoming grid
- capture_incoming_->grid_features_ = std::make_shared<vision_utils::FeatureIdxGrid>(capture_incoming_->getImage().rows, capture_incoming_->getImage().cols, params_tracker_feature_trifocal_->n_cells_v, params_tracker_feature_trifocal_->n_cells_h);
-
- capture_incoming_->grid_features_->insert(capture_incoming_->keypoints_);
-
- // If last_ptr_ is not null, then we can do some computation here.
- if (last_ptr_ != nullptr)
- {
- // Get capture
- capture_last_ = std::static_pointer_cast<CaptureImage>(last_ptr_);
-
- // Match full image (faster)
- // We exchange the order of the descriptors to fill better the map hereafter (map does not allow a single key)
- DMatchVector matches_incoming_from_last;
-
- capture_incoming_->matches_normalized_scores_ = mat_ptr_->robustMatch(capture_incoming_->keypoints_,
- capture_last_->keypoints_,
- capture_incoming_->descriptors_,
- capture_last_->descriptors_,
- des_ptr_->getSize(),
- matches_incoming_from_last);
-
- capture_incoming_->matches_from_precedent_ = matches_incoming_from_last;
-
- // Set capture map of match indices
- for (auto match : capture_incoming_->matches_from_precedent_)
- capture_last_->map_index_to_next_[match.trainIdx] = match.queryIdx; // map[last] = incoming
- }
-}
-
-void ProcessorTrackerFeatureTrifocal::postProcess()
-{
- // DEBUG
- std::vector<vision_utils::KeyPointEnhanced> kps_e;
-
- for (auto const & feat_base : last_ptr_->getFeatureList())
- {
- FeaturePointImagePtr feat = std::static_pointer_cast<FeaturePointImage>(feat_base);
- unsigned int id = feat->id();
- unsigned int track_id = feat->trackId();
- vision_utils::KeyPointEnhanced kp_e(feat->getKeypoint(), id, track_id, track_matrix_.trackSize(track_id), feat->getMeasurementCovariance());
- kps_e.push_back(kp_e);
- }
-
- // DEBUG
- cv::Mat img = (std::static_pointer_cast<CaptureImage>(last_ptr_))->getImage();
- cv::Mat img_proc = vision_utils::buildImageProcessed(img, kps_e);
-
- cv::imshow("DEBUG VIEW", img_proc);
- cv::waitKey(1);
-}
-
-FactorBasePtr ProcessorTrackerFeatureTrifocal::createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
-{
- // NOTE: This function cannot be implemented because the API lacks an input to feature_prev_origin.
- // Therefore, we implement establishFactors() instead and do all the job there.
- // This function remains empty, and with a warning or even an error issued in case someone calls it.
- std::cout << "033[1;33m [WARN]:033[0m ProcessorTrackerFeatureTrifocal::createFactor is empty." << std::endl;
- FactorBasePtr return_var{}; //TODO: fill this variable
- return return_var;
-}
-
-void ProcessorTrackerFeatureTrifocal::establishFactors()
-{
- if (initialized_)
- {
- // get tracks between prev, origin and last
- TrackMatches matches = track_matrix_.matches(prev_origin_ptr_, last_ptr_); // it's guaranteed by construction that the track also includes origin
-
- for (auto pair_trkid_match : matches) // OMG this will add potentially a loooot of factors! TODO see a smarter way of adding factors
- { // Currently reduced by creating factors for large tracks
- // get track ID
- SizeStd trk_id = pair_trkid_match.first;
-
- if (track_matrix_.trackSize(trk_id)>params_tracker_feature_trifocal_->min_track_length_for_factor)
- {
- // get the three features for this track
- // FeatureBasePtr ftr_prev = track_matrix_.feature(trk_id, prev_origin_ptr_); // left here for ref, but implemented in a quicker way below
- // FeatureBasePtr ftr_last = track_matrix_.feature(trk_id, last_ptr_); // same here
- FeatureBasePtr ftr_prev = pair_trkid_match.second.first;
- FeatureBasePtr ftr_orig = track_matrix_.feature(trk_id, origin_ptr_); // because it's a tracker, this feature in the middle of prev and last exists for sure!
- FeatureBasePtr ftr_last = pair_trkid_match.second.second;
-
- // make factor
- FactorAutodiffTrifocalPtr ctr = std::make_shared<FactorAutodiffTrifocal>(ftr_prev, ftr_orig, ftr_last, shared_from_this(), false, FAC_ACTIVE);
-
- // link to wolf tree
- ftr_last->addFactor(ctr);
- ftr_orig->addConstrainedBy(ctr);
- ftr_prev->addConstrainedBy(ctr);
- }
- }
- }
-}
-
-void ProcessorTrackerFeatureTrifocal::setParams(const ProcessorParamsTrackerFeatureTrifocalPtr _params)
-{
- params_tracker_feature_trifocal_ = _params;
-}
-
-void ProcessorTrackerFeatureTrifocal::configure(SensorBasePtr _sensor)
-{
- _sensor->setNoiseStd(Vector2s::Ones() * params_tracker_feature_trifocal_->pixel_noise_std);
-}
-
-ProcessorBasePtr ProcessorTrackerFeatureTrifocal::create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr _sensor_ptr)
-{
- const auto params = std::static_pointer_cast<ProcessorParamsTrackerFeatureTrifocal>(_params);
-
- ProcessorBasePtr prc_ptr = std::make_shared<ProcessorTrackerFeatureTrifocal>(params);
- prc_ptr->setName(_unique_name);
- prc_ptr->configure(_sensor_ptr);
- return prc_ptr;
-}
-
-} // namespace wolf
-
-// Register in the ProcessorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("TRACKER FEATURE TRIFOCAL", ProcessorTrackerFeatureTrifocal)
-} // namespace wolf
diff --git a/src/processor/processor_tracker_landmark_apriltag.cpp b/src/processor/processor_tracker_landmark_apriltag.cpp
deleted file mode 100644
index 27f6be72295c60e01ccce2dbacdaaeae5f215068..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_landmark_apriltag.cpp
+++ /dev/null
@@ -1,867 +0,0 @@
-#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/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp
deleted file mode 100644
index aeba30f8e908a72a57aad5b5f617edfa249b8982..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_landmark_corner.cpp
+++ /dev/null
@@ -1,428 +0,0 @@
-#include "base/processor/processor_tracker_landmark_corner.h"
-#include "base/rotations.h"
-
-namespace wolf
-{
-
-void ProcessorTrackerLandmarkCorner::preProcess()
-{
- // extract corners of incoming
- extractCorners(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_), corners_incoming_);
-
- // compute transformations
- t_world_robot_ = getProblem()->getState(incoming_ptr_->getTimeStamp());
-
- // world_robot
- Eigen::Matrix3s R_world_robot = Eigen::Matrix3s::Identity();
- R_world_robot.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_world_robot_(2)).matrix();
-
- // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
- || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
- {
- t_robot_sensor_.head<2>() = getSensor()->getP()->getState();
- t_robot_sensor_(2) = getSensor()->getO()->getState()(0);
- R_robot_sensor_.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_robot_sensor_(2)).matrix();
- extrinsics_transformation_computed_ = true;
- }
-
- // global_sensor
- R_world_sensor_.topLeftCorner<2, 2>() = R_world_robot.topLeftCorner<2, 2>() * R_robot_sensor_.topLeftCorner<2, 2>();
- t_world_sensor_ = t_world_robot_ + R_world_robot * t_robot_sensor_;
-
- // sensor_global
- R_sensor_world_.topLeftCorner<2, 2>() = R_robot_sensor_.topLeftCorner<2, 2>().transpose() * R_world_robot.topLeftCorner<2, 2>().transpose();
- t_sensor_world_ = -R_sensor_world_ * t_world_robot_ - R_robot_sensor_.transpose() * t_robot_sensor_;
-
- //std::cout << "t_robot_sensor_" << t_robot_sensor_.transpose() << std::endl;
- //std::cout << "t_world_robot_" << t_world_robot_.transpose() << std::endl;
- //std::cout << "t_world_sensor_" << t_world_sensor_.transpose() << std::endl;
- //std::cout << "PreProcess: incoming new features: " << corners_incoming_.size() << std::endl;
-}
-
-unsigned int ProcessorTrackerLandmarkCorner::findLandmarks(const LandmarkBasePtrList& _landmarks_corner_searched,
- FeatureBasePtrList& _features_corner_found,
- LandmarkMatchMap& _feature_landmark_correspondences)
-{
- //std::cout << "ProcessorTrackerLandmarkCorner::findLandmarks: " << _landmarks_corner_searched.size() << " features: " << corners_incoming_.size() << std::endl;
-
- // NAIVE FIRST NEAREST NEIGHBOR MATCHING
- LandmarkBasePtrList not_matched_landmarks = _landmarks_corner_searched;
- Scalar dm2;
-
- // COMPUTING ALL EXPECTED FEATURES
- std::map<LandmarkBasePtr, Eigen::Vector4s, std::less<LandmarkBasePtr>, Eigen::aligned_allocator<std::pair<LandmarkBasePtr, Eigen::Vector4s> > > expected_features;
- std::map<LandmarkBasePtr, Eigen::Matrix3s, std::less<LandmarkBasePtr>, Eigen::aligned_allocator<std::pair<LandmarkBasePtr, Eigen::Matrix3s> > > expected_features_covs;
- for (auto landmark : not_matched_landmarks)
- expectedFeature(landmark, expected_features[landmark], expected_features_covs[landmark]);
-
- auto next_feature_it = corners_incoming_.begin();
- auto feature_it = next_feature_it++;
- while (feature_it != corners_incoming_.end())
- {
- LandmarkBaseIter closest_landmark = not_matched_landmarks.end();
- Scalar closest_dm2 = 1e3;
- for (auto landmark_it = not_matched_landmarks.begin(); landmark_it != not_matched_landmarks.end(); landmark_it++)
- {
- if ((*landmark_it)->getType() == "CORNER 2D" &&
- fabs(pi2pi((std::static_pointer_cast<FeatureCorner2D>(*feature_it))->getAperture() - (*landmark_it)->getDescriptor(0))) < aperture_error_th_)
- {
- dm2 = computeSquaredMahalanobisDistances((*feature_it), expected_features[*landmark_it],
- expected_features_covs[*landmark_it],
- Eigen::MatrixXs::Zero(3, 1))(0); //Mahalanobis squared
- if (dm2 < 0.5 && (closest_landmark == not_matched_landmarks.end() || dm2 < closest_dm2))
- {
- //std::cout << "pair feature " << (*feature_it)->id() << " & landmark " << (*landmark_it)->id() << std::endl;
- //std::cout << "pair SqMahalanobisDist = " << dm2 << std::endl;
- closest_dm2 = dm2;
- closest_landmark = landmark_it;
- }
- }
- }
- //std::cout << "all landmarks checked with feature " << (*feature_it)->id() << std::endl;
- if (closest_landmark != not_matched_landmarks.end())
- {
- //std::cout << "closest landmark: " << (*closest_landmark)->id() << std::endl;
- // match
- matches_landmark_from_incoming_[*feature_it] = std::make_shared<LandmarkMatch>(*closest_landmark, closest_dm2);
- // erase from the landmarks to be found
- not_matched_landmarks.erase(closest_landmark);
- // move corner feature to output list
- _features_corner_found.splice(_features_corner_found.end(), corners_incoming_, feature_it);
- }
- //else
- // std::cout << "no landmark close enough found." << std::endl;
- feature_it = next_feature_it++;
- }
-
-/*
- // MATCHING FROM MAP
- if (!corners_incoming_.empty() && !_landmarks_corner_searched.empty())
- {
- //local declarations
- Scalar prob, dm2;
- unsigned int ii, jj;
-
- // COMPUTING ALL EXPECTED FEATURES
- std::map<LandmarkBasePtr, Eigen::Vector4s, std::less<LandmarkBasePtr>, Eigen::aligned_allocator<std::pair<LandmarkBasePtr, Eigen::Vector4s> > > expected_features;
- std::map<LandmarkBasePtr, Eigen::Matrix3s, std::less<LandmarkBasePtr>, Eigen::aligned_allocator<std::pair<LandmarkBasePtr, Eigen::Matrix3s> > > expected_features_covs;
- for (auto landmark : _landmarks_corner_searched)
- expectedFeature(landmark, expected_features[landmark], expected_features_covs[landmark]);
- //std::cout << "expected features!" << std::endl;
-
- // SETTING ASSOCIATION TREE
- std::map<unsigned int, FeatureBaseIter> features_map;
- std::map<unsigned int, LandmarkBaseIter> landmarks_map;
- std::map<unsigned int, unsigned int> landmarks_index_map;
- //tree object allocation and sizing
- AssociationTree tree;
- tree.resize(corners_incoming_.size(), _landmarks_corner_searched.size());
- //set independent probabilities between feature-landmark pairs
- ii = 0;
- for (auto feature_it = corners_incoming_.begin(); feature_it != corners_incoming_.end();
- feature_it++, ii++) //ii runs over extracted feature
- {
- features_map[ii] = feature_it;
- //std::cout << "Feature: " << (*i_it)->id() << std::endl << (*i_it)->getMeasurement().head(3).transpose() << std::endl;
- jj = 0;
- for (auto landmark_it = _landmarks_corner_searched.begin(); landmark_it != _landmarks_corner_searched.end();
- landmark_it++, jj++)
- {
- if ((*landmark_it)->getType() == "CORNER 2D")
- {
- landmarks_map[jj] = landmark_it;
- landmarks_index_map[jj] = 0;
- //std::cout << "Landmark: " << (*j_it)->id() << " - jj: " << jj << std::endl;
- //If aperture difference is small enough, proceed with Mahalanobis distance. Otherwise Set prob to 0 to force unassociation
- if (fabs(
- pi2pi(((FeatureCorner2D*)(((*feature_it))))->getAperture()
- - (*landmark_it)->getDescriptor(0))) < aperture_error_th_)
- {
- dm2 = computeSquaredMahalanobisDistances(*feature_it, expected_features[*landmark_it],
- expected_features_covs[*landmark_it],
- Eigen::MatrixXs::Zero(3, 1))(0); //Mahalanobis squared
- //prob = (dm2 < 5 * 5 ? 5 * erfc(sqrt(dm2 / 2)) : 0); //prob = erfc( sqrt(dm2/2) ); //prob = erfc( sqrt(dm2)/1.4142136 );// sqrt(2) = 1.4142136
- prob = 10*erfc( sqrt(dm2/2) );
- if (dm2 < 0.5)
- {
- std::cout << "pair feature " << (*features_map[ii])->id() << " & landmark " << (*landmarks_map[jj])->id() << std::endl;
- std::cout << "pair SqMahalanobisDist = " << dm2 << std::endl;
- std::cout << "pair probability = " << prob << std::endl;
-
- }
- tree.setScore(ii, jj, prob);
- }
- else
- tree.setScore(ii, jj, 0.); //prob to 0
- }
- }
- }
- // Grows tree and make association pairs
- std::map<unsigned int, unsigned int> ft_lk_pairs;
- std::vector<bool> associated_mask;
- //std::cout << "solving tree" << std::endl;
- tree.solve(ft_lk_pairs, associated_mask);
- //print tree & score table
- //std::cout << "------------- TREE SOLVED ---------" << std::endl;
- //std::cout << corners_incoming_.size() << " new corners in incoming:" << std::endl;
- //for (auto new_feature : corners_incoming_)
- // std::cout << "\tcorner " << new_feature->id() << std::endl;
- std::cout << ft_lk_pairs.size() << " pairs:" << std::endl;
- for (auto pair : ft_lk_pairs)
- std::cout << "\tfeature " << (*features_map[pair.first])->id() << " & landmark " << (*landmarks_map[pair.second])->id() << std::endl;
- //tree.printTree();
- //tree.printScoreTable();
- // Vector of new landmarks to be created
- std::vector<FeatureCorner2D*> new_corner_landmarks(0);
-
- // ESTABLISH CORRESPONDENCES
- for (auto pair : ft_lk_pairs)
- {
- // match
- matches_landmark_from_incoming_[*features_map[pair.first]] = new LandmarkMatch(
- *landmarks_map[pair.second], tree.getScore(pair.first, pair.second));
- // move matched feature to list
- _features_corner_found.splice(_features_corner_found.end(), corners_incoming_, features_map[pair.first]);
- }
- //std::cout << corners_incoming_.size() << " remaining new corners in incoming:" << std::endl;
- //for (auto new_feature : corners_incoming_)
- // std::cout << "\tcorner " << new_feature->id() << std::endl;
- }
- else
- std::cout << "0 corners incoming or 0 landmarks to search" <<std::endl;
-*/
- return matches_landmark_from_incoming_.size();
-}
-
-bool ProcessorTrackerLandmarkCorner::voteForKeyFrame()
-{
- // option 1: more than TH new features in last
- if (corners_last_.size() >= new_corners_th_)
- {
- std::cout << "------------- NEW KEY FRAME: Option 1 - Enough new features" << std::endl;
- //std::cout << "\tnew features in last = " << corners_last_.size() << std::endl;
- return true;
- }
- // option 2: loop closure (if the newest frame from which a matched landmark was observed is old enough)
- for (auto new_feat : new_features_last_)
- {
- if (last_ptr_->getFrame()->id() - matches_landmark_from_last_[new_feat]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getFrame()->id() > loop_frames_th_)
- {
- std::cout << "------------- NEW KEY FRAME: Option 2 - Loop closure" << std::endl;
- return true;
- }
- }
- //// option 3: less than half matched in origin, matched in incoming (more than half in last)
- //if (matches_landmark_from_incoming_.size()*2 < origin_ptr_->getFeatureList().size() && matches_landmark_from_last_.size()*2 > origin_ptr_->getFeatureList().size())
- //{
- // std::cout << "------------- NEW KEY FRAME: Option 3 - " << std::endl;
- // //std::cout << "\tmatches in incoming = " << matches_landmark_from_incoming_.size() << std::endl<< "\tmatches in origin = " << origin_ptr_->getFeatureList().size() << std::endl;
- // return true;
- //}
- return false;
-}
-
-void ProcessorTrackerLandmarkCorner::extractCorners(CaptureLaser2DPtr _capture_laser_ptr,
- FeatureBasePtrList& _corner_list)
-{
- // TODO: sort corners by bearing
- std::list<laserscanutils::CornerPoint> corners;
-
- corner_finder_.findCorners(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), line_finder_, corners);
-
- Eigen::Vector4s measurement;
- for (auto corner : corners)
- {
- measurement.head<2>() = corner.point_.head<2>();
- measurement(2)=corner.orientation_;
- measurement(3)=corner.aperture_;
-
- _corner_list.push_back(std::make_shared<FeatureCorner2D>(measurement, corner.covariance_));
-
- //std::cout << "new corner detected: " << measurement.head<3>().transpose() << std::endl;
- }
-
-/* //variables
- std::list<laserscanutils::Corner> corners;
- //extract corners from range data
- laserscanutils::extractCorners(scan_params_, corner_alg_params_, _capture_laser_ptr->getRanges(), corners);
- //std::cout << corners.size() << " corners extracted" << std::endl;
- Eigen::Matrix4s measurement_cov;
- Eigen::Matrix3s R = Eigen::Matrix3s::Identity();
- Eigen::Vector4s measurement;
- for (auto corner : corners)
- {
- measurement.head(2) = corner.pt_.head(2);
- measurement(2) = corner.orientation_;
- measurement(3) = corner.aperture_;
- // TODO: maybe in line object?
- Scalar L1 = corner.line_1_.length();
- Scalar L2 = corner.line_2_.length();
- Scalar cov_angle_line1 = 12 * corner.line_1_.error_
- / (pow(L1, 2) * (pow(corner.line_1_.np_, 3) - pow(corner.line_1_.np_, 2)));
- Scalar cov_angle_line2 = 12 * corner.line_2_.error_
- / (pow(L2, 2) * (pow(corner.line_2_.np_, 3) - pow(corner.line_2_.np_, 2)));
- //init cov in corner coordinates
- measurement_cov << corner.line_1_.error_ + cov_angle_line1 * L1 * L1 / 4, 0, 0, 0, 0, corner.line_2_.error_
- + cov_angle_line2 * L2 * L2 / 4, 0, 0, 0, 0, cov_angle_line1 + cov_angle_line2, 0, 0, 0, 0, cov_angle_line1
- + cov_angle_line2;
- measurement_cov = 10 * measurement_cov;
- //std::cout << "New feature: " << meas.transpose() << std::endl;
- //std::cout << measurement_cov << std::endl;
- // Rotate covariance
- R.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(corner.orientation_).matrix();
- measurement_cov.topLeftCorner<3, 3>() = R.transpose() * measurement_cov.topLeftCorner<3, 3>() * R;
- //std::cout << "rotated covariance: " << std::endl;
- //std::cout << measurement_cov << std::endl;
- _corner_list.push_back(new FeatureCorner2D(measurement, measurement_cov));
- }*/
-}
-
-void ProcessorTrackerLandmarkCorner::expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::Vector4s& expected_feature_,
- Eigen::Matrix3s& expected_feature_cov_)
-{
- //std::cout << "ProcessorTrackerLandmarkCorner::expectedFeature: " << std::endl;
- //std::cout << "Landmark global pose: " << _landmark_ptr->getP()->getVector().transpose() << " " << _landmark_ptr->getO()->getVector() << std::endl;
- //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
- //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
-
- // ------------ expected feature measurement
- expected_feature_.head<2>() = R_sensor_world_.topLeftCorner<2,2>() * (_landmark_ptr->getP()->getState() - t_world_sensor_.head<2>());
- expected_feature_(2) = _landmark_ptr->getO()->getState()(0) - t_world_sensor_(2);
- expected_feature_(3) = (std::static_pointer_cast<LandmarkCorner2D>(_landmark_ptr))->getAperture();
- //std::cout << "Expected feature pose: " << expected_feature_.head<3>().transpose() << std::endl;
-
- // ------------ expected feature covariance
- Eigen::MatrixXs Sigma = Eigen::MatrixXs::Zero(6, 6);
- // closer keyframe with computed covariance
- FrameBasePtr key_frame_ptr = (origin_ptr_ != nullptr ? origin_ptr_->getFrame() : nullptr);
- // If all covariance blocks are stored wolfproblem (filling upper diagonal only)
- if (key_frame_ptr != nullptr &&
- // Sigma_rr
- getProblem()->getCovarianceBlock(key_frame_ptr->getP(), key_frame_ptr->getP(), Sigma, 3, 3) &&
- getProblem()->getCovarianceBlock(key_frame_ptr->getP(), key_frame_ptr->getO(), Sigma, 3, 5) &&
- getProblem()->getCovarianceBlock(key_frame_ptr->getO(), key_frame_ptr->getO(), Sigma, 5, 5) &&
- // Sigma_ll
- getProblem()->getCovarianceBlock(_landmark_ptr->getP(), _landmark_ptr->getP(), Sigma, 0, 0) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getP(), _landmark_ptr->getO(), Sigma, 0, 2) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getO(), _landmark_ptr->getO(), Sigma, 2, 2) &&
- // Sigma_lr
- getProblem()->getCovarianceBlock(_landmark_ptr->getP(), key_frame_ptr->getP(), Sigma, 0, 3) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getP(), key_frame_ptr->getO(), Sigma, 0, 5) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getO(), key_frame_ptr->getP(), Sigma, 2, 3) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getO(), key_frame_ptr->getO(), Sigma, 2, 5) )
- {
- // Jacobian
- Eigen::Vector2s p_robot_landmark = t_world_robot_.head<2>() - _landmark_ptr->getP()->getState();
- Eigen::Matrix<Scalar, 3, 6> Jlr = Eigen::Matrix<Scalar, 3, 6>::Zero();
- Jlr.block<3, 3>(0, 3) = -R_world_sensor_.transpose();
- Jlr.block<3, 3>(0, 3) = R_world_sensor_.transpose();
- Jlr(0, 2) = -p_robot_landmark(0) * sin(t_world_sensor_(2)) + p_robot_landmark(1) * cos(t_world_sensor_(2));
- Jlr(1, 2) = -p_robot_landmark(0) * cos(t_world_sensor_(2)) - p_robot_landmark(1) * sin(t_world_sensor_(2));
- // measurement covariance
- expected_feature_cov_ = Jlr * Sigma.selfadjointView<Eigen::Upper>() * Jlr.transpose();
- }
- else
- // Any covariance block is not stored in wolfproblem -> Identity()
- expected_feature_cov_ = Eigen::Matrix3s::Identity()*0.1;
-}
-
-Eigen::VectorXs ProcessorTrackerLandmarkCorner::computeSquaredMahalanobisDistances(const FeatureBasePtr _feature_ptr,
- const Eigen::Vector4s& _expected_feature,
- const Eigen::Matrix3s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu)
-{
- const Eigen::Vector2s& p_feature = _feature_ptr->getMeasurement().head(2);
- const Scalar& o_feature = _feature_ptr->getMeasurement()(2);
- // ------------------------ d
- Eigen::Vector3s d;
- d.head(2) = p_feature - _expected_feature.head(2);
- d(2) = pi2pi(o_feature - _expected_feature(2));
- // ------------------------ Sigma_d
- Eigen::Matrix3s iSigma_d =
- (_feature_ptr->getMeasurementCovariance().topLeftCorner<3, 3>() + _expected_feature_cov).inverse();
- Eigen::VectorXs squared_mahalanobis_distances(_mu.cols());
- for (unsigned int i = 0; i < _mu.cols(); i++)
- {
- squared_mahalanobis_distances(i) = (d - _mu.col(i)).transpose() * iSigma_d * (d - _mu.col(i));
- //if ((d - _mu.col(i)).norm() < 1)
- //{
- // std::cout << "distance: " << (d - _mu.col(i)).norm() << std::endl;
- // std::cout << "iSigma_d: " << std::endl << iSigma_d << std::endl;
- // std::cout << "mahalanobis: " << squared_mahalanobis_distances(i) << std::endl;
- //}
- }
-
- return squared_mahalanobis_distances;
-}
-
-ProcessorBasePtr ProcessorTrackerLandmarkCorner::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
-{
- ProcessorParamsLaserPtr params = std::static_pointer_cast<ProcessorParamsLaser>(_params);
- ProcessorTrackerLandmarkCornerPtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkCorner>(params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBasePtr _feature_ptr)
-{
- //std::cout << "ProcessorTrackerLandmarkCorner::createLandmark" << std::endl;
- // compute feature global pose
- Eigen::Vector3s feature_global_pose = R_world_sensor_ * _feature_ptr->getMeasurement().head<3>() + t_world_sensor_;
- // Create new landmark
- return std::make_shared<LandmarkCorner2D>(std::make_shared<StateBlock>(feature_global_pose.head(2)),
- std::make_shared<StateBlock>(feature_global_pose.tail(1)),
- _feature_ptr->getMeasurement()(3));
-}
-
-unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
-{
- // already computed since each scan is computed in preprocess()
- _features_incoming_out = std::move(corners_last_);
- return _features_incoming_out.size();
-}
-
-FactorBasePtr ProcessorTrackerLandmarkCorner::createFactor(FeatureBasePtr _feature_ptr,
- LandmarkBasePtr _landmark_ptr)
-{
- assert(_feature_ptr != nullptr && _landmark_ptr != nullptr
- && "ProcessorTrackerLandmarkCorner::createFactor: feature and landmark pointers can not be nullptr!");
- return std::make_shared<FactorCorner2D>(_feature_ptr,
- std::static_pointer_cast<LandmarkCorner2D>((_landmark_ptr)),
- shared_from_this());
-}
-
-ProcessorTrackerLandmarkCorner::~ProcessorTrackerLandmarkCorner()
-{
- while (!corners_last_.empty())
- {
- corners_last_.front()->remove();
- corners_last_.pop_front(); // TODO check if this is needed
- }
- while (!corners_incoming_.empty())
- {
- corners_incoming_.front()->remove();
- corners_incoming_.pop_front(); // TODO check if this is needed
- }
-}
-
-ProcessorTrackerLandmarkCorner::ProcessorTrackerLandmarkCorner(ProcessorParamsLaserPtr _params_laser) :
- ProcessorTrackerLandmark("TRACKER LANDMARK CORNER", _params_laser),
- line_finder_(_params_laser->line_finder_params_),
- new_corners_th_(_params_laser->new_corners_th),
- loop_frames_th_(_params_laser->loop_frames_th),
- R_sensor_world_(Eigen::Matrix3s::Identity()),
- R_world_sensor_(Eigen::Matrix3s::Identity()),
- R_robot_sensor_(Eigen::Matrix3s::Identity()),
- extrinsics_transformation_computed_(false)
-{
- //
-}
-
-} //namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("TRACKER LANDMARK CORNER", ProcessorTrackerLandmarkCorner)
-} // namespace wolf
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
deleted file mode 100644
index a88ec47c932f3c1c7f9098a532285c20940dbeda..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ /dev/null
@@ -1,508 +0,0 @@
-#include "base/processor/processor_tracker_landmark_image.h"
-
-#include "base/capture/capture_image.h"
-#include "base/factor/factor_AHP.h"
-#include "base/feature/feature_base.h"
-#include "base/feature/feature_point_image.h"
-#include "base/frame_base.h"
-#include "base/logging.h"
-#include "base/map_base.h"
-#include "base/pinhole_tools.h"
-#include "base/problem.h"
-#include "base/sensor/sensor_camera.h"
-#include "base/state_block.h"
-#include "base/time_stamp.h"
-
-// vision_utils
-#include <detectors.h>
-#include <descriptors.h>
-#include <matchers.h>
-#include <algorithms.h>
-
-#include <Eigen/Geometry>
-#include <iomanip> //setprecision
-
-namespace wolf
-{
-
-ProcessorTrackerLandmarkImage::ProcessorTrackerLandmarkImage(ProcessorParamsTrackerLandmarkImagePtr _params_tracker_landmark_image) :
- ProcessorTrackerLandmark("IMAGE LANDMARK", _params_tracker_landmark_image),
- cell_width_(0),
- cell_height_(0),
- params_tracker_landmark_image_(_params_tracker_landmark_image),
- n_feature_(0),
- landmark_idx_non_visible_(0)
-{
- // Detector
- std::string det_name = vision_utils::readYamlType(params_tracker_landmark_image_->yaml_file_params_vision_utils, "detector");
- det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_tracker_landmark_image_->yaml_file_params_vision_utils);
-
- if (det_name.compare("ORB") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorORB>(det_ptr_);
- else if (det_name.compare("FAST") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorFAST>(det_ptr_);
- else if (det_name.compare("SIFT") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSIFT>(det_ptr_);
- else if (det_name.compare("SURF") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSURF>(det_ptr_);
- else if (det_name.compare("BRISK") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorBRISK>(det_ptr_);
- else if (det_name.compare("MSER") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorMSER>(det_ptr_);
- else if (det_name.compare("GFTT") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorGFTT>(det_ptr_);
- else if (det_name.compare("HARRIS") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorHARRIS>(det_ptr_);
- else if (det_name.compare("SBD") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorSBD>(det_ptr_);
- else if (det_name.compare("KAZE") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorKAZE>(det_ptr_);
- else if (det_name.compare("AKAZE") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorAKAZE>(det_ptr_);
- else if (det_name.compare("AGAST") == 0)
- det_ptr_ = std::static_pointer_cast<vision_utils::DetectorAGAST>(det_ptr_);
-
- // Descriptor
- std::string des_name = vision_utils::readYamlType(params_tracker_landmark_image_->yaml_file_params_vision_utils, "descriptor");
- des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_tracker_landmark_image_->yaml_file_params_vision_utils);
-
- if (des_name.compare("ORB") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorORB>(des_ptr_);
- else if (des_name.compare("SIFT") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorSIFT>(des_ptr_);
- else if (des_name.compare("SURF") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorSURF>(des_ptr_);
- else if (des_name.compare("BRISK") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorBRISK>(des_ptr_);
- else if (des_name.compare("KAZE") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorKAZE>(des_ptr_);
- else if (des_name.compare("AKAZE") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorAKAZE>(des_ptr_);
- else if (des_name.compare("LATCH") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorLATCH>(des_ptr_);
- else if (des_name.compare("FREAK") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorFREAK>(des_ptr_);
- else if (des_name.compare("BRIEF") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorBRIEF>(des_ptr_);
- else if (des_name.compare("DAISY") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorDAISY>(des_ptr_);
- else if (des_name.compare("LUCID") == 0)
- des_ptr_ = std::static_pointer_cast<vision_utils::DescriptorLUCID>(des_ptr_);
-
- // Matcher
- std::string mat_name = vision_utils::readYamlType(params_tracker_landmark_image_->yaml_file_params_vision_utils, "matcher");
- mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_tracker_landmark_image_->yaml_file_params_vision_utils);
-
- if (mat_name.compare("FLANNBASED") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherFLANNBASED>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_L1") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_L1>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_HAMMING") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING>(mat_ptr_);
- if (mat_name.compare("BRUTEFORCE_HAMMING_2") == 0)
- mat_ptr_ = std::static_pointer_cast<vision_utils::MatcherBRUTEFORCE_HAMMING_2>(mat_ptr_);
-
- // Active search grid
- vision_utils::AlgorithmBasePtr alg_ptr = vision_utils::setupAlgorithm("ACTIVESEARCH", "ACTIVESEARCH algorithm", params_tracker_landmark_image_->yaml_file_params_vision_utils);
- active_search_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmACTIVESEARCH>(alg_ptr);
-}
-
-ProcessorTrackerLandmarkImage::~ProcessorTrackerLandmarkImage()
-{
- //
-}
-
-void ProcessorTrackerLandmarkImage::configure(SensorBasePtr _sensor)
-{
- SensorCameraPtr camera(std::static_pointer_cast<SensorCamera>(_sensor));
- image_.width_ = camera->getImgWidth();
- image_.height_ = camera->getImgHeight();
-
- active_search_ptr_->initAlg(camera->getImgWidth(), camera->getImgHeight(), det_ptr_->getPatternRadius() );
-
- params_tracker_landmark_image_activesearch_ptr_ = std::static_pointer_cast<vision_utils::AlgorithmParamsACTIVESEARCH>( active_search_ptr_->getParams() );
-
- cell_width_ = image_.width_ / params_tracker_landmark_image_activesearch_ptr_->n_cells_h;
- cell_height_ = image_.height_ / params_tracker_landmark_image_activesearch_ptr_->n_cells_v;
-}
-
-void ProcessorTrackerLandmarkImage::preProcess()
-{
- image_incoming_ = std::static_pointer_cast<CaptureImage>(incoming_ptr_)->getImage();
-
- active_search_ptr_->renew();
-
- detector_roi_.clear();
- feat_lmk_found_.clear();
-}
-
-void ProcessorTrackerLandmarkImage::postProcess()
-{
- detector_roi_.clear();
- feat_lmk_found_.clear();
-}
-
-unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
- FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences)
-{
- KeyPointVector candidate_keypoints;
- cv::Mat candidate_descriptors;
- DMatchVector cv_matches;
-
- Eigen::VectorXs current_state = getProblem()->getState(incoming_ptr_->getTimeStamp());
-
- for (auto landmark_in_ptr : _landmarks_in)
- {
-
- // project landmark into incoming capture
- LandmarkAHPPtr landmark_ptr = std::static_pointer_cast<LandmarkAHP>(landmark_in_ptr);
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(this->getSensor());
- Eigen::Vector4s point3D_hmg;
- Eigen::Vector2s pixel;
-
- landmarkInCurrentCamera(current_state, landmark_ptr, point3D_hmg);
-
- pixel = pinhole::projectPoint(camera->getIntrinsic()->getState(),
- camera->getDistortionVector(),
- point3D_hmg.head<3>());
-
- if(pinhole::isInImage(pixel, image_.width_, image_.height_))
- {
- cv::Rect roi = vision_utils::setRoi(pixel[0], pixel[1], cell_width_, cell_height_);
-
- active_search_ptr_->hitCell(pixel);
-
- cv::Mat target_descriptor = landmark_ptr->getCvDescriptor();
-
- if (detect(image_incoming_, roi, candidate_keypoints, candidate_descriptors))
- {
- Scalar normalized_score = match(target_descriptor,candidate_descriptors,cv_matches);
-
- if (normalized_score > mat_ptr_->getParams()->min_norm_score )
- {
- FeaturePointImagePtr incoming_point_ptr = std::make_shared<FeaturePointImage>(
- candidate_keypoints[cv_matches[0].trainIdx],
- cv_matches[0].trainIdx,
- candidate_descriptors.row(cv_matches[0].trainIdx),
- Eigen::Matrix2s::Identity()*params_tracker_landmark_image_->pixel_noise_var);
-
- incoming_point_ptr->setTrackId(landmark_in_ptr->id());
- incoming_point_ptr->setLandmarkId(landmark_in_ptr->id());
- incoming_point_ptr->setScore(normalized_score);
- incoming_point_ptr->setExpectation(pixel);
-
- _features_incoming_out.push_back(incoming_point_ptr);
-
- _feature_landmark_correspondences[incoming_point_ptr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, normalized_score);
-
- feat_lmk_found_.push_back(incoming_point_ptr);
-
- // To visualize
- cv::Rect roi2 = roi;
- vision_utils::trimRoi(image_.width_, image_.height_, roi2);
- incoming_point_ptr->setTrackerRoi(roi2);
- }
-// else
-// std::cout << "NOT FOUND" << std::endl;
- }
-// else
-// std::cout << "NOT DETECTED/FOUND" << std::endl;
- }
-// else
-// std::cout << "NOT in the image" << std::endl;
- }
-// std::cout << "\tNumber of Features tracked: " << _features_incoming_out.size() << std::endl;
- landmarks_tracked_ = _features_incoming_out.size();
- return _features_incoming_out.size();
-}
-
-bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
-{
- return false;
-// return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe;
-}
-
-unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
-{
- cv::Rect roi;
- KeyPointVector new_keypoints;
- cv::Mat new_descriptors;
- cv::KeyPointsFilter keypoint_filter;
- unsigned int n_new_features = 0;
-
- for (unsigned int n_iterations = 0; n_iterations < _max_features; n_iterations++)
- {
- if (active_search_ptr_->pickEmptyRoi(roi))
- {
- detector_roi_.push_back(roi);
- if (detect(image_last_, roi, new_keypoints, new_descriptors))
- {
- KeyPointVector list_keypoints = new_keypoints;
- unsigned int index = 0;
- keypoint_filter.retainBest(new_keypoints,1);
- for(unsigned int i = 0; i < list_keypoints.size(); i++)
- {
- if(list_keypoints[i].pt == new_keypoints[0].pt)
- index = i;
- }
-
- if(new_keypoints[0].response > params_tracker_landmark_image_activesearch_ptr_->min_response_new_feature)
- {
- list_response_.push_back(new_keypoints[0].response);
- FeaturePointImagePtr point_ptr = std::make_shared<FeaturePointImage>(
- new_keypoints[0],
- 0,
- new_descriptors.row(index),
- Eigen::Matrix2s::Identity()*params_tracker_landmark_image_->pixel_noise_var);
- point_ptr->setIsKnown(false);
- point_ptr->setTrackId(point_ptr->id());
- point_ptr->setExpectation(Eigen::Vector2s(new_keypoints[0].pt.x,new_keypoints[0].pt.y));
- _features_incoming_out.push_back(point_ptr);
-
- active_search_ptr_->hitCell(new_keypoints[0]);
-
- n_new_features++;
- }
-
- }
- else
- active_search_ptr_->blockCell(roi);
- }
- else
- break;
- }
-
- WOLF_DEBUG( "Number of new features detected: " , n_new_features );
- return n_new_features;
-}
-
-LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _feature_ptr)
-{
-
- FeaturePointImagePtr feat_point_image_ptr = std::static_pointer_cast<FeaturePointImage>( _feature_ptr);
- FrameBasePtr anchor_frame = getLast()->getFrame();
-
- Eigen::Vector2s point2D;
- point2D[0] = feat_point_image_ptr->getKeypoint().pt.x;
- point2D[1] = feat_point_image_ptr->getKeypoint().pt.y;
-
- Scalar distance = params_tracker_landmark_image_->distance; // arbitrary value
- Eigen::Vector4s vec_homogeneous;
-
- point2D = pinhole::depixellizePoint(getSensor()->getIntrinsic()->getState(),point2D);
- point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),point2D);
-
- Eigen::Vector3s point3D;
- point3D.head<2>() = point2D;
- point3D(2) = 1;
-
- point3D.normalize();
-
- vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
-
- LandmarkAHPPtr lmk_ahp_ptr = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, getSensor(), feat_point_image_ptr->getDescriptor());
- _feature_ptr->setLandmarkId(lmk_ahp_ptr->id());
- return lmk_ahp_ptr;
-}
-
-FactorBasePtr ProcessorTrackerLandmarkImage::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
-{
-
- if ((std::static_pointer_cast<LandmarkAHP>(_landmark_ptr))->getAnchorFrame() == last_ptr_->getFrame())
- {
- return FactorBasePtr();
- }
- else
- {
- assert (last_ptr_ && "bad last ptr");
- assert (_landmark_ptr && "bad lmk ptr");
-
- LandmarkAHPPtr landmark_ahp = std::static_pointer_cast<LandmarkAHP>(_landmark_ptr);
-
- FactorAHPPtr factor_ptr = FactorAHP::create(_feature_ptr, landmark_ahp, shared_from_this(), true);
-
- return factor_ptr;
- }
-}
-
-// ==================================================================== My own functions
-
-void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorXs& _current_state,
- const LandmarkAHPPtr _landmark,
- Eigen::Vector4s& _point3D_hmg)
-{
- using namespace Eigen;
-
- /*
- * Rationale: we transform the landmark from anchor camera to current camera:
- *
- * C0 ---> R0 ---> W ---> R1 ---> C1
- *
- * where
- * '0' is 'anchor'
- * '1' is 'current',
- * 'R' is 'robot'
- * 'C' is 'camera'
- * 'W' is 'world',
- *
- * by concatenating the individual transforms:
- *
- * T_W_R0,
- * T_W_R1,
- * T_R0_C0,
- * T_R1_C1
- *
- * We use Eigen::Transform which is like using homogeneous transform matrices with a simpler API
- */
-
- // Assert frame is 3D with at least PQ
- assert((_current_state.size() == 7 || _current_state.size() == 16) && "Wrong state size! Should be 7 for 3D pose or 16 for IMU.");
-
- // ALL TRANSFORMS
- Transform<Scalar,3,Eigen::Affine> T_W_R0, T_W_R1, T_R0_C0, T_R1_C1;
-
- // world to anchor robot frame
- Translation<Scalar,3> t_w_r0(_landmark->getAnchorFrame()->getP()->getState()); // sadly we cannot put a Map over a translation
- const Quaternions q_w_r0(Eigen::Vector4s(_landmark->getAnchorFrame()->getO()->getState()));
- T_W_R0 = t_w_r0 * q_w_r0;
-
- // world to current robot frame
- Translation<Scalar,3> t_w_r1(_current_state.head<3>());
- Map<const Quaternions> q_w_r1(_current_state.data() + 3);
- T_W_R1 = t_w_r1 * q_w_r1;
-
- // anchor robot to anchor camera
- Translation<Scalar,3> t_r0_c0(_landmark->getAnchorSensor()->getP()->getState());
- const Quaternions q_r0_c0(Eigen::Vector4s(_landmark->getAnchorSensor()->getO()->getState()));
- T_R0_C0 = t_r0_c0 * q_r0_c0;
-
- // current robot to current camera
- Translation<Scalar,3> t_r1_c1(this->getSensor()->getP()->getState());
- const Quaternions q_r1_c1(Eigen::Vector4s(this->getSensor()->getO()->getState()));
- T_R1_C1 = t_r1_c1 * q_r1_c1;
-
- // Transform lmk from c0 to c1 and exit
- Vector4s landmark_hmg_c0 = _landmark->getP()->getState(); // lmk in anchor frame
- _point3D_hmg = T_R1_C1.inverse(Eigen::Affine) * T_W_R1.inverse(Eigen::Affine) * T_W_R0 * T_R0_C0 * landmark_hmg_c0;
-}
-
-Scalar ProcessorTrackerLandmarkImage::match(const cv::Mat _target_descriptor, const cv::Mat _candidate_descriptors, DMatchVector& _cv_matches)
-{
- std::vector<Scalar> normalized_scores = mat_ptr_->match(_target_descriptor, _candidate_descriptors, des_ptr_->getSize(), _cv_matches);
- return normalized_scores[0];
-}
-
-unsigned int ProcessorTrackerLandmarkImage::detect(const cv::Mat _image, cv::Rect& _roi, KeyPointVector& _new_keypoints, cv::Mat& new_descriptors)
-{
- _new_keypoints = det_ptr_->detect(_image, _roi);
- new_descriptors = des_ptr_->getDescriptor(_image, _new_keypoints);
- return _new_keypoints.size();
-}
-
-void ProcessorTrackerLandmarkImage::drawTrackerRoi(cv::Mat _image, cv::Scalar _color)
-{
- CaptureImagePtr _capture = std::static_pointer_cast<CaptureImage>(last_ptr_);
- if (last_ptr_!=nullptr)
- {
- for (auto feature : _capture->getFeatureList())
- cv::rectangle(_image, std::static_pointer_cast<FeaturePointImage>(feature)->getTrackerRoi(), _color, 1, 8, 0);
- }
-}
-
-void ProcessorTrackerLandmarkImage::drawRoi(cv::Mat _image, std::list<cv::Rect> _roi_list, cv::Scalar _color)
-{
- if (last_ptr_!=nullptr)
- {
- for (auto roi : _roi_list)
- cv::rectangle(_image, roi, _color, 1, 8, 0);
- }
-}
-
-void ProcessorTrackerLandmarkImage::drawFeaturesFromLandmarks(cv::Mat _image)
-{
- if (last_ptr_!=nullptr)
- {
- FeaturePointImagePtr ftr;
- for(auto feature_point : feat_lmk_found_)
- {
- ftr = std::static_pointer_cast<FeaturePointImage>(feature_point);
-
- cv::Point2f point = ftr->getKeypoint().pt;
- cv::circle(_image, point, 2, cv::Scalar(255.0, 255.0, 0.0), -1, 8, 0);
-
- cv::Point2f point2 = point;
- point2.x = point2.x - 16;
- cv::putText(_image, std::to_string(ftr->landmarkId()) + "/" + std::to_string((int)(100*ftr->getScore())), point2,
- cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255.0, 255.0, 0.0));
- }
- }
-}
-
-void ProcessorTrackerLandmarkImage::drawLandmarks(cv::Mat _image)
-{
- if (last_ptr_!=nullptr)
- {
- unsigned int num_lmks_in_img = 0;
-
- Eigen::VectorXs current_state = last_ptr_->getFrame()->getState();
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensor());
-
- for (auto landmark_base_ptr : getProblem()->getMap()->getLandmarkList())
- {
- LandmarkAHPPtr landmark_ptr = std::static_pointer_cast<LandmarkAHP>(landmark_base_ptr);
-
- Eigen::Vector4s point3D_hmg;
- landmarkInCurrentCamera(current_state, landmark_ptr, point3D_hmg);
-
- Eigen::Vector2s point2D = pinhole::projectPoint(camera->getIntrinsic()->getState(), // k
- camera->getDistortionVector(), // d
- point3D_hmg.head(3)); // v
-
- if(pinhole::isInImage(point2D,image_.width_,image_.height_))
- {
- num_lmks_in_img++;
-
- cv::Point2f point;
- point.x = point2D[0];
- point.y = point2D[1];
-
- cv::circle(_image, point, 4, cv::Scalar(51.0, 51.0, 255.0), 1, 3, 0);
- cv::putText(_image, std::to_string(landmark_ptr->id()), point, cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(100.0, 100.0, 255.0) );
- }
- }
- cv::Point label_for_landmark_point;
- label_for_landmark_point.x = 3;
- label_for_landmark_point.y = 10;
- cv::putText(_image, std::to_string(landmarks_tracked_), label_for_landmark_point,
- cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255.0, 0.0, 255.0));
-
- cv::Point label_for_landmark_point2;
- label_for_landmark_point2.x = 3;
- label_for_landmark_point2.y = 20;
- cv::putText(_image, std::to_string(num_lmks_in_img), label_for_landmark_point2,
- cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255.0, 0.0, 255.0));
-
-// std::cout << "\t\tTotal landmarks: " << counter << std::endl;
- }
-}
-
-//namespace wolf{
-
-ProcessorBasePtr ProcessorTrackerLandmarkImage::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
-{
- ProcessorTrackerLandmarkImagePtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkImage>(std::static_pointer_cast<ProcessorParamsTrackerLandmarkImage>(_params));
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("IMAGE LANDMARK", ProcessorTrackerLandmarkImage)
-} // namespace wolf
-
diff --git a/src/processor/processor_tracker_landmark_polyline.cpp b/src/processor/processor_tracker_landmark_polyline.cpp
deleted file mode 100644
index c3f91c0278eda1a7a73657f4d18d40710c59f999..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_landmark_polyline.cpp
+++ /dev/null
@@ -1,1020 +0,0 @@
-#include "base/processor/processor_tracker_landmark_polyline.h"
-
-namespace wolf
-{
-
-void ProcessorTrackerLandmarkPolyline::preProcess()
-{
- //std::cout << "PreProcess: " << std::endl;
-
- // extract corners of incoming
- extractPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_), polylines_incoming_);
-
- // compute transformations
- computeTransformations(incoming_ptr_->getTimeStamp());
-
- //std::cout << "PreProcess: incoming new features: " << polylines_incoming_.size() << std::endl;
-}
-
-void ProcessorTrackerLandmarkPolyline::computeTransformations(const TimeStamp& _ts)
-{
- Eigen::Vector3s vehicle_pose = getProblem()->getState(_ts);
- t_world_robot_ = vehicle_pose.head<2>();
-
- // world_robot
- Eigen::Matrix2s R_world_robot = Eigen::Rotation2Ds(vehicle_pose(2)).matrix();
-
- // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
- || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
- {
- t_robot_sensor_ = getSensor()->getP()->getState();
- R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix();
- extrinsics_transformation_computed_ = true;
- }
-
- // global_sensor
- R_world_sensor_ = R_world_robot * R_robot_sensor_;
- t_world_sensor_ = t_world_robot_ + R_world_robot * t_robot_sensor_;
-
- // sensor_global
- R_sensor_world_ = R_robot_sensor_.transpose() * R_world_robot.transpose();
- t_sensor_world_ = -R_robot_sensor_.transpose() * t_robot_sensor_ -R_sensor_world_ * t_world_robot_ ;
-
- //std::cout << "t_world_robot_ " << t_world_robot_.transpose() << std::endl;
- //std::cout << "t_robot_sensor_ " << t_robot_sensor_.transpose() << std::endl;
- //std::cout << "R_robot_sensor_ " << std::endl << R_robot_sensor_ << std::endl;
- //std::cout << "t_world_sensor_ " << t_world_sensor_.transpose() << std::endl;
- //std::cout << "R_world_sensor_ " << std::endl << R_world_sensor_ << std::endl;
- //std::cout << "t_sensor_world_ " << t_sensor_world_.transpose() << std::endl;
- //std::cout << "R_sensor_world_ " << std::endl << R_sensor_world_ << std::endl;
-
-}
-
-unsigned int ProcessorTrackerLandmarkPolyline::findLandmarks(const LandmarkBasePtrList& _landmarks_searched,
- FeatureBasePtrList& _features_found,
- LandmarkMatchMap& _feature_landmark_correspondences)
-{
- //std::cout << "ProcessorTrackerLandmarkPolyline::findLandmarks: " << _landmarks_searched.size() << " features: " << polylines_incoming_.size() << std::endl;
-
- // COMPUTING ALL EXPECTED FEATURES
- std::map<LandmarkBasePtr, Eigen::MatrixXs> expected_features;
- std::map<LandmarkBasePtr, Eigen::MatrixXs> expected_features_covs;
- for (auto landmark : _landmarks_searched)
- if (landmark->getType() == "POLYLINE 2D")
- {
- expected_features[landmark] = Eigen::MatrixXs(3, (std::static_pointer_cast<LandmarkPolyline2D>(landmark))->getNPoints());
- expected_features_covs[landmark] = Eigen::MatrixXs(2, 2*(std::static_pointer_cast<LandmarkPolyline2D>(landmark))->getNPoints());
- expectedFeature(landmark, expected_features[landmark], expected_features_covs[landmark]);
- }
-
- // NAIVE NEAREST NEIGHBOR MATCHING
- LandmarkPolylineMatchPtr best_match = nullptr;
- FeaturePolyline2DPtr polyline_feature;
- LandmarkPolyline2DPtr polyline_landmark;
-
- auto next_feature_it = polylines_incoming_.begin();
- auto feature_it = next_feature_it++;
- int max_ftr, max_lmk, max_offset, min_offset, offset, from_ftr, from_lmk, to_ftr, to_lmk, N_overlapped;
-
- // iterate over all polylines features
- while (feature_it != polylines_incoming_.end())
- {
- polyline_feature = std::static_pointer_cast<FeaturePolyline2D>(*feature_it);
- max_ftr = polyline_feature->getNPoints() - 1;
-
- // Check with all landmarks
- for (auto landmark_it = _landmarks_searched.begin(); landmark_it != _landmarks_searched.end(); landmark_it++)
- {
- polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(*landmark_it);
-
- // Open landmark polyline
- if (!polyline_landmark->isClosed())
- {
- //std::cout << "MATCHING WITH OPEN LANDMARK" << std::endl;
- //std::cout << "\tfeature " << polyline_feature->id() << ": 0-" << max_ftr << std::endl;
- //std::cout << "\tlandmark " << polyline_landmark->id() << ": 0-" << polyline_landmark->getNPoints() - 1 << std::endl;
- max_lmk = polyline_landmark->getNPoints() - 1;
- max_offset = max_ftr;
- min_offset = -max_lmk;
-
- // Check all overlapping positions between each feature-landmark pair
- for (offset = min_offset; offset <= max_offset; offset++)
- {
- if (offset == min_offset && !polyline_landmark->isLastDefined() && !polyline_feature->isFirstDefined())
- continue;
-
- if (offset == max_offset && !polyline_landmark->isFirstDefined() && !polyline_feature->isLastDefined())
- continue;
-
- from_lmk = std::max(0, -offset);
- from_ftr = std::max(0, offset);
- N_overlapped = std::min(max_ftr - from_ftr, max_lmk - from_lmk)+1;
- to_lmk = from_lmk+N_overlapped-1;
- to_ftr = from_ftr+N_overlapped-1;
-
- //std::cout << "\t\toffset " << offset << std::endl;
- //std::cout << "\t\t\tfrom_lmk " << from_lmk << std::endl;
- //std::cout << "\t\t\tfrom_ftr " << from_ftr << std::endl;
- //std::cout << "\t\t\tN_overlapped " << N_overlapped << std::endl;
-
- // Compute the squared distance for all overlapped points
- Eigen::ArrayXXd d = (polyline_feature->getPoints().block(0,from_ftr, 2,N_overlapped) -
- expected_features[*landmark_it].block(0,from_lmk, 2, N_overlapped)).array();
-
- Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2);
- //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- if (offset != min_offset && offset != max_offset)
- {
- // Point-to-line first distance
- bool from_ftr_not_defined = (from_ftr == 0 && !polyline_feature->isFirstDefined());
- bool from_lmk_not_defined = (from_lmk == 0 && !polyline_landmark->isFirstDefined());
- //std::cout << "\t\tfrom_ftr_not_defined " << from_ftr_not_defined << (from_ftr == 0) << !polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\t\tfrom_lmk_not_defined " << from_lmk_not_defined << (from_lmk == 0) << !polyline_landmark->isFirstDefined() << std::endl;
- if (from_ftr_not_defined || from_lmk_not_defined)
- {
- //std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl;
- //std::cout << "\t\t\tA" << expected_features[*landmark_it].col(from_lmk).transpose() << std::endl;
- //std::cout << "\t\t\tAaux" << expected_features[*landmark_it].col(from_lmk+1).transpose() << std::endl;
- //std::cout << "\t\t\tB" << polyline_feature->getPoints().col(from_ftr).transpose() << std::endl;
- dist2(0) = sqDistPointToLine(expected_features[*landmark_it].col(from_lmk),
- expected_features[*landmark_it].col(from_lmk+1),
- polyline_feature->getPoints().col(from_ftr),
- !from_lmk_not_defined,
- !from_ftr_not_defined);
- }
-
- // Point-to-line last distance
- bool last_ftr_not_defined = !polyline_feature->isLastDefined() && to_ftr == max_ftr;
- bool last_lmk_not_defined = !polyline_landmark->isLastDefined() && to_lmk == max_lmk;
- //std::cout << "\t\tlast_ftr_not_defined " << last_ftr_not_defined << (to_ftr == max_ftr) << !polyline_feature->isLastDefined() << std::endl;
- //std::cout << "\t\tlast_lmk_not_defined " << last_lmk_not_defined << (to_lmk == max_lmk) << !polyline_landmark->isLastDefined() << std::endl;
- if (last_ftr_not_defined || last_lmk_not_defined)
- {
- //std::cout << "\t\t\tLast feature not defined distance to line" << std::endl;
- //std::cout << "\t\t\tA" << expected_features[*landmark_it].col(to_lmk).transpose() << std::endl;
- //std::cout << "\t\t\tAaux" << expected_features[*landmark_it].col(to_lmk-1).transpose() << std::endl;
- //std::cout << "\t\t\tB" << polyline_feature->getPoints().col(to_ftr).transpose() << std::endl;
- dist2(N_overlapped-1) = sqDistPointToLine(expected_features[*landmark_it].col(to_lmk),
- expected_features[*landmark_it].col(to_lmk-1),
- polyline_feature->getPoints().col(to_ftr),
- !last_lmk_not_defined,
- !last_ftr_not_defined);
- }
- }
- //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- // All squared distances should be witin a threshold
- // Choose the most overlapped one
- if ((dist2 < params_->position_error_th*params_->position_error_th).all() && (best_match == nullptr ||
- (N_overlapped >= best_match->feature_match_to_id_-best_match->feature_match_from_id_+1 &&
- dist2.mean() < best_match->normalized_score_ )))
- {
- //std::cout << "BEST MATCH" << std::endl;
- best_match = std::make_shared<LandmarkPolylineMatch>();
- best_match->feature_match_from_id_= from_ftr;
- best_match->landmark_match_from_id_= from_lmk+polyline_landmark->getFirstId();
- best_match->feature_match_to_id_= from_ftr+N_overlapped-1;
- best_match->landmark_match_to_id_= from_lmk+N_overlapped-1+polyline_landmark->getFirstId();
- best_match->landmark_ptr_=polyline_landmark;
- best_match->normalized_score_ = dist2.mean();
- }
- }
- }
- // Closed landmark polyline
- else
- {
- if (polyline_feature->getNPoints() > polyline_landmark->getNPoints())
- continue;
-
- //std::cout << "MATCHING WITH CLOSED LANDMARK" << std::endl;
- //std::cout << "\tfeature " << polyline_feature->id() << ": 0-" << max_ftr << std::endl;
- //std::cout << "\tlandmark " << polyline_landmark->id() << ": 0-" << polyline_landmark->getNPoints() - 1 << std::endl;
-
- max_offset = 0;
- min_offset = -polyline_landmark->getNPoints() + 1;
-
- // Check all overlapping positions between each feature-landmark pair
- for (offset = min_offset; offset <= max_offset; offset++)
- {
- from_lmk = -offset;
- to_lmk = from_lmk+polyline_feature->getNPoints()-1;
- if (to_lmk >= polyline_landmark->getNPoints())
- to_lmk -= polyline_landmark->getNPoints();
-
- //std::cout << "\t\toffset " << offset << std::endl;
- //std::cout << "\t\t\tfrom_lmk " << from_lmk << std::endl;
- //std::cout << "\t\t\tto_lmk " << to_lmk << std::endl;
-
- // Compute the squared distance for all overlapped points
- Eigen::ArrayXXd d = polyline_feature->getPoints().topRows(2).array();
- if (to_lmk > from_lmk)
- d -= expected_features[*landmark_it].block(0,from_lmk, 2, polyline_feature->getNPoints()).array();
- else
- {
- d.leftCols(polyline_landmark->getNPoints()-from_lmk) -= expected_features[*landmark_it].block(0,from_lmk, 2, polyline_landmark->getNPoints()-from_lmk).array();
- d.rightCols(to_lmk+1) -= expected_features[*landmark_it].block(0, 0, 2, to_lmk+1).array();
- }
- Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2);
- //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- // Point-to-line first distance
- if (!polyline_feature->isFirstDefined())
- {
- int next_from_lmk = (from_lmk+1 == polyline_landmark->getNPoints() ? 0 : from_lmk+1);
- dist2(0) = sqDistPointToLine(expected_features[*landmark_it].col(from_lmk),
- expected_features[*landmark_it].col(next_from_lmk),
- polyline_feature->getPoints().col(0),
- true,
- false);
- }
-
- // Point-to-line last distance
- if (!polyline_feature->isLastDefined())
- {
- int prev_to_lmk = (to_lmk == 0 ? polyline_landmark->getNPoints()-1 : to_lmk-1);
- dist2(polyline_feature->getNPoints()-1) = sqDistPointToLine(expected_features[*landmark_it].col(to_lmk),
- expected_features[*landmark_it].col(prev_to_lmk),
- polyline_feature->getPoints().col(polyline_feature->getNPoints()-1),
- true,
- false);
- }
- //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- // All squared distances should be witin a threshold
- // Choose the most overlapped one
- if ((dist2 < params_->position_error_th*params_->position_error_th).all() && (best_match == nullptr || dist2.mean() < best_match->normalized_score_ ))
- {
- //std::cout << "BEST MATCH" << std::endl;
- best_match = std::make_shared<LandmarkPolylineMatch>();
- best_match->feature_match_from_id_= 0;
- best_match->landmark_match_from_id_= from_lmk+polyline_landmark->getFirstId();
- best_match->feature_match_to_id_= polyline_feature->getNPoints()-1;
- best_match->landmark_match_to_id_= to_lmk+polyline_landmark->getFirstId();
- best_match->landmark_ptr_=polyline_landmark;
- best_match->normalized_score_ = dist2.mean();
- }
- }
- }
-
- //std::cout << "landmark " << (*landmark_it)->id() << ": 0-" << max_lmk << " - defined " << polyline_landmark->isFirstDefined() << polyline_landmark->isLastDefined() << std::endl;
- //std::cout << "feature " << (*feature_it)->id() << ": 0-" << max_ftr << " - defined " << polyline_feature->isFirstDefined() << polyline_feature->isLastDefined() << std::endl;
- //std::cout << expected_features[*landmark_it] << std::endl;
- //std::cout << "\tmax_offset " << max_offset << std::endl;
- //std::cout << "\tmin_offset " << min_offset << std::endl;
- //if (!polyline_landmark->isFirstDefined() && !polyline_feature->isLastDefined())
- // std::cout << "\tLIMITED max offset " << max_offset << std::endl;
- //if (!polyline_feature->isFirstDefined() && !polyline_landmark->isLastDefined())
- // std::cout << "\tLIMITED min offset " << min_offset << std::endl;
-
- }
- // Match found for this feature
- if (best_match != nullptr)
- {
- //std::cout << "\tclosest landmark: " << best_match->landmark_ptr_->id() << std::endl;
- // match
- matches_landmark_from_incoming_[*feature_it] = best_match;
- // move corner feature to output list
- _features_found.splice(_features_found.end(), polylines_incoming_, feature_it);
- // reset match
- best_match = nullptr;
- }
- //else
- //{
- // std::cout << "\t-------------------------->NO LANDMARK CLOSE ENOUGH!!!!" << std::endl;
- // std::getchar();
- //}
- feature_it = next_feature_it++;
- }
- return matches_landmark_from_incoming_.size();
-}
-
-bool ProcessorTrackerLandmarkPolyline::voteForKeyFrame()
-{
- //std::cout << "------------- ProcessorTrackerLandmarkPolyline::voteForKeyFrame:" << std::endl;
- //std::cout << "polylines_last_.size():" << polylines_last_.size()<< std::endl;
- // option 1: more than TH new features in last
- if (polylines_last_.size() >= params_->new_features_th)
- {
- std::cout << "------------- NEW KEY FRAME: Option 1 - Enough new features" << std::endl;
- //std::cout << "\tnew features in last = " << corners_last_.size() << std::endl;
- return true;
- }
- // option 2: loop closure (if the newest frame from which a matched landmark was observed is old enough)
- for (auto new_ftr : new_features_last_)
- {
- if (last_ptr_->getFrame()->id() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getFrame()->id() > params_->loop_frames_th)
- {
- std::cout << "------------- NEW KEY FRAME: Option 2 - Loop closure" << std::endl;
- return true;
- }
- }
- return false;
-}
-
-void ProcessorTrackerLandmarkPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr,
- FeatureBasePtrList& _polyline_list)
-{
- //std::cout << "ProcessorTrackerLandmarkPolyline::extractPolylines: " << std::endl;
- // TODO: sort corners by bearing
- std::list<laserscanutils::Polyline> polylines;
-
- line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines);
-
- for (auto&& pl : polylines)
- {
- //std::cout << "new polyline detected: Defined" << pl.first_defined_ << pl.last_defined_ << std::endl;
- //std::cout << "covs: " << std::endl << pl.covs_ << std::endl;
- _polyline_list.push_back(std::make_shared<FeaturePolyline2D>(pl.points_, pl.covs_, pl.first_defined_, pl.last_defined_));
- //std::cout << "new polyline detected: " << std::endl;
- }
- //std::cout << _polyline_list.size() << " polylines extracted" << std::endl;
-}
-
-void ProcessorTrackerLandmarkPolyline::expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
- Eigen::MatrixXs& expected_feature_cov_)
-{
- assert(_landmark_ptr->getType() == "POLYLINE 2D" && "ProcessorTrackerLandmarkPolyline::expectedFeature: Bad landmark type");
- LandmarkPolyline2DPtr polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(_landmark_ptr);
- assert(expected_feature_.cols() == polyline_landmark->getNPoints() && expected_feature_.rows() == 3 && "ProcessorTrackerLandmarkPolyline::expectedFeature: bad expected_feature_ sizes");
-
- //std::cout << "ProcessorTrackerLandmarkPolyline::expectedFeature" << std::endl;
- //std::cout << "t_world_sensor_: " << t_world_sensor_.transpose() << std::endl;
- //std::cout << "R_sensor_world_: " << std::endl << R_sensor_world_ << std::endl;
-
- expected_feature_ = Eigen::MatrixXs::Zero(3,polyline_landmark->getNPoints());
- expected_feature_cov_ = Eigen::MatrixXs::Zero(2,2*polyline_landmark->getNPoints());
- Eigen::Vector3s col = Eigen::Vector3s::Ones();
-
- ////////// global coordinates points
- if (polyline_landmark->getClassification() == UNCLASSIFIED)
- for (auto i = 0; i < polyline_landmark->getNPoints(); i++)
- {
- //std::cout << "Point " << i+polyline_landmark->getFirstId() << std::endl;
- //std::cout << "First Point " << polyline_landmark->getFirstId() << std::endl;
- //std::cout << "Landmark global position: " << polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()).transpose() << std::endl;
-
- // ------------ expected feature point
- col.head<2>() = R_sensor_world_ * (polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()) - t_world_sensor_);
- expected_feature_.col(i) = col;
-
- //std::cout << "Expected point " << i << ": " << expected_feature_.col(i).transpose() << std::endl;
- // ------------ expected feature point covariance
- // TODO
- expected_feature_cov_.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2);
- }
-
- ////////// landmark with origin
- else
- {
- Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(polyline_landmark->getO()->getState()(0)).matrix();
- const Eigen::VectorXs& t_world_points = polyline_landmark->getP()->getState();
-
- for (auto i = 0; i < polyline_landmark->getNPoints(); i++)
- {
- //std::cout << "Point " << i+polyline_landmark->getFirstId() << std::endl;
- //std::cout << "First Point " << polyline_landmark->getFirstId() << std::endl;
- //std::cout << "Landmark global position: " << polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()).transpose() << std::endl;
-
- // ------------ expected feature point
- col.head<2>() = R_sensor_world_ * (R_world_points * polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()) + t_world_points - t_world_sensor_);
- expected_feature_.col(i) = col;
-
- //std::cout << "Expected point " << i << ": " << expected_feature_.col(i).transpose() << std::endl;
- // ------------ expected feature point covariance
- // TODO
- expected_feature_cov_.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2);
- }
- }
-}
-
-Eigen::VectorXs ProcessorTrackerLandmarkPolyline::computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
- const Eigen::Matrix2s& _feature_cov,
- const Eigen::Vector2s& _expected_feature,
- const Eigen::Matrix2s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu)
-{
- // ------------------------ d
- Eigen::Vector2s d = _feature - _expected_feature;
-
- // ------------------------ Sigma_d
- Eigen::Matrix2s iSigma_d = (_feature_cov + _expected_feature_cov).inverse();
- Eigen::VectorXs squared_mahalanobis_distances(_mu.cols());
- for (unsigned int i = 0; i < _mu.cols(); i++)
- {
- squared_mahalanobis_distances(i) = (d - _mu.col(i)).transpose() * iSigma_d * (d - _mu.col(i));
- //if ((d - _mu.col(i)).norm() < 1)
- //{
- // std::cout << "distance: " << (d - _mu.col(i)).norm() << std::endl;
- // std::cout << "iSigma_d: " << std::endl << iSigma_d << std::endl;
- // std::cout << "mahalanobis: " << squared_mahalanobis_distances(i) << std::endl;
- //}
- }
-
- return squared_mahalanobis_distances;
-}
-
-Scalar ProcessorTrackerLandmarkPolyline::sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
- const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined)
-{
- /* Squared distance from B to the line A_aux-A (match evaluated is A-B)
- *
- * No matter if A_aux is defined or not.
- *
- * Case 1: B not defined
- *
- * The projection of B over the line AAaux must be in [A_aux, inf(in A direction)). Otherwise, return squared distance Aaux-B.
- * Check: the angle BAauxA is <= 90º:
- * (BA)² <= (BAaux)² + (AAaux)²
- *
- * Case 1.1: A not defined
- *
- * No more restrictions: return distance line to point.
- *
- * Case 1.2: A defined
- *
- * The projection of B over the line AAaux must be in (inf(in Aaux direction), A]. Otherwise, return squared distance A-B.
- * Additional check: the angle BAAaux is <= 90º:
- * (BAaux)² <= (BA)² + (AAaux)²
- *
- * Case 2: B is defined and A is not
- *
- * Returns the distance B-Line if the projection of B to the line is in [A, inf). Otherwise, return squared distance A-B.
- * Checks if the angle BAAaux is >= 90º: (BAaux)² >= (BA)² + (AAaux)²
- *
- * ( Case B and A are defined is not point-to-line, is point to point -> assertion )
- *
- */
-
- assert((!_A_defined || !_B_defined) && "ProcessorTrackerLandmarkPolyline::sqDistPointToLine: at least one point must not be defined.");
-
- Scalar AB_sq = (_B-_A).head<2>().squaredNorm();
- Scalar AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
- Scalar AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
-
- // Case 1
- if (!_B_defined)
- {
- if (AB_sq <= AauxB_sq + AAaux_sq)
- {
- // Case 1.1
- if (!_A_defined)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line
- // Case 1.2
- else if (AauxB_sq <= AB_sq + AAaux_sq)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line
- }
- }
- // Case 2
- else if (!_A_defined && _B_defined)
- if (AauxB_sq >= AB_sq + AAaux_sq)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line
-
- // Default return A-B squared distance
- return (_A.head<2>() - _B.head<2>()).squaredNorm();
-}
-
-void ProcessorTrackerLandmarkPolyline::createNewLandmarks()
-{
- //std::cout << "ProcessorTrackerLandmarkPolyline::createNewLandmarks" << std::endl;
- FeaturePolyline2DPtr polyline_ft_ptr;
- LandmarkBasePtr new_lmk_ptr;
- for (auto new_feature_ptr : new_features_last_)
- {
- // create new landmark
- new_lmk_ptr = createLandmark(new_feature_ptr);
- //std::cout << "\tfeature: " << new_feature_ptr->id() << std::endl;
- //std::cout << "\tnew_landmark: " << new_lmk_ptr->id() << ": "<< ((LandmarkPolyline2D*)new_lmk_ptr)->getNPoints() << " points" << std::endl;
- new_landmarks_.push_back(new_lmk_ptr);
- // cast
- polyline_ft_ptr = std::static_pointer_cast<FeaturePolyline2D>(new_feature_ptr);
- // create new correspondence
- LandmarkPolylineMatchPtr match = std::make_shared<LandmarkPolylineMatch>();
- match->feature_match_from_id_= 0; // all points match
- match->landmark_match_from_id_ = 0;
- match->feature_match_to_id_= polyline_ft_ptr->getNPoints()-1; // all points match
- match->landmark_match_to_id_ = polyline_ft_ptr->getNPoints()-1;
- match->normalized_score_ = 1.0; // max score
- match->landmark_ptr_ = new_lmk_ptr;
- matches_landmark_from_last_[new_feature_ptr] = match;
- }
-}
-
-LandmarkBasePtr ProcessorTrackerLandmarkPolyline::createLandmark(FeatureBasePtr _feature_ptr)
-{
- assert(_feature_ptr->getType() == "POLYLINE 2D");
- //std::cout << "ProcessorTrackerLandmarkPolyline::createLandmark" << std::endl;
- //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
- //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
-
- FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr);
- // compute feature global pose
- Eigen::MatrixXs points_global = R_world_sensor_ * polyline_ptr->getPoints().topRows<2>() +
- t_world_sensor_ * Eigen::VectorXs::Ones(polyline_ptr->getNPoints()).transpose();
-
- //std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl;
- //std::cout << "Landmark global points: " << std::endl << points_global << std::endl;
- //std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl;
-
- // Create new landmark
- return std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true), std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true), points_global, polyline_ptr->isFirstDefined(), polyline_ptr->isLastDefined());
-}
-
-ProcessorTrackerLandmarkPolyline::~ProcessorTrackerLandmarkPolyline()
-{
- while (!polylines_last_.empty())
- {
- polylines_last_.front()->remove();
- polylines_last_.pop_front(); // TODO see if this is needed
- }
- while (!polylines_incoming_.empty())
- {
- polylines_incoming_.front()->remove();
- polylines_incoming_.pop_front(); // TODO see if this is needed
- }
-}
-
-void ProcessorTrackerLandmarkPolyline::establishFactors()
-{
- //TODO: update with new index in landmarks
-
- //std::cout << "ProcessorTrackerLandmarkPolyline::establishFactors" << std::endl;
- LandmarkPolylineMatchPtr polyline_match;
- FeaturePolyline2DPtr polyline_feature;
- LandmarkPolyline2DPtr polyline_landmark;
-
- for (auto last_feature : last_ptr_->getFeatureList())
- {
- polyline_feature = std::static_pointer_cast<FeaturePolyline2D>(last_feature);
- polyline_match = std::static_pointer_cast<LandmarkPolylineMatch>(matches_landmark_from_last_[last_feature]);
- polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(polyline_match->landmark_ptr_);
-
- assert(polyline_landmark != nullptr && polyline_match != nullptr);
-
- // Modify landmark (only for not closed)
- if (!polyline_landmark->isClosed())
- {
- //std::cout << std::endl << "MODIFY LANDMARK" << std::endl;
- //std::cout << "feature " << polyline_feature->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl;
- //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl;
- //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl;
-
- Eigen::MatrixXs points_global = R_world_sensor_ * polyline_feature->getPoints().topRows<2>() +
- t_world_sensor_ * Eigen::VectorXs::Ones(polyline_feature->getNPoints()).transpose();
- // GROW/CLOSE LANDMARK
- // -----------------Front-----------------
- bool check_front_closing = // Sufficient conditions
- // condition 1: feature first defined point not matched
- (polyline_feature->isFirstDefined() && polyline_match->feature_match_from_id_ > 0) ||
- // condition 2: feature second point not matched
- (polyline_match->feature_match_from_id_ > 1) ||
- // condition 3: matched front points but feature front point defined and landmark front point not defined
- (polyline_match->landmark_match_from_id_ == polyline_landmark->getFirstId() && polyline_feature->isFirstDefined() && !polyline_landmark->isFirstDefined());
-
- // Check closing with landmark's last points
- if (check_front_closing)
- {
- //std::cout << "---------------- Trying to close polyline..." << std::endl;
- //std::cout << "feature " << polyline_feature->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl;
- //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl;
- //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl;
-
- int feat_point_id_matching = polyline_feature->isFirstDefined() ? 0 : 1;
- int lmk_last_defined_point = polyline_landmark->getLastId() - (polyline_landmark->isLastDefined() ? 0 : 1);
- //std::cout << std::endl << "\tfeat point matching " << feat_point_id_matching << std::endl;
- //std::cout << std::endl << "\tlmk last defined point " << lmk_last_defined_point << std::endl;
-
- for (int id_lmk = lmk_last_defined_point; id_lmk > polyline_match->landmark_match_to_id_; id_lmk--)
- {
- //std::cout << "\t\tid_lmk " << id_lmk << std::endl;
- //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_->position_error_th*params_->position_error_th << std::endl;
-
- if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_->position_error_th*params_->position_error_th)
- {
- std::cout << "CLOSING POLYLINE" << std::endl;
-
- unsigned int N_back_overlapped = polyline_landmark->getLastId() - id_lmk + 1;
- int N_feature_new_points = polyline_match->feature_match_from_id_ - feat_point_id_matching - N_back_overlapped;
-
- // define first point (if not defined and no points have to be merged)
- if (!polyline_landmark->isFirstDefined() && N_feature_new_points >= 0)
- polyline_landmark->setFirst(points_global.col(polyline_match->feature_match_from_id_), true);
-
- // add other points (if there are)
- if (N_feature_new_points > 0)
- polyline_landmark->addPoints(points_global.middleCols(feat_point_id_matching + N_back_overlapped, N_feature_new_points), // points matrix in global coordinates
- N_feature_new_points-1, // last index to be added
- true, // defined
- false); // front (!back)
-
- // define last point (if not defined and no points have to be merged)
- if (!polyline_landmark->isLastDefined() && N_feature_new_points >= 0)
- polyline_landmark->setLast(points_global.col(feat_point_id_matching + N_back_overlapped - 1), true);
-
- // close landmark
- polyline_landmark->setClosed();
-
- polyline_match->landmark_match_from_id_ = id_lmk - (polyline_feature->isFirstDefined() ? 0 : 1);
- polyline_match->feature_match_from_id_ = 0;
- break;
- }
- }
- }
- // Add new front points (if not matched feature points)
- if (polyline_match->feature_match_from_id_ > 0) // && !polyline_landmark->isClosed()
- {
- assert(!polyline_landmark->isClosed() && "feature not matched points in a closed landmark");
- //std::cout << "Add new front points. Defined: " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
-
- polyline_landmark->addPoints(points_global, // points matrix in global coordinates
- polyline_match->feature_match_from_id_-1, // last feature point index to be added
- polyline_feature->isFirstDefined(), // is defined
- false); // front
-
- polyline_match->landmark_match_from_id_ = polyline_landmark->getFirstId();
- polyline_match->feature_match_from_id_ = 0;
- //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
- }
- // Change first point
- else if (polyline_match->landmark_match_from_id_ == polyline_landmark->getFirstId() && !polyline_landmark->isFirstDefined())
- {
- //std::cout << "Change first point. Defined: " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tpoint " << polyline_landmark->getFirstId() << ": " << polyline_landmark->getPointVector(polyline_landmark->getFirstId()).transpose() << std::endl;
- //std::cout << "\tpoint " << polyline_landmark->getFirstId()+1 << ": " << polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1).transpose() << std::endl;
- //std::cout << "\tfeature point: " << points_global.topLeftCorner(2,1).transpose() << std::endl;
-
- if (// new defined first
- polyline_feature->isFirstDefined() ||
- // new not defined first
- (points_global.topLeftCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1)).squaredNorm() >
- (points_global.topLeftCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId())).squaredNorm() +
- (polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId())).squaredNorm())
- polyline_landmark->setFirst(points_global.leftCols(1), polyline_feature->isFirstDefined());
-
- }
- // -----------------Back-----------------
- bool check_back_closing = // Sufficient conditions
- // condition 1: feature last defined point not matched
- (polyline_feature->isLastDefined() && polyline_match->feature_match_to_id_ < polyline_feature->getNPoints()-1) ||
- // condition 2: feature second last point not matched
- (polyline_match->feature_match_to_id_ < polyline_feature->getNPoints() - 2) ||
- // condition 3: matched back points but feature last point defined and landmark last point not defined
- (polyline_match->landmark_match_to_id_ == polyline_landmark->getLastId() && polyline_feature->isLastDefined() && !polyline_landmark->isLastDefined());
-
- // Necessary condition: still open landmark
- check_back_closing = check_back_closing && !polyline_landmark->isClosed();
-
- // Check closing with landmark's first points
- if (check_back_closing)
- {
- int feat_point_id_matching = polyline_feature->getNPoints() - (polyline_feature->isLastDefined() ? 1 : 2);
- int lmk_first_defined_point = polyline_landmark->getFirstId() + (polyline_landmark->isFirstDefined() ? 0 : 1);
- //std::cout << std::endl << "\tfeat point matching " << feat_point_id_matching << std::endl;
- //std::cout << std::endl << "\tlmk first defined point " << lmk_first_defined_point << std::endl;
-
- for (int id_lmk = lmk_first_defined_point; id_lmk < polyline_match->landmark_match_from_id_; id_lmk++)
- {
- //std::cout << "\t\tid_lmk " << id_lmk << std::endl;
- //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_->position_error_th*params_->position_error_th << std::endl;
-
- if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_->position_error_th*params_->position_error_th)
- {
- std::cout << "CLOSING POLYLINE" << std::endl;
-
- unsigned int N_front_overlapped = id_lmk - polyline_landmark->getFirstId() + 1;
- int N_feature_new_points = feat_point_id_matching - polyline_match->feature_match_to_id_ - N_front_overlapped;
-
- // define first point (if not defined and no points have to be merged)
- if (!polyline_landmark->isFirstDefined() && N_feature_new_points >= 0)
- polyline_landmark->setFirst(points_global.col(feat_point_id_matching - N_front_overlapped + 1), true);
-
- // add other points (if there are)
- if (N_feature_new_points > 0)
- polyline_landmark->addPoints(points_global.middleCols(polyline_match->feature_match_to_id_ + 1, N_feature_new_points), // points matrix in global coordinates
- N_feature_new_points-1, // last index to be added
- true, // defined
- false); // front (!back)
-
- // define last point (if not defined and no points have to be merged)
- if (!polyline_landmark->isLastDefined() && N_feature_new_points >= 0)
- polyline_landmark->setLast(points_global.col(polyline_match->feature_match_to_id_), true);
-
- // close landmark
- polyline_landmark->setClosed();
-
- polyline_match->landmark_match_to_id_ = id_lmk + (polyline_feature->isLastDefined() ? 0 : 1);
- polyline_match->feature_match_to_id_ = polyline_feature->getNPoints() - 1;
- break;
- }
- }
- }
- // Add new back points (if it wasn't closed)
- if (polyline_match->feature_match_to_id_ < polyline_feature->getNPoints()-1)
- {
- assert(!polyline_landmark->isClosed() && "feature not matched points in a closed landmark");
- //std::cout << "Add back points. Defined: " << polyline_feature->isLastDefined() << std::endl;
- //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
-
- polyline_landmark->addPoints(points_global, // points matrix in global coordinates
- polyline_match->feature_match_to_id_+1, // last feature point index to be added
- polyline_feature->isLastDefined(), // is defined
- true); // back
-
- polyline_match->landmark_match_to_id_ = polyline_landmark->getLastId();
- polyline_match->feature_match_to_id_ = polyline_feature->getNPoints()-1;
- //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
- }
- // Change last point
- else if (polyline_match->landmark_match_to_id_ == polyline_landmark->getLastId() && !polyline_landmark->isLastDefined()) //&& polyline_match->feature_match_to_id_ == polyline_feature->getNPoints()-1
- {
- //std::cout << "Change last point. Defined: " << (polyline_feature->isLastDefined() ? 1 : 0) << std::endl;
- //std::cout << "\tpoint " << polyline_landmark->getLastId() << ": " << polyline_landmark->getPointVector(polyline_landmark->getLastId()).transpose() << std::endl;
- //std::cout << "\tpoint " << polyline_landmark->getLastId()-1 << ": " << polyline_landmark->getPointVector(polyline_landmark->getLastId()-1).transpose() << std::endl;
- //std::cout << "\tfeature point: " << points_global.topRightCorner(2,1).transpose() << std::endl;
- if (// new defined last
- polyline_feature->isLastDefined() ||
- // new not defined last
- (points_global.topRightCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getLastId()-1)).squaredNorm() >
- (points_global.topRightCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getLastId())).squaredNorm() +
- (polyline_landmark->getPointVector(polyline_landmark->getLastId()-1)-polyline_landmark->getPointVector(polyline_landmark->getLastId())).squaredNorm())
- polyline_landmark->setLast(points_global.rightCols(1), polyline_feature->isLastDefined());
- }
-
- //std::cout << "MODIFIED LANDMARK" << std::endl;
- //std::cout << "feature " << polyline_feature->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl;
- //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl;
- }
-
- // ESTABLISH CONSTRAINTS
- //std::cout << "ESTABLISH CONSTRAINTS" << std::endl;
- //std::cout << "\tfeature " << polyline_feature->id() << std::endl;
- //std::cout << "\tlandmark " << polyline_landmark->id() << std::endl;
- //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl;
-
- // Factors for all inner and defined feature points
- int lmk_point_id = polyline_match->landmark_match_from_id_;
-
- for (int ftr_point_id = 0; ftr_point_id < polyline_feature->getNPoints(); ftr_point_id++, lmk_point_id++)
- {
- if (lmk_point_id > polyline_landmark->getLastId())
- lmk_point_id -= polyline_landmark->getNPoints();
- if (lmk_point_id < polyline_landmark->getFirstId())
- lmk_point_id += polyline_landmark->getNPoints();
-
- //std::cout << "feature point " << ftr_point_id << std::endl;
- //std::cout << "landmark point " << lmk_point_id << std::endl;
-
- // First not defined point
- if (ftr_point_id == 0 && !polyline_feature->isFirstDefined())
- // first point to line factor
- {
- int lmk_next_point_id = lmk_point_id+1;
- if (lmk_next_point_id > polyline_landmark->getLastId())
- lmk_next_point_id -= polyline_landmark->getNPoints();
- //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_next_point_id << std::endl;
- last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
- ftr_point_id, lmk_point_id, lmk_next_point_id));
- //std::cout << "factor added" << std::endl;
- }
-
- // Last not defined point
- else if (ftr_point_id == polyline_feature->getNPoints()-1 && !polyline_feature->isLastDefined())
- // last point to line factor
- {
- int lmk_prev_point_id = lmk_point_id-1;
- if (lmk_prev_point_id < polyline_landmark->getFirstId())
- lmk_prev_point_id += polyline_landmark->getNPoints();
- //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl;
- last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
- ftr_point_id, lmk_point_id, lmk_prev_point_id));
- //std::cout << "factor added" << std::endl;
- }
-
- // Defined point
- else
- // point to point factor
- {
- //std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
- //std::cout << "landmark first id:" << polyline_landmark->getFirstId() << std::endl;
- //std::cout << "landmark last id:" << polyline_landmark->getLastId() << std::endl;
- //std::cout << "landmark n points:" << polyline_landmark->getNPoints() << std::endl;
- last_feature->addFactor(std::make_shared<FactorPoint2D>(polyline_feature, polyline_landmark, shared_from_this(),
- ftr_point_id, lmk_point_id));
- //std::cout << "factor added" << std::endl;
- }
- }
- //std::cout << "Factors established" << std::endl;
- }
-}
-
-void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBasePtrList& _lmk_list)
-{
- //std::cout << "ProcessorTrackerLandmarkPolyline::classifyPolilines: " << _lmk_list->size() << std::endl;
- std::vector<Scalar> object_L({12, 5.9, 1.2});
- std::vector<Scalar> object_W({2.345, 2.345, 0.9});
- std::vector<Scalar> object_D({sqrt(12*12+2.345*2.345), sqrt(5.9*5.9+2.345*2.345), sqrt(0.9*0.9+1.2*1.2)});
- std::vector<LandmarkClassification> object_class({CONTAINER, SMALL_CONTAINER, PALLET});
-
- for (auto lmk_ptr : _lmk_list)
- if (lmk_ptr->getType() == "POLYLINE 2D")
- {
- LandmarkPolyline2DPtr polyline_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_ptr);
- auto n_defined_points = polyline_ptr->getNPoints() - (polyline_ptr->isFirstDefined() ? 0 : 1) - (polyline_ptr->isLastDefined() ? 0 : 1);
- auto A_id = polyline_ptr->getFirstId() + (polyline_ptr->isFirstDefined() ? 0 : 1);
- auto B_id = A_id + 1;
- auto C_id = B_id + 1;
- auto D_id = C_id + 1;
-
- // necessary conditions
- if (polyline_ptr->getClassification() != UNCLASSIFIED ||
- n_defined_points < 3 ||
- n_defined_points > 4 )
- continue;
-
- //std::cout << "landmark " << lmk_ptr->id() << std::endl;
-
- // consider 3 first defined points
- Scalar dAB = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(B_id)).norm();
- Scalar dBC = (polyline_ptr->getPointVector(B_id) - polyline_ptr->getPointVector(C_id)).norm();
- Scalar dAC = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(C_id)).norm();
-
- //std::cout << "dAB = " << dAB << " error 1: " << fabs(dAB-CONT_L) << " error 2: " << fabs(dAB-CONT_W) << std::endl;
- //std::cout << "dBC = " << dBC << " error 1: " << fabs(dBC-CONT_W) << " error 2: " << fabs(dBC-CONT_L) << std::endl;
- //std::cout << "dAC = " << dAC << " error 1&2: " << fabs(dAC-CONT_D) << std::endl;
-
- auto classification = -1;
- bool configuration;
-
- for (unsigned int i = 0; i < object_L.size(); i++)
- {
- // check configuration 1
- if(fabs(dAB-object_L[i]) < params_->position_error_th &&
- fabs(dBC-object_W[i]) < params_->position_error_th &&
- fabs(dAC-object_D[i]) < params_->position_error_th)
- {
- configuration = true;
- classification = i;
- break;
- }
-
- // check configuration 2
- if(fabs(dAB-object_W[i]) < params_->position_error_th &&
- fabs(dBC-object_L[i]) < params_->position_error_th &&
- fabs(dAC-object_D[i]) < params_->position_error_th)
- {
- configuration = false;
- classification = i;
- break;
- }
- }
-
- // any container position fitted
- if (classification != -1)
- {
- // if 4 defined checking
- if (n_defined_points == 4)
- {
- //std::cout << "checking with 4th point... " << std::endl;
-
- Scalar dAD = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(D_id)).norm();
- Scalar dBD = (polyline_ptr->getPointVector(B_id) - polyline_ptr->getPointVector(D_id)).norm();
- Scalar dCD = (polyline_ptr->getPointVector(C_id) - polyline_ptr->getPointVector(D_id)).norm();
-
- // necessary conditions
- if (fabs(dAD-dBC) > params_->position_error_th ||
- fabs(dBD-dAC) > params_->position_error_th ||
- fabs(dCD-dAB) > params_->position_error_th)
- continue;
- }
-
- // if not 4 defined add/define points
- else
- {
- //std::cout << "adding/defining points... " << std::endl;
- if (!polyline_ptr->isFirstDefined())
- {
- polyline_ptr->defineExtreme(false);
- D_id = polyline_ptr->getFirstId();
- }
- else if (!polyline_ptr->isLastDefined())
- polyline_ptr->defineExtreme(true);
- else
- polyline_ptr->addPoint(Eigen::Vector2s::Zero(), true, true);
- }
- //std::cout << "landmark " << lmk_ptr->id() << " classified as " << object_class[classification] << " in configuration " << configuration << std::endl;
-
- // Close
- polyline_ptr->setClosed();
-
- // Classify
- polyline_ptr->classify(object_class[classification]);
-
- // Unfix origin
- polyline_ptr->getP()->unfix();
- polyline_ptr->getO()->unfix();
-
- // Move origin to B
- polyline_ptr->getP()->setState(polyline_ptr->getPointVector((configuration ? B_id : A_id)));
- Eigen::Vector2s frame_x = (configuration ? polyline_ptr->getPointVector(A_id)-polyline_ptr->getPointVector(B_id) : polyline_ptr->getPointVector(C_id)-polyline_ptr->getPointVector(B_id));
- polyline_ptr->getO()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0))));
-
- //std::cout << "A: " << polyline_ptr->getPointVector(A_id).transpose() << std::endl;
- //std::cout << "B: " << polyline_ptr->getPointVector(B_id).transpose() << std::endl;
- //std::cout << "C: " << polyline_ptr->getPointVector(C_id).transpose() << std::endl;
- //std::cout << "frame_x: " << frame_x.transpose() << std::endl;
- //std::cout << "frame position: " << polyline_ptr->getP()->getVector().transpose() << std::endl;
- //std::cout << "frame orientation: " << polyline_ptr->getO()->getVector() << std::endl;
-
- // Fix polyline points to its respective relative positions
- if (configuration)
- {
- polyline_ptr->getPointStateBlock(A_id)->setState(Eigen::Vector2s(object_L[classification], 0));
- polyline_ptr->getPointStateBlock(B_id)->setState(Eigen::Vector2s(0, 0));
- polyline_ptr->getPointStateBlock(C_id)->setState(Eigen::Vector2s(0, object_W[classification]));
- polyline_ptr->getPointStateBlock(D_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification]));
- }
- else
- {
- polyline_ptr->getPointStateBlock(A_id)->setState(Eigen::Vector2s(0, 0));
- polyline_ptr->getPointStateBlock(B_id)->setState(Eigen::Vector2s(0, object_W[classification]));
- polyline_ptr->getPointStateBlock(C_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification]));
- polyline_ptr->getPointStateBlock(D_id)->setState(Eigen::Vector2s(object_L[classification], 0));
- }
- for (auto id = polyline_ptr->getFirstId(); id <= polyline_ptr->getLastId(); id++)
- {
- polyline_ptr->getPointStateBlock(id)->fix();
- }
- }
- }
-}
-
-void ProcessorTrackerLandmarkPolyline::postProcess()
-{
- //std::cout << "postProcess: " << std::endl;
- //std::cout << "New Last features: " << getNewFeaturesListLast().size() << std::endl;
- //std::cout << "Last features: " << last_ptr_->getFeatureList().size() << std::endl;
- classifyPolilines(getProblem()->getMap()->getLandmarkList());
-}
-
-FactorBasePtr ProcessorTrackerLandmarkPolyline::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
-{
- // not used
- return nullptr;
-}
-
-ProcessorBasePtr ProcessorTrackerLandmarkPolyline::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
-{
- ProcessorParamsPolylinePtr params = std::static_pointer_cast<ProcessorParamsPolyline>(_params);
- ProcessorTrackerLandmarkPolylinePtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkPolyline>(params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-} //namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("POLYLINE", ProcessorTrackerLandmarkPolyline)
-} // namespace wolf
diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp
deleted file mode 100644
index a7a5677af497ccb2f925dce9ae993e160b860fd5..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_laser_2D.cpp
+++ /dev/null
@@ -1,90 +0,0 @@
-#include "base/sensor/sensor_laser_2D.h"
-#include "base/state_block.h"
-
-namespace wolf {
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
- SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8)
-{
- setDefaultScanParams();
-}
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _angle_min, const double& _angle_max, const double& _angle_step, const double& _scan_time, const double& _range_min, const double& _range_max, const double& _range_std_dev, const double& _angle_std_dev) :
- SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8),
- scan_params_({ _angle_min, _angle_max, _angle_step, _scan_time, _range_min, _range_max, _range_std_dev, _angle_std_dev })
-{
- //
-}
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params) :
- SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8),
- scan_params_(_params)
-{
- //
-}
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params) :
- SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8),
- scan_params_(_params.scan_params)
-{
- //
-}
-
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params) :
- SensorLaser2D(_p_ptr, _o_ptr, *_params)
-{
- //
-}
-
-SensorLaser2D::~SensorLaser2D()
-{
- //
-}
-
-void SensorLaser2D::setDefaultScanParams()
-{
- scan_params_.angle_min_ = M_PI_2;
- scan_params_.angle_max_ = -M_PI_2;
- scan_params_.angle_step_ = -M_PI/720;
- scan_params_.scan_time_ = 0.01;//not relevant
- scan_params_.range_min_ = 0.2;
- scan_params_.range_max_ = 100;
- scan_params_.range_std_dev_ = 0.01;
-
- setNoiseStd(Eigen::VectorXs::Constant(1,scan_params_.range_std_dev_));
-}
-
-void SensorLaser2D::setScanParams(const laserscanutils::LaserScanParams & _params)
-{
- scan_params_ = _params;
-}
-
-const laserscanutils::LaserScanParams& SensorLaser2D::getScanParams() const
-{
- return scan_params_;
-}
-
-// Define the factory method
-SensorBasePtr SensorLaser2D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
- StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
- // cast intrinsics into derived type
- IntrinsicsLaser2DPtr params = std::static_pointer_cast<IntrinsicsLaser2D>(_intrinsics);
- SensorLaser2DPtr sen = std::make_shared<SensorLaser2D>(pos_ptr, ori_ptr, params->scan_params);
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory and the ParameterFactory
-#include "base/sensor/sensor_factory.h"
-//#include "intrinsics_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("LASER 2D", SensorLaser2D)
-//const bool registered_laser_params = IntrinsicsFactory::get().registerCreator("LASER 2D", createIntrinsicsLaser2D);
-} // namespace wolf