Mercurial Hosting > traffic-intelligence

annotate trajectorymanagement/src/CanberraMetric.h @ 1222:69b531c7a061

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added methods to reset trajectories and change object coordinates (including features)
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 20 Jun 2023 15:42:19 -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 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_ */