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

add func

Browse Source
This commit is contained in:
zhongchao
2022-06-08 18:10:35 +08:00
committed by liujing
parent ffc352a605
commit f165359408
17 changed files with 188 additions and 145 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/CircleQueue.java
View File

@@ -1,7 +1,6 @@
package com.mogo.eagle.core.function.map;
import android.util.Log;
import java.util.List;
import java.util.Vector;
public class CircleQueue {
@@ -14,8 +13,12 @@ public class CircleQueue {
objQueue = new Vector<>(maxSize);
}
public int size() {
return objQueue.size();
}
public void addQueue(ObjQueue obj) {
if(objQueue.size() == maxSize){
if (objQueue.size() == maxSize) {
objQueue.remove(0);
}
objQueue.add(obj);
@@ -25,9 +28,19 @@ public class CircleQueue {
objQueue.remove(obj);
}
public void show() {
for (ObjQueue queue : objQueue) {
Log.i("CircleQueue","show : " + queue.getType());
}
public List<ObjQueue> getLastThreeFrame() {
return objQueue.subList(objQueue.size() - 3, objQueue.size());
}
public ObjQueue getLastFrame() {
return objQueue.lastElement();
}
@Override
public String toString() {
return "CircleQueue{" +
"objQueue=" + objQueue +
"size=" + objQueue.size() +
'}';
}
}

89
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java
View File

@@ -35,15 +35,6 @@ public class IdentifyDataDrawer {
*/
private static final ConcurrentHashMap<String, MessagePad.TrackedObject> mMarkersCaches = new ConcurrentHashMap<>();
/**
* kalman缓存数据
*/
private static final ConcurrentHashMap<String, KalmanFilter> algoCache = new ConcurrentHashMap<>();
/**
* 记录每次实际绘制的交通元素UUID
*/
private final ArrayList<String> trafficDataUuidList = new ArrayList<>();
/**
* 过滤后的数据集合
*/
@@ -75,31 +66,34 @@ public class IdentifyDataDrawer {
*/
@SuppressLint("NewApi")
public void renderAdasRecognizedResult(List<MessagePad.TrackedObject> resultList) {
if (resultList == null || resultList.isEmpty()) {
clearOldMarker();
CallerLogger.INSTANCE.w(TAG, "感知数据为空无需渲染……");
return;
}
// if (resultList == null || resultList.isEmpty()) {
// TrackManager.getInstance().clearAll(mContext);
// CallerLogger.INSTANCE.w(TAG, "感知数据为空无需渲染……");
// return;
// }
//
// if (!MogoApisHandler.getInstance().getApis().getStatusManagerApi().isVrMode()) {
// TrackManager.getInstance().clearAll(mContext);
// CallerLogger.INSTANCE.w(TAG, "渲染 adas 识别的数据 当前不是VR模式");
// return;
// }
if (!MogoApisHandler.getInstance().getApis().getStatusManagerApi().isVrMode()) {
clearOldMarker();
CallerLogger.INSTANCE.w(TAG, "渲染 adas 识别的数据 当前不是VR模式");
return;
}
// //清除缓存
// for (MessagePad.TrackedObject data : resultList) {
// if (trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())) {
// trafficDataUuidList.remove("" + data.getUuid());
// }
// }
// trafficDataUuidList.forEach(uuid -> {
// mMarkersCaches.remove(uuid);
// algoCache.remove(uuid);
// });
//清除缓存
for (MessagePad.TrackedObject data : resultList) {
if (trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())) {
trafficDataUuidList.remove("" + data.getUuid());
}
}
trafficDataUuidList.forEach(uuid -> {
mMarkersCaches.remove(uuid);
algoCache.remove(uuid);
});
ArrayList<MessagePad.TrackedObject> filterList = filterTrafficData(resultList);
TrackManager.getInstance().clearCache(mContext);
// ArrayList<MessagePad.TrackedObject> filterList = filterTrafficData(resultList);
ArrayList<MessagePad.TrackedObject> filterList = TrackManager.getInstance().filterTrafficData(resultList);
Log.i("0608arrow","size : " + filterList.size() + " , filterList : " + filterList.toString());
if (filterList.size() > 0) {
// 绘制新数据
MogoMarkerManager.getInstance(mContext)
@@ -114,7 +108,6 @@ public class IdentifyDataDrawer {
*/
private ArrayList<MessagePad.TrackedObject> filterTrafficData(List<MessagePad.TrackedObject> trafficData) {
mFilterTrafficData.clear();
trafficDataUuidList.clear();
for (MessagePad.TrackedObject data : trafficData) {
// 过滤掉未知感知数据
if (!FunctionBuildConfig.isDrawUnknownIdentifyData && data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_WEI_ZHI.getType()) {
@@ -133,45 +126,15 @@ public class IdentifyDataDrawer {
//更新已存在的感知物体数据
}
mMarkersCaches.put(uuid, data);
trafficDataUuidList.add(uuid);
}
return mFilterTrafficData;
}
//todo 相信滤波的定位点做验证，将原始data修改经纬度和航向角返回
private MessagePad.TrackedObject kalmanCorrectData(MessagePad.TrackedObject data) {
String uuid = "" + data.getUuid();
if (algoCache.containsKey(uuid)) {
Object o = algoCache.get(uuid);
KalmanFilter kf = (KalmanFilter) o;
assert kf != null;
algoCache.put(uuid, kf);
MessagePad.TrackedObject cacheTrackObj = mMarkersCaches.get(uuid);
assert cacheTrackObj != null;
if (data.getSpeed() < 0.5) {
return data.toBuilder().setHeading(cacheTrackObj.getHeading()).setLongitude(cacheTrackObj.getLongitude()).setLatitude(cacheTrackObj.getLatitude()).build();
} else {
return data;
}
} else {
double r = 0.000005;
if (AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficBus || AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficTruck) {
r = 0.00001;
}
algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), r));
return data;
}
}
/**
* 清除旧的 marker 数据
*/
public void clearOldMarker() {
for (String uuid : trafficDataUuidList) {
MogoMarkerManager.getInstance(mContext)
.removeMarker(uuid);
}
trafficDataUuidList.clear();
TrackManager.getInstance().clearAll(mContext);
}
}

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

@@ -1,11 +1,17 @@
package com.mogo.eagle.core.function.map;
import android.content.Context;
import android.util.ArrayMap;
import android.util.Log;
import com.mogo.eagle.core.data.config.FunctionBuildConfig;
import com.mogo.eagle.core.data.enums.TrafficTypeEnum;
import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager;
import com.mogo.map.MogoMarkerManager;
import java.text.DecimalFormat;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;
import mogo.telematics.pad.MessagePad;
@@ -20,6 +26,9 @@ public class TrackManager {
return TrackOwner.trackManager;
}
public static final DecimalFormat DF = new DecimalFormat("0.000000");
public static final int DISTANCE = 6371000;
/**
* marker缓存队列
*/
@@ -30,17 +39,18 @@ public class TrackManager {
*/
private final ArrayList<MessagePad.TrackedObject> mFilterTrafficData = new ArrayList<>();
public ArrayList<MessagePad.TrackedObject> filterTrafficData(List<MessagePad.TrackedObject> trafficData) {
//清空上次返回数据，做到缓存复用
mFilterTrafficData.clear();
//进入过滤机制的感知物体，首先从缓存队列中进行查找 uuid
for (MessagePad.TrackedObject data : trafficData) {
// todo 过滤掉未知感知数据，后面会依据危险等级显示
if (!FunctionBuildConfig.isDrawUnknownIdentifyData && data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_WEI_ZHI.getType()) {
//CallerLogger.INSTANCE.w(TAG, "未知感知类型数据，丢弃，不渲染");
continue;
}
String uuid = "" + data.getUuid();
TrackObj trackObj = mMarkersCaches.get(uuid);
if (trackObj != null) {
@@ -49,11 +59,33 @@ public class TrackManager {
} else {
trackObj = new TrackObj(data);
//todo 判断是否有重合元素 google s2
}
mMarkersCaches.put(uuid, trackObj);
}
return mFilterTrafficData;
}
public void clearCache(Context mContext) {
Iterator it = mMarkersCaches.keySet().iterator();
while (it.hasNext()) {
String key = (String) it.next();
TrackObj trackObj = mMarkersCaches.get(key);
if (CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84SatelliteTime() - trackObj.getRecentlyTime() > 1000) {
Log.d("track","clearCache uuid : " + key);
mMarkersCaches.remove(key);
MogoMarkerManager.getInstance(mContext)
.removeMarker(key);
}
}
}
public void clearAll(Context mContext) {
Iterator it = mMarkersCaches.keySet().iterator();
while (it.hasNext()) {
String key = (String) it.next();
mMarkersCaches.remove(key);
MogoMarkerManager.getInstance(mContext)
.removeMarker(key);
}
}
}

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

@@ -1,8 +1,15 @@
package com.mogo.eagle.core.function.map;
import static com.mogo.eagle.core.function.map.TrackManager.DISTANCE;
import android.util.Log;
import com.mogo.eagle.core.utilcode.geometry.S2CellId;
import com.mogo.eagle.core.utilcode.geometry.S2LatLng;
import java.math.BigDecimal;
import java.util.List;
import mogo.telematics.pad.MessagePad;
public class TrackObj {
@@ -10,56 +17,119 @@ public class TrackObj {
private final CircleQueue circleQueue = new CircleQueue(10);
private final KalmanFilter kalmanFilter; //卡尔曼结果
private S2CellId s2CellId; //s2 id权重
private long recentlyTime;
private S2LatLng s2LatLng; //s2 经纬度
private long recentlyTime; //用于缓存帧数判断，暂定缓存1秒数据，中间如果有物体未出现，1秒后删除
private double headingDelta; //航向角德尔塔
private double speedDelta; //速度德尔塔
private double typeWeight; //类型权重
private final int[] observationType = new int[3]; //类型变化的观测数组
private String uuid;
public TrackObj(MessagePad.TrackedObject data) {
uuid = "" + data.getUuid();
kalmanFilter = new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005);
circleQueue.addQueue(new ObjQueue(data.getHeading(), data.getSpeed(), data.getType()));
recentlyTime = Double.valueOf(data.getSatelliteTime()).longValue();
recentlyTime = BigDecimal.valueOf(data.getSatelliteTime()).longValue();
S2LatLng s2LatLng = S2LatLng.fromDegrees(data.getLatitude(), data.getLongitude());
s2CellId = S2CellId.fromLatLng(s2LatLng).parent(21); //需要验证21前后
s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22); //需要验证22前后
}
private MessagePad.TrackedObject cacheData;
//先处理kalman数据，将经纬度校准后，放入缓存队列，然后基于后序策略将各个项进行校准
public MessagePad.TrackedObject updateObj(MessagePad.TrackedObject data) {
cacheData = data.toBuilder().build();
recentlyTime = BigDecimal.valueOf(data.getSatelliteTime()).longValue();//todo 毫秒没有了
correct();
circleQueue.addQueue(new ObjQueue(cacheData.getHeading(), cacheData.getSpeed(), cacheData.getType()));
return cacheData.toBuilder().build();
}
private void correct() {
calLoc();
calHeading();
calType();
}
private void calLoc() {
//距离计算,位置修正
double[] lonLat = kalmanFilter.filter(cacheData.getLongitude(), cacheData.getLatitude());
if (s2LatLng != null) {
double distance = s2LatLng.getDistance(S2LatLng.fromDegrees(lonLat[0], lonLat[1])).distance(DISTANCE);
// DF.format(distance)
//速度小于0.5m/s,并且距离在15米范围内，则认为是相对静止状态(注意调整阈值)，不更新缓存点信息
if (cacheData.getSpeed() < 0.5 && distance < 15) {
Log.i("track", "uuid : " + uuid + " , 静止物体相对上一帧 distance : " + distance + " lon : " + s2LatLng.latDegrees() + " lat : " + s2LatLng.lngDegrees());
cacheData = cacheData.toBuilder().setLongitude(s2LatLng.latDegrees()).setLatitude(s2LatLng.lngDegrees()).build();
} else {
//不在阈值内则更新，代表物体移动，使用卡尔曼滤波经纬度数据
s2LatLng = S2LatLng.fromDegrees(lonLat[0], lonLat[1]);
s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22);
cacheData = cacheData.toBuilder().setLongitude(lonLat[0]).setLatitude(lonLat[1]).build();
}
} else {
//首次更新
s2LatLng = S2LatLng.fromDegrees(lonLat[0], lonLat[1]);
s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22);
}
}
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();
newDelta = Math.abs(cacheData.getHeading() - lastObj.getHeading());
//按帧与帧之间的顺序变化
double abs = Math.abs(firstDelta - secondDelta);
//存在180度转向(有一帧出现错误)
if (Math.abs(abs - 180) < 5) {
headingDelta = firstDelta - secondDelta;
} else if (abs < 5) { //两帧之间差量比较均匀
headingDelta = firstDelta - secondDelta;
} else if (Math.abs(abs - 180) > 5 && newDelta < 5) { //前两帧数据中出现异常值，相信后序帧
headingDelta = newDelta;
}
} else {
lastObj = circleQueue.getLastFrame();
speedAverage = lastObj.getSpeed();
newDelta = Math.abs(cacheData.getHeading() - lastObj.getHeading());
headingDelta = newDelta;
}
//更正数据,速度小于0.5使用上一帧数据
if (speedAverage < 0.5) {
cacheData = cacheData.toBuilder().setHeading(circleQueue.getLastFrame().getHeading()).build();
}
//速度大于1.5并出现大幅度转向使用缓存帧和delta数据
if (cacheData.getSpeed() > 1.5 && newDelta > 10 && headingDelta != 0.0) {
cacheData = cacheData.toBuilder().setHeading(lastObj.getHeading() + headingDelta).build();
}
}
private void calType() {
}
public long getRecentlyTime() {
return recentlyTime;
}
public void setRecentlyTime(long recentlyTime) {
this.recentlyTime = recentlyTime;
}
public double getHeadingDelta() {
return headingDelta;
}
public void setHeadingDelta(double headingDelta) {
this.headingDelta = headingDelta;
}
public double getSpeedDelta() {
return speedDelta;
}
public void setSpeedDelta(double speedDelta) {
this.speedDelta = speedDelta;
}
public double getTypeWeight() {
return typeWeight;
}
public void setTypeWeight(double typeWeight) {
this.typeWeight = typeWeight;
}
//先处理kalman数据，将经纬度校准后，放入缓存队列，然后基于后序策略将各个项进行校准
public MessagePad.TrackedObject updateObj(MessagePad.TrackedObject data) {
// assert kf != null;
// double[] lonLat = kf.filter(data.getLongitude(), data.getLatitude());
return data;
@Override
public String toString() {
return "TrackObj{" +
"circleQueue=" + circleQueue +
", s2CellId=" + s2CellId +
", recentlyTime=" + recentlyTime +
", cacheData=" + cacheData +
'}';
}
}

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

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