Mercurial > hg > nsaunier > traffic-intelligence

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

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