Mercurial Hosting > traffic-intelligence
annotate trajectorymanagement/src/TrajectoryWriter.h @ 1269:ca70a79688ae
adding a speed threshold to avoid computing TTC at very low speeds
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 05 Jun 2024 10:12:43 -0400 |
parents | e1e7acef8eab |
children |
rev | line source |
---|---|
1159
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
1 #ifndef TRAJECTORYWRITER_H_ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
2 #define TRAJECTORYWRITER_H_ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
3 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
4 #include <fstream> |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
5 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
6 #include "Trajectory.h" |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
7 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
8 /** |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
9 * TrajectoryWriter class. |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
10 * |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
11 * The TrajectoryWriter class allows to save trajectories in different formats like image files, Matlab, Scilab or FreeMat files. |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
12 */ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
13 class TrajectoryWriter |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
14 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
15 public: |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
16 /** |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
17 * Save the trajectory in the image. |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
18 * |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
19 * @param[in] trajectory trajectory |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
20 * @param[in] color color of the trajectory |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
21 * @param[in] img image on which the trajectory should be saved |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
22 * @return image with the saved trajectory |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
23 */ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
24 template<typename T> |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
25 void write(const Trajectory<T> &trajectory, CvScalar color, cv::Mat& img = cv::Mat::Mat()) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
26 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
27 if (img.empty()) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
28 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
29 img = createImg(trajectory); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
30 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
31 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
32 writeOnNotNullImg(trajectory, color, img); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
33 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
34 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
35 /** |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
36 * Save trajectories in the image. |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
37 * |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
38 * @param[in] img image on which the trajectory should be saved |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
39 * @param[in] trajectories trajectories |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
40 * @param[in] colors colors of trajectories |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
41 * @return image with saved trajectories |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
42 */ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
43 template<typename T> |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
44 void saveInIplImage(cv::Mat& img, std::vector<Trajectory<T> > *trajectories, std::vector<CvScalar> *colors) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
45 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
46 assert( !img.empty()); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
47 assert( trajectories != NULL ); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
48 assert( trajectories->size() > 0 ); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
49 assert( colors != NULL ); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
50 assert( colors->size() > 0 ); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
51 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
52 unsigned nbOfColors = colors->size(); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
53 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
54 for (unsigned i = 0; i < trajectories->size(); ++i) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
55 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
56 Trajectory<T> trajectory = (*trajectories)[i]; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
57 CvScalar color = (*colors)[i % nbOfColors]; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
58 writeOnNotNullImg(trajectory, color, img); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
59 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
60 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
61 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
62 /** |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
63 * Save trajectories in the Matlab format file. |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
64 * |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
65 * @param[in] file name of the file |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
66 * @param[in] trajectories trajectories |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
67 * @param[in] writeWithVisualization information if functions to visualize data should be written into the file |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
68 * @param[in] plotCommand2D command to plot in 2D |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
69 * @param[in] plotCommand3D command to plot in 3D |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
70 */ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
71 template<typename T> |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
72 void saveInMatlabFormat(const char *file, std::vector<Trajectory<T> > *trajectories, bool writeWithVisualization = true, const char *plotCommand2D = "plot", const char *plotCommand3D = "plot3d") |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
73 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
74 saveInMatlabScilabFormat(file, trajectories, writeWithVisualization, plotCommand2D, plotCommand3D, true); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
75 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
76 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
77 /** |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
78 * Save trajectories in the Scilab format file. |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
79 * |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
80 * @param[in] file name of the file |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
81 * @param[in] trajectories trajectories |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
82 * @param[in] writeWithVisualization information if functions to visualize data should be written into the file |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
83 * @param[in] plotCommand2D command to plot in 2D |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
84 * @param[in] plotCommand3D command to plot in 3D |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
85 */ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
86 template<typename T> |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
87 void saveInScilabFormat(const char *file, std::vector<Trajectory<T> > *trajectories, bool writeWithVisualization = true, const char *plotCommand2D = "plot", const char *plotCommand3D = "plot3d") |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
88 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
89 saveInMatlabScilabFormat(file, trajectories, writeWithVisualization, plotCommand2D, plotCommand3D, false); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
90 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
91 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
92 private: |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
93 /** |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
94 * Create image on which the trajectory can be saved. |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
95 * |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
96 * @param[in] trajectory trajectory |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
97 * @return image |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
98 */ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
99 template<typename T> |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
100 cv::Mat createImg(const Trajectory<cv::Point_<T> > &trajectory) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
101 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
102 cv::Point_<T> _min = min(trajectory); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
103 cv::Point_<T> _max = max(trajectory); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
104 CvSize imgSize = cvSize(_max.x + 1, _max.y + 1); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
105 cv::Mat img = cv::Mat::zeros(imgSize, CV_8UC3); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
106 return img; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
107 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
108 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
109 /** |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
110 * Save the trajectory in the image. |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
111 * |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
112 * @param[in] trajectory trajectory |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
113 * @param[in] color color of the trajectory |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
114 * @param[in] img image on which the trajectory should be saved |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
115 * @return image with the saved trajectory |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
116 */ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
117 template<typename T> |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
118 void writeOnNotNullImg(const Trajectory<T> &trajectory, CvScalar color, cv::Mat& img) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
119 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
120 assert( !img.empty() ); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
121 for (unsigned int i = 1; i < trajectory.size(); ++i) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
122 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
123 const T p1 = trajectory.getPoint(i - 1); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
124 const T p2 = trajectory.getPoint(i); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
125 cv::line(img, p1, p2, color); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
126 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
127 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
128 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
129 /** |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
130 * Save trajectories in the Matlab or Scilab format file. |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
131 * |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
132 * @param[in] file name of the file |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
133 * @param[in] trajectories trajectories |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
134 * @param[in] writeWithVisualization information if functions to visualize data should be written into the file |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
135 * @param[in] plotCommand2D command to plot in 2D |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
136 * @param[in] plotCommand3D command to plot in 3D |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
137 * @param[in] matlab information whether trajectories should be saved in Matlab or Scilab format |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
138 */ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
139 template<typename T> |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
140 void saveInMatlabScilabFormat(const char *file, std::vector<Trajectory<T> > *trajectories, bool writeWithVisualization, const char *plotCommand2D, const char *plotCommand3D, bool matlab) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
141 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
142 if (trajectories == NULL) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
143 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
144 assert( false ); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
145 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
146 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
147 std::stringstream matlabFileSS; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
148 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
149 if (matlab) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
150 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
151 matlabFileSS << "hold on;\n"; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
152 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
153 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
154 for (unsigned i = 0; i < trajectories->size(); ++i) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
155 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
156 matlabFileSS << "T"; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
157 matlabFileSS << i << "=["; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
158 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
159 for (unsigned j = 0; j < (*trajectories)[i].size(); ++j) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
160 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
161 matlabFileSS << (*trajectories)[i].getPoint(j); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
162 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
163 if (j < (*trajectories)[i].size() - 1) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
164 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
165 matlabFileSS << ";"; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
166 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
167 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
168 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
169 matlabFileSS << "];\n"; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
170 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
171 if (writeWithVisualization) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
172 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
173 matlabFileSS << "if size(T" << i << ",2) == 2\n"; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
174 matlabFileSS << "\t\t" << plotCommand2D << "( T" << i << "(:,1), T" << i << "(:,2) );\n"; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
175 matlabFileSS << "end;\n"; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
176 matlabFileSS << "if size(T" << i << ",2) == 3\n"; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
177 matlabFileSS << "\t\t" << plotCommand3D << "( T" << i << "(:,1), T" << i << "(:,2), T" << i << "(:,3) );\n"; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
178 matlabFileSS << "end;\n"; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
179 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
180 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
181 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
182 std::ofstream matlabFile; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
183 matlabFile.open(file, std::ios::out); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
184 matlabFile << matlabFileSS.str(); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
185 matlabFile.flush(); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
186 matlabFile.close(); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
187 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
188 }; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
189 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
190 #endif /* TRAJECTORYWRITER_H_ */ |