diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.kt b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.kt index 76084e0da4..e39ae44c44 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.kt +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.kt @@ -1,6 +1,7 @@ package com.mogo.eagle.core.function.map.identify import android.annotation.SuppressLint +import androidx.collection.ArraySet import com.mogo.commons.AbsMogoApplication import com.mogo.eagle.core.data.config.FunctionBuildConfig import com.mogo.eagle.core.data.enums.TrafficTypeEnum @@ -37,7 +38,7 @@ class IdentifyOriginDataDrawer : Identify { /** * 记录每次实际绘制的交通元素UUID */ - private val trafficDataUuidList = ArrayList() + private val trafficDataUuidList = ArraySet() /** * 过滤后的数据集合 diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java index e13df39911..b22d89de6c 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java @@ -14,7 +14,6 @@ import com.mogo.eagle.core.data.config.FunctionBuildConfig; import com.mogo.eagle.core.data.enums.TrafficTypeEnum; import com.mogo.map.MogoMarkerManager; -import java.text.DecimalFormat; import java.util.ArrayList; import java.util.Iterator; import java.util.List; @@ -32,8 +31,6 @@ public class TrackManager { return TrackOwner.trackManager; } - public static final DecimalFormat DF = new DecimalFormat("0.000000"); - public static final int DISTANCE = 6371000; public static double LIMIT_SPEED = 0.5; /** @@ -46,7 +43,7 @@ public class TrackManager { */ private final BiMap cellIdCaches = HashBiMap.create(); - private final ArrayMap recentCaches = new ArrayMap<>(); +// private final ArrayMap recentCaches = new ArrayMap<>(); /** * 记录每次实际绘制的交通元素UUID @@ -78,16 +75,25 @@ public class TrackManager { mFilterTrafficData.add(data); } else { trackObj = new TrackObj(data); - // 判断是否有重合元素 google s2 + // 融合逻辑 : 判断是否有重合元素 google s2 if (cellIdCaches.containsValue(trackObj.getCellIdPos())) { String findSameValue = cellIdCaches.inverse().get(trackObj.getCellIdPos()); - if (data.getUuid() - Integer.parseInt(findSameValue) > 0) { -// Log.d("0609", "uuid : " + findSameValue + " 与新感知物 : " + uuid + " , 出现相同pos : " + trackObj.getCellIdPos()); - uuid = findSameValue; - data = data.toBuilder().setUuid(Integer.parseInt(findSameValue)).build(); - data = trackObj.updateObj(data); - mFilterTrafficData.add(data); + Log.d("hy uuid : " + findSameValue, " 与新感知物 : " + uuid + "相同"); + //uuid处理 + data = data.toBuilder().setUuid(Integer.parseInt(findSameValue)).build(); + MessagePad.TrackedObject cache = mMarkersCaches.get(findSameValue).getCache(); + if (cache != null) { + //相对静止物体不改变 + TrackObj cacheTrack = mMarkersCaches.get(findSameValue); + if (cacheTrack.relativeStatic()) { + data = cache; + trackObj = cacheTrack; + } + //todo OBU相关后面通过增加字段判断数据来源进行融合，不融合衡阳验收分支功能 } + uuid = findSameValue; + data = trackObj.updateObj(data); + mFilterTrafficData.add(data); } } cellIdCaches.forcePut(uuid, trackObj.getCellIdPos()); @@ -126,10 +132,7 @@ public class TrackManager { } trafficDataUuidList.forEach(uuid -> { Log.d("hy uuid : " + uuid, " 移除物体"); - mMarkersCaches.remove(uuid); - cellIdCaches.remove(uuid); - MogoMarkerManager.getInstance(AbsMogoApplication.getApp()) - .removeMarker(uuid); + removeKey(uuid); }); //todo bus存在时间回溯，将id重置，会有id复用问题，导致鹰眼展示元素缺少 // Iterator it = mMarkersCaches.keySet().iterator(); @@ -149,16 +152,20 @@ public class TrackManager { // } } + public void removeKey(String key) { + mMarkersCaches.remove(key); + cellIdCaches.remove(key); +// recentCaches.remove(key); + MogoMarkerManager.getInstance(AbsMogoApplication.getApp()) + .removeMarker(key); + } + public void clearAll() { trafficDataUuidList.clear(); Iterator it = mMarkersCaches.keySet().iterator(); while (it.hasNext()) { String key = (String) it.next(); - mMarkersCaches.remove(key); - cellIdCaches.remove(key); - recentCaches.remove(key); - MogoMarkerManager.getInstance(AbsMogoApplication.getApp()) - .removeMarker(key); + removeKey(key); } } } diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java index e4f8b94915..aab75dffd4 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java @@ -1,14 +1,19 @@ package com.mogo.eagle.core.function.map.identify; -import static com.mogo.eagle.core.function.map.identify.TrackManager.DISTANCE; import static com.mogo.eagle.core.function.map.identify.TrackManager.LIMIT_SPEED; +import android.annotation.SuppressLint; +import android.util.Log; + +import com.mogo.eagle.core.data.enums.TrafficTypeEnum; +import com.mogo.eagle.core.data.map.CenterLine; import com.mogo.eagle.core.function.call.map.CallerHDMapManager; import com.mogo.eagle.core.utilcode.geometry.S2CellId; import com.mogo.eagle.core.utilcode.geometry.S2LatLng; -import com.mogo.module.service.Utils; -import com.mogo.eagle.core.data.map.CenterLine; +import com.mogo.map.utils.PointInterpolatorUtil; +import com.zhidaoauto.map.sdk.open.data.MapDataApi; +import java.util.Arrays; import java.util.List; import mogo.telematics.pad.MessagePad; @@ -22,12 +27,12 @@ public class TrackObj { private long recentlyTime; //用于缓存帧数判断，暂定缓存1秒数据，中间如果有物体未出现，1秒后删除 private double roadAngle; //道路航向 private double headingDelta; //航向角德尔塔 + private int[] typeArray = new int[5]; private double typeWeight; //类型权重 private double lat; private double lon; private double speedAverage; -// private SinglePointRoadInfo singlePointRoadInfo; -// private double[] matchedPoint; + private com.zhidaoauto.map.sdk.open.road.CenterLine centerLineInfo = null; public TrackObj(MessagePad.TrackedObject data) { kalmanFilter = new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005); @@ -58,93 +63,72 @@ public class TrackObj { } private void correct() { - calAverageSpeed(); + calAverageSpeedAndType(); calLoc(); calHeading(); - calType(); } - private void calAverageSpeed() { - //计算平均速度 + @SuppressLint("NewApi") + private void calAverageSpeedAndType() { + if (circleQueue.size() >= 5) { + //计算平均速度 List objQueueList = circleQueue.getLastFiveFrame(); speedAverage = (objQueueList.get(0).getSpeed() + objQueueList.get(1).getSpeed() + objQueueList.get(2).getSpeed() + objQueueList.get(3).getSpeed() + objQueueList.get(4).getSpeed()) / 5; + //计算类型 + typeArray[0] = objQueueList.get(0).getType(); + typeArray[1] = objQueueList.get(1).getType(); + typeArray[2] = objQueueList.get(2).getType(); + typeArray[3] = objQueueList.get(3).getType(); + typeArray[4] = objQueueList.get(4).getType(); } else { double cal = 0; List objQueueList = circleQueue.getPreFrame(); - for (ObjQueue obj : objQueueList) { - cal += obj.getSpeed(); + for (int i = 0; i < objQueueList.size() - 1; i++) { + cal += objQueueList.get(i).getSpeed(); + typeArray[i] = objQueueList.get(i).getType(); } speedAverage = cal / objQueueList.size(); -// speedAverage = circleQueue.getLastFrame().getSpeed(); } + Log.d("hy uuid : " + cacheData.getUuid()," 类型 : " + Arrays.toString(typeArray)); + typeArray = Arrays.stream(typeArray).sorted().toArray(); + typeWeight = typeArray[typeArray.length / 2]; } private void calLoc() { //距离计算,位置修正 - //todo bus250 taxi上测试下面注释掉内容 - //double[] lonLat = kalmanFilter.filter(cacheData.getLongitude(), cacheData.getLatitude()); -// double distance = s2LatLng.getDistance(S2LatLng.fromDegrees(lonLat[1], lonLat[0])).distance(DISTANCE); -// double distance = s2LatLng.getDistance(S2LatLng.fromDegrees(cacheData.getLatitude(), cacheData.getLongitude())).distance(DISTANCE); - //todo 重新计算速度值(如果连续几帧distance累加到一定值，速度没变化，需要重新计算速度，防止锁死) -// if (relativeStatic()) { -// double tempDis = distance; -// if (distance >= 4) { //(150km/h) 41.6m/s x 0.1s = 4.16m 约等于 4 -// tempDis = 4; -// } -// double calSpeed = cacheData.getSpeed(); -// if (cacheData.getSpeed() != 0.0) { -// calSpeed = tempDis / ((Double.valueOf(cacheData.getSatelliteTime() * 1000).longValue() - recentlyTime) / 1000.0); -//// Log.d("calSpeed uuid : " + cacheData.getUuid(), " tempDis : " + tempDis + " , 重新赋值 calSpeed : " + DF.format(calSpeed) + " , time : " + (Double.valueOf(cacheData.getSatelliteTime() * 1000).longValue() - recentlyTime) + " , 原速度 : " + cacheData.getSpeed()); -// if (calSpeed > cacheData.getSpeed()) { -// calSpeed = cacheData.getSpeed(); -//// Log.d("calSpeed uuid : " + cacheData.getUuid(), " 二次重新赋值 calSpeed : " + DF.format(calSpeed)); -// } -//// if (calSpeed > 2) { -//// calSpeed = 2; -////// Log.d("calSpeed uuid : " + cacheData.getUuid(), " 三次重新赋值 calSpeed : " + DF.format(calSpeed)); -//// } -// } -// cacheData = cacheData.toBuilder().setSpeed(calSpeed).build(); -// } - //todo 等后序速度优化结果值可用，使用计算结果 -// double calDistance = (cacheData.getSpeed() * (Double.valueOf(cacheData.getSatelliteTime() * 1000).longValue() - recentlyTime)) / 1000.0; -// double calDistance = Utils.calculateLineDistance(lon, lat, cacheData.getLongitude(), cacheData.getLatitude()); -// Log.d("calLoc uuid : " + cacheData.getUuid() + " calDistance : " + DF.format(calDistance), (calDistance * 2 < distance) ? "超出范围" : "正常值"); - //速度小于0.5m/s,并且距离在计算合理范围内超出2倍，则认为是相对静止状态(注意调整阈值)，不更新缓存点信息 -// if (cacheData.getSpeed() < LIMIT_SPEED || relativeStatic() || calDistance * 2 < distance) { if (relativeStatic()) { -// if (singlePointRoadInfo == null) { -// double angle = roadAngle != 0 ? roadAngle : cacheData.getHeading(); -// long cost = System.nanoTime(); -// singlePointRoadInfo = MapDataApi.INSTANCE.getSinglePointMatchRoad(lon, lat, (float) angle, true, true); -// Log.d("hy create cost", " " + (System.nanoTime() - cost) / 1000000); -// } -// if (singlePointRoadInfo != null && singlePointRoadInfo.getCoords() != null && !singlePointRoadInfo.getCoords().isEmpty()) { -// if(matchedPoint == null || matchedPoint.length == 0){ -// long cost = System.nanoTime(); -// matchedPoint = PointInterpolatorUtil.mergeToRoad(cacheData.getLongitude(), cacheData.getLatitude(), singlePointRoadInfo.getCoords()); -// Log.d("hy matchedPoint cost", " " + (System.nanoTime() - cost) / 1000000); -// Log.d("hy uuid : " + cacheData.getUuid(), "道路经纬度 lon : " + matchedPoint[0] + " lat : " + matchedPoint[1] + " distance : " + matchedPoint[2] + " , 原数据 lon : " + lon + " lat : " + lat); -// }else{ -// if(matchedPoint[0] == 0 || matchedPoint[1] == 0){ -// cacheData = cacheData.toBuilder().setLongitude(lon).setLatitude(lat).build(); -// }else{ -// cacheData = cacheData.toBuilder().setLongitude(matchedPoint[0]).setLatitude(matchedPoint[1]).build(); -// } -// } -// lat = matchedPoint[1]; -// lon = matchedPoint[0]; -// } else { -// Log.d("hy uuid : " + cacheData.getUuid(), "未匹配到道路数据,使用原数据 lon : " + lon + " lat : " + lat); -// } - cacheData = cacheData.toBuilder().setLongitude(lon).setLatitude(lat).build(); + double angle = roadAngle != 0 ? roadAngle : cacheData.getHeading(); + if (centerLineInfo == null && isFourWheelType()) { + centerLineInfo = MapDataApi.INSTANCE.getCenterLineInfo(lon, lat, (float) angle); + if (centerLineInfo != null && centerLineInfo.getPoints() != null && !centerLineInfo.getPoints().isEmpty()) { + double[] matchedPoint = PointInterpolatorUtil.mergeToRoad(cacheData.getLongitude(), cacheData.getLatitude(), centerLineInfo.getPoints()); + if (matchedPoint[0] > 0 || matchedPoint[1] > 0) { + lon = matchedPoint[0]; + lat = matchedPoint[1]; + s2LatLng = S2LatLng.fromDegrees(cacheData.getLatitude(), cacheData.getLongitude()); + s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22); + Log.d("hy uuid : " + cacheData.getUuid(), " 更新定位 lon : " + matchedPoint[0] + " , lat : " + matchedPoint[1]); + } else { + centerLineInfo = null; + Log.d("hy uuid : " + cacheData.getUuid(), "计算结果出现问题 : " + matchedPoint[0] + " , " + matchedPoint[1]); + } + } else { + centerLineInfo = null; + Log.d("hy uuid : " + cacheData.getUuid(), "未获取到道路数据"); + } + } + Log.d("hy uuid : " + cacheData.getUuid(), " 静止使用定位数据 lon : " + lon + " , lat : " + lat); + cacheData = cacheData.toBuilder().setLongitude(lon).setLatitude(lat).build(); } else { + centerLineInfo = null; //不在阈值内则更新，代表物体移动，使用卡尔曼滤波经纬度数据 + //double[] lonLat = kalmanFilter.filter(cacheData.getLongitude(), cacheData.getLatitude()); lat = cacheData.getLatitude(); lon = cacheData.getLongitude(); s2LatLng = S2LatLng.fromDegrees(cacheData.getLatitude(), cacheData.getLongitude()); s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22); + Log.d("hy uuid : " + cacheData.getUuid(), " 开始移动 lon : " + lon + " , lat : " + lat); // cacheData = cacheData.toBuilder().setLongitude(lonLat[0]).setLatitude(lonLat[1]).build(); } } @@ -193,15 +177,11 @@ public class TrackObj { } //速度大于LIMIT_SPEED并出现大幅度转向使用缓存帧和delta数据 if (cacheData.getSpeed() >= LIMIT_SPEED && newDelta > 10 && headingDelta != 0.0) { -// Log.i("0609", "uuid : " + cacheData.getUuid() + " 修正航向角 last : " + lastObj.getHeading() + " , 增益 : " + headingDelta); +// Log.i("hy uuid : " + cacheData.getUuid() , " 修正航向角 last : " + lastObj.getHeading() + " , 增益 : " + headingDelta); cacheData = cacheData.toBuilder().setHeading(lastObj.getHeading() + headingDelta).build(); } } - private void calType() { - - } - public long getRecentlyTime() { return recentlyTime; } @@ -218,6 +198,12 @@ public class TrackObj { return speedAverage < LIMIT_SPEED; } + public boolean isFourWheelType() { + return typeWeight != TrafficTypeEnum.TYPE_TRAFFIC_ID_PEOPLE.getType() + || typeWeight != TrafficTypeEnum.TYPE_TRAFFIC_ID_BICYCLE.getType() + || typeWeight != TrafficTypeEnum.TYPE_TRAFFIC_ID_MOTO.getType(); + } + @Override public String toString() { return "TrackObj{" +