diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackManager.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackManager.java index 2b2b7f827d..994f4a96e6 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackManager.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackManager.java @@ -15,6 +15,7 @@ import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListener import com.mogo.eagle.core.utilcode.mogo.AppIdentityModeUtils; import com.mogo.map.MogoMarkerManager; +import java.text.DecimalFormat; import java.util.ArrayList; import java.util.Iterator; import java.util.List; @@ -31,6 +32,7 @@ public class TrackManager { return TrackOwner.trackManager; } + public static final DecimalFormat DF = new DecimalFormat("0.000000"); public static final int DISTANCE = 6371000; public static double LIMIT_SPEED = AppIdentityModeUtils.isBus(FunctionBuildConfig.appIdentityMode) ? 0.7 : 0.5; @@ -74,13 +76,16 @@ public class TrackManager { } else { trackObj = new TrackObj(data); // 判断是否有重合元素 google s2 -// if (cellIdCaches.containsValue(trackObj.getCellIdPos())) { -// String findSameValue = cellIdCaches.inverse().get(trackObj.getCellIdPos()); -// Log.d("0609", "uuid : " + findSameValue + " 与新感知物 : " + uuid + " , 出现相同pos : " + trackObj.getCellIdPos()); -// data = data.toBuilder().setUuid(Integer.parseInt(findSameValue)).build(); -// data = trackObj.updateObj(data); -// mFilterTrafficData.add(data); -// } + if (cellIdCaches.containsValue(trackObj.getCellIdPos())) { + String findSameValue = cellIdCaches.inverse().get(trackObj.getCellIdPos()); + if(data.getUuid() - Integer.parseInt(findSameValue) > 0){ + Log.d("0609", "uuid : " + findSameValue + " 与新感知物 : " + uuid + " , 出现相同pos : " + trackObj.getCellIdPos()); + uuid = findSameValue; + data = data.toBuilder().setUuid(Integer.parseInt(findSameValue)).build(); + data = trackObj.updateObj(data); + mFilterTrafficData.add(data); + } + } } cellIdCaches.forcePut(uuid, trackObj.getCellIdPos()); mMarkersCaches.put(uuid, trackObj); @@ -101,11 +106,13 @@ public class TrackManager { mMarkersCaches.remove(uuid); cellIdCaches.remove(uuid); }); + + //todo bus存在时间回溯，将id重置，会有id复用问题，导致鹰眼展示元素缺少，此处暂时不开启。 // Iterator it = mMarkersCaches.keySet().iterator(); // while (it.hasNext()) { // String key = (String) it.next(); // TrackObj trackObj = mMarkersCaches.get(key); -// if (trackObj != null && Math.abs(CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84SatelliteTime() - trackObj.getRecentlyTime()) >= 500) { +// if (trackObj != null && Math.abs(CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84SatelliteTime() - trackObj.getRecentlyTime()) >= 1000) { // Log.d("track", "clearCache uuid : " + key); // mMarkersCaches.remove(key); // cellIdCaches.remove(key); diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackObj.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackObj.java index fe687f473d..48bf638874 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackObj.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackObj.java @@ -1,11 +1,16 @@ package com.mogo.eagle.core.function.map; +import static com.mogo.eagle.core.function.map.TrackManager.DF; +import static com.mogo.eagle.core.function.map.TrackManager.DISTANCE; import static com.mogo.eagle.core.function.map.TrackManager.LIMIT_SPEED; import android.util.Log; +import com.mogo.eagle.core.function.call.map.CallerHDMapManager; import com.mogo.eagle.core.utilcode.geometry.S2CellId; import com.mogo.eagle.core.utilcode.geometry.S2LatLng; +import com.mogo.module.service.Utils; +import com.mogo.eagle.core.data.map.CenterLine; import java.util.List; @@ -22,6 +27,7 @@ public class TrackObj { private double typeWeight; //类型权重 private double lat; private double lon; + private double speedAverage; public TrackObj(MessagePad.TrackedObject data) { kalmanFilter = new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005); @@ -42,27 +48,62 @@ 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; } private void correct() { + calAverageSpeed(); calLoc(); calHeading(); calType(); } + private void calAverageSpeed() { + //计算平均速度 + if (circleQueue.size() >= 3) { + List objQueueList = circleQueue.getLastThreeFrame(); + speedAverage = (objQueueList.get(0).getSpeed() + objQueueList.get(1).getSpeed() + objQueueList.get(2).getSpeed()) / 3; + } else { + speedAverage = circleQueue.getLastFrame().getSpeed(); + } + } + private void calLoc() { //距离计算,位置修正 //todo bus250 taxi上测试下面注释掉内容 - // double[] lonLat = kalmanFilter.filter(cacheData.getLongitude(), cacheData.getLatitude()); + //double[] lonLat = kalmanFilter.filter(cacheData.getLongitude(), cacheData.getLatitude()); // double distance = s2LatLng.getDistance(S2LatLng.fromDegrees(lonLat[1], lonLat[0])).distance(DISTANCE); + double distance = s2LatLng.getDistance(S2LatLng.fromDegrees(cacheData.getLatitude(), cacheData.getLongitude())).distance(DISTANCE); + //todo 重新计算速度值(如果连续几帧distance累加到一定值，速度没变化，需要重新计算速度，防止锁死) + if (speedAverage < LIMIT_SPEED) { + double tempDis = distance; + if (distance >= 4) { //(150km/h) 41.6m/s x 0.1s = 4.16m 约等于 4 + tempDis = 4; + } + double calSpeed = cacheData.getSpeed(); + if (cacheData.getSpeed() != 0.0) { + calSpeed = tempDis / ((Double.valueOf(cacheData.getSatelliteTime() * 1000).longValue() - recentlyTime) / 1000.0); +// Log.d("calSpeed uuid : " + cacheData.getUuid(), " tempDis : " + tempDis + " , 重新赋值 calSpeed : " + DF.format(calSpeed) + " , time : " + (Double.valueOf(cacheData.getSatelliteTime() * 1000).longValue() - recentlyTime) + " , 原速度 : " + cacheData.getSpeed()); + if (calSpeed > cacheData.getSpeed()) { + calSpeed = cacheData.getSpeed(); +// Log.d("calSpeed uuid : " + cacheData.getUuid(), " 二次重新赋值 calSpeed : " + DF.format(calSpeed)); + } + if (calSpeed > 2) { + calSpeed = 2; +// Log.d("calSpeed uuid : " + cacheData.getUuid(), " 三次重新赋值 calSpeed : " + DF.format(calSpeed)); + } + } + cacheData = cacheData.toBuilder().setSpeed(calSpeed).build(); + } + //todo 等后序速度优化结果值可用，使用计算结果 // double calDistance = (cacheData.getSpeed() * (Double.valueOf(cacheData.getSatelliteTime() * 1000).longValue() - recentlyTime)) / 1000.0; + double calDistance = Utils.calculateLineDistance(lon, lat, cacheData.getLongitude(), cacheData.getLatitude()); +// Log.d("calLoc uuid : " + cacheData.getUuid() + " calDistance : " + DF.format(calDistance), (calDistance * 2 < distance) ? "超出范围" : "正常值"); //速度小于0.5m/s,并且距离在计算合理范围内超出2倍，则认为是相对静止状态(注意调整阈值)，不更新缓存点信息 -// if (cacheData.getSpeed() < LIMIT_SPEED || calDistance * 2 < distance) { -// Log.i("uuid : " + uuid , cacheData.getSpeed() < LIMIT_SPEED ? " 减速到静止" : "拉回来了 cal :" + calDistance + " , distance : " + distance + " , lon : " + lonLat[0] + " , lat : " + lonLat[1]); - if (cacheData.getSpeed() < LIMIT_SPEED) { -// cacheData = cacheData.toBuilder().setLongitude(s2LatLng.lngDegrees()).setLatitude(s2LatLng.latDegrees()).build(); + if (cacheData.getSpeed() < LIMIT_SPEED || speedAverage < LIMIT_SPEED || calDistance * 2 < distance) { +// Log.i("calLoc uuid : " + cacheData.getUuid(), cacheData.getSpeed() < LIMIT_SPEED ? " 减速到静止" : "拉回来了 cal :" + calDistance + " , distance : " + distance + " , lon : " + lon + " , lat : " + lat); cacheData = cacheData.toBuilder().setLongitude(lon).setLatitude(lat).build(); } else { //不在阈值内则更新，代表物体移动，使用卡尔曼滤波经纬度数据 @@ -76,15 +117,12 @@ public class TrackObj { //todo 对航向角做 10帧 原数据缓存，取众数 private void calHeading() { - double speedAverage; double newDelta; ObjQueue lastObj; if (circleQueue.size() >= 3) { //计算差量 List objQueueList = circleQueue.getLastThreeFrame(); lastObj = objQueueList.get(2); - //计算平均速度 - speedAverage = (objQueueList.get(0).getSpeed() + objQueueList.get(1).getSpeed() + objQueueList.get(2).getSpeed()) / 3; double firstDelta = objQueueList.get(1).getHeading() - objQueueList.get(0).getHeading(); double secondDelta = objQueueList.get(2).getHeading() - objQueueList.get(1).getHeading(); @@ -101,17 +139,23 @@ public class TrackObj { } } else { lastObj = circleQueue.getLastFrame(); - speedAverage = lastObj.getSpeed(); newDelta = Math.abs(cacheData.getHeading() - lastObj.getHeading()); headingDelta = newDelta; } //更正数据,速度小于0.5使用上一帧数据 - if (speedAverage < LIMIT_SPEED && circleQueue.size() != 1) { - cacheData = cacheData.toBuilder().setHeading(circleQueue.getLastFrame().getHeading()).build(); + if (speedAverage < LIMIT_SPEED) { + CenterLine centerLine = CallerHDMapManager.INSTANCE.getCenterLineInfo(lon, lat, -1); + if (centerLine != null && centerLine.getAngle() != 0 && Math.abs(centerLine.getAngle() - circleQueue.getLastFrame().getHeading()) >= 10) { +// Log.d("calHeading uuid : " + cacheData.getUuid(), " road angle : " + centerLine.getAngle() + " , last heading : " + circleQueue.getLastFrame().getHeading() + " queue size : " + circleQueue.size()); + cacheData = cacheData.toBuilder().setHeading(centerLine.getAngle()).build(); + } else { +// Log.d("calHeading uuid : " + cacheData.getUuid(), "使用上一帧heading : " + circleQueue.getLastFrame().getHeading()); + cacheData = cacheData.toBuilder().setHeading(circleQueue.getLastFrame().getHeading()).build(); + } } - //速度大于1.5并出现大幅度转向使用缓存帧和delta数据 - if (cacheData.getSpeed() > 1.5 && newDelta > 10 && headingDelta != 0.0) { - Log.i("0609", "uuid : " + cacheData.getUuid() + " 修正航向角 last : " + lastObj.getHeading() + " , 增益 : " + headingDelta); + //速度大于LIMIT_SPEED并出现大幅度转向使用缓存帧和delta数据 + if (cacheData.getSpeed() >= LIMIT_SPEED && newDelta > 10 && headingDelta != 0.0) { +// Log.i("0609", "uuid : " + cacheData.getUuid() + " 修正航向角 last : " + lastObj.getHeading() + " , 增益 : " + headingDelta); cacheData = cacheData.toBuilder().setHeading(lastObj.getHeading() + headingDelta).build(); } } diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/log b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/log deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/track b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/track deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/gradle.properties b/gradle.properties index 763978f753..472e674e45 100644 --- a/gradle.properties +++ b/gradle.properties @@ -60,7 +60,7 @@ HOOK_LOG_VERSION=1.6.1 SERVICE_CHAIN_VERSION=1.1.0 ################ 外部依赖引用 ################ # loglib -LOGLIB_VERSION=1.3.0 +LOGLIB_VERSION=1.3.1 ######## MogoAiCloudSDK Version ######## # 网络请求LOGLIB_VERSION MOGO_NETWORK_VERSION=1.4.3.3