Mercurial Hosting > traffic-intelligence
annotate trajectorymanagement/src/CanberraMetric.h @ 1251:2b1c8fe8f7e4
added function for KITTI 2D format (just a simple csv-like format with xy coordinates)
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Fri, 15 Mar 2024 17:05:54 -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 CANBERRAMETRIC_H_ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
2 #define CANBERRAMETRIC_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 "Metric.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 #include "TrajectoryExceptions.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 * CanberraMetric 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 Canberra metric measures the similarity between two trajectories. |
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 template<typename Tr> |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
14 class CanberraMetric: public Metric<Tr, double> |
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 public: |
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 * Compute distance between two trajectories. |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
19 * |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
20 * @param[in] a input trajectory |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
21 * @param[in] b input trajectory |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
22 * @param[out] result distance between two trajectories |
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 void distance(const Trajectory<Tr> *a, const Trajectory<Tr> *b, double &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
|
25 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
26 result = double(0); |
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 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
|
29 checkTrajectoryLength(a->size(), b->size()); |
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 double diff = double(0); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
32 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
|
33 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
34 cv::Point3_<typeof(static_cast<Tr>(a->getPoint(i)).x)> A(a->getPoint(i)); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
35 cv::Point3_<typeof(static_cast<Tr>(a->getPoint(i)).x)> B(b->getPoint(i)); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
36 diff += calculate(A.x, B.x); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
37 diff += calculate(A.y, B.y); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
38 diff += calculate(A.z, B.z); |
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 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
41 result = diff; |
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 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
44 private: |
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 * Check the length of the trajectories. |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
47 * |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
48 * @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
|
49 * @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
|
50 */ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
51 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
|
52 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
53 if (a != b || a == 0) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
54 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
55 throw TrajectoryLengthErrorException(); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
56 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
57 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
58 |
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 * Compute similarity between two values. |
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 * @param a first value |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
63 * @param b second value |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
64 * @return result |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
65 */ |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
66 double calculate(double a, double b) |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
67 { |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
68 double result = std::abs(a - b); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
69 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
70 a = std::fabs(a); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
71 b = std::fabs(b); |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
72 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
73 a += b; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
74 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
75 if (a == 0) |
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 return 0; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
78 } |
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 result /= a; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
81 |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
82 return result; |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
83 } |
e1e7acef8eab
moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
84 }; |
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 #endif /* CANBERRAMETRIC_H_ */ |