Program Listing for File fractalposetracker.h
↰ Return to documentation for file (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