001// License: GPL. For details, see LICENSE file.
002package org.openstreetmap.josm.data.gpx;
003
004import java.util.ArrayList;
005import java.util.Collection;
006import java.util.List;
007import java.util.concurrent.TimeUnit;
008
009import org.openstreetmap.josm.data.coor.LatLon;
010import org.openstreetmap.josm.data.projection.Projection;
011import org.openstreetmap.josm.data.projection.ProjectionRegistry;
012import org.openstreetmap.josm.spi.preferences.Config;
013import org.openstreetmap.josm.tools.Logging;
014import org.openstreetmap.josm.tools.Pair;
015import org.openstreetmap.josm.tools.Utils;
016
017/**
018 * Correlation logic for {@code CorrelateGpxWithImages}.
019 * @since 14205
020 */
021public final class GpxImageCorrelation {
022
023    private GpxImageCorrelation() {
024        // Hide public constructor
025    }
026
027    /**
028     * Match a list of photos to a gpx track with given settings.
029     * All images need a exifTime attribute and the List must be sorted according to these times.
030     * @param images images to match
031     * @param selectedGpx selected GPX data
032     * @param settings correlation settings
033     * @return number of matched points
034     */
035    public static int matchGpxTrack(List<? extends GpxImageEntry> images, GpxData selectedGpx, GpxImageCorrelationSettings settings) {
036        int ret = 0;
037
038        if (Logging.isDebugEnabled()) {
039            Logging.debug("Correlating {0} images to {1} GPX track segments using {2}",
040                    images.size(), selectedGpx.getTrackSegsCount(), settings);
041        }
042
043        boolean trkInt, trkTag, segInt, segTag;
044        int trkTime, trkDist, trkTagTime, segTime, segDist, segTagTime;
045
046        if (settings.isForceTags()) {
047            // temporary option to override advanced settings and activate all possible interpolations / tagging methods
048            trkInt = trkTag = segInt = segTag = true;
049            trkTime = trkDist = trkTagTime = segTime = segDist = segTagTime = Integer.MAX_VALUE;
050        } else {
051            // Load the settings
052            trkInt = Config.getPref().getBoolean("geoimage.trk.int", false);
053            trkTime = Config.getPref().getBoolean("geoimage.trk.int.time", false) ?
054                    Config.getPref().getInt("geoimage.trk.int.time.val", 60) : Integer.MAX_VALUE;
055            trkDist = Config.getPref().getBoolean("geoimage.trk.int.dist", false) ?
056                    Config.getPref().getInt("geoimage.trk.int.dist.val", 50) : Integer.MAX_VALUE;
057
058            trkTag = Config.getPref().getBoolean("geoimage.trk.tag", true);
059            trkTagTime = Config.getPref().getBoolean("geoimage.trk.tag.time", true) ?
060                    Config.getPref().getInt("geoimage.trk.tag.time.val", 2) : Integer.MAX_VALUE;
061
062            segInt = Config.getPref().getBoolean("geoimage.seg.int", true);
063            segTime = Config.getPref().getBoolean("geoimage.seg.int.time", true) ?
064                    Config.getPref().getInt("geoimage.seg.int.time.val", 60) : Integer.MAX_VALUE;
065            segDist = Config.getPref().getBoolean("geoimage.seg.int.dist", true) ?
066                    Config.getPref().getInt("geoimage.seg.int.dist.val", 50) : Integer.MAX_VALUE;
067
068            segTag = Config.getPref().getBoolean("geoimage.seg.tag", true);
069            segTagTime = Config.getPref().getBoolean("geoimage.seg.tag.time", true) ?
070                    Config.getPref().getInt("geoimage.seg.tag.time.val", 2) : Integer.MAX_VALUE;
071        }
072
073        final GpxImageDirectionPositionSettings dirpos = settings.getDirectionPositionSettings();
074        final long offset = settings.getOffset();
075
076        boolean isFirst = true;
077        long prevWpTime = 0;
078        WayPoint prevWp = null;
079
080        for (List<List<WayPoint>> segs : loadTracks(selectedGpx.getTracks())) {
081            boolean firstSegment = true;
082            for (List<WayPoint> wps : segs) {
083                int size = wps.size();
084                for (int i = 0; i < size; i++) {
085                    final WayPoint curWp = wps.get(i);
086                    // Interpolate timestamps in the segment, if one or more waypoints miss them
087                    if (!curWp.hasDate()) {
088                        //check if any of the following waypoints has a timestamp...
089                        if (i > 0 && wps.get(i - 1).hasDate()) {
090                            long prevWpTimeNoOffset = wps.get(i - 1).getTimeInMillis();
091                            double totalDist = 0;
092                            List<Pair<Double, WayPoint>> nextWps = new ArrayList<>();
093                            for (int j = i; j < size; j++) {
094                                totalDist += wps.get(j - 1).getCoor().greatCircleDistance(wps.get(j).getCoor());
095                                nextWps.add(new Pair<>(totalDist, wps.get(j)));
096                                if (wps.get(j).hasDate()) {
097                                    // ...if yes, interpolate everything in between
098                                    long timeDiff = wps.get(j).getTimeInMillis() - prevWpTimeNoOffset;
099                                    for (Pair<Double, WayPoint> pair : nextWps) {
100                                        pair.b.setTimeInMillis((long) (prevWpTimeNoOffset + (timeDiff * (pair.a / totalDist))));
101                                    }
102                                    break;
103                                }
104                            }
105                            if (!curWp.hasDate()) {
106                                break; //It's pointless to continue with this segment, because none of the following waypoints had a timestamp
107                            }
108                        } else {
109                            // Timestamps on waypoints without preceding timestamps in the same segment can not be interpolated, so try next one
110                            continue;
111                        }
112                    }
113
114                    final long curWpTime = curWp.getTimeInMillis() + offset;
115                    boolean interpolate = true;
116                    int tagTime = 0;
117                    if (i == 0) {
118                        if (firstSegment) {
119                            // First segment of the track, so apply settings for tracks
120                            firstSegment = false;
121                            if (!trkInt || isFirst || prevWp == null ||
122                                    Math.abs(curWpTime - prevWpTime) > TimeUnit.MINUTES.toMillis(trkTime) ||
123                                    prevWp.getCoor().greatCircleDistance(curWp.getCoor()) > trkDist) {
124                                isFirst = false;
125                                interpolate = false;
126                                if (trkTag) {
127                                    tagTime = trkTagTime;
128                                }
129                            }
130                        } else {
131                            // Apply settings for segments
132                            if (!segInt || prevWp == null ||
133                                    Math.abs(curWpTime - prevWpTime) > TimeUnit.MINUTES.toMillis(segTime) ||
134                                    prevWp.getCoor().greatCircleDistance(curWp.getCoor()) > segDist) {
135                                interpolate = false;
136                                if (segTag) {
137                                    tagTime = segTagTime;
138                                }
139                            }
140                        }
141                    }
142                    WayPoint nextWp = i < size - 1 ? wps.get(i + 1) : null;
143                    ret += matchPoints(images, prevWp, prevWpTime, curWp, curWpTime, offset, interpolate, tagTime, nextWp, dirpos);
144                    prevWp = curWp;
145                    prevWpTime = curWpTime;
146                }
147            }
148        }
149        if (trkTag && prevWp != null) {
150            ret += matchPoints(images, prevWp, prevWpTime, prevWp, prevWpTime, offset, false, trkTagTime, null, dirpos);
151        }
152        Logging.debug("Correlated {0} total points", ret);
153        return ret;
154    }
155
156    static List<List<List<WayPoint>>> loadTracks(Collection<IGpxTrack> tracks) {
157        List<List<List<WayPoint>>> trks = new ArrayList<>();
158        for (IGpxTrack trk : tracks) {
159            List<List<WayPoint>> segs = new ArrayList<>();
160            for (IGpxTrackSegment seg : trk.getSegments()) {
161                List<WayPoint> wps = new ArrayList<>(seg.getWayPoints());
162                if (!wps.isEmpty()) {
163                    //remove waypoints at the beginning of the track/segment without timestamps
164                    int wp;
165                    for (wp = 0; wp < wps.size(); wp++) {
166                        if (wps.get(wp).hasDate()) {
167                            break;
168                        }
169                    }
170                    if (wp == 0) {
171                        segs.add(wps);
172                    } else if (wp < wps.size()) {
173                        segs.add(wps.subList(wp, wps.size()));
174                    }
175                }
176            }
177            //sort segments by first waypoint
178            if (!segs.isEmpty()) {
179                segs.sort((o1, o2) -> {
180                    if (o1.isEmpty() || o2.isEmpty())
181                        return 0;
182                    return o1.get(0).compareTo(o2.get(0));
183                });
184                trks.add(segs);
185            }
186        }
187        //sort tracks by first waypoint of first segment
188        trks.sort((o1, o2) -> {
189            if (o1.isEmpty() || o1.get(0).isEmpty()
190             || o2.isEmpty() || o2.get(0).isEmpty())
191                return 0;
192            return o1.get(0).get(0).compareTo(o2.get(0).get(0));
193        });
194        return trks;
195    }
196
197    static Double getElevation(WayPoint wp) {
198        if (wp != null) {
199            String value = wp.getString(GpxConstants.PT_ELE);
200            if (!Utils.isEmpty(value)) {
201                try {
202                    return Double.valueOf(value);
203                } catch (NumberFormatException e) {
204                    Logging.warn(e);
205                }
206            }
207        }
208        return null;
209    }
210
211    private static int matchPoints(List<? extends GpxImageEntry> images, WayPoint prevWp, long prevWpTime, WayPoint curWp, long curWpTime,
212            long offset, boolean interpolate, int tagTime, WayPoint nextWp, GpxImageDirectionPositionSettings dirpos) {
213
214        final boolean isLast = nextWp == null;
215
216        // i is the index of the timewise last photo that has the same or earlier EXIF time
217        int i;
218        if (isLast) {
219            i = images.size() - 1;
220        } else {
221            i = getLastIndexOfListBefore(images, curWpTime);
222        }
223
224        if (Logging.isDebugEnabled()) {
225            Logging.debug("Correlating images for i={1} - curWp={2}/{3} - prevWp={4}/{5} - nextWp={6} - tagTime={7} - interpolate={8}",
226                    i, curWp, curWpTime, prevWp, prevWpTime, nextWp, tagTime, interpolate);
227        }
228
229        // no photos match
230        if (i < 0) {
231            Logging.debug("Correlated nothing, no photos match");
232            return 0;
233        }
234
235        int ret = 0;
236        Double speed = null;
237        Double prevElevation = null;
238
239        if (prevWp != null && interpolate) {
240            double distance = prevWp.getCoor().greatCircleDistance(curWp.getCoor());
241            // This is in km/h, 3.6 * m/s
242            if (curWpTime > prevWpTime) {
243                speed = 3600 * distance / (curWpTime - prevWpTime);
244            }
245            prevElevation = getElevation(prevWp);
246        }
247
248        final Double curElevation = getElevation(curWp);
249
250        if (!interpolate || isLast) {
251            final long half = Math.abs(curWpTime - prevWpTime) / 2;
252            while (i >= 0) {
253                final GpxImageEntry curImg = images.get(i);
254                final GpxImageEntry curTmp = curImg.getTmp();
255                final long time = curImg.getExifInstant().toEpochMilli();
256                if ((!isLast && time > curWpTime) || time < prevWpTime) {
257                    break;
258                }
259                long tagms = TimeUnit.MINUTES.toMillis(tagTime);
260                if (!curTmp.hasNewGpsData() &&
261                        (Math.abs(time - curWpTime) <= tagms
262                        || Math.abs(prevWpTime - time) <= tagms)) {
263                    if (prevWp != null && time < curWpTime - half) {
264                        curTmp.setPos(prevWp.getCoor());
265                    } else {
266                        curTmp.setPos(curWp.getCoor());
267                    }
268                    if (nextWp != null && dirpos.isSetImageDirection()) {
269                        double direction = curWp.getCoor().bearing(nextWp.getCoor());
270                        curTmp.setExifImgDir(computeDirection(direction, dirpos.getImageDirectionAngleOffset()));
271                    }
272                    curTmp.setGpsTime(curImg.getExifInstant().minusMillis(offset));
273                    curTmp.flagNewGpsData();
274                    curImg.tmpUpdated();
275
276                    ret++;
277                }
278                i--;
279            }
280        } else if (prevWp != null) {
281            // This code gives a simple linear interpolation of the coordinates between current and
282            // previous track point assuming a constant speed in between
283            @SuppressWarnings("null")
284            LatLon nextCoorForDirection = nextWp.getCoor();
285            while (i >= 0) {
286                final GpxImageEntry curImg = images.get(i);
287                final long imgTime = curImg.getExifInstant().toEpochMilli();
288                if (imgTime < prevWpTime) {
289                    break;
290                }
291                final GpxImageEntry curTmp = curImg.getTmp();
292                if (!curTmp.hasNewGpsData()) {
293                    // The values of timeDiff are between 0 and 1, it is not seconds but a dimensionless variable
294                    final double timeDiff = (double) (imgTime - prevWpTime) / Math.abs(curWpTime - prevWpTime);
295                    final boolean shiftXY = dirpos.getShiftImageX() != 0d || dirpos.getShiftImageY() != 0d;
296                    final LatLon prevCoor = prevWp.getCoor();
297                    final LatLon curCoor = curWp.getCoor();
298                    LatLon position = prevCoor.interpolate(curCoor, timeDiff);
299                    if (nextCoorForDirection != null && (shiftXY || dirpos.isSetImageDirection())) {
300                        double direction = position.bearing(nextCoorForDirection);
301                        if (dirpos.isSetImageDirection()) {
302                            curTmp.setExifImgDir(computeDirection(direction, dirpos.getImageDirectionAngleOffset()));
303                        }
304                        if (shiftXY) {
305                            final Projection proj = ProjectionRegistry.getProjection();
306                            final double offsetX = dirpos.getShiftImageX();
307                            final double offsetY = dirpos.getShiftImageY();
308                            final double r = Math.sqrt(offsetX * offsetX + offsetY * offsetY);
309                            final double orientation = (direction + LatLon.ZERO.bearing(new LatLon(offsetX, offsetY))) % (2 * Math.PI);
310                            position = proj.eastNorth2latlon(proj.latlon2eastNorth(position)
311                                    .add(r * Math.sin(orientation), r * Math.cos(orientation)));
312                        }
313                    }
314                    curTmp.setPos(position);
315                    curTmp.setSpeed(speed);
316                    if (curElevation != null && prevElevation != null) {
317                        curTmp.setElevation(prevElevation + (curElevation - prevElevation) * timeDiff + dirpos.getElevationShift());
318                    }
319                    curTmp.setGpsTime(curImg.getExifInstant().minusMillis(offset));
320                    curTmp.flagNewGpsData();
321                    curImg.tmpUpdated();
322
323                    nextCoorForDirection = curCoor;
324                    ret++;
325                }
326                i--;
327            }
328        }
329        Logging.debug("Correlated {0} image(s)", ret);
330        return ret;
331    }
332
333    private static double computeDirection(double direction, double angleOffset) {
334        return (Utils.toDegrees(direction) + angleOffset) % 360d;
335    }
336
337    private static int getLastIndexOfListBefore(List<? extends GpxImageEntry> images, long searchedTime) {
338        int lstSize = images.size();
339
340        // No photos or the first photo taken is later than the search period
341        if (lstSize == 0 || searchedTime < images.get(0).getExifInstant().toEpochMilli())
342            return -1;
343
344        // The search period is later than the last photo
345        if (searchedTime > images.get(lstSize - 1).getExifInstant().toEpochMilli())
346            return lstSize-1;
347
348        // The searched index is somewhere in the middle, do a binary search from the beginning
349        int curIndex;
350        int startIndex = 0;
351        int endIndex = lstSize-1;
352        while (endIndex - startIndex > 1) {
353            curIndex = (endIndex + startIndex) / 2;
354            if (searchedTime > images.get(curIndex).getExifInstant().toEpochMilli()) {
355                startIndex = curIndex;
356            } else {
357                endIndex = curIndex;
358            }
359        }
360        if (searchedTime < images.get(endIndex).getExifInstant().toEpochMilli())
361            return startIndex;
362
363        // This final loop is to check if photos with the exact same EXIF time follows
364        while ((endIndex < (lstSize - 1)) && (images.get(endIndex).getExifInstant().toEpochMilli()
365                == images.get(endIndex + 1).getExifInstant().toEpochMilli())) {
366            endIndex++;
367        }
368        return endIndex;
369    }
370}