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

wait to test

Browse Source
This commit is contained in:
zhongchao
2022-04-27 16:15:48 +08:00
parent c0082e3ce1
commit 93cf7a721c
2 changed files with 12 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -88,7 +88,6 @@ public class IdentifyDataDrawer {
return;
}
long start = System.currentTimeMillis();
//清除缓存
for (MessagePad.TrackedObject data : resultList) {
if (trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())) {
@@ -100,18 +99,13 @@ public class IdentifyDataDrawer {
algoCache.remove(uuid);
});
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "origin data size : " + resultList.size());
ArrayList<MessagePad.TrackedObject> filterList = filterTrafficData(resultList);
long cost = System.currentTimeMillis() - start;
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "cost : " + cost);
if (filterList.size() > 0) {
// 绘制新数据
MogoMarkerManager.getInstance(mContext)
.updateBatchMarkerPosition(filterList);
}
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "mMarkersCaches : " + mMarkersCaches.size());
}
/**
@@ -158,9 +152,11 @@ 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]);
double correct = Math.abs(heading - data.getHeading()) > 30 && Math.abs(heading - data.getHeading()) < 120 ? heading : data.getHeading();//todo test
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(heading).setLongitude(lonLat[0]).setLatitude(lonLat[1]).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 {
return data;
}
@@ -169,7 +165,7 @@ public class IdentifyDataDrawer {
if (AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficBus || AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficTruck) {
r = 0.00001;
}
algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), r));
algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), data.getHeading(), r));
return data;
}
}

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

@@ -10,14 +10,17 @@ public class KalmanFilter {
double[][] k = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}};
int idx = 1;
public KalmanFilter(double lon, double lat, double r) {
double cacheHeading;
public KalmanFilter(double lon, double lat, double originHeading, double r) {
this.xhat[0][0] = lon;
this.xhat[0][1] = lat;
this.r = r;
this.cacheHeading = originHeading;
}
public double[] filter(double lon, double lat) {
for(int i = 0; i < 2; ++i) {
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);
@@ -31,4 +34,5 @@ public class KalmanFilter {
this.idx = 1 - this.idx;
return new double[]{lon1, lat1};
}
}