checkout hengyang_yingyan logic
This commit is contained in:
zhongchao
@@ -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复用问题，导致鹰眼展示元素缺少
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user