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}