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

Merge branch 'dev_robotaxi-d-app-module_280_220608_2.8.0' of gitlab.zhidaoauto.com:zhjt/AndroidApp/MoGoEagleEye into dev_robotaxi-d-app-module_280_220608_2.8.0

Browse Source
This commit is contained in:
xuxinchao
2022-06-23 16:08:04 +08:00
parent ebb88145aa e9e54c6b56
commit 6c3b341542
2 changed files with 38 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java
View File

@@ -31,7 +31,7 @@ public class TrackManager {
return TrackOwner.trackManager;
}
public static double LIMIT_SPEED = 0.5;
public static double LIMIT_SPEED = 0.3;
/**
* marker缓存队列
@@ -89,7 +89,16 @@ public class TrackManager {
data = cache;
trackObj = cacheTrack;
}
//todo OBU相关后面通过增加字段判断数据来源进行融合，不融合衡阳验收分支功能
// 颜色处理 OBU相关 先处理缓存帧，后处理新感知帧，因为存在obu感知更新
// String cacheId = "" + cache.getUuid();
// if (cacheId.startsWith("7476") && cache.getUuid() > 10000) {
// Log.d("hy uuid : " + cacheId, "缓存物为OBU数据，更新物体类型和颜色为OBU");
// data = data.toBuilder().setType(cache.getType()).setColor(cache.getColor()).build();
// }
// if (uuid.startsWith("7476") && data.getUuid() > 10000) {
// Log.d("hy uuid : " + uuid, "出现OBU感知物体");
// data = data.toBuilder().setType(data.getType()).setColor(data.getColor()).build();
// }
}
uuid = findSameValue;
data = trackObj.updateObj(data);
@@ -98,7 +107,6 @@ public class TrackManager {
}
cellIdCaches.forcePut(uuid, trackObj.getCellIdPos());
mMarkersCaches.put(uuid, trackObj);
Log.d("hy uuid : " + uuid, " 显示物体,塞入set");
trafficDataUuidList.add(uuid);
}
//todo 将上次没被删除掉物体加入集合，造成延迟删除，对运动物体不友好
@@ -131,7 +139,6 @@ public class TrackManager {
}
}
trafficDataUuidList.forEach(uuid -> {
Log.d("hy uuid : " + uuid, " 移除物体");
removeKey(uuid);
});
//todo bus存在时间回溯，将id重置，会有id复用问题，导致鹰眼展示元素缺少

50
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java
View File

@@ -21,21 +21,21 @@ import mogo.telematics.pad.MessagePad;
public class TrackObj {
private final CircleQueue circleQueue = new CircleQueue(10);
private final KalmanFilter kalmanFilter; //卡尔曼结果
// private final KalmanFilter kalmanFilter; //卡尔曼结果
private S2CellId s2CellId; //s2 id权重
private S2LatLng s2LatLng; //s2 经纬度
private long recentlyTime; //用于缓存帧数判断，暂定缓存1秒数据，中间如果有物体未出现，1秒后删除
private double roadAngle; //道路航向
private double headingDelta; //航向角德尔塔
private int[] typeArray = new int[5];
private double typeWeight; //类型权重
private int[] typeArray = new int[3];
private int typeWeight; //类型权重
private double lat;
private double lon;
private double speedAverage;
private com.zhidaoauto.map.sdk.open.road.CenterLine centerLineInfo = null;
public TrackObj(MessagePad.TrackedObject data) {
kalmanFilter = new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005);
// kalmanFilter = new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005);
circleQueue.addQueue(new ObjQueue(data.getHeading(), data.getSpeed(), data.getType()));
recentlyTime = Double.valueOf(data.getSatelliteTime() * 1000).longValue();
lat = data.getLatitude();
@@ -57,7 +57,6 @@ public class TrackObj {
correct();
recentlyTime = Double.valueOf(data.getSatelliteTime() * 1000).longValue();
// Log.d("calHeading uuid : " + cacheData.getUuid(), "result heading : " + cacheData.getHeading() + " speed : " + cacheData.getSpeed());
circleQueue.addQueue(new ObjQueue(cacheData.getHeading(), cacheData.getSpeed(), cacheData.getType()));
return cacheData;
}
@@ -70,17 +69,14 @@ public class TrackObj {
@SuppressLint("NewApi")
private void calAverageSpeedAndType() {
if (circleQueue.size() >= 5) {
if (circleQueue.size() >= 3) {
//计算平均速度
List<ObjQueue> objQueueList = circleQueue.getLastFiveFrame();
speedAverage = (objQueueList.get(0).getSpeed() + objQueueList.get(1).getSpeed() + objQueueList.get(2).getSpeed() + objQueueList.get(3).getSpeed() + objQueueList.get(4).getSpeed()) / 5;
List<ObjQueue> objQueueList = circleQueue.getLastThreeFrame();
speedAverage = (objQueueList.get(0).getSpeed() + objQueueList.get(1).getSpeed() + objQueueList.get(2).getSpeed()) / 3;
//计算类型
typeArray[0] = objQueueList.get(0).getType();
typeArray[1] = objQueueList.get(1).getType();
typeArray[2] = objQueueList.get(2).getType();
typeArray[3] = objQueueList.get(3).getType();
typeArray[4] = objQueueList.get(4).getType();
} else {
double cal = 0;
List<ObjQueue> objQueueList = circleQueue.getPreFrame();
@@ -90,9 +86,16 @@ public class TrackObj {
}
speedAverage = cal / objQueueList.size();
}
Log.d("hy uuid : " + cacheData.getUuid()," 类型 : " + Arrays.toString(typeArray));
typeArray = Arrays.stream(typeArray).sorted().toArray();
typeWeight = typeArray[typeArray.length / 2];
typeWeight = typeArray[typeArray.length / 2] == 0 ? cacheData.getType() : typeArray[typeArray.length / 2];
// 重新给静止物体赋值速度
if (relativeStatic()) {
double calSpeed = circleQueue.getLastFrame().getSpeed();
if (cacheData.getSpeed() != 0.0 && calSpeed == 0.0 && cacheData.getSpeed() >= 0.19444) {
cacheData = cacheData.toBuilder().setSpeed(0.19444).build();
}
}
}
private void calLoc() {
@@ -100,7 +103,11 @@ public class TrackObj {
if (relativeStatic()) {
double angle = roadAngle != 0 ? roadAngle : cacheData.getHeading();
if (centerLineInfo == null && isFourWheelType()) {
centerLineInfo = MapDataApi.INSTANCE.getCenterLineInfo(lon, lat, (float) angle);
try {
centerLineInfo = MapDataApi.INSTANCE.getCenterLineInfo(lon, lat, (float) angle);
}catch (Exception e){
Log.d("hy uuid : " + cacheData.getUuid(), "道路获取异常");
}
if (centerLineInfo != null && centerLineInfo.getPoints() != null && !centerLineInfo.getPoints().isEmpty()) {
double[] matchedPoint = PointInterpolatorUtil.mergeToRoad(cacheData.getLongitude(), cacheData.getLatitude(), centerLineInfo.getPoints());
if (matchedPoint[0] > 0 || matchedPoint[1] > 0) {
@@ -108,17 +115,16 @@ public class TrackObj {
lat = matchedPoint[1];
s2LatLng = S2LatLng.fromDegrees(cacheData.getLatitude(), cacheData.getLongitude());
s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22);
Log.d("hy uuid : " + cacheData.getUuid(), " 更新定位 lon : " + matchedPoint[0] + " , lat : " + matchedPoint[1]);
// Log.d("hy uuid : " + cacheData.getUuid(), " 更新定位 lon : " + matchedPoint[0] + " , lat : " + matchedPoint[1]);
} else {
centerLineInfo = null;
Log.d("hy uuid : " + cacheData.getUuid(), "计算结果出现问题 : " + matchedPoint[0] + " , " + matchedPoint[1]);
// Log.d("hy uuid : " + cacheData.getUuid(), "计算结果出现问题 : " + matchedPoint[0] + " , " + matchedPoint[1]);
}
} else {
centerLineInfo = null;
Log.d("hy uuid : " + cacheData.getUuid(), "未获取到道路数据");
// Log.d("hy uuid : " + cacheData.getUuid(), "未获取到道路数据");
}
}
Log.d("hy uuid : " + cacheData.getUuid(), " 静止使用定位数据 lon : " + lon + " , lat : " + lat);
cacheData = cacheData.toBuilder().setLongitude(lon).setLatitude(lat).build();
} else {
centerLineInfo = null;
@@ -128,7 +134,7 @@ public class TrackObj {
lon = cacheData.getLongitude();
s2LatLng = S2LatLng.fromDegrees(cacheData.getLatitude(), cacheData.getLongitude());
s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22);
Log.d("hy uuid : " + cacheData.getUuid(), " 开始移动 lon : " + lon + " , lat : " + lat);
// Log.d("hy uuid : " + cacheData.getUuid(), " 开始移动 lon : " + lon + " , lat : " + lat);
// cacheData = cacheData.toBuilder().setLongitude(lonLat[0]).setLatitude(lonLat[1]).build();
}
}
@@ -166,11 +172,9 @@ public class TrackObj {
if (centerLine != null && centerLine.getAngle() != 0) {
cacheData = cacheData.toBuilder().setHeading(centerLine.getAngle()).build();
} else {
// Log.d("hy uuid : " + cacheData.getUuid(), "未获取到道路航向,使用上一帧 : " + circleQueue.getLastFrame().getHeading());
cacheData = cacheData.toBuilder().setHeading(circleQueue.getLastFrame().getHeading()).build();
}
} else {
// Log.d("hy uuid : " + cacheData.getUuid(), "未获取到道路航向,使用上一帧 : " + circleQueue.getLastFrame().getHeading());
cacheData = cacheData.toBuilder().setHeading(circleQueue.getLastFrame().getHeading()).build();
}
@@ -200,8 +204,8 @@ public class TrackObj {
public boolean isFourWheelType() {
return typeWeight != TrafficTypeEnum.TYPE_TRAFFIC_ID_PEOPLE.getType()
|| typeWeight != TrafficTypeEnum.TYPE_TRAFFIC_ID_BICYCLE.getType()
|| typeWeight != TrafficTypeEnum.TYPE_TRAFFIC_ID_MOTO.getType();
&& typeWeight != TrafficTypeEnum.TYPE_TRAFFIC_ID_BICYCLE.getType()
&& typeWeight != TrafficTypeEnum.TYPE_TRAFFIC_ID_MOTO.getType();
}
@Override