RelativeTrackerPoseRegistrationEstimator.hh

 
#ifndef GM_TRACK_RELATIVETRACKERPOSEREGISTRATIONESTIMATOR
#define GM_TRACK_RELATIVETRACKERPOSEREGISTRATIONESTIMATOR
 
#include <gmTrack/config.hh>
 
#include <gmCore/io_eigen.hh>
#include <gmCore/OFactory.hh>
#include <gmCore/Updateable.hh>
 
#include <gmTrack/MultiPoseTracker.hh>
#include <gmTrack/SinglePoseTracker.hh>
#include <gmTrack/MultiPoseTracker.hh>
 
BEGIN_NAMESPACE_GMTRACK;
 
class RelativeTrackerPoseRegistrationEstimator : public gmCore::Object {
 
public:
  RelativeTrackerPoseRegistrationEstimator();
  ~RelativeTrackerPoseRegistrationEstimator();
 
  void addSinglePoseTracker(std::shared_ptr<SinglePoseTracker> tracker);
 
  void addMultiPoseTracker(std::shared_ptr<MultiPoseTracker> tracker);
 
  void setSamplesPerSecond(float n);
 
  void setWarningThreshold(float d);
 
  void setInlierThreshold(float r);
 
  void setOrientationWarningThreshold(float d);
 
  void setOrientationInlierThreshold(float r);
 
  struct Pose {
    Eigen::Vector3f position;
    Eigen::Quaternionf orientation;
  };
 
  size_t getSinglePoseTrackerCount() const;
 
  Pose getSinglePoseTrackerPose(size_t idx) const;
 
  size_t getMultiPoseTrackerCount() const;
 
  std::map<int, Pose> getMultiPoseTrackerPose(size_t idx) const;
 
  void traverse(Visitor *visitor) override;
 
  GM_OFI_DECLARE;
 
private:
  struct Impl;
  std::unique_ptr<Impl> _impl;
};
 
std::ostream &
operator<<(std::ostream &out,
           const RelativeTrackerPoseRegistrationEstimator::Pose &pose) {
  Eigen::AngleAxisf aa(pose.orientation);
  return out << "TPose(P=" << pose.position.transpose() << ", R=" << aa.angle()
             << "/" << aa.axis().transpose() << ")";
}
 
END_NAMESPACE_GMTRACK;
 
#endif