source: osm/applications/editors/josm/plugins/NanoLog/src/nanolog/Correlator.java@ 32638

Last change on this file since 32638 was 32638, checked in by donvip, 8 years ago

checkstyle
File size: 11.3 KB
Line 
1package nanolog;
2
3import static org.openstreetmap.josm.tools.I18n.tr;
4
5import java.util.ArrayList;
6import java.util.Collections;
7import java.util.List;
8
9import javax.swing.JOptionPane;
10
11import org.openstreetmap.josm.Main;
12import org.openstreetmap.josm.data.coor.EastNorth;
13import org.openstreetmap.josm.data.coor.LatLon;
14import org.openstreetmap.josm.data.gpx.GpxData;
15import org.openstreetmap.josm.data.gpx.GpxTrack;
16import org.openstreetmap.josm.data.gpx.GpxTrackSegment;
17import org.openstreetmap.josm.data.gpx.WayPoint;
18import org.openstreetmap.josm.tools.UncheckedParseException;
19import org.openstreetmap.josm.tools.date.DateUtils;
20
21/**
22 * A class that establishes correlation between GPS trace and NanoLog. Mostly copied from
23 * {@link org.openstreetmap.josm.gui.layer.geoimage.CorrelateGpxWithImages}, thus licensed GPL.
24 *
25 * @author zverik
26 */
27public final class Correlator {
28
29 private Correlator() {
30 // Hide default constructor for utilities classes
31 }
32
33 /**
34 * Matches entries to GPX so most points are on the trace.
35 */
36 public static long crudeMatch(List<NanoLogEntry> entries, GpxData data) {
37 List<NanoLogEntry> sortedEntries = new ArrayList<>(entries);
38 Collections.sort(sortedEntries);
39 long firstExifDate = sortedEntries.get(0).getTime().getTime();
40 long firstGPXDate = -1;
41 outer:
42 for (GpxTrack trk : data.tracks) {
43 for (GpxTrackSegment segment : trk.getSegments()) {
44 for (WayPoint curWp : segment.getWayPoints()) {
45 String curDateWpStr = (String) curWp.attr.get("time");
46 if (curDateWpStr == null) {
47 continue;
48 }
49
50 try {
51 firstGPXDate = DateUtils.fromString(curDateWpStr).getTime();
52 break outer;
53 } catch (Exception e) {
54 Main.warn(e);
55 }
56 }
57 }
58 }
59
60 // No GPX timestamps found, exit
61 if (firstGPXDate < 0) {
62 JOptionPane.showMessageDialog(Main.parent,
63 tr("The selected GPX track does not contain timestamps. Please select another one."),
64 tr("GPX Track has no time information"), JOptionPane.WARNING_MESSAGE);
65 return 0;
66 }
67
68 return firstExifDate - firstGPXDate;
69 }
70
71 public static void revertPos(List<NanoLogEntry> entries) {
72 for (NanoLogEntry entry : entries) {
73 entry.setPos(entry.getBasePos());
74 }
75 }
76
77 /**
78 * Offset is in 1/1000 of a second.
79 */
80 public static void correlate(List<NanoLogEntry> entries, GpxData data, long offset) {
81 List<NanoLogEntry> sortedEntries = new ArrayList<>(entries);
82 //int ret = 0;
83 Collections.sort(sortedEntries);
84 for (GpxTrack track : data.tracks) {
85 for (GpxTrackSegment segment : track.getSegments()) {
86 long prevWpTime = 0;
87 WayPoint prevWp = null;
88
89 for (WayPoint curWp : segment.getWayPoints()) {
90
91 String curWpTimeStr = (String) curWp.attr.get("time");
92 if (curWpTimeStr != null) {
93 try {
94 long curWpTime = DateUtils.fromString(curWpTimeStr).getTime() + offset;
95 /*ret +=*/ matchPoints(sortedEntries, prevWp, prevWpTime, curWp, curWpTime, offset);
96
97 prevWp = curWp;
98 prevWpTime = curWpTime;
99
100 } catch (UncheckedParseException e) {
101 Main.error("Error while parsing date \"" + curWpTimeStr + '"');
102 Main.error(e);
103 prevWp = null;
104 prevWpTime = 0;
105 }
106 } else {
107 prevWp = null;
108 prevWpTime = 0;
109 }
110 }
111 }
112 }
113 }
114
115 private static int matchPoints(List<NanoLogEntry> entries, WayPoint prevWp, long prevWpTime,
116 WayPoint curWp, long curWpTime, long offset) {
117 // Time between the track point and the previous one, 5 sec if first point, i.e. photos take
118 // 5 sec before the first track point can be assumed to be take at the starting position
119 long interval = prevWpTime > 0 ? Math.abs(curWpTime - prevWpTime) : 5 * 1000;
120 int ret = 0;
121
122 // i is the index of the timewise last photo that has the same or earlier EXIF time
123 int i = getLastIndexOfListBefore(entries, curWpTime);
124
125 // no photos match
126 if (i < 0)
127 return 0;
128
129 Integer direction = null;
130 if (prevWp != null) {
131 direction = Long.valueOf(Math.round(180.0 / Math.PI * prevWp.getCoor().heading(curWp.getCoor()))).intValue();
132 }
133
134 // First trackpoint, then interval is set to five seconds, i.e. photos up to five seconds
135 // before the first point will be geotagged with the starting point
136 if (prevWpTime == 0 || curWpTime <= prevWpTime) {
137 while (true) {
138 if (i < 0) {
139 break;
140 }
141 final NanoLogEntry curImg = entries.get(i);
142 long time = curImg.getTime().getTime();
143 if (time > curWpTime || time < curWpTime - interval) {
144 break;
145 }
146 if (curImg.getPos() == null) {
147 curImg.setPos(curWp.getCoor());
148 curImg.setDirection(direction);
149 ret++;
150 }
151 i--;
152 }
153 return ret;
154 }
155
156 // This code gives a simple linear interpolation of the coordinates between current and
157 // previous track point assuming a constant speed in between
158 while (true) {
159 if (i < 0) {
160 break;
161 }
162 NanoLogEntry curImg = entries.get(i);
163 long imgTime = curImg.getTime().getTime();
164 if (imgTime < prevWpTime) {
165 break;
166 }
167
168 if (curImg.getPos() == null && prevWp != null) {
169 // The values of timeDiff are between 0 and 1, it is not seconds but a dimensionless variable
170 double timeDiff = (double) (imgTime - prevWpTime) / interval;
171 curImg.setPos(prevWp.getCoor().interpolate(curWp.getCoor(), timeDiff));
172 curImg.setDirection(direction);
173
174 ret++;
175 }
176 i--;
177 }
178 return ret;
179 }
180
181 private static int getLastIndexOfListBefore(List<NanoLogEntry> entries, long searchedTime) {
182 int lstSize = entries.size();
183
184 // No photos or the first photo taken is later than the search period
185 if (lstSize == 0 || searchedTime < entries.get(0).getTime().getTime())
186 return -1;
187
188 // The search period is later than the last photo
189 if (searchedTime > entries.get(lstSize - 1).getTime().getTime())
190 return lstSize-1;
191
192 // The searched index is somewhere in the middle, do a binary search from the beginning
193 int curIndex = 0;
194 int startIndex = 0;
195 int endIndex = lstSize-1;
196 while (endIndex - startIndex > 1) {
197 curIndex = (endIndex + startIndex) / 2;
198 if (searchedTime > entries.get(curIndex).getTime().getTime()) {
199 startIndex = curIndex;
200 } else {
201 endIndex = curIndex;
202 }
203 }
204 if (searchedTime < entries.get(endIndex).getTime().getTime())
205 return startIndex;
206
207 // This final loop is to check if photos with the exact same EXIF time follows
208 while ((endIndex < (lstSize-1)) && (entries.get(endIndex).getTime().getTime()
209 == entries.get(endIndex + 1).getTime().getTime())) {
210 endIndex++;
211 }
212 return endIndex;
213 }
214
215 /**
216 * Returns date of a potential point on GPX track (which can be between points).
217 */
218 public static long getGpxDate(GpxData data, LatLon pos) {
219 EastNorth en = Main.getProjection().latlon2eastNorth(pos);
220 for (GpxTrack track : data.tracks) {
221 for (GpxTrackSegment segment : track.getSegments()) {
222 long prevWpTime = 0;
223 WayPoint prevWp = null;
224 for (WayPoint curWp : segment.getWayPoints()) {
225 String curWpTimeStr = (String) curWp.attr.get("time");
226 if (curWpTimeStr != null) {
227 try {
228 long curWpTime = DateUtils.fromString(curWpTimeStr).getTime();
229 if (prevWp != null) {
230 EastNorth c1 = Main.getProjection().latlon2eastNorth(prevWp.getCoor());
231 EastNorth c2 = Main.getProjection().latlon2eastNorth(curWp.getCoor());
232 if (!c1.equals(c2)) {
233 EastNorth middle = getSegmentAltitudeIntersection(c1, c2, en);
234 if (middle != null && en.distance(middle) < 1) {
235 // found our point, no further search is neccessary
236 double prop = c1.east() == c2.east()
237 ? (middle.north() - c1.north()) / (c2.north() - c1.north())
238 : (middle.east() - c1.east()) / (c2.east() - c1.east());
239 if (prop >= 0 && prop <= 1) {
240 return Math.round(prevWpTime + prop * (curWpTime - prevWpTime));
241 }
242 }
243 }
244 }
245
246 prevWp = curWp;
247 prevWpTime = curWpTime;
248 } catch (UncheckedParseException e) {
249 Main.error("Error while parsing date \"" + curWpTimeStr + '"');
250 Main.error(e);
251 prevWp = null;
252 prevWpTime = 0;
253 }
254 } else {
255 prevWp = null;
256 prevWpTime = 0;
257 }
258 }
259 }
260 }
261 return 0;
262 }
263
264 /**
265 * Returns the coordinate of intersection of segment p1-p2 and an altitude
266 * to it starting at point p. If the line defined with p1-p2 intersects
267 * its altitude out of p1-p2, null is returned.
268 * @return Intersection coordinate or null
269 **/
270 public static EastNorth getSegmentAltitudeIntersection(EastNorth p1, EastNorth p2, EastNorth point) {
271 double ldx = p2.getX() - p1.getX();
272 double ldy = p2.getY() - p1.getY();
273
274 if (ldx == 0 && ldy == 0) //segment zero length
275 return p1;
276
277 double pdx = point.getX() - p1.getX();
278 double pdy = point.getY() - p1.getY();
279
280 double offset = (pdx * ldx + pdy * ldy) / (ldx * ldx + ldy * ldy);
281
282 if (offset < -1e-8 || offset > 1+1e-8) return null;
283 if (offset < 1e-8)
284 return p1;
285 else if (offset > 1-1e-8)
286 return p2;
287 else
288 return new EastNorth(p1.getX() + ldx * offset, p1.getY() + ldy * offset);
289 }
290
291}
Note: See TracBrowser for help on using the repository browser.