Program Listing for File posetracker.h

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

#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