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

merge

Browse Source
This commit is contained in:
zhongchao
2022-04-13 21:53:48 +08:00
parent bb1baa023b
commit 8c7cc19d74
1 changed files with 40 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -9,8 +9,11 @@ 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.eagle.core.utilcode.util.UiThreadHandler;
import com.mogo.map.MogoMap;
import com.mogo.map.MogoMarkerManager;
import com.mogo.module.common.MogoApisHandler;
import com.mogo.module.common.constants.AdasRecognizedType;
import com.mogo.module.common.utils.DrivingDirectionUtils;
import java.util.ArrayList;
@@ -35,6 +38,8 @@ public class IdentifyDataDrawer {
*/
private static final ConcurrentHashMap<String, MessagePad.TrackedObject> mMarkersCaches = new ConcurrentHashMap<>();
private static final ConcurrentHashMap<String, KalmanFilter> algoCache = new ConcurrentHashMap<>();
/**
* 已经感知不到的脏数据
*/
@@ -95,13 +100,14 @@ public class IdentifyDataDrawer {
}
trafficDataUuidList.forEach(uuid -> {
mMarkersCaches.remove(uuid);
algoCache.remove(uuid);
});
CallerLogger.INSTANCE.d(M_HMI + "arrow47", "origin data size : " + resultList.size());
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 + "arrow47", "cost : " + cost);
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "cost : " + cost);
if (filterList.size() > 0) {
// 绘制新数据
@@ -109,7 +115,7 @@ public class IdentifyDataDrawer {
.updateBatchMarkerPosition(filterList);
}
CallerLogger.INSTANCE.d(M_HMI + "arrow47", "first data size : " + firstData.size() + " , mMarkersCaches : " + mMarkersCaches.size());
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "first data size : " + firstData.size() + " , mMarkersCaches : " + mMarkersCaches.size());
// 首次未添加的感知物在调用完绘制方法后再塞入cache map
mMarkersCaches.putAll(firstData);
}
@@ -136,15 +142,16 @@ public class IdentifyDataDrawer {
MessagePad.TrackedObject correctData = null;
//todo 进行修正
if (Math.abs(cacheData.getHeading() - data.getHeading()) > 40 && Math.abs(cacheData.getHeading() - data.getHeading()) < 90) {
CallerLogger.INSTANCE.d(M_HMI + "arrow47", "uuid: " + data.getUuid() + " , 40~90差值范围 , 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading());
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "uuid: " + data.getUuid() + " , 40~90差值范围 , 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading());
}
if (Math.abs(cacheData.getHeading() - data.getHeading()) > 90) {
int degree = DrivingDirectionUtils.getDegreeOfCar2Poi(cacheData.getLongitude(), cacheData.getLatitude(), data.getLongitude(), data.getLatitude(), Double.valueOf(cacheData.getHeading()).intValue());
if (degree > 90) {
CallerLogger.INSTANCE.d(M_HMI + "arrow47", "uuid: " + data.getUuid() + " , 夹角 : " + degree + " , 修正 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading());
correctData = data.toBuilder().setHeading(cacheData.getHeading()).build();
double heading = kalmanCorrect(data);
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "uuid: " + data.getUuid() + " , 夹角 : " + degree + " , kalmanCorrect : " + heading + " , 修正 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading());
correctData = data.toBuilder().setHeading(heading).build();
} else {
CallerLogger.INSTANCE.d(M_HMI + "arrow47", "uuid: " + data.getUuid() + " , 夹角 : " + degree + " , 未修正 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading());
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "uuid: " + data.getUuid() + " , 夹角 : " + degree + " , 未修正 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading());
}
}
if (correctData != null) {
@@ -164,6 +171,32 @@ public class IdentifyDataDrawer {
return mFilterTrafficData;
}
private double kalmanCorrect(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;
if (data.getSpeed() >= 4) {
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;
} else {
return data.getHeading();
}
} else {
double r = 0.00005;
if (AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficBus || AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficTruck) {
r = 0.0001;
}
algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), r));
return data.getHeading();
}
}
/**
* 清除旧的 marker 数据
@@ -175,5 +208,4 @@ public class IdentifyDataDrawer {
}
trafficDataUuidList.clear();
}
}