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

fix track bug and log crash

Browse Source
This commit is contained in:
zhongchao
2022-06-14 13:43:08 +08:00
parent 0f1019645a
commit 52bd1df733
5 changed files with 74 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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);

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

@@ -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<ObjQueue> 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<ObjQueue> 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();
}
}

0
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/log
View File

0
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/track
View File