--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trajectorymanagement/src/Trajectory.h	Mon Feb 22 22:09:35 2021 -0500
@@ -0,0 +1,504 @@
+#ifndef TRAJECTORY_H_
+#define TRAJECTORY_H_
+
+#include "TrajectoryElement.h"
+#include "TrajectoryExceptions.h"
+
+#include "opencv2/core/core.hpp"
+
+#include <ostream>
+#include <vector>
+#include <cassert>
+#include <algorithm>
+
+/**
+ * Trajectory class.
+ *
+ * The Trajectory class is a container to keep information about a trajectory.
+ * defined as a sequence of not necessarily consecutive trajectory elements (instand + point coordinates)
+ * 
+ * \todo check (and remove) stuff about ascending frame numbers
+ */
+template<typename Type>
+class Trajectory
+{
+public:
+  /// Constructor.
+ Trajectory() : id(0), checkAscFrameNumber(false) {}
+
+  /**
+   * Constructor.
+   *
+   * @param trajectory trajectory
+   */
+  Trajectory(const Trajectory &itrajectory) {
+    setId(itrajectory.getId());
+    for (unsigned int i = 0; i < itrajectory.size(); ++i)
+      trajectory.push_back(itrajectory.getTrajectoryElement(i)); // justified because we directly copy the first and last instants
+    
+    setCheckAscFrameNumber(itrajectory.getCheckAscFrameNumber());
+  }
+
+  /**
+   * Constructor.
+   *
+   * @param trajectory trajectory
+   */
+ Trajectory(const int& id, const std::vector<TrajectoryElement<Type> >& trajectoryElements):
+  id(id), checkAscFrameNumber(false) {
+    for (typename std::vector<TrajectoryElement<Type> >::const_iterator iter = trajectoryElements.begin();
+	 iter != trajectoryElements.end(); iter++)
+      add(*iter);
+  }
+
+  /**
+   * Get id of the trajectory.
+   *
+   * @return id
+   */
+  unsigned int getId(void) const { return id;}
+
+  /**
+   * Set id of the trajectory.
+   *
+   * @param[in] id id
+   */
+  void setId(const unsigned int& iid) { id = iid;}
+
+  /**
+   * Get element of the trajectory.
+   *
+   * @param[in] position index of the element of the trajectory
+   * @return element of the trajectory#include "OpenCVPointTypeSupport.h"
+   */
+  const TrajectoryElement<Type> getTrajectoryElement(unsigned int position) const
+  {
+    return trajectory[position];
+  }
+  
+  /**
+   * Add an element to the trajectory.
+   *
+   * @param[in] frameNumber number of the frame of the new element of the trajectory
+   * @param[in] point position of the new element of the trajectory
+   */
+  void add(unsigned int frameNumber, const Type &point)
+  {
+    TrajectoryElement<Type> trajectoryElement = TrajectoryElement<Type> (frameNumber, point);
+    
+    if (getCheckAscFrameNumber())
+      {
+	ascFrameNumberAddCheck(frameNumber);
+      }
+    trajectory.push_back(trajectoryElement);
+  }
+  
+  /**
+   * Add an element to the trajectory.
+   *
+   * @param[in] point position of the new element of the trajectory
+   */
+  void add(const Type &point)
+  {
+    unsigned int frameNumber = 1;
+    add(frameNumber, point);
+  }
+  
+  void add(const TrajectoryElement<Type> &trajectoryElement)
+  {
+    int frameNumber = trajectoryElement.getFrameNumber();
+    if (getCheckAscFrameNumber())
+      {
+	ascFrameNumberAddCheck(frameNumber);
+      }
+    trajectory.push_back(trajectoryElement);
+  }
+
+  /**
+   * Get number of the frame of the element of the trajectory.
+   *
+   * @param[in] position index of the elesize_t ment of the trajectory
+   * @return number of the frame of the element of the trajectory.
+   */
+  unsigned int getFrameNumber(unsigned int position) const
+  {
+    return trajectory[position].getFrameNumber();
+  }
+
+  /**
+   * Get position of the element of the trajectory.
+   *
+   * @param[in] position index of the element of the trajectory
+   * @return position of the element of the trajectory
+   */
+  const Type &getPoint(unsigned int position) const
+  {
+    return trajectory[position].getPoint();
+  }
+
+  /** Get position of the element of the trajectory. */
+  const Type& getPointAtInstant(const unsigned int& t) const
+  {
+    typename std::vector<TrajectoryElement<Type> >::const_iterator iter = trajectory.begin();
+    while (iter != trajectory.end() && iter->getFrameNumber() != t)      
+      iter++;
+    if (iter != trajectory.end())
+      return iter->getPoint();
+    else {
+      throw TrajectoryOutOfRangeErrorException(t);
+    }
+  }
+
+  bool getCheckAscFrameNumber() const
+  {
+    return checkAscFrameNumber;
+  }
+
+  void setCheckAscFrameNumber(bool icheckAscFrameNumber)
+  {
+    if (icheckAscFrameNumber)
+      {
+	ascFrameNumberCheck();
+      }
+
+    checkAscFrameNumber = icheckAscFrameNumber;
+  }
+
+  /**
+   * Get number of elements in the trajectory.
+   *
+   * @return number of elements in the trajectory
+   */
+  unsigned int size() const
+  {
+    return trajectory.size();
+  }
+
+  bool empty() const
+  {
+    return trajectory.empty();
+  }
+
+  void insert(unsigned int position, unsigned int frameNumber, const Type &point)
+  {
+    trajectoryRangeLeEqCheck(position);
+
+    if (getCheckAscFrameNumber())
+      {
+	ascFrameNumberInsertCheck(position, frameNumber);
+      }
+
+    trajectory.insert(trajectory.begin() + position, TrajectoryElement<Type> (frameNumber, point));
+  }
+
+  /**
+   * Erase an element from the trajectory.
+   *TrajectoryFrameNumberErrorException
+   * @param[in] position index of the trajectory to be removed
+   */
+  void erase(unsigned int position)
+  {
+    trajectoryRangeLeCheck(position);
+    trajectory.erase(trajectory.begin() + position);
+  }
+
+  /**
+   * Erase the last element from the trajectory.
+   */
+  void pop_back()
+  {
+    trajectoryNotEmptyCheck();
+    trajectory.pop_back();
+  }
+
+  /**
+   * Clear the trajectory.
+   */
+  void clear()
+  {
+    trajectory.clear();
+  }
+
+  /**
+   * Get position of the element of the trajectory.
+   *
+   * @param[in] i index of the element of the trajectory
+   * @return position of the element of the trajectory
+   */
+  Type operator [](unsigned int i) const
+  {
+    return trajectory[i].getPoint();
+  }
+
+  /**
+   * Get position of the element of the trajectory.
+   *
+   * @param[in] i index of the element of the trajectory
+   * @return position of the element of the trajectory
+   */
+  const Type& at(unsigned int i) const
+  {
+    trajectoryRangeLeCheck(i);
+    return trajectory[i].getPoint();
+  }
+
+  /**
+   * Shift each element of the trajectory.
+   *
+   * @param[in] t vector by which the position of each element of the trajectory should be shifted.
+   */
+  void shift(const Type& t)
+  {
+    for (unsigned int i = 0; i < trajectory.size(); ++i)
+      {
+	trajectory[i].shift(t);
+      }
+  }
+
+   /** Computes the first and last instants */
+  void computeInstants(unsigned int& firstInstant, unsigned int& lastInstant) {
+    if (trajectory.empty()) {
+      firstInstant = 0;
+      lastInstant = 0;
+    } else {
+      typename std::vector<TrajectoryElement<Type> >::iterator iter = trajectory.begin();
+      firstInstant = iter->getFrameNumber();
+      lastInstant = iter->getFrameNumber();
+      iter++;
+      while (iter != trajectory.end()) {
+	unsigned int frameNumber = iter->getFrameNumber();
+	if (frameNumber < firstInstant)
+	  firstInstant = frameNumber;
+	else if (frameNumber > lastInstant)
+	  lastInstant = frameNumber;
+	iter++;
+      }
+    }
+  }
+
+  /** Smoothes the trajectory positions (parameter is the half-window used for moving average) */
+  void movingAverage(const unsigned int& nFramesSmoothing) {
+    if (!trajectory.empty() && nFramesSmoothing >= 1) {
+      // todo check the frames are in increasing order
+      std::vector<TrajectoryElement<Type> > smoothedTrajectory;
+      unsigned int nPositions = trajectory.size();
+      for(unsigned int i=0; i<nPositions; ++i) {
+      	Type p(0,0);
+      	unsigned int delta = std::min(nFramesSmoothing, std::min(i, nPositions-1-i));
+      	for (unsigned int j=i-delta; j<=i+delta; ++j)
+      	  p = p+getPoint(j);
+      	smoothedTrajectory.push_back(TrajectoryElement<Type>(trajectory[i].getFrameNumber(), p*(1./(1.+2.*static_cast<float>(delta)))));
+      }
+      trajectory = smoothedTrajectory;
+    }
+  }
+
+protected:
+  /**
+   * Check the length of the trajectories.
+   *
+   * @param[in] a the size of the trajectory
+   * @param[in] b the size of the trajectory
+   */
+  void trajectoryNotEmptyCheck() const
+  {
+    if (trajectory.size() == 0)
+      {
+	throw TrajectoryLengthErrorException();
+      }
+  }
+
+  /**
+   * Check the range of the trajectory.
+   *
+   * @param[in] n index of the element of the trajectory
+   */
+  void trajectoryRangeLeEqCheck(unsigned int n) const
+  {
+    if (n > trajectory.size())
+      {
+	throw TrajectoryOutOfRangeErrorException();
+      }
+  }
+
+  /**
+   * Check the range of the trajectory.
+   *
+   * @param[in] n index of the element of the trajectory
+   */
+  void trajectoryRangeLeCheck(unsigned int n) const
+  {
+    if (n >= trajectory.size())
+      {
+	throw TrajectoryOutOfRangeErrorException();
+      }
+  }
+
+  void ascFrameNumberCheck(unsigned int prevFrameNumber, unsigned int currFrameNumber) const
+  {
+    if (prevFrameNumber >= currFrameNumber)
+      {
+	throw TrajectoryFrameNumberErrorException();
+      }
+  }
+
+  void ascFrameNumberCheck() const
+  {
+    for (unsigned int i = 1; i < trajectory.size(); ++i)
+      {
+	ascFrameNumberCheck(trajectory[i - 1].getFrameNumber(), trajectory[i].getFrameNumber());
+      }
+  }
+
+  void ascFrameNumberAddCheck(unsigned int frameNumber) const
+  {
+    if (!trajectory.empty())
+      {
+	ascFrameNumberCheck(trajectory.back().getFrameNumber(), frameNumber);
+      }
+  }
+
+  void ascFrameNumberInsertCheck(unsigned int position, unsigned int frameNumber) const
+  {
+    if (position > 0)
+      {
+	ascFrameNumberCheck(trajectory[position - 1].getFrameNumber(), frameNumber);
+      }
+
+    if (position < trajectory.size())
+      {
+	ascFrameNumberCheck(frameNumber, trajectory[position].getFrameNumber());
+      }
+  }
+
+ private:
+
+  /// Id of the trajectory.
+  unsigned int id;
+
+  /**
+   * Trajectory.
+   */
+  std::vector<TrajectoryElement<Type> > trajectory;
+
+  bool checkAscFrameNumber;
+};
+
+typedef Trajectory<cv::Point_<int> > TrajectoryPoint2i;
+typedef TrajectoryPoint2i TrajectoryPoint;
+typedef Trajectory<cv::Point_<float> > TrajectoryPoint2f;
+typedef Trajectory<cv::Point_<double> > TrajectoryPoint2d;
+typedef Trajectory<cv::Point3_<int> > TrajectoryPoint3i;
+typedef Trajectory<cv::Point3_<float> > TrajectoryPoint3f;
+typedef Trajectory<cv::Point3_<double> > TrajectoryPoint3d;
+
+/**
+ * Compare two trajectories.
+ *
+ * @param[in] t trajectory trajectory
+ * @return information whether trajectories are equal or notPoint_<_Tp>
+ */
+template<typename T>
+static inline bool operator ==(const Trajectory<T>& a, const Trajectory<T>& b)
+{
+  if (a.getId() != b.getId() || a.getCheckAscFrameNumber() != b.getCheckAscFrameNumber() || a.size() != b.size())
+    {
+      return false;
+    }
+
+  for (unsigned int i = 0; i < a.size(); ++i)
+    {
+      if (a.getTrajectoryElement(i) != b.getTrajectoryElement(i))
+	{
+	  return false;
+	}
+    }
+
+  return true;
+}
+
+template<typename T>
+static inline Trajectory<T>& operator +=(Trajectory<T>& a, const T& b)
+{
+  a.add(b);
+  return a;
+}
+
+template<typename T>
+static inline Trajectory<T>& operator +=(Trajectory<T>& a, const TrajectoryElement<T>& b)
+{
+  a.add(b);
+  return a;
+}
+
+template<typename Ti, typename Tr>
+  std::basic_istream<Ti>& operator>>(std::basic_istream<Ti>& in, Trajectory<Tr>& trajectory)
+{
+  unsigned int id;
+  if (in >> id)
+    {
+      trajectory.setId(id);
+      TrajectoryElement<Tr> trajectoryElement;
+      while (in >> trajectoryElement)
+	{
+	  trajectory.add(trajectoryElement);
+	}
+    }
+  return in;
+}
+
+template<typename Tr>
+std::ostream& operator<<(std::ostream& out, const Trajectory<Tr>& trajectory)
+{
+  out << trajectory.getId();
+  for (unsigned int i = 0; i < trajectory.size(); ++i)
+    {
+      out << " ";
+      out << trajectory.getTrajectoryElement(i);
+    }
+  return out;
+}
+
+/**
+ * Compute the minimum and the maximum position of the trajectory.
+ *
+ * @param[out] _min minimum position of the trajectory
+ * @param[out] _max maximum position of the trajectory
+ */
+template<typename T>
+static inline T min(const Trajectory<T>& trajectory)
+{
+  if (trajectory.empty())
+    {
+      throw TrajectoryLengthErrorException();
+    }
+
+  T point = trajectory.getPoint(0);
+
+  for (unsigned int i = 1; i < trajectory.size(); ++i)
+    {
+      point = min(point, trajectory.getPoint(i));
+    }
+
+  return point;
+}
+
+template<typename T>
+static inline T max(const Trajectory<T>& trajectory)
+{
+  if (trajectory.empty())
+    {
+      throw TrajectoryLengthErrorException();
+    }
+
+  T point = trajectory.getPoint(0);
+
+  for (unsigned int i = 1; i < trajectory.size(); ++i)
+    {
+      point = max(point, trajectory.getPoint(i));
+    }
+
+  return point;
+}
+
+#endif /* TRAJECTORY_H_ */