Mercurial Hosting > traffic-intelligence
annotate trajectorymanagement/src/SquaredEuclideanMetric.h @ 1270:20a5e1292321
added smoothing functions and velocity generation to dltrack
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 10 Jun 2024 16:44:19 -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 SQUAREDEUCLIDIANMETRIC_H_ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
2 #define SQUAREDEUCLIDIANMETRIC_H_ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
3 #include "Metric.h" |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
4 #include "TrajectoryExceptions.h" |
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 /** |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
7 * SquaredEuclideanMetric class. |
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 * The Squared Euclidean metric measures the similarity between two trajectories. |
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 template<typename Tr, typename To> |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
12 class SquaredEuclideanMetric: public Metric<Tr, To> |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
13 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
14 public: |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
15 /** |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
16 * Compute distance between two trajectories. |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
17 * |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
18 * @param[in] a input trajectory |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
19 * @param[in] b input trajectory |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
20 * @param[out] result distance between two trajectories |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
21 */ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
22 void distance(const Trajectory<Tr> *a, const Trajectory<Tr> *b, To &result, unsigned int nbOfPoints = std::numeric_limits<unsigned int>::max()) |
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 result = To(0); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
25 if (nbOfPoints == std::numeric_limits<unsigned int>::max()) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
26 checkTrajectoryLength(a->size(), b->size()); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
27 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
28 To diff = To(0); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
29 for (unsigned int i = 0; i < a->size() and i < nbOfPoints; ++i) |
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 cv::Point3_<typeof(static_cast<Tr>(a->getPoint(i)).x)> p(a->getPoint(i) - b->getPoint(i)); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
32 diff += pow(p.x, 2) + pow(p.y, 2) + pow(p.z, 2); |
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 result = diff; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
36 } |
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 private: |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
39 /** |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
40 * Check the length of the trajectories. |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
41 * |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
42 * @param[in] a the size of the trajectory |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
43 * @param[in] b the size of the trajectory |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
44 */ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
45 void checkTrajectoryLength(size_t a, size_t b) const |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
46 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
47 if (a != b || a == 0) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
48 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
49 throw TrajectoryLengthErrorException(); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
50 } |
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 }; |
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 #endif /* SQUAREDEUCLIDIANMETRIC_H_ */ |