diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java index b22d89de6c..6493eff69a 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackManager.java @@ -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复用问题，导致鹰眼展示元素缺少 diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java index aab75dffd4..2675b96016 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/TrackObj.java @@ -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 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 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 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