Program Listing for File markermap.h

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

#ifndef _Aruco_MarkerMap_h
#define _Aruco_MarkerMap_h

#include "aruco_export.h"
#include "marker.h"

#include <opencv2/core/core.hpp>

#include <string>
#include <vector>

namespace aruco
{
class ARUCO_EXPORT Marker3DInfo
{
public:
  std::vector<cv::Point3f> points;
  int id;  // maker id

  Marker3DInfo();
  Marker3DInfo(int _id);
  inline bool operator==(const Marker3DInfo& MI)
  {
    return id == MI.id;
  }

  // returns the distance of the marker side
  inline float getMarkerSize() const
  {
    return static_cast<float>(cv::norm(points[0] - points[1]));
  }
  inline cv::Point3f at(size_t idx) const
  {
    return points.at(idx);
  }
  inline cv::Point3f& operator[](size_t idx)
  {
    return points[idx];
  }
  inline const cv::Point3f& operator[](size_t idx) const
  {
    return points[idx];
  }
  inline void push_back(const cv::Point3f& p)
  {
    points.push_back(p);
  }
  inline size_t size() const
  {
    return points.size();
  }

public:
  inline void toStream(std::ostream& str)
  {
    str << id << " " << size() << " ";
    for (size_t i = 0; i < size(); i++)
      str << at(i).x << " " << at(i).y << " " << at(i).z << " ";
  }
  inline void fromStream(std::istream& str)
  {
    int s;
    str >> id >> s;
    points.resize(s);
    for (size_t i = 0; i < size(); i++)
      str >> points[i].x >> points[i].y >> points[i].z;
  }
};

class ARUCO_EXPORT MarkerMap : public std::vector<Marker3DInfo>
{
public:
  MarkerMap();

  MarkerMap(std::string filePath);

  bool isExpressedInMeters() const
  {
    return mInfoType == METERS;
  }
  bool isExpressedInPixels() const
  {
    return mInfoType == PIX;
  }
  MarkerMap convertToMeters(float markerSize) const;
  // simple way of knowing which elements detected in an image are from this markermap
  // returns the indices of the elements in the vector 'markers' that belong to this set
  // Example: The set has the elements with ids 10,21,31,41,92
  // The input vector has the markers with ids 10,88,9,12,41
  // function returns {0,4}, because element 0 (10) of the vector belongs to the set, and
  // also element 4 (41) belongs to the set
  std::vector<int> getIndices(const vector<Marker>& markers) const;

  const Marker3DInfo& getMarker3DInfo(int id) const;

  int getIndexOfMarkerId(int id) const;
  void getIdList(vector<int>& ids, bool append = true) const;

  cv::Mat getImage(float METER2PIX = 0) const;

  void saveToFile(std::string sfile);
  void readFromFile(std::string sfile);

  // calculates the camera location w.r.t. the map using the information provided
  // returns the <rvec,tvec>
  std::pair<cv::Mat, cv::Mat> calculateExtrinsics(const std::vector<aruco::Marker>& markers,
                                                  float markerSize, cv::Mat CameraMatrix,
                                                  cv::Mat Distorsion);

  // returns string indicating the dictionary
  inline std::string getDictionary() const
  {
    return dictionary;
  }

  enum Marker3DInfoType
  {
    NONE = -1,
    PIX = 0,
    METERS = 1
  };  // indicates if the data in MakersInfo is expressed in meters or in pixels so as to do conversion internally
  // returns string indicating the dictionary
  void setDictionary(std::string d)
  {
    dictionary = d;
  }

  // variable indicates if the data in MakersInfo is expressed in meters or in pixels so
  // as to do conversion internally
  int mInfoType;

private:
  // dictionary it belongs to (if any)
  std::string dictionary;

private:
  void saveToFile(cv::FileStorage& fs);
  void readFromFile(cv::FileStorage& fs);

public:
  void toStream(std::ostream& str);
  void fromStream(std::istream& str);
};
}  // namespace aruco

#endif