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

fix bug of identify data angle problem

Browse Source
This commit is contained in:
zhongchao
2022-04-16 16:41:20 +08:00
parent bcec86632c
commit da36054265
1 changed files with 15 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -9,12 +9,10 @@ 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;
import java.util.List;
@@ -40,10 +38,6 @@ public class IdentifyDataDrawer {
private static final ConcurrentHashMap<String, KalmanFilter> algoCache = new ConcurrentHashMap<>();
/**
* 已经感知不到的脏数据
*/
private final ConcurrentHashMap<String, MessagePad.TrackedObject> firstData = new ConcurrentHashMap<>();
/**
* 记录每次实际绘制的交通元素UUID
*/
@@ -114,10 +108,7 @@ public class IdentifyDataDrawer {
MogoMarkerManager.getInstance(mContext)
.updateBatchMarkerPosition(filterList);
}
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "first data size : " + firstData.size() + " , mMarkersCaches : " + mMarkersCaches.size());
// 首次未添加的感知物在调用完绘制方法后再塞入cache map
mMarkersCaches.putAll(firstData);
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "mMarkersCaches : " + mMarkersCaches.size());
}
/**
@@ -126,7 +117,6 @@ public class IdentifyDataDrawer {
* @return 过滤后的数据集合
*/
private ArrayList<MessagePad.TrackedObject> filterTrafficData(List<MessagePad.TrackedObject> trafficData) {
firstData.clear();
mFilterTrafficData.clear();
trafficDataUuidList.clear();
for (MessagePad.TrackedObject data : trafficData) {
@@ -135,38 +125,24 @@ public class IdentifyDataDrawer {
//CallerLogger.INSTANCE.w(TAG, "未知感知类型数据，丢弃，不渲染");
continue;
}
String uuid = "" + data.getUuid();
//首次过来的数据不添加,首次未添加的感知物在调用完绘制方法后再塞入cache map
MessagePad.TrackedObject cacheData = mMarkersCaches.get("" + data.getUuid());
MessagePad.TrackedObject cacheData = mMarkersCaches.get(uuid);
if (cacheData != null) {
MessagePad.TrackedObject correctData = null;
MessagePad.TrackedObject correctData;
//todo 进行修正
if (Math.abs(cacheData.getHeading() - data.getHeading()) > 40 && Math.abs(cacheData.getHeading() - data.getHeading()) < 90) {
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "uuid: " + data.getUuid() + " , 40~90差值范围 , 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading());
double correctHeading = kalmanCorrect(data);
if(correctHeading != cacheData.getHeading()){
CallerLogger.INSTANCE.d(M_HMI + "arrow48", "uuid: " + uuid + " , kalmanCorrect : " + correctHeading + " , 修正 上一帧 : " + 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) {
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 + "arrow48", "uuid: " + data.getUuid() + " , 夹角 : " + degree + " , 未修正 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading());
}
}
if (correctData != null) {
mFilterTrafficData.add(correctData);
//更新已存在的感知物体数据
mMarkersCaches.put("" + data.getUuid(), correctData);
} else {
mFilterTrafficData.add(data);
//更新已存在的感知物体数据
mMarkersCaches.put("" + data.getUuid(), data);
}
} else {
firstData.put("" + data.getUuid(), data);
correctData = data.toBuilder().setHeading(correctHeading).build();
mFilterTrafficData.add(correctData);
//更新已存在的感知物体数据
mMarkersCaches.put(uuid, correctData);
}else{
mMarkersCaches.put(uuid, data);
}
trafficDataUuidList.add("" + data.getUuid());
trafficDataUuidList.add(uuid);
}
return mFilterTrafficData;
}
@@ -181,7 +157,7 @@ public class IdentifyDataDrawer {
algoCache.put(uuid, kf);
MessagePad.TrackedObject cacheTrackObj = mMarkersCaches.get(uuid);
assert cacheTrackObj != null;
if (data.getSpeed() >= 4) {
if (data.getSpeed() >= 3) {
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;