nsaunier/traffic-intelligence: trajectorymanagement/src/Trajectory.h comparison

comparison trajectorymanagement/src/Trajectory.h @ 1159:e1e7acef8eab

moved trajectory management library into Traffic Intelligence

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Mon, 22 Feb 2021 22:09:35 -0500
parents
children

comparison

equal deleted inserted replaced

-:7eb972942f22
+:e1e7acef8eab
+#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_ */

Mercurial > hg > nsaunier > traffic-intelligence

comparison trajectorymanagement/src/Trajectory.h @ 1159:e1e7acef8eab