Weighted tracks
[blender.git] / extern / libmv / libmv / simple_pipeline / tracks.cc
1 // Copyright (c) 2011 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20
21 #include "libmv/simple_pipeline/tracks.h"
22
23 #include <algorithm>
24 #include <vector>
25 #include <iterator>
26
27 #include "libmv/numeric/numeric.h"
28
29 namespace libmv {
30
31 Tracks::Tracks(const Tracks &other) {
32   markers_ = other.markers_;
33 }
34
35 Tracks::Tracks(const vector<Marker> &markers) : markers_(markers) {}
36
37 void Tracks::Insert(int image, int track, double x, double y, double weight) {
38   // TODO(keir): Wow, this is quadratic for repeated insertions. Fix this by
39   // adding a smarter data structure like a set<>.
40   for (int i = 0; i < markers_.size(); ++i) {
41     if (markers_[i].image == image &&
42         markers_[i].track == track) {
43       markers_[i].x = x;
44       markers_[i].y = y;
45       return;
46     }
47   }
48   Marker marker = { image, track, x, y, weight };
49   markers_.push_back(marker);
50 }
51
52 vector<Marker> Tracks::AllMarkers() const {
53   return markers_;
54 }
55
56 vector<Marker> Tracks::MarkersInImage(int image) const {
57   vector<Marker> markers;
58   for (int i = 0; i < markers_.size(); ++i) {
59     if (image == markers_[i].image) {
60       markers.push_back(markers_[i]);
61     }
62   }
63   return markers;
64 }
65
66 vector<Marker> Tracks::MarkersForTrack(int track) const {
67   vector<Marker> markers;
68   for (int i = 0; i < markers_.size(); ++i) {
69     if (track == markers_[i].track) {
70       markers.push_back(markers_[i]);
71     }
72   }
73   return markers;
74 }
75
76 vector<Marker> Tracks::MarkersInBothImages(int image1, int image2) const {
77   vector<Marker> markers;
78   for (int i = 0; i < markers_.size(); ++i) {
79     int image = markers_[i].image;
80     if (image == image1 || image == image2)
81       markers.push_back(markers_[i]);
82   }
83   return markers;
84 }
85
86 vector<Marker> Tracks::MarkersForTracksInBothImages(int image1,
87                                                     int image2) const {
88   std::vector<int> image1_tracks;
89   std::vector<int> image2_tracks;
90
91   for (int i = 0; i < markers_.size(); ++i) {
92     int image = markers_[i].image;
93     if (image == image1) {
94       image1_tracks.push_back(markers_[i].track);
95     } else if (image == image2) {
96       image2_tracks.push_back(markers_[i].track);
97     }
98   }
99
100   std::sort(image1_tracks.begin(), image1_tracks.end());
101   std::sort(image2_tracks.begin(), image2_tracks.end());
102
103   std::vector<int> intersection;
104   std::set_intersection(image1_tracks.begin(), image1_tracks.end(),
105                         image2_tracks.begin(), image2_tracks.end(),
106                         std::back_inserter(intersection));
107
108   vector<Marker> markers;
109   for (int i = 0; i < markers_.size(); ++i) {
110     if ((markers_[i].image == image1 || markers_[i].image == image2) &&
111         std::binary_search(intersection.begin(), intersection.end(),
112                            markers_[i].track)) {
113       markers.push_back(markers_[i]);
114     }
115   }
116   return markers;
117 }
118
119 Marker Tracks::MarkerInImageForTrack(int image, int track) const {
120   for (int i = 0; i < markers_.size(); ++i) {
121     if (markers_[i].image == image && markers_[i].track == track) {
122       return markers_[i];
123     }
124   }
125   Marker null = { -1, -1, -1, -1, 0.0 };
126   return null;
127 }
128
129 void Tracks::RemoveMarkersForTrack(int track) {
130   int size = 0;
131   for (int i = 0; i < markers_.size(); ++i) {
132     if (markers_[i].track != track) {
133       markers_[size++] = markers_[i];
134     }
135   }
136   markers_.resize(size);
137 }
138
139 void Tracks::RemoveMarker(int image, int track) {
140   int size = 0;
141   for (int i = 0; i < markers_.size(); ++i) {
142     if (markers_[i].image != image || markers_[i].track != track) {
143       markers_[size++] = markers_[i];
144     }
145   }
146   markers_.resize(size);
147 }
148
149 int Tracks::MaxImage() const {
150   // TODO(MatthiasF): maintain a max_image_ member (updated on Insert)
151   int max_image = 0;
152   for (int i = 0; i < markers_.size(); ++i) {
153     max_image = std::max(markers_[i].image, max_image);
154   }
155   return max_image;
156 }
157
158 int Tracks::MaxTrack() const {
159   // TODO(MatthiasF): maintain a max_track_ member (updated on Insert)
160   int max_track = 0;
161   for (int i = 0; i < markers_.size(); ++i) {
162     max_track = std::max(markers_[i].track, max_track);
163   }
164   return max_track;
165 }
166
167 int Tracks::NumMarkers() const {
168   return markers_.size();
169 }
170
171 void CoordinatesForMarkersInImage(const vector<Marker> &markers,
172                                   int image,
173                                   Mat *coordinates) {
174   vector<Vec2> coords;
175   for (int i = 0; i < markers.size(); ++i) {
176     const Marker &marker = markers[i];
177     if (markers[i].image == image) {
178       coords.push_back(Vec2(marker.x, marker.y));
179     }
180   }
181   coordinates->resize(2, coords.size());
182   for (int i = 0; i < coords.size(); i++) {
183     coordinates->col(i) = coords[i];
184   }
185 }
186
187 }  // namespace libmv