Mercurial Hosting > traffic-intelligence

annotate trajectorymanagement/src/TrajectoryWriter.h @ 1219:8a626226793e

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
update where optimization uses either nomad-parameter file depending on optimizing 1 or 2 steps
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 19 Jun 2023 17:09:56 -0400
parents e1e7acef8eab
children
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
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_ */