fiducial_tracker.hpp

// Copyright 2023 Jacob Hartzer
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.
 
#ifndef TRACKERS__FIDUCIAL_TRACKER_HPP_
#define TRACKERS__FIDUCIAL_TRACKER_HPP_
 
#include <map>
#include <memory>
#include <string>
#include <vector>
 
#include <opencv2/aruco.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/opencv.hpp>
 
#include "ekf/ekf.hpp"
#include "ekf/types.hpp"
#include "ekf/update/fiducial_updater.hpp"
#include "infrastructure/debug_logger.hpp"
#include "trackers/tracker.hpp"
 
enum class FiducialType
{
  ARUCO_BOARD,
  CHARUCO_BOARD
};
 
class FiducialTracker : public Tracker
{
public:
  typedef struct Parameters : public Tracker::Parameters
  {
    FiducialType detector_type;                     
    unsigned int predefined_dict{0};                
    unsigned int squares_x {1};                     
    unsigned int squares_y {1};                     
    double square_length {1.0};                     
    double marker_length {1.0};                     
    unsigned int id{0};                             
    Eigen::Vector3d pos_f_in_l;                     
    Eigen::Quaterniond ang_f_to_l;                  
    bool is_extrinsic{false};                       
    Eigen::VectorXd variance {{1, 1, 1, 1, 1, 1}};  
    bool is_cam_extrinsic{false};  
  } Parameters;
 
  explicit FiducialTracker(FiducialTracker::Parameters params);
 
  void Track(
    double time,
    unsigned int frame_id,
    const cv::Mat & img_in,
    cv::Mat & img_out
  );
 
  int InterpolateCorners(
    std::vector<std::vector<cv::Point2f>> & marker_corners,
    std::vector<int> & marker_ids,
    cv::Mat image,
    cv::Ptr<cv::aruco::Board> & board,
    std::vector<cv::Point2f> & corners,
    std::vector<int> & ids,
    cv::Mat camera_matrix,
    cv::Mat dist_coefficients
  ) const;
 
  void DrawDetectedCorners(
    cv::Mat image,
    std::vector<std::vector<cv::Point2f>> & marker_corners,
    std::vector<cv::Point2f> & corners,
    std::vector<int> & ids,
    cv::Scalar corner_color
  ) const;
 
  bool EstimatePoseBoard(
    std::vector<std::vector<cv::Point2f>> & marker_corners,
    cv::InputArray & corners,
    cv::InputArray & ids,
    cv::Ptr<cv::aruco::Board> board,
    cv::InputArray & camera_matrix,
    cv::InputArray & dist_coefficients,
    cv::Vec3d & r_vec,
    cv::Vec3d & t_vec
  ) const;
 
  cv::Ptr<cv::aruco::Dictionary> m_dict;  
  cv::Ptr<cv::aruco::Board> m_board;      
 
protected:
  FiducialUpdater m_fiducial_updater;  
  FiducialType m_detector_type;        
 
private:
  Eigen::Vector3d m_pos_error;
  Eigen::Vector3d m_ang_error;
};
 
#endif  // TRACKERS__FIDUCIAL_TRACKER_HPP_