yangyakun921/MoGoEagleEye
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

cherry pick eagle

Browse Source
This commit is contained in:
zhongchao
2022-06-30 15:51:14 +08:00
parent e491dc1617
commit 70a8ea89c6
6 changed files with 91 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/Identify.kt
View File

@@ -5,7 +5,9 @@ import mogo.telematics.pad.MessagePad.TrackedObject
interface Identify {
fun renderPlanningWarningObj(planningObjects: List<MessagePad.PlanningObject>?)
fun renderPlanningWarningObj(planningObjects: List<MessagePad.PlanningObject>?){
}
fun renderAdasRecognizedResult(resultList: List<TrackedObject>?)

9
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyBeautifyDataDrawer.kt
View File

@@ -17,10 +17,6 @@ class IdentifyBeautifyDataDrawer : Identify {
private const val TAG = "IdentifyDataDrawer"
}
override fun renderPlanningWarningObj(planningObjects: List<MessagePad.PlanningObject>?) {
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())

4
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.kt
View File

@@ -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)
}
}

144
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java
View File

@@ -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<String, TrackObj> mMarkersCaches = new ArrayMap<>();
private final ConcurrentHashMap<String, TrackObj> mMarkersCaches = new ConcurrentHashMap<>();
/**
* marker s2 cellId缓存队列，空间换时间
*/
private final BiMap<String, Long> cellIdCaches = HashBiMap.create();
// private final ArrayMap<String, TrackObj> recentCaches = new ArrayMap<>();
/**
* 记录每次实际绘制的交通元素UUID
*/
private final Set<String> trafficDataUuidList = new ArraySet<>();
private final List<String> trafficDataUuid = Collections.synchronizedList(new ArrayList<>());
/**
* 过滤后的数据集合
*/
private final ArrayList<MessagePad.TrackedObject> mFilterTrafficData = new ArrayList<>();
public ArrayList<MessagePad.TrackedObject> filterTrafficData(List<MessagePad.TrackedObject> trafficData) {
@SuppressLint("NewApi")
public synchronized ArrayList<MessagePad.TrackedObject> filterTrafficData(List<MessagePad.TrackedObject> 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<MessagePad.PlanningObject> planningObjects) {
}
@RequiresApi(api = Build.VERSION_CODES.N)
public void clearCache(List<MessagePad.TrackedObject> 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));
}
}

40
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java
View File

@@ -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<ObjQueue> 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() {