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

hy track

Browse Source
This commit is contained in:
zhongchao
2022-06-22 11:21:42 +08:00
parent dec5799449
commit ad3aef1644
3 changed files with 88 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/identify/IdentifyOriginDataDrawer.kt
View File

@@ -1,6 +1,7 @@
package com.mogo.eagle.core.function.map.identify
import android.annotation.SuppressLint
import androidx.collection.ArraySet
import com.mogo.commons.AbsMogoApplication
import com.mogo.eagle.core.data.config.FunctionBuildConfig
import com.mogo.eagle.core.data.enums.TrafficTypeEnum
@@ -37,7 +38,7 @@ class IdentifyOriginDataDrawer : Identify {
/**
* 记录每次实际绘制的交通元素UUID
*/
private val trafficDataUuidList = ArrayList<String>()
private val trafficDataUuidList = ArraySet<String>()
/**
* 过滤后的数据集合

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

@@ -14,7 +14,6 @@ import com.mogo.eagle.core.data.config.FunctionBuildConfig;
import com.mogo.eagle.core.data.enums.TrafficTypeEnum;
import com.mogo.map.MogoMarkerManager;
import java.text.DecimalFormat;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;
@@ -32,8 +31,6 @@ 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 = 0.5;
/**
@@ -46,7 +43,7 @@ public class TrackManager {
*/
private final BiMap<String, Long> cellIdCaches = HashBiMap.create();
private final ArrayMap<String, TrackObj> recentCaches = new ArrayMap<>();
// private final ArrayMap<String, TrackObj> recentCaches = new ArrayMap<>();
/**
* 记录每次实际绘制的交通元素UUID
@@ -78,16 +75,25 @@ public class TrackManager {
mFilterTrafficData.add(data);
} else {
trackObj = new TrackObj(data);
// 判断是否有重合元素 google s2
// 融合逻辑 : 判断是否有重合元素 google s2
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);
Log.d("hy uuid : " + findSameValue, " 与新感知物 : " + uuid + "相同");
//uuid处理
data = data.toBuilder().setUuid(Integer.parseInt(findSameValue)).build();
MessagePad.TrackedObject cache = mMarkersCaches.get(findSameValue).getCache();
if (cache != null) {
//相对静止物体不改变
TrackObj cacheTrack = mMarkersCaches.get(findSameValue);
if (cacheTrack.relativeStatic()) {
data = cache;
trackObj = cacheTrack;
}
//todo OBU相关后面通过增加字段判断数据来源进行融合，不融合衡阳验收分支功能
}
uuid = findSameValue;
data = trackObj.updateObj(data);
mFilterTrafficData.add(data);
}
}
cellIdCaches.forcePut(uuid, trackObj.getCellIdPos());
@@ -126,10 +132,7 @@ public class TrackManager {
}
trafficDataUuidList.forEach(uuid -> {
Log.d("hy uuid : " + uuid, " 移除物体");
mMarkersCaches.remove(uuid);
cellIdCaches.remove(uuid);
MogoMarkerManager.getInstance(AbsMogoApplication.getApp())
.removeMarker(uuid);
removeKey(uuid);
});
//todo bus存在时间回溯，将id重置，会有id复用问题，导致鹰眼展示元素缺少
// Iterator it = mMarkersCaches.keySet().iterator();
@@ -149,16 +152,20 @@ public class TrackManager {
// }
}
public void removeKey(String key) {
mMarkersCaches.remove(key);
cellIdCaches.remove(key);
// recentCaches.remove(key);
MogoMarkerManager.getInstance(AbsMogoApplication.getApp())
.removeMarker(key);
}
public void clearAll() {
trafficDataUuidList.clear();
Iterator it = mMarkersCaches.keySet().iterator();
while (it.hasNext()) {
String key = (String) it.next();
mMarkersCaches.remove(key);
cellIdCaches.remove(key);
recentCaches.remove(key);
MogoMarkerManager.getInstance(AbsMogoApplication.getApp())
.removeMarker(key);
removeKey(key);
}
}
}

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

@@ -1,14 +1,19 @@
package com.mogo.eagle.core.function.map.identify;
import static com.mogo.eagle.core.function.map.identify.TrackManager.DISTANCE;
import static com.mogo.eagle.core.function.map.identify.TrackManager.LIMIT_SPEED;
import android.annotation.SuppressLint;
import android.util.Log;
import com.mogo.eagle.core.data.enums.TrafficTypeEnum;
import com.mogo.eagle.core.data.map.CenterLine;
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 com.mogo.map.utils.PointInterpolatorUtil;
import com.zhidaoauto.map.sdk.open.data.MapDataApi;
import java.util.Arrays;
import java.util.List;
import mogo.telematics.pad.MessagePad;
@@ -22,12 +27,12 @@ public class TrackObj {
private long recentlyTime; //用于缓存帧数判断，暂定缓存1秒数据，中间如果有物体未出现，1秒后删除
private double roadAngle; //道路航向
private double headingDelta; //航向角德尔塔
private int[] typeArray = new int[5];
private double typeWeight; //类型权重
private double lat;
private double lon;
private double speedAverage;
// private SinglePointRoadInfo singlePointRoadInfo;
// private double[] matchedPoint;
private com.zhidaoauto.map.sdk.open.road.CenterLine centerLineInfo = null;
public TrackObj(MessagePad.TrackedObject data) {
kalmanFilter = new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005);
@@ -58,93 +63,72 @@ public class TrackObj {
}
private void correct() {
calAverageSpeed();
calAverageSpeedAndType();
calLoc();
calHeading();
calType();
}
private void calAverageSpeed() {
//计算平均速度
@SuppressLint("NewApi")
private void calAverageSpeedAndType() {
if (circleQueue.size() >= 5) {
//计算平均速度
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;
//计算类型
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();
for (ObjQueue obj : objQueueList) {
cal += obj.getSpeed();
for (int i = 0; i < objQueueList.size() - 1; i++) {
cal += objQueueList.get(i).getSpeed();
typeArray[i] = objQueueList.get(i).getType();
}
speedAverage = cal / objQueueList.size();
// speedAverage = circleQueue.getLastFrame().getSpeed();
}
Log.d("hy uuid : " + cacheData.getUuid()," 类型 : " + Arrays.toString(typeArray));
typeArray = Arrays.stream(typeArray).sorted().toArray();
typeWeight = typeArray[typeArray.length / 2];
}
private void calLoc() {
//距离计算,位置修正
//todo bus250 taxi上测试下面注释掉内容
//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 (relativeStatic()) {
// 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 || relativeStatic() || calDistance * 2 < distance) {
if (relativeStatic()) {
// if (singlePointRoadInfo == null) {
// double angle = roadAngle != 0 ? roadAngle : cacheData.getHeading();
// long cost = System.nanoTime();
// singlePointRoadInfo = MapDataApi.INSTANCE.getSinglePointMatchRoad(lon, lat, (float) angle, true, true);
// Log.d("hy create cost", " " + (System.nanoTime() - cost) / 1000000);
// }
// if (singlePointRoadInfo != null && singlePointRoadInfo.getCoords() != null && !singlePointRoadInfo.getCoords().isEmpty()) {
// if(matchedPoint == null || matchedPoint.length == 0){
// long cost = System.nanoTime();
// matchedPoint = PointInterpolatorUtil.mergeToRoad(cacheData.getLongitude(), cacheData.getLatitude(), singlePointRoadInfo.getCoords());
// Log.d("hy matchedPoint cost", " " + (System.nanoTime() - cost) / 1000000);
// Log.d("hy uuid : " + cacheData.getUuid(), "道路经纬度 lon : " + matchedPoint[0] + " lat : " + matchedPoint[1] + " distance : " + matchedPoint[2] + " , 原数据 lon : " + lon + " lat : " + lat);
// }else{
// if(matchedPoint[0] == 0 || matchedPoint[1] == 0){
// cacheData = cacheData.toBuilder().setLongitude(lon).setLatitude(lat).build();
// }else{
// cacheData = cacheData.toBuilder().setLongitude(matchedPoint[0]).setLatitude(matchedPoint[1]).build();
// }
// }
// lat = matchedPoint[1];
// lon = matchedPoint[0];
// } else {
// Log.d("hy uuid : " + cacheData.getUuid(), "未匹配到道路数据,使用原数据 lon : " + lon + " lat : " + lat);
// }
cacheData = cacheData.toBuilder().setLongitude(lon).setLatitude(lat).build();
double angle = roadAngle != 0 ? roadAngle : cacheData.getHeading();
if (centerLineInfo == null && isFourWheelType()) {
centerLineInfo = MapDataApi.INSTANCE.getCenterLineInfo(lon, lat, (float) angle);
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);
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]);
}
} else {
centerLineInfo = null;
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;
//不在阈值内则更新，代表物体移动，使用卡尔曼滤波经纬度数据
//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();
}
}
@@ -193,15 +177,11 @@ public class TrackObj {
}
//速度大于LIMIT_SPEED并出现大幅度转向使用缓存帧和delta数据
if (cacheData.getSpeed() >= LIMIT_SPEED && newDelta > 10 && headingDelta != 0.0) {
// Log.i("0609", "uuid : " + cacheData.getUuid() + " 修正航向角 last : " + lastObj.getHeading() + " , 增益 : " + headingDelta);
// Log.i("hy uuid : " + cacheData.getUuid() , " 修正航向角 last : " + lastObj.getHeading() + " , 增益 : " + headingDelta);
cacheData = cacheData.toBuilder().setHeading(lastObj.getHeading() + headingDelta).build();
}
}
private void calType() {
}
public long getRecentlyTime() {
return recentlyTime;
}
@@ -218,6 +198,12 @@ public class TrackObj {
return speedAverage < LIMIT_SPEED;
}
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();
}
@Override
public String toString() {
return "TrackObj{" +