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

close the tracker of heading cal

Browse Source
This commit is contained in:
zhongchao
2022-12-13 09:59:44 +08:00
parent 09d23df950
commit e90cd53b29
1 changed files with 1 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -20,21 +20,17 @@ import mogo.telematics.pad.MessagePad;
public class TrackObj {
private final CircleQueue circleQueue = new CircleQueue(6);
// private final KalmanFilter kalmanFilter; //卡尔曼结果
private S2CellId s2CellId; //s2 id权重
private S2LatLng s2LatLng; //s2 经纬度
private double recentlyTime; //用于缓存帧数判断，暂定缓存1秒数据，中间如果有物体未出现，1秒后删除
private double roadAngle; //道路航向
// private double headingDelta; //航向角德尔塔
private int[] typeArray = new int[3];
private int typeWeight; //类型权重
private double lat;
private double lon;
private double speedAverage;
// private CenterLine centerLineInfo = null;
public TrackObj(MessagePad.TrackedObject data) {
// kalmanFilter = new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005);
circleQueue.addQueue(new ObjQueue(data.getHeading(), data.getSpeed(), data.getLatitude(), data.getLongitude(), data.getType()));
recentlyTime = data.getSatelliteTime();
lat = data.getLatitude();
@@ -61,15 +57,13 @@ public class TrackObj {
private void correct() {
calAverageSpeedAndType();
calLoc();
calHeading();
// calHeading();
}
@SuppressLint("NewApi")
private void calAverageSpeedAndType() {
if (circleQueue.size() >= 3) {
List<ObjQueue> objQueueList = circleQueue.getLastThreeFrame();
// 计算平均速度
// speedAverage = (objQueueList.get(0).getSpeed() + objQueueList.get(1).getSpeed() + objQueueList.get(2).getSpeed()) / 3;
// 使用中值滤波
objQueueList.sort(Comparator.comparing(ObjQueue::getSpeed));
speedAverage = objQueueList.get(1).getSpeed();
@@ -101,38 +95,13 @@ public class TrackObj {
private void calLoc() {
//距离计算,位置修正
if (relativeStatic()) {
// double angle = roadAngle != 0 ? roadAngle : cacheData.getHeading();
// if (centerLineInfo == null && isFourWheelType()) {
// try {
// centerLineInfo = CallerHDMapManager.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) {
// lon = matchedPoint[0];
// lat = matchedPoint[1];
// s2LatLng = S2LatLng.fromDegrees(cacheData.getLatitude(), cacheData.getLongitude());
// s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22);
// } else {
// centerLineInfo = null;
// }
// } else {
// centerLineInfo = null;
// }
// }
cacheData = cacheData.toBuilder().setLongitude(lon).setLatitude(lat).build();
} else {
// centerLineInfo = null;
//不在阈值内则更新，代表物体移动，使用卡尔曼滤波经纬度数据
//double[] lonLat = kalmanFilter.filter(cacheData.getLongitude(), cacheData.getLatitude());
lat = cacheData.getLatitude();
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);
// cacheData = cacheData.toBuilder().setLongitude(lonLat[0]).setLatitude(lonLat[1]).build();
}
}
@@ -175,7 +144,6 @@ public class TrackObj {
public boolean relativeStatic() {
if (speedAverage < LIMIT_SPEED) {
// Log.d("emArrow-Track", "relativeStatic return" + " , uuid : " + cacheData.getUuid());
return true;
} else {
return isInRange();
@@ -192,7 +160,6 @@ public class TrackObj {
return false;
}
double dis = CoordinateUtils.calculateLineDistance(center[0], center[1], cacheData.getLongitude(), cacheData.getLatitude());
// Log.d("emArrow-Track", "uuid : " + cacheData.getUuid() + " , type : " + cacheData.getType() + " , list size : " + objQueueList.size() + " , dis : " + dis);
if (cacheData.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS.getType() || cacheData.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK.getType()) {
return dis <= 0.6;
} else {