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

teting

Browse Source
This commit is contained in:
zhongchao
2022-04-27 11:20:46 +08:00
parent 5292f9ec4c
commit 57cbbe7042
2 changed files with 17 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java
View File

@@ -36,6 +36,9 @@ public class IdentifyDataDrawer {
*/
private static final ConcurrentHashMap<String, MessagePad.TrackedObject> mMarkersCaches = new ConcurrentHashMap<>();
/**
* kalman缓存数据
*/
private static final ConcurrentHashMap<String, KalmanFilter> algoCache = new ConcurrentHashMap<>();
/**
@@ -108,7 +111,7 @@ public class IdentifyDataDrawer {
MogoMarkerManager.getInstance(mContext)
.updateBatchMarkerPosition(filterList);
}
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "mMarkersCaches : " + mMarkersCaches.size());
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "mMarkersCaches : " + mMarkersCaches.size());
}
/**
@@ -130,16 +133,11 @@ public class IdentifyDataDrawer {
MessagePad.TrackedObject cacheData = mMarkersCaches.get(uuid);
if (cacheData != null) {
MessagePad.TrackedObject correctData;
//todo 进行修正
double correctHeading = kalmanCorrect(data);
if(correctHeading != cacheData.getHeading()){
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "uuid: " + uuid + " , kalmanCorrect : " + correctHeading + " , 修正 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading());
}
correctData = data.toBuilder().setHeading(correctHeading).build();
correctData = kalmanCorrectData(data);
mFilterTrafficData.add(correctData);
//更新已存在的感知物体数据
mMarkersCaches.put(uuid, correctData);
}else{
} else {
mMarkersCaches.put(uuid, data);
}
trafficDataUuidList.add(uuid);
@@ -147,7 +145,8 @@ public class IdentifyDataDrawer {
return mFilterTrafficData;
}
private double kalmanCorrect(MessagePad.TrackedObject data) {
//todo 相信滤波的定位点做验证，将原始data修改经纬度和航向角返回
private MessagePad.TrackedObject kalmanCorrectData(MessagePad.TrackedObject data) {
String uuid = "" + data.getUuid();
if (algoCache.containsKey(uuid)) {
Object o = algoCache.get(uuid);
@@ -159,18 +158,19 @@ public class IdentifyDataDrawer {
assert cacheTrackObj != null;
if (data.getSpeed() >= 1.5) {
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;
double correct = heading - data.getHeading() > 30 ? heading : data.getHeading();//todo test
CallerLogger.INSTANCE.d(M_HMI + "arrow48", " uuid : " + uuid + " , origin heading : " + data.getHeading() + " , cal heading : " + heading + ", correct : " + correct + " , heading - data.getHeading() > 30 : " + (heading - data.getHeading() > 30));
return data.toBuilder().setHeading(heading).setLongitude(lonLat[0]).setLatitude(lonLat[1]).build();
} else {
return data.getHeading();
return data;
}
} else {
double r = 0.00005;
double r = 0.000005;
if (AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficBus || AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficTruck) {
r = 0.0001;
r = 0.00001;
}
algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), r));
return data.getHeading();
return data;
}
}
@@ -185,3 +185,4 @@ public class IdentifyDataDrawer {
trafficDataUuidList.clear();
}
}