Program Listing for File fractalposetracker.h

↰ Return to documentation for file (/tmp/ws/src/aruco_ros/aruco/include/aruco/fractallabelers/fractalposetracker.h)

#include <opencv2/imgproc/imgproc.hpp>
#include "../picoflann.h"
#include "../cameraparameters.h"
#include "../markerdetector.h"
#include "fractalmarkerset.h"
#include "../aruco_export.h"
namespace aruco
{


void kfilter(std::vector<cv::KeyPoint> &kpoints);

void assignClass(const cv::Mat &im, std::vector<cv::KeyPoint> &kpoints,
                 float sizeNorm = 0.f, int wsize = 5);

struct PicoFlann_KeyPointAdapter
{
  inline float operator()(const cv::KeyPoint &elem, int dim) const
  {
    return dim == 0 ? elem.pt.x : elem.pt.y;
  }
  inline float operator()(const cv::Point2f &elem, int dim) const
  {
    return dim == 0 ? elem.x : elem.y;
  }
};

class ARUCO_EXPORT FractalPoseTracker
{
public:
  FractalPoseTracker();
  void setParams(const CameraParameters &cam_params, const FractalMarkerSet &msconf,
                 const float markerSize = -1);
  bool fractalInnerPose(const cv::Ptr<MarkerDetector> markerDetector,
                        const std::vector<aruco::Marker> &markers, bool refinement = true);
  bool ROI(const std::vector<cv::Mat> imagePyramid, cv::Mat &img,
           std::vector<cv::Point2f> &innerPoints2d, cv::Point2f &offset, float &ratio);

  cv::Mat fractal_solve_ransac(int ninners,
                               std::vector<std::pair<uint, std::vector<uint>>> inner_kpnt,
                               std::vector<cv::KeyPoint> kpnts, uint32_t maxIter = 500,
                               float _minInliers = 0.2f, float _thresInliers = 0.7f);

  void drawKeyPoints(const cv::Mat image, std::vector<cv::KeyPoint> kpoints,
                     bool text = false, bool transf = false);

  bool fractalRefinement(const cv::Ptr<MarkerDetector> markerDetector, int markerWarpPix = 10);

  // return the rotation vector. Returns an empty matrix if last call to estimatePose returned false
  const cv::Mat getRvec() const
  {
    return _rvec;
  }

  // return the translation vector. Returns an empty matrix if last call to estimatePose returned false
  const cv::Mat getTvec() const
  {
    return _tvec;
  }

  // return all corners from fractal marker
  const std::vector<cv::Point3f> getInner3d()
  {
    return _innerp3d;
  }

  // is the pose valid?
  bool isPoseValid() const
  {
    return !_rvec.empty() && !_tvec.empty();
  }

  FractalMarkerSet getFractal()
  {
    return _fractalMarker;
  }

private:
  FractalMarkerSet _fractalMarker;                       // FractalMarkerSet configuration
  aruco::CameraParameters _cam_params;                   // Camera parameters.
  cv::Mat _rvec, _tvec;                                  // current poses
  std::map<int, std::vector<cv::Point3f>> _id_innerp3d;  // Id_marker-Inners_corners
  std::vector<cv::Point3f> _innerp3d;                    // All inners corners
  std::vector<cv::KeyPoint> _innerkpoints;               // All inners keypoints
  picoflann::KdTreeIndex<2, PicoFlann_KeyPointAdapter> _kdtree;
  std::map<int, double> _id_radius;  // Idmarker_Radius(Optimus)
  std::map<int, float> _id_area;     // Idmarker_projectedArea(Optimus)
  float _preRadius = 0;              // radius used previous iteration
};
}  // namespace aruco