From c06b17cb82a44540f2f8e12d2e97d82eefecc86c Mon Sep 17 00:00:00 2001 From: zhongchao Date: Fri, 8 Apr 2022 12:27:43 +0800 Subject: [PATCH] merge --- .../core/function/map/IdentifyDataDrawer.java | 8 +- .../function/map/IdentifyDataDrawerTest.java | 203 ++++++++++++++++++ .../eagle/core/function/map/KalmanFilter.java | 34 +++ .../function/map/MapIdentifySubscriber.kt | 4 +- .../src/main/java/com/mogo/map/IMogoMap.java | 1 - .../uicontroller/IMogoMapUIController.java | 6 + .../java/com/mogo/map/AMapViewWrapper.java | 5 + .../com/mogo/map/MogoMapUIController.java | 9 + .../map/uicontroller/AMapUIController.java | 8 + 9 files changed, 274 insertions(+), 4 deletions(-) create mode 100644 core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTest.java create mode 100644 core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java 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 128c7aa062..7e63c599e8 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 @@ -87,10 +87,11 @@ public class IdentifyDataDrawer { return; } + long start = System.currentTimeMillis(); //清除缓存 for (MessagePad.TrackedObject data : resultList) { if(trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())){ - trafficDataUuidList.remove(data.getUuid()); + trafficDataUuidList.remove("" + data.getUuid()); } } trafficDataUuidList.forEach(uuid -> { @@ -99,11 +100,16 @@ public class IdentifyDataDrawer { CallerLogger.INSTANCE.d(M_HMI + "arrow47", "origin data size : " + resultList.size()); ArrayList filterList = filterTrafficData(resultList); + + long cost = System.currentTimeMillis() - start; + CallerLogger.INSTANCE.d(M_HMI + "arrow47", "cost : " + cost); + if (filterList.size() > 0) { // 绘制新数据 MogoMarkerManager.getInstance(mContext) .updateBatchMarkerPosition(filterList); } + CallerLogger.INSTANCE.d(M_HMI + "arrow47", "first data size : " + firstData.size() + " , mMarkersCaches : " + mMarkersCaches.size()); // 首次未添加的感知物在调用完绘制方法后再塞入cache map mMarkersCaches.putAll(firstData); diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTest.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTest.java new file mode 100644 index 0000000000..85bd82d6fc --- /dev/null +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTest.java @@ -0,0 +1,203 @@ +package com.mogo.eagle.core.function.map; + +import static com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant.M_HMI; + +import android.annotation.SuppressLint; +import android.content.Context; + +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.function.call.map.CallerMapLocationListenerManager; +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 java.util.ArrayList; +import java.util.List; +import java.util.concurrent.ConcurrentHashMap; + +import mogo.telematics.pad.MessagePad; + +/** + * @author xiaoyuzhou + * @date 2021/10/19 10:45 上午 + * 域控制器识别信息绘制 + */ +public class IdentifyDataDrawerTest { + private static final String TAG = "IdentifyDataDrawer"; + + protected final Context mContext; + private static volatile IdentifyDataDrawerTest sInstance; + + /** + * 上一帧数据的缓存 + */ + private static final ConcurrentHashMap mMarkersCaches = new ConcurrentHashMap<>(); + + private static final ConcurrentHashMap algoCache = new ConcurrentHashMap<>(); + + /** + * 记录每次实际绘制的交通元素UUID + */ + private final ArrayList trafficDataUuidList = new ArrayList<>(); + /** + * 过滤后的数据集合 + */ + private final ArrayList mFilterTrafficData = new ArrayList<>(); + + private IdentifyDataDrawerTest() { + mContext = AbsMogoApplication.getApp(); + addPreVehicleModel(); + } + + public static IdentifyDataDrawerTest getInstance() { + if (sInstance == null) { + synchronized (IdentifyDataDrawerTest.class) { + if (sInstance == null) { + sInstance = new IdentifyDataDrawerTest(); + } + } + } + return sInstance; + } + + public synchronized void release() { + sInstance = null; + } + + /** + * 渲染 adas 识别的数据 + * + * @param resultList adas感知融合数据 + */ + @SuppressLint("NewApi") + public void renderAdasRecognizedResult(List resultList) { + if (resultList == null || resultList.isEmpty()) { + clearOldMarker(); + CallerLogger.INSTANCE.w(TAG, "感知数据为空无需渲染……"); + return; + } + + if (!MogoApisHandler.getInstance().getApis().getStatusManagerApi().isVrMode()) { + clearOldMarker(); + CallerLogger.INSTANCE.w(TAG, "渲染 adas 识别的数据 当前不是VR模式"); + return; + } + + //清除缓存 + for (MessagePad.TrackedObject data : resultList) { + if (trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())) { + trafficDataUuidList.remove("" + data.getUuid()); + } + } + trafficDataUuidList.forEach(uuid -> { + mMarkersCaches.remove(uuid); + algoCache.remove(uuid); + }); + + ArrayList filterList = filterTrafficData(resultList); + if (filterList.size() > 0) { + // 绘制新数据 + MogoMarkerManager.getInstance(mContext) + .updateBatchMarkerPosition(filterList); + } + } + + /** + * 数据过滤器 + * + * @return 过滤后的数据集合 + */ + private ArrayList filterTrafficData(List trafficData) { + mFilterTrafficData.clear(); + trafficDataUuidList.clear(); + for (MessagePad.TrackedObject data : trafficData) { + // 过滤掉未知感知数据 + if (!FunctionBuildConfig.isDrawUnknownIdentifyData && data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_WEI_ZHI.getType()) { + //CallerLogger.INSTANCE.w(TAG, "未知感知类型数据，丢弃，不渲染"); + continue; + } + double heading = kalmanFilter(data); + MessagePad.TrackedObject correctData = data.toBuilder().setHeading(heading).build(); + mFilterTrafficData.add(correctData); + //更新已存在的感知物体数据 + mMarkersCaches.put("" + correctData.getUuid(), correctData); + trafficDataUuidList.add("" + correctData.getUuid()); + } + return mFilterTrafficData; + } + + private double kalmanFilter(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; + double heading = MogoMap.getInstance().getMogoMap().getUIController().getAngle(cacheTrackObj.getLongitude(), cacheTrackObj.getLatitude(), lonLat[0], lonLat[1]); + CallerLogger.INSTANCE.d(M_HMI + "arrow47", " uuid : " + uuid + " , origin heading : " + data.getHeading() + " , correct heading : " + heading + " ---- " + (data.getHeading() - heading)); + return Math.abs((data.getHeading() - heading)) >= 180 ? heading : data.getHeading(); + } else { + algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.00005)); + return data.getHeading(); + } + } + + /** + * 清除旧的 marker 数据 + */ + public void clearOldMarker() { + for (String uuid : trafficDataUuidList) { + MogoMarkerManager.getInstance(mContext) + .removeMarker(uuid); + } + trafficDataUuidList.clear(); + } + + private void addPreVehicleModel() { + CallerLogger.INSTANCE.d(TAG, "添加感知模型到地图中……"); + addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_WEI_ZHI, "添加感知模型到地图中……preVehicleStrWeiZhi="); + + addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_PEOPLE, "添加感知模型到地图中……preVehicleStrPeople="); + + addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_BICYCLE, "添加感知模型到地图中……preVehicleStrBicycle="); + + addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_TA_CHE, "添加感知模型到地图中……preVehicleStrTaChe="); + + addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_MOTO, "添加感知模型到地图中……preVehicleStrMoto="); + + addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS, "添加感知模型到地图中……preVehicleStrBus="); + + addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK, "添加感知模型到地图中……preVehicleStrTruck="); + } + + /** + * 添加模型到地图中 + * + * @param typeTrafficIdWeiZhi + * @param s + */ + private void addPreVehicleModelWeiZhi(TrafficTypeEnum typeTrafficIdWeiZhi, String s) { + String preVehicleStrWeiZhi = MogoMarkerManager.getInstance(mContext) + .addPreVehicleModel(typeTrafficIdWeiZhi.getType(), + typeTrafficIdWeiZhi.getTraffic3DIconId()); + CallerLogger.INSTANCE.d(TAG, s + preVehicleStrWeiZhi); + + if (preVehicleStrWeiZhi == null) { + UiThreadHandler.postDelayed(new Runnable() { + @Override + public void run() { + CallerLogger.INSTANCE.w(TAG, "添加感知模型到地图中失败，尝试重复添加……"); + addPreVehicleModelWeiZhi(typeTrafficIdWeiZhi, s); + } + }, 1000L); + } + } + +} diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java new file mode 100644 index 0000000000..08605437fa --- /dev/null +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java @@ -0,0 +1,34 @@ +package com.mogo.eagle.core.function.map; + +public class KalmanFilter { + private final double q = 1.0E-6D; + double r = 5.0E-5D; + double[][] xhat = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}}; + double[][] p = new double[][]{{1.0D, 1.0D}, {0.0D, 0.0D}}; + double[][] xhatminus = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}}; + double[][] pMinus = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}}; + double[][] k = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}}; + int idx = 1; + + public KalmanFilter(double lon, double lat, double r) { + this.xhat[0][0] = lon; + this.xhat[0][1] = lat; + this.r = r; + } + + public double[] filter(double lon, double lat) { + for(int i = 0; i < 2; ++i) { + this.xhatminus[this.idx][i] = this.xhat[1 - this.idx][i]; + this.pMinus[this.idx][i] = this.p[1 - this.idx][i] + 1.0E-6D; + this.k[this.idx][i] = this.pMinus[this.idx][i] / (this.pMinus[this.idx][i] + this.r); + double value = i == 0 ? lon : lat; + this.xhat[this.idx][i] = this.xhatminus[this.idx][i] + this.k[this.idx][i] * (value - this.xhatminus[this.idx][i]); + this.p[this.idx][i] = (1.0D - this.k[this.idx][i]) * this.pMinus[this.idx][i]; + } + + double lon1 = this.xhat[this.idx][0]; + double lat1 = this.xhat[this.idx][1]; + this.idx = 1 - this.idx; + return new double[]{lon1, lat1}; + } +} diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/MapIdentifySubscriber.kt b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/MapIdentifySubscriber.kt index 5eb0df67c8..662e3c6136 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/MapIdentifySubscriber.kt +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/MapIdentifySubscriber.kt @@ -41,9 +41,9 @@ class MapIdentifySubscriber private constructor() : IMoGoSubscriber, IMoGoAutopi override fun onAutopilotIdentifyDataUpdate(trafficData: List?) { try { if (FunctionBuildConfig.isDrawIdentifyData) { - ThreadUtils.getSinglePool().execute { IdentifyDataDrawer.getInstance().renderAdasRecognizedResult(trafficData) } + ThreadUtils.getSinglePool().execute { IdentifyDataDrawerTest.getInstance().renderAdasRecognizedResult(trafficData) } } else { - IdentifyDataDrawer.getInstance().clearOldMarker() + IdentifyDataDrawerTest.getInstance().clearOldMarker() } } catch (e: Exception) { e.printStackTrace() diff --git a/libraries/mogo-map-api/src/main/java/com/mogo/map/IMogoMap.java b/libraries/mogo-map-api/src/main/java/com/mogo/map/IMogoMap.java index dcf235541d..11b968b840 100644 --- a/libraries/mogo-map-api/src/main/java/com/mogo/map/IMogoMap.java +++ b/libraries/mogo-map-api/src/main/java/com/mogo/map/IMogoMap.java @@ -1,6 +1,5 @@ package com.mogo.map; -import com.mogo.eagle.core.data.traffic.TrafficData; import com.mogo.map.marker.IMogoMarker; import com.mogo.map.marker.MogoMarkerOptions; import com.mogo.map.overlay.IMogoPolyline; diff --git a/libraries/mogo-map-api/src/main/java/com/mogo/map/uicontroller/IMogoMapUIController.java b/libraries/mogo-map-api/src/main/java/com/mogo/map/uicontroller/IMogoMapUIController.java index 65cb31af57..9ae3db58a1 100644 --- a/libraries/mogo-map-api/src/main/java/com/mogo/map/uicontroller/IMogoMapUIController.java +++ b/libraries/mogo-map-api/src/main/java/com/mogo/map/uicontroller/IMogoMapUIController.java @@ -352,4 +352,10 @@ public interface IMogoMapUIController { * 设置地图视线角度 */ void setMapDAngle(float angle); + + /** + * 获取行车方向 + * @return + */ + float getAngle(double startLon, double startLat, double endLon, double endLat); } diff --git a/libraries/mogo-map/src/main/java/com/mogo/map/AMapViewWrapper.java b/libraries/mogo-map/src/main/java/com/mogo/map/AMapViewWrapper.java index 6881bc8434..f18918c184 100644 --- a/libraries/mogo-map/src/main/java/com/mogo/map/AMapViewWrapper.java +++ b/libraries/mogo-map/src/main/java/com/mogo/map/AMapViewWrapper.java @@ -1079,4 +1079,9 @@ public class AMapViewWrapper implements IMogoMapView, mMapView.getMapAutoViewHelper().setMapDAngle(angle); } + @Override + public float getAngle(double startLon, double startLat, double endLon, double endLat) { + return MapAutoApi.INSTANCE.getAngle(startLon,startLat,endLon,endLat); + } + } diff --git a/libraries/mogo-map/src/main/java/com/mogo/map/MogoMapUIController.java b/libraries/mogo-map/src/main/java/com/mogo/map/MogoMapUIController.java index d0e275cd76..0eaa831de4 100644 --- a/libraries/mogo-map/src/main/java/com/mogo/map/MogoMapUIController.java +++ b/libraries/mogo-map/src/main/java/com/mogo/map/MogoMapUIController.java @@ -454,4 +454,13 @@ public class MogoMapUIController implements IMogoMapUIController { mDelegate.setMapDAngle(angle); } } + + @Override + public float getAngle(double startLon, double startLat, double endLon, double endLat) { + initDelegate(); + if (mDelegate != null) { + return mDelegate.getAngle(startLon, startLat, endLon, endLat); + } + return 0.0f; + } } diff --git a/libraries/mogo-map/src/main/java/com/mogo/map/uicontroller/AMapUIController.java b/libraries/mogo-map/src/main/java/com/mogo/map/uicontroller/AMapUIController.java index 39051ccaf9..38fc9ebdae 100644 --- a/libraries/mogo-map/src/main/java/com/mogo/map/uicontroller/AMapUIController.java +++ b/libraries/mogo-map/src/main/java/com/mogo/map/uicontroller/AMapUIController.java @@ -374,4 +374,12 @@ public class AMapUIController implements IMogoMapUIController { mClient.setMapDAngle(angle); } } + + @Override + public float getAngle(double startLon, double startLat, double endLon, double endLat) { + if (mClient != null) { + return mClient.getAngle(startLon,startLat,endLon,endLat); + } + return 0; + } }