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

fix identify

Browse Source
This commit is contained in:
zhongchao
2022-04-29 10:09:06 +08:00
parent 241305437e
commit d3af38d7dc
2 changed files with 9 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -126,14 +126,13 @@ public class IdentifyDataDrawer {
//首次过来的数据不添加,首次未添加的感知物在调用完绘制方法后再塞入cache map //首次过来的数据不添加,首次未添加的感知物在调用完绘制方法后再塞入cache map
MessagePad.TrackedObject cacheData = mMarkersCaches.get(uuid); MessagePad.TrackedObject cacheData = mMarkersCaches.get(uuid);
if (cacheData != null) { if (cacheData != null) {
MessagePad.TrackedObject correctData; if (data.getSpeed() < 0.5) {
correctData = kalmanCorrectData(data); data.toBuilder().setHeading(cacheData.getHeading()).setLongitude(cacheData.getLongitude()).setLatitude(cacheData.getLatitude()).build();
mFilterTrafficData.add(correctData); }
mFilterTrafficData.add(data);
//更新已存在的感知物体数据 //更新已存在的感知物体数据
mMarkersCaches.put(uuid, correctData);
} else {
mMarkersCaches.put(uuid, data);
} }
mMarkersCaches.put(uuid, data);
trafficDataUuidList.add(uuid); trafficDataUuidList.add(uuid);
} }
return mFilterTrafficData; return mFilterTrafficData;
@@ -146,17 +145,11 @@ public class IdentifyDataDrawer {
Object o = algoCache.get(uuid); Object o = algoCache.get(uuid);
KalmanFilter kf = (KalmanFilter) o; KalmanFilter kf = (KalmanFilter) o;
assert kf != null; assert kf != null;
double[] lonLat = kf.filter(data.getLongitude(), data.getLatitude());
algoCache.put(uuid, kf); algoCache.put(uuid, kf);
MessagePad.TrackedObject cacheTrackObj = mMarkersCaches.get(uuid); MessagePad.TrackedObject cacheTrackObj = mMarkersCaches.get(uuid);
assert cacheTrackObj != null; assert cacheTrackObj != null;
if (data.getSpeed() >= 1.5) { if (data.getSpeed() < 0.5) {
double heading = MogoMap.getInstance().getMogoMap().getUIController().getAngle(cacheTrackObj.getLongitude(), cacheTrackObj.getLatitude(), lonLat[0], lonLat[1]); return data.toBuilder().setHeading(cacheTrackObj.getHeading()).setLongitude(cacheTrackObj.getLongitude()).setLatitude(cacheTrackObj.getLatitude()).build();
double correct = Math.abs(heading - data.getHeading()) > 30 && Math.abs(heading - data.getHeading()) < 120 ? heading : data.getHeading();//todo test, 需要对每个uuid做航向角缓存
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 + " , origin H : " + data.getHeading() + " , cal H : " + heading + ", correct : " + correct + " , 使用滤波角度 : " + (Math.abs(heading - data.getHeading()) > 30 && Math.abs(heading - data.getHeading()) < 120));
}
return data.toBuilder().setHeading(correct).setLongitude(lonLat[0]).setLatitude(lonLat[1]).build();
} else { } else {
return data; return data;
} }
@@ -165,7 +158,7 @@ public class IdentifyDataDrawer {
if (AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficBus || AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficTruck) { if (AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficBus || AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficTruck) {
r = 0.00001; r = 0.00001;
} }
algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), data.getHeading(), r)); algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), r));
return data; return data;
} }
} }

5
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java
View File

@@ -10,13 +10,10 @@ public class KalmanFilter {
double[][] k = 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; int idx = 1;
double cacheHeading; public KalmanFilter(double lon, double lat, double r) {
public KalmanFilter(double lon, double lat, double originHeading, double r) {
this.xhat[0][0] = lon; this.xhat[0][0] = lon;
this.xhat[0][1] = lat; this.xhat[0][1] = lat;
this.r = r; this.r = r;
this.cacheHeading = originHeading;
} }
public double[] filter(double lon, double lat) { public double[] filter(double lon, double lat) {