From 8c7cc19d74a4e1205620eaa8872fa00cd77b1b06 Mon Sep 17 00:00:00 2001 From: zhongchao Date: Wed, 13 Apr 2022 21:53:48 +0800 Subject: [PATCH] merge --- .../core/function/map/IdentifyDataDrawer.java | 48 +++++++++++++++---- 1 file changed, 40 insertions(+), 8 deletions(-) diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java index f8e0efc87b..24f86f7a47 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java @@ -9,8 +9,11 @@ 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.mogo.logger.CallerLogger; +import com.mogo.eagle.core.utilcode.util.UiThreadHandler; +import com.mogo.map.MogoMap; import com.mogo.map.MogoMarkerManager; import com.mogo.module.common.MogoApisHandler; +import com.mogo.module.common.constants.AdasRecognizedType; import com.mogo.module.common.utils.DrivingDirectionUtils; import java.util.ArrayList; @@ -35,6 +38,8 @@ public class IdentifyDataDrawer { */ private static final ConcurrentHashMap mMarkersCaches = new ConcurrentHashMap<>(); + private static final ConcurrentHashMap algoCache = new ConcurrentHashMap<>(); + /** * 已经感知不到的脏数据 */ @@ -95,13 +100,14 @@ public class IdentifyDataDrawer { } trafficDataUuidList.forEach(uuid -> { mMarkersCaches.remove(uuid); + algoCache.remove(uuid); }); - CallerLogger.INSTANCE.d(M_HMI + "arrow47", "origin data size : " + resultList.size()); + CallerLogger.INSTANCE.d(M_HMI + "arrow48", "origin data size : " + resultList.size()); ArrayList filterList = filterTrafficData(resultList); long cost = System.currentTimeMillis() - start; - CallerLogger.INSTANCE.d(M_HMI + "arrow47", "cost : " + cost); + CallerLogger.INSTANCE.d(M_HMI + "arrow48", "cost : " + cost); if (filterList.size() > 0) { // 绘制新数据 @@ -109,7 +115,7 @@ public class IdentifyDataDrawer { .updateBatchMarkerPosition(filterList); } - CallerLogger.INSTANCE.d(M_HMI + "arrow47", "first data size : " + firstData.size() + " , mMarkersCaches : " + mMarkersCaches.size()); + CallerLogger.INSTANCE.d(M_HMI + "arrow48", "first data size : " + firstData.size() + " , mMarkersCaches : " + mMarkersCaches.size()); // 首次未添加的感知物在调用完绘制方法后再塞入cache map mMarkersCaches.putAll(firstData); } @@ -136,15 +142,16 @@ public class IdentifyDataDrawer { MessagePad.TrackedObject correctData = null; //todo 进行修正 if (Math.abs(cacheData.getHeading() - data.getHeading()) > 40 && Math.abs(cacheData.getHeading() - data.getHeading()) < 90) { - CallerLogger.INSTANCE.d(M_HMI + "arrow47", "uuid: " + data.getUuid() + " , 40~90差值范围 , 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading()); + CallerLogger.INSTANCE.d(M_HMI + "arrow48", "uuid: " + data.getUuid() + " , 40~90差值范围 , 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading()); } if (Math.abs(cacheData.getHeading() - data.getHeading()) > 90) { int degree = DrivingDirectionUtils.getDegreeOfCar2Poi(cacheData.getLongitude(), cacheData.getLatitude(), data.getLongitude(), data.getLatitude(), Double.valueOf(cacheData.getHeading()).intValue()); if (degree > 90) { - CallerLogger.INSTANCE.d(M_HMI + "arrow47", "uuid: " + data.getUuid() + " , 夹角 : " + degree + " , 修正 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading()); - correctData = data.toBuilder().setHeading(cacheData.getHeading()).build(); + double heading = kalmanCorrect(data); + CallerLogger.INSTANCE.d(M_HMI + "arrow48", "uuid: " + data.getUuid() + " , 夹角 : " + degree + " , kalmanCorrect : " + heading + " , 修正 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading()); + correctData = data.toBuilder().setHeading(heading).build(); } else { - CallerLogger.INSTANCE.d(M_HMI + "arrow47", "uuid: " + data.getUuid() + " , 夹角 : " + degree + " , 未修正 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading()); + CallerLogger.INSTANCE.d(M_HMI + "arrow48", "uuid: " + data.getUuid() + " , 夹角 : " + degree + " , 未修正 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading()); } } if (correctData != null) { @@ -164,6 +171,32 @@ public class IdentifyDataDrawer { return mFilterTrafficData; } + private double kalmanCorrect(MessagePad.TrackedObject data) { + String uuid = "" + data.getUuid(); + if (algoCache.containsKey(uuid)) { + Object o = algoCache.get(uuid); + KalmanFilter kf = (KalmanFilter) o; + assert kf != null; + double[] lonLat = kf.filter(data.getLongitude(), data.getLatitude()); + algoCache.put(uuid, kf); + MessagePad.TrackedObject cacheTrackObj = mMarkersCaches.get(uuid); + assert cacheTrackObj != null; + if (data.getSpeed() >= 4) { + double heading = MogoMap.getInstance().getMogoMap().getUIController().getAngle(cacheTrackObj.getLongitude(), cacheTrackObj.getLatitude(), lonLat[0], lonLat[1]); + CallerLogger.INSTANCE.d(M_HMI + "arrow48", " uuid : " + uuid + " , origin heading : " + data.getHeading() + " , correct heading : " + heading + " ---- " + (data.getHeading() - heading)); + return heading; + } else { + return data.getHeading(); + } + } else { + double r = 0.00005; + if (AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficBus || AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficTruck) { + r = 0.0001; + } + algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), r)); + return data.getHeading(); + } + } /** * 清除旧的 marker 数据 @@ -175,5 +208,4 @@ public class IdentifyDataDrawer { } trafficDataUuidList.clear(); } - }