From 70a8ea89c63f645644f6e96578441d8eed74664f Mon Sep 17 00:00:00 2001 From: zhongchao Date: Thu, 30 Jun 2022 15:51:14 +0800 Subject: [PATCH] cherry pick eagle --- .../core/function/map/identify/Identify.kt | 4 +- .../identify/IdentifyBeautifyDataDrawer.kt | 9 -- .../map/identify/IdentifyOriginDataDrawer.kt | 4 + .../function/map/identify/TrackManager.java | 144 +++++++----------- .../core/function/map/identify/TrackObj.java | 40 +++-- .../mogo/map/utils/PointInterpolatorUtil.java | 5 +- 6 files changed, 91 insertions(+), 115 deletions(-) diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.kt b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.kt index 9a6f593406..f66e8b1243 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.kt +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.kt @@ -5,7 +5,9 @@ import mogo.telematics.pad.MessagePad.TrackedObject interface Identify { - fun renderPlanningWarningObj(planningObjects: List?) + fun renderPlanningWarningObj(planningObjects: List?){ + + } fun renderAdasRecognizedResult(resultList: List?) diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.kt b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.kt index 6d7dbf42c9..928a999498 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.kt +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.kt @@ -17,10 +17,6 @@ class IdentifyBeautifyDataDrawer : Identify { private const val TAG = "IdentifyDataDrawer" } - override fun renderPlanningWarningObj(planningObjects: List?) { - TrackManager.getInstance().filterWarningData(planningObjects) - } - /** * 渲染 adas 识别的数据 * @@ -41,12 +37,7 @@ class IdentifyBeautifyDataDrawer : Identify { //清除缓存 TrackManager.getInstance().clearCache(resultList) -// val cost = System.nanoTime() val filterList = TrackManager.getInstance().filterTrafficData(resultList) -// Log.d( -// "time cost", -// " " + (System.nanoTime() - cost) / 1000000 + " , 处理了" + resultList.size + "条数据" -// ) if (filterList.size > 0) { // 绘制新数据 MogoMarkerManager.getInstance(AbsMogoApplication.getApp()) 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 36ed626e02..7e575fcaee 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 @@ -128,6 +128,10 @@ class IdentifyOriginDataDrawer : Identify, IMoGoAutopilotStatusListener { //清除缓存 for (data in resultList) { if (trafficDataUuidList.size > 0 && trafficDataUuidList.contains("" + data.uuid)) { + if (!FunctionBuildConfig.isDrawUnknownIdentifyData && data.type == TrafficTypeEnum.TYPE_TRAFFIC_ID_WEI_ZHI.type) { + //CallerLogger.INSTANCE.w(TAG, "未知感知类型数据，丢弃，不渲染"); + continue + } trafficDataUuidList.remove("" + data.uuid) } } 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 6493eff69a..71fa51821d 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 @@ -1,23 +1,27 @@ package com.mogo.eagle.core.function.map.identify; +import android.annotation.SuppressLint; import android.os.Build; -import android.util.ArrayMap; import android.util.Log; import androidx.annotation.RequiresApi; -import androidx.collection.ArraySet; import com.google.common.collect.BiMap; import com.google.common.collect.HashBiMap; import com.mogo.commons.AbsMogoApplication; import com.mogo.eagle.core.data.config.FunctionBuildConfig; import com.mogo.eagle.core.data.enums.TrafficTypeEnum; +import com.mogo.eagle.core.utilcode.geometry.S2CellId; +import com.mogo.eagle.core.utilcode.geometry.S2LatLng; import com.mogo.map.MogoMarkerManager; import java.util.ArrayList; +import java.util.Collections; import java.util.Iterator; import java.util.List; import java.util.Set; +import java.util.concurrent.ConcurrentHashMap; +import java.util.function.Consumer; import mogo.telematics.pad.MessagePad; @@ -31,148 +35,112 @@ public class TrackManager { return TrackOwner.trackManager; } - public static double LIMIT_SPEED = 0.3; + public static double LIMIT_SPEED = 0.45; /** * marker缓存队列 */ - private final ArrayMap mMarkersCaches = new ArrayMap<>(); + private final ConcurrentHashMap mMarkersCaches = new ConcurrentHashMap<>(); /** * marker s2 cellId缓存队列，空间换时间 */ private final BiMap cellIdCaches = HashBiMap.create(); -// private final ArrayMap recentCaches = new ArrayMap<>(); - /** * 记录每次实际绘制的交通元素UUID */ - private final Set trafficDataUuidList = new ArraySet<>(); + private final List trafficDataUuid = Collections.synchronizedList(new ArrayList<>()); /** * 过滤后的数据集合 */ private final ArrayList mFilterTrafficData = new ArrayList<>(); - public ArrayList filterTrafficData(List trafficData) { + @SuppressLint("NewApi") + public synchronized ArrayList filterTrafficData(List trafficData) { //清空上次返回数据，做到缓存复用 mFilterTrafficData.clear(); - trafficDataUuidList.clear(); //进入过滤机制的感知物体，首先从缓存队列中进行查找 uuid for (MessagePad.TrackedObject data : trafficData) { - - // todo 过滤掉未知感知数据，后面会依据危险等级显示 if (!FunctionBuildConfig.isDrawUnknownIdentifyData && data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_WEI_ZHI.getType()) { - //CallerLogger.INSTANCE.w(TAG, "未知感知类型数据，丢弃，不渲染"); continue; } - String uuid = "" + data.getUuid(); TrackObj trackObj = mMarkersCaches.get(uuid); if (trackObj != null) { - data = trackObj.updateObj(data); - mFilterTrafficData.add(data); + trackObj.updateObj(data); } else { - trackObj = new TrackObj(data); - // 融合逻辑 : 判断是否有重合元素 google s2 - if (cellIdCaches.containsValue(trackObj.getCellIdPos())) { - String findSameValue = cellIdCaches.inverse().get(trackObj.getCellIdPos()); + S2LatLng s2LatLng = S2LatLng.fromDegrees(data.getLatitude(), data.getLongitude()); + S2CellId s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22); //需要验证22前后 + long pos = s2CellId.pos(); + // 融合逻辑 : 判断是否有重合元素 + if (cellIdCaches.containsValue(pos)) { + String findSameValue = cellIdCaches.inverse().get(pos); 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; + TrackObj cacheTrack = mMarkersCaches.get(findSameValue); + if (cacheTrack != null) { + trackObj = cacheTrack; + MessagePad.TrackedObject cache = cacheTrack.getCache(); + if (cache != null) { + //相对静止物体并且非obu数据，则不改变，为感知融合同位置物体，使用缓存数据做覆盖 + if (cacheTrack.relativeStatic()) { + data = cache; + } } - // 颜色处理 OBU相关 先处理缓存帧，后处理新感知帧，因为存在obu感知更新 -// String cacheId = "" + cache.getUuid(); -// if (cacheId.startsWith("7476") && cache.getUuid() > 10000) { -// Log.d("hy uuid : " + cacheId, "缓存物为OBU数据，更新物体类型和颜色为OBU"); -// data = data.toBuilder().setType(cache.getType()).setColor(cache.getColor()).build(); -// } -// if (uuid.startsWith("7476") && data.getUuid() > 10000) { -// Log.d("hy uuid : " + uuid, "出现OBU感知物体"); -// data = data.toBuilder().setType(data.getType()).setColor(data.getColor()).build(); -// } + uuid = findSameValue; + trackObj.updateObj(data); + } else { + trackObj = new TrackObj(data); } - uuid = findSameValue; - data = trackObj.updateObj(data); - mFilterTrafficData.add(data); + } else { + trackObj = new TrackObj(data); } } + mFilterTrafficData.add(trackObj.getCache()); cellIdCaches.forcePut(uuid, trackObj.getCellIdPos()); mMarkersCaches.put(uuid, trackObj); - trafficDataUuidList.add(uuid); + trafficDataUuid.add(uuid); } - //todo 将上次没被删除掉物体加入集合，造成延迟删除，对运动物体不友好 -// Iterator it = recentCaches.keySet().iterator(); -// while (it.hasNext()) { -// String key = (String) it.next(); -// TrackObj trackObj = recentCaches.get(key); -// if(trackObj == null){ -// continue; -// } -// if(!trackObj.relativeStatic()){ -// continue; -// } -// mFilterTrafficData.add(trackObj.getCache()); -// } return mFilterTrafficData; } - //todo reset color - public void filterWarningData(List planningObjects) { - - } - @RequiresApi(api = Build.VERSION_CODES.N) public void clearCache(List resultList) { - //清除缓存 + //过滤现有元素 for (MessagePad.TrackedObject data : resultList) { - if (trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())) { - trafficDataUuidList.remove("" + data.getUuid()); + String uuid = "" + data.getUuid(); + if (!FunctionBuildConfig.isDrawUnknownIdentifyData && data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_WEI_ZHI.getType()) { + continue; + } + if (trafficDataUuid.size() > 0) { + trafficDataUuid.remove(uuid); } } - trafficDataUuidList.forEach(uuid -> { - removeKey(uuid); - }); - //todo bus存在时间回溯，将id重置，会有id复用问题，导致鹰眼展示元素缺少 -// Iterator it = mMarkersCaches.keySet().iterator(); -// while (it.hasNext()) { -// String key = (String) it.next(); -// TrackObj trackObj = mMarkersCaches.get(key); -// if (trackObj != null && Math.abs(CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84SatelliteTime() - trackObj.getRecentlyTime()) >= 2000) { -//// Log.d("track", "clearCache uuid : " + key + " ， time : " + (CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84SatelliteTime() - trackObj.getRecentlyTime())); -// mMarkersCaches.remove(key); -// cellIdCaches.remove(key); -// recentCaches.remove(key); -// MogoMarkerManager.getInstance(AbsMogoApplication.getApp()) -// .removeMarker(key); -// } else { -// recentCaches.put(key, trackObj); -// } -// } + + //清除缓存,删除marker + Iterator it = trafficDataUuid.iterator(); + while (it.hasNext()) { + String key = (String) it.next(); + it.remove(); + removeKey(key); + } } - public void removeKey(String key) { + public synchronized void removeKey(String key) { mMarkersCaches.remove(key); cellIdCaches.remove(key); -// recentCaches.remove(key); MogoMarkerManager.getInstance(AbsMogoApplication.getApp()) .removeMarker(key); } + @SuppressLint("NewApi") public void clearAll() { - trafficDataUuidList.clear(); - Iterator it = mMarkersCaches.keySet().iterator(); - while (it.hasNext()) { - String key = (String) it.next(); - removeKey(key); - } + trafficDataUuid.clear(); + mMarkersCaches.forEach((uuid, trackObj) -> removeKey(uuid)); } + + } 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 2675b96016..dba607bc4c 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 @@ -14,6 +14,7 @@ import com.mogo.map.utils.PointInterpolatorUtil; import com.zhidaoauto.map.sdk.open.data.MapDataApi; import java.util.Arrays; +import java.util.Comparator; import java.util.List; import mogo.telematics.pad.MessagePad; @@ -24,7 +25,7 @@ public class TrackObj { // private final KalmanFilter kalmanFilter; //卡尔曼结果 private S2CellId s2CellId; //s2 id权重 private S2LatLng s2LatLng; //s2 经纬度 - private long recentlyTime; //用于缓存帧数判断，暂定缓存1秒数据，中间如果有物体未出现，1秒后删除 + private double recentlyTime; //用于缓存帧数判断，暂定缓存1秒数据，中间如果有物体未出现，1秒后删除 private double roadAngle; //道路航向 private double headingDelta; //航向角德尔塔 private int[] typeArray = new int[3]; @@ -32,12 +33,12 @@ public class TrackObj { private double lat; private double lon; private double speedAverage; - private com.zhidaoauto.map.sdk.open.road.CenterLine centerLineInfo = null; + private CenterLine centerLineInfo = null; public TrackObj(MessagePad.TrackedObject data) { // kalmanFilter = new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005); circleQueue.addQueue(new ObjQueue(data.getHeading(), data.getSpeed(), data.getType())); - recentlyTime = Double.valueOf(data.getSatelliteTime() * 1000).longValue(); + recentlyTime = data.getSatelliteTime(); lat = data.getLatitude(); lon = data.getLongitude(); s2LatLng = S2LatLng.fromDegrees(data.getLatitude(), data.getLongitude()); @@ -51,14 +52,11 @@ public class TrackObj { private MessagePad.TrackedObject cacheData; //先处理kalman数据，将经纬度校准后，放入缓存队列，然后基于后序策略将各个项进行校准 - public MessagePad.TrackedObject updateObj(MessagePad.TrackedObject data) { + public void updateObj(MessagePad.TrackedObject data) { cacheData = data; - correct(); - - recentlyTime = Double.valueOf(data.getSatelliteTime() * 1000).longValue(); + recentlyTime = data.getSatelliteTime(); circleQueue.addQueue(new ObjQueue(cacheData.getHeading(), cacheData.getSpeed(), cacheData.getType())); - return cacheData; } private void correct() { @@ -70,9 +68,12 @@ public class TrackObj { @SuppressLint("NewApi") private void calAverageSpeedAndType() { if (circleQueue.size() >= 3) { - //计算平均速度 List objQueueList = circleQueue.getLastThreeFrame(); - speedAverage = (objQueueList.get(0).getSpeed() + objQueueList.get(1).getSpeed() + objQueueList.get(2).getSpeed()) / 3; + // 计算平均速度 +// speedAverage = (objQueueList.get(0).getSpeed() + objQueueList.get(1).getSpeed() + objQueueList.get(2).getSpeed()) / 3; + // 使用中值滤波 + objQueueList.sort(Comparator.comparing(ObjQueue::getSpeed)); + speedAverage = objQueueList.get(1).getSpeed(); //计算类型 typeArray[0] = objQueueList.get(0).getType(); typeArray[1] = objQueueList.get(1).getType(); @@ -104,8 +105,8 @@ public class TrackObj { double angle = roadAngle != 0 ? roadAngle : cacheData.getHeading(); if (centerLineInfo == null && isFourWheelType()) { try { - centerLineInfo = MapDataApi.INSTANCE.getCenterLineInfo(lon, lat, (float) angle); - }catch (Exception e){ + centerLineInfo = CallerHDMapManager.INSTANCE.getCenterLineInfo(lon, lat, (float) angle); + } catch (Exception e) { Log.d("hy uuid : " + cacheData.getUuid(), "道路获取异常"); } if (centerLineInfo != null && centerLineInfo.getPoints() != null && !centerLineInfo.getPoints().isEmpty()) { @@ -186,7 +187,7 @@ public class TrackObj { } } - public long getRecentlyTime() { + public double getRecentlyTime() { return recentlyTime; } @@ -194,12 +195,21 @@ public class TrackObj { return s2CellId.pos(); } + /** + * 首帧数据不返回 + * + * @return TrackedObject + */ public MessagePad.TrackedObject getCache() { - return cacheData; + if (circleQueue.size() > 1) { + return cacheData; + } else { + return null; + } } public boolean relativeStatic() { - return speedAverage < LIMIT_SPEED; + return speedAverage < LIMIT_SPEED | cacheData.getSpeed() < LIMIT_SPEED; } public boolean isFourWheelType() { diff --git a/libraries/mogo-map/src/main/java/com/mogo/map/utils/PointInterpolatorUtil.java b/libraries/mogo-map/src/main/java/com/mogo/map/utils/PointInterpolatorUtil.java index 206e780bd6..063da4fb7c 100644 --- a/libraries/mogo-map/src/main/java/com/mogo/map/utils/PointInterpolatorUtil.java +++ b/libraries/mogo-map/src/main/java/com/mogo/map/utils/PointInterpolatorUtil.java @@ -1,10 +1,11 @@ package com.mogo.map.utils; import com.mogo.cloud.commons.utils.CoordinateUtils; +import com.mogo.eagle.core.data.map.LonLatPoint; import com.mogo.eagle.core.data.map.MogoLatLng; import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger; -import com.zhidaoauto.map.sdk.open.query.LonLatPoint; +import java.util.ArrayList; import java.util.List; /** @@ -66,7 +67,7 @@ public class PointInterpolatorUtil { * @param road 目标道路数据 * @return double[]{吸附后的经度，吸附后的纬度，目标经纬度距离道路的垂直距离} */ - public static double[] mergeToRoad(double lon, double lat, List road) { + public static double[] mergeToRoad(double lon, double lat, ArrayList road) { int closeStart = 0; int closeEnd = road.size() - 1; int[] result = getCloseTwoPoint(closeStart, closeEnd, lon, lat, road);