posetracker.h

Go to the documentation of this file.

 
#ifndef ARUCO_POSETRACKER
#define ARUCO_POSETRACKER
 
#include "aruco_export.h"
#include "cameraparameters.h"
#include "marker.h"
#include "markermap.h"
 
#include <map>
#include <opencv2/core/core.hpp>
 
namespace aruco
{
class ARUCO_EXPORT MarkerPoseTracker
{
public:
  bool estimatePose(Marker& m, const CameraParameters& cam_params, float markerSize,
                    float minErrorRatio = 10 /*tau_e in paper*/);
 
  // returns the 4x4 transform matrix. Returns an empty matrix if last call to
  // estimatePose returned false
  cv::Mat getRTMatrix() const;
  // 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;
  }
 
private:
  cv::Mat _rvec, _tvec;  // current poses
  double solve_pnp(const std::vector<cv::Point3f>& p3d,
                   const std::vector<cv::Point2f>& p2d, const cv::Mat& cam_matrix,
                   const cv::Mat& dist, cv::Mat& r_io, cv::Mat& t_io);
};
class ARUCO_EXPORT MarkerMapPoseTracker
{
public:
  MarkerMapPoseTracker();
  // Sets the parameters required for operation
  // If the msconf has data expressed in meters, then the markerSize parameter is not
  // required. If it is in
  // pixels, the markersize will be used to
  // transform to meters
  // Throws exception if wrong configuraiton
  void setParams(const CameraParameters& cam_params, const MarkerMap& msconf,
                 float markerSize = -1);
  // indicates if the call to setParams has been successfull and this object is ready to
  // call estimatePose
  bool isValid() const
  {
    return _isValid;
  }
 
  // resets current state
  void reset()
  {
    _isValid = false;
    _rvec = cv::Mat();
    _tvec = cv::Mat();
  }
 
  // estimates camera pose wrt the markermap
  // returns true if pose has been obtained and false otherwise
  bool estimatePose(const std::vector<Marker>& v_m);
 
  // returns the 4x4 transform matrix. Returns an empty matrix if last call to
  // estimatePose returned false
  cv::Mat getRTMatrix() const;
  // 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;
  }
  // prevents from big jumps. If the difference between current and previous positions are
  // greater than the value indicated
  // assumes no good tracking and the pose will be set as null
  void setMaxTrackingDifference(float maxTranslation, float maxAngle)
  {
    _maxTranslation = maxTranslation;
    _maxAngle = maxAngle;
  }
 
  void setMinErrorRatio(float minErrorRatio)
  {
    aruco_minerrratio_valid = minErrorRatio;
  }
 
  double getInitialErr()
  {
    return _initial_err;
  }
 
protected:
  cv::Mat _rvec, _tvec;  // current poses
  aruco::CameraParameters _cam_params;
  MarkerMap _msconf;
  std::map<int, Marker3DInfo> _map_mm;
  bool _isValid;
  cv::Mat relocalization(const std::vector<Marker>& v_m);
  float aruco_minerrratio_valid;           /*tau_e in paper*/
  std::map<uint32_t, cv::Mat> marker_m2g;  // for each marker, the transform from the
                                           // global ref system to the marker ref system
  float _maxTranslation = -1, _maxAngle = -1;
  double _initial_err;
};
};  // namespace aruco
 
#endif