From d45f528aa93ea6cad284e9448685b4c0a8593074 Mon Sep 17 00:00:00 2001 From: zhongchao Date: Thu, 2 Jun 2022 11:40:05 +0800 Subject: [PATCH] wait to test --- .../eagle/core/function/map/CircleQueue.java | 33 +++ .../core/function/map/IdentifyDataDrawer.java | 9 +- .../function/map/IdentifyDataDrawerTrack.java | 213 ++++++++++++++++++ .../eagle/core/function/map/ObjQueue.java | 47 ++++ .../eagle/core/function/map/TrackObj.java | 47 ++++ 5 files changed, 344 insertions(+), 5 deletions(-) create mode 100644 core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/CircleQueue.java create mode 100644 core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTrack.java create mode 100644 core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/ObjQueue.java create mode 100644 core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackObj.java diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/CircleQueue.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/CircleQueue.java new file mode 100644 index 0000000000..2494ef19f3 --- /dev/null +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/CircleQueue.java @@ -0,0 +1,33 @@ +package com.mogo.eagle.core.function.map; + +import android.util.Log; + +import java.util.Vector; + +public class CircleQueue { + + private final Vector objQueue; + private final int maxSize; + + public CircleQueue(int maxSize) { + this.maxSize = maxSize; + objQueue = new Vector<>(maxSize); + } + + public void addQueue(ObjQueue obj) { + if(objQueue.size() == maxSize){ + objQueue.remove(0); + } + objQueue.add(obj); + } + + public void deleteObj(ObjQueue obj) { + objQueue.remove(obj); + } + + public void show() { + for (ObjQueue queue : objQueue) { + Log.i("CircleQueue","show : " + queue.getType()); + } + } +} 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 465578c37c..97423f314b 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 @@ -1,15 +1,13 @@ 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.utilcode.mogo.AppIdentityModeUtils; import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger; -import com.mogo.map.MogoMap; import com.mogo.map.MogoMarkerManager; import com.mogo.module.common.MogoApisHandler; import com.mogo.module.common.constants.AdasRecognizedType; @@ -126,8 +124,9 @@ public class IdentifyDataDrawer { //首次过来的数据不添加,首次未添加的感知物在调用完绘制方法后再塞入cache map MessagePad.TrackedObject cacheData = mMarkersCaches.get(uuid); if (cacheData != null) { - if (data.getSpeed() < 0.5) { - data = data.toBuilder().setHeading(cacheData.getHeading()).setLongitude(cacheData.getLongitude()).setLatitude(cacheData.getLatitude()).build(); + double limitSpeed = AppIdentityModeUtils.isBus(FunctionBuildConfig.appIdentityMode) ? 0.7 : 0.5; + if (data.getSpeed() < limitSpeed) { + data.toBuilder().setHeading(cacheData.getHeading()).setLongitude(cacheData.getLongitude()).setLatitude(cacheData.getLatitude()).build(); } mFilterTrafficData.add(data); //更新已存在的感知物体数据 diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTrack.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTrack.java new file mode 100644 index 0000000000..3edbc8c530 --- /dev/null +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTrack.java @@ -0,0 +1,213 @@ +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.utilcode.mogo.logger.CallerLogger; +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 IdentifyDataDrawerTrack { + private static final String TAG = "IdentifyDataDrawer"; + + protected final Context mContext; + private static volatile IdentifyDataDrawerTrack sInstance; + + /** + * 上一帧数据的缓存 + */ + private static final ConcurrentHashMap mMarkersCaches = new ConcurrentHashMap<>(); + + /** + * kalman缓存数据 + */ + private static final ConcurrentHashMap algoCache = new ConcurrentHashMap<>(); + + /** + * 记录每次实际绘制的交通元素UUID + */ + private final ArrayList trafficDataUuidList = new ArrayList<>(); + /** + * 过滤后的数据集合 + */ + private final ArrayList mFilterTrafficData = new ArrayList<>(); + + private IdentifyDataDrawerTrack() { + mContext = AbsMogoApplication.getApp(); + } + + public static IdentifyDataDrawerTrack getInstance() { + if (sInstance == null) { + synchronized (IdentifyDataDrawerTrack.class) { + if (sInstance == null) { + sInstance = new IdentifyDataDrawerTrack(); + } + } + } + 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; + } + String uuid = "" + data.getUuid(); + //首次过来的数据不添加,首次未添加的感知物在调用完绘制方法后再塞入cache map + MessagePad.TrackedObject cacheData = mMarkersCaches.get(uuid); + if (cacheData != null) { //todo 代码不要推到云端，本地测试即可！！！！！！！！！！ + MessagePad.TrackedObject correctData; + correctData = kalmanCorrectData(data); + mFilterTrafficData.add(correctData); + //更新已存在的感知物体数据 + mMarkersCaches.put(uuid, correctData); + } else { + mMarkersCaches.put(uuid, data); + } + trafficDataUuidList.add(uuid); + } + return mFilterTrafficData; + } + + //todo 相信滤波的定位点做验证，将原始data修改经纬度和航向角返回 + private MessagePad.TrackedObject kalmanCorrectData(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; + //todo 代码不要推到云端，本地测试即可！！！！！！！！！！ + if (data.getSpeed() >= 1.5 && (data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TA_CHE.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_MOTO.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BICYCLE.getType())) { + double kalH = MogoMap.getInstance().getMogoMap().getUIController().getAngle(cacheTrackObj.getLongitude(), cacheTrackObj.getLatitude(), lonLat[0], lonLat[1]); + if ((cacheTrackObj.getHeading() / 360 > 0.75 && data.getHeading() / 360 < 0.25) || (cacheTrackObj.getHeading() / 360 < 0.25 && data.getHeading() / 360 > 0.75)) { + return data; + } + if (Math.abs(cacheTrackObj.getHeading() - data.getHeading()) > 165 && Math.abs(cacheTrackObj.getHeading() - data.getHeading()) < 195) { + return data; + } + if (Math.abs(cacheTrackObj.getHeading() - data.getHeading()) > 40 && (Math.abs(kalH - cacheTrackObj.getHeading())) > 40) { + if (data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TA_CHE.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK.getType()) { + CallerLogger.INSTANCE.d(M_HMI + "type : " + data.getType(), " uuid : " + uuid + " , cacheH : " + cacheTrackObj.getHeading() + " , origin H : " + data.getHeading() + " , kalH H : " + kalH + " , 使用卡尔曼" + " ， speed : " + data.getSpeed()); + } + return data.toBuilder().setHeading(kalH).build(); + } + if (Math.abs(cacheTrackObj.getHeading() - data.getHeading()) > 40 && (Math.abs(kalH - data.getHeading())) > 40) { //存在前后帧误差 + double correct = Math.abs(cacheTrackObj.getHeading() - kalH) - Math.abs(data.getHeading() - kalH) > 0 ? data.getHeading() : cacheTrackObj.getHeading(); + if (data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TA_CHE.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK.getType()) { + CallerLogger.INSTANCE.d(M_HMI + "type : " + data.getType(), " uuid : " + uuid + " , cacheH : " + cacheTrackObj.getHeading() + " , origin H : " + data.getHeading() + " , correct H : " + correct + " , 使用 : " + (Math.abs(cacheTrackObj.getHeading() - kalH) - Math.abs(data.getHeading() - kalH) > 0 ? "当前帧" : "缓存帧" + " ， speed : " + data.getSpeed())); + } + return data.toBuilder().setHeading(correct).build(); + } + } else if (data.getSpeed() <= 0.5) { + double roadAngle = MogoMap.getInstance().getMogoMap().getUIController().getRoadAngle(cacheTrackObj.getLongitude(), cacheTrackObj.getLatitude(), -1); + double result = roadAngle != 0 ? roadAngle : cacheTrackObj.getHeading(); + CallerLogger.INSTANCE.d(M_HMI + "type : " + data.getType()," uuid : " + uuid + " , <0.5 result : " + result); + return data.toBuilder().setHeading(cacheTrackObj.getHeading()).setLongitude(cacheTrackObj.getLongitude()).setLatitude(cacheTrackObj.getLatitude()).build(); + } else { + return data; + } + } else { + algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005)); + return data; + } + return data; + } + + private double getAngle(double lat_a, double lng_a, double lat_b, double lng_b) { + double y = Math.sin(lng_b - lng_a) * Math.cos(lat_b); + double x = Math.cos(lat_a) * Math.sin(lat_b) - Math.sin(lat_a) * Math.cos(lat_b) * Math.cos(lng_b - lng_a); + double bearing = Math.atan2(y, x); + bearing = Math.toDegrees(bearing); + if (bearing < 0) { + bearing = bearing + 360; + } + return 360 - bearing; + } + + /** + * 清除旧的 marker 数据 + */ + public void clearOldMarker() { + for (String uuid : trafficDataUuidList) { + MogoMarkerManager.getInstance(mContext) + .removeMarker(uuid); + } + trafficDataUuidList.clear(); + } +} + + + diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/ObjQueue.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/ObjQueue.java new file mode 100644 index 0000000000..f34d5599e4 --- /dev/null +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/ObjQueue.java @@ -0,0 +1,47 @@ +package com.mogo.eagle.core.function.map; + +public class ObjQueue { + + private double heading; + private double speed; + private int type; + + public ObjQueue(double heading, double speed, int type) { + this.heading = heading; + this.speed = speed; + this.type = type; + } + + public double getHeading() { + return heading; + } + + public void setHeading(double heading) { + this.heading = heading; + } + + public double getSpeed() { + return speed; + } + + public void setSpeed(double speed) { + this.speed = speed; + } + + public int getType() { + return type; + } + + public void setType(int type) { + this.type = type; + } + + @Override + public String toString() { + return "ObjQueue{" + + "heading=" + heading + + ", speed=" + speed + + ", type=" + type + + '}'; + } +} diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackObj.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackObj.java new file mode 100644 index 0000000000..a34ac2afce --- /dev/null +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackObj.java @@ -0,0 +1,47 @@ +package com.mogo.eagle.core.function.map; + +public class TrackObj { + + private final CircleQueue circleQueue = new CircleQueue(10); + private final int[] observationType = new int[3]; //类型变化的观测数组 + private long recentlyTime; + private double headingDelta; //航向角德尔塔 + private double speedDelta; //速度德尔塔 + private double typeWeight; //类型权重 + + public long getRecentlyTime() { + return recentlyTime; + } + + public void setRecentlyTime(long recentlyTime) { + this.recentlyTime = recentlyTime; + } + + public double getHeadingDelta() { + return headingDelta; + } + + public void setHeadingDelta(double headingDelta) { + this.headingDelta = headingDelta; + } + + public double getSpeedDelta() { + return speedDelta; + } + + public void setSpeedDelta(double speedDelta) { + this.speedDelta = speedDelta; + } + + public double getTypeWeight() { + return typeWeight; + } + + public void setTypeWeight(double typeWeight) { + this.typeWeight = typeWeight; + } + + public void updateObj() { + + } +}