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

merge

Browse Source
This commit is contained in:
zhongchao
2022-04-08 12:27:43 +08:00
parent 1b72b6c9a2
commit c06b17cb82
9 changed files with 274 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -87,10 +87,11 @@ public class IdentifyDataDrawer {
return;
}
long start = System.currentTimeMillis();
//清除缓存
for (MessagePad.TrackedObject data : resultList) {
if(trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())){
trafficDataUuidList.remove(data.getUuid());
trafficDataUuidList.remove("" + data.getUuid());
}
}
trafficDataUuidList.forEach(uuid -> {
@@ -99,11 +100,16 @@ public class IdentifyDataDrawer {
CallerLogger.INSTANCE.d(M_HMI + "arrow47", "origin data size : " + resultList.size());
ArrayList<MessagePad.TrackedObject> filterList = filterTrafficData(resultList);
long cost = System.currentTimeMillis() - start;
CallerLogger.INSTANCE.d(M_HMI + "arrow47", "cost : " + cost);
if (filterList.size() > 0) {
// 绘制新数据
MogoMarkerManager.getInstance(mContext)
.updateBatchMarkerPosition(filterList);
}
CallerLogger.INSTANCE.d(M_HMI + "arrow47", "first data size : " + firstData.size() + " , mMarkersCaches : " + mMarkersCaches.size());
// 首次未添加的感知物在调用完绘制方法后再塞入cache map
mMarkersCaches.putAll(firstData);

203
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawerTest.java Normal file
View File

@@ -0,0 +1,203 @@
package com.mogo.eagle.core.function.map;
import static com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant.M_HMI;
import android.annotation.SuppressLint;
import android.content.Context;
import com.mogo.commons.AbsMogoApplication;
import com.mogo.eagle.core.data.config.FunctionBuildConfig;
import com.mogo.eagle.core.data.enums.TrafficTypeEnum;
import com.mogo.eagle.core.function.call.map.CallerMapLocationListenerManager;
import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger;
import com.mogo.eagle.core.utilcode.util.UiThreadHandler;
import com.mogo.map.MogoMap;
import com.mogo.map.MogoMarkerManager;
import com.mogo.module.common.MogoApisHandler;
import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.ConcurrentHashMap;
import mogo.telematics.pad.MessagePad;
/**
* @author xiaoyuzhou
* @date 2021/10/19 10:45 上午
* 域控制器识别信息绘制
*/
public class IdentifyDataDrawerTest {
private static final String TAG = "IdentifyDataDrawer";
protected final Context mContext;
private static volatile IdentifyDataDrawerTest sInstance;
/**
* 上一帧数据的缓存
*/
private static final ConcurrentHashMap<String, MessagePad.TrackedObject> mMarkersCaches = new ConcurrentHashMap<>();
private static final ConcurrentHashMap<String, KalmanFilter> algoCache = new ConcurrentHashMap<>();
/**
* 记录每次实际绘制的交通元素UUID
*/
private final ArrayList<String> trafficDataUuidList = new ArrayList<>();
/**
* 过滤后的数据集合
*/
private final ArrayList<MessagePad.TrackedObject> mFilterTrafficData = new ArrayList<>();
private IdentifyDataDrawerTest() {
mContext = AbsMogoApplication.getApp();
addPreVehicleModel();
}
public static IdentifyDataDrawerTest getInstance() {
if (sInstance == null) {
synchronized (IdentifyDataDrawerTest.class) {
if (sInstance == null) {
sInstance = new IdentifyDataDrawerTest();
}
}
}
return sInstance;
}
public synchronized void release() {
sInstance = null;
}
/**
* 渲染 adas 识别的数据
*
* @param resultList adas感知融合数据
*/
@SuppressLint("NewApi")
public void renderAdasRecognizedResult(List<MessagePad.TrackedObject> resultList) {
if (resultList == null || resultList.isEmpty()) {
clearOldMarker();
CallerLogger.INSTANCE.w(TAG, "感知数据为空无需渲染……");
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);
});
ArrayList<MessagePad.TrackedObject> filterList = filterTrafficData(resultList);
if (filterList.size() > 0) {
// 绘制新数据
MogoMarkerManager.getInstance(mContext)
.updateBatchMarkerPosition(filterList);
}
}
/**
* 数据过滤器
*
* @return 过滤后的数据集合
*/
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()) {
//CallerLogger.INSTANCE.w(TAG, "未知感知类型数据，丢弃，不渲染");
continue;
}
double heading = kalmanFilter(data);
MessagePad.TrackedObject correctData = data.toBuilder().setHeading(heading).build();
mFilterTrafficData.add(correctData);
//更新已存在的感知物体数据
mMarkersCaches.put("" + correctData.getUuid(), correctData);
trafficDataUuidList.add("" + correctData.getUuid());
}
return mFilterTrafficData;
}
private double kalmanFilter(MessagePad.TrackedObject data) {
String uuid = "" + data.getUuid();
if (algoCache.containsKey(uuid)) {
Object o = algoCache.get(uuid);
KalmanFilter kf = (KalmanFilter) o;
assert kf != null;
double[] lonLat = kf.filter(data.getLongitude(), data.getLatitude());
algoCache.put(uuid, kf);
MessagePad.TrackedObject cacheTrackObj = mMarkersCaches.get(uuid);
assert cacheTrackObj != null;
double heading = MogoMap.getInstance().getMogoMap().getUIController().getAngle(cacheTrackObj.getLongitude(), cacheTrackObj.getLatitude(), lonLat[0], lonLat[1]);
CallerLogger.INSTANCE.d(M_HMI + "arrow47", " uuid : " + uuid + " , origin heading : " + data.getHeading() + " , correct heading : " + heading + " ---- " + (data.getHeading() - heading));
return Math.abs((data.getHeading() - heading)) >= 180 ? heading : data.getHeading();
} else {
algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.00005));
return data.getHeading();
}
}
/**
* 清除旧的 marker 数据
*/
public void clearOldMarker() {
for (String uuid : trafficDataUuidList) {
MogoMarkerManager.getInstance(mContext)
.removeMarker(uuid);
}
trafficDataUuidList.clear();
}
private void addPreVehicleModel() {
CallerLogger.INSTANCE.d(TAG, "添加感知模型到地图中……");
addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_WEI_ZHI, "添加感知模型到地图中……preVehicleStrWeiZhi=");
addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_PEOPLE, "添加感知模型到地图中……preVehicleStrPeople=");
addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_BICYCLE, "添加感知模型到地图中……preVehicleStrBicycle=");
addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_TA_CHE, "添加感知模型到地图中……preVehicleStrTaChe=");
addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_MOTO, "添加感知模型到地图中……preVehicleStrMoto=");
addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS, "添加感知模型到地图中……preVehicleStrBus=");
addPreVehicleModelWeiZhi(TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK, "添加感知模型到地图中……preVehicleStrTruck=");
}
/**
* 添加模型到地图中
*
* @param typeTrafficIdWeiZhi
* @param s
*/
private void addPreVehicleModelWeiZhi(TrafficTypeEnum typeTrafficIdWeiZhi, String s) {
String preVehicleStrWeiZhi = MogoMarkerManager.getInstance(mContext)
.addPreVehicleModel(typeTrafficIdWeiZhi.getType(),
typeTrafficIdWeiZhi.getTraffic3DIconId());
CallerLogger.INSTANCE.d(TAG, s + preVehicleStrWeiZhi);
if (preVehicleStrWeiZhi == null) {
UiThreadHandler.postDelayed(new Runnable() {
@Override
public void run() {
CallerLogger.INSTANCE.w(TAG, "添加感知模型到地图中失败，尝试重复添加……");
addPreVehicleModelWeiZhi(typeTrafficIdWeiZhi, s);
}
}, 1000L);
}
}
}

34
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java Normal file
View File

@@ -0,0 +1,34 @@
package com.mogo.eagle.core.function.map;
public class KalmanFilter {
private final double q = 1.0E-6D;
double r = 5.0E-5D;
double[][] xhat = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}};
double[][] p = new double[][]{{1.0D, 1.0D}, {0.0D, 0.0D}};
double[][] xhatminus = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}};
double[][] pMinus = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}};
double[][] k = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}};
int idx = 1;
public KalmanFilter(double lon, double lat, double r) {
this.xhat[0][0] = lon;
this.xhat[0][1] = lat;
this.r = r;
}
public double[] filter(double lon, double lat) {
for(int i = 0; i < 2; ++i) {
this.xhatminus[this.idx][i] = this.xhat[1 - this.idx][i];
this.pMinus[this.idx][i] = this.p[1 - this.idx][i] + 1.0E-6D;
this.k[this.idx][i] = this.pMinus[this.idx][i] / (this.pMinus[this.idx][i] + this.r);
double value = i == 0 ? lon : lat;
this.xhat[this.idx][i] = this.xhatminus[this.idx][i] + this.k[this.idx][i] * (value - this.xhatminus[this.idx][i]);
this.p[this.idx][i] = (1.0D - this.k[this.idx][i]) * this.pMinus[this.idx][i];
}
double lon1 = this.xhat[this.idx][0];
double lat1 = this.xhat[this.idx][1];
this.idx = 1 - this.idx;
return new double[]{lon1, lat1};
}
}

4
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/MapIdentifySubscriber.kt
View File

@@ -41,9 +41,9 @@ class MapIdentifySubscriber private constructor() : IMoGoSubscriber, IMoGoAutopi
override fun onAutopilotIdentifyDataUpdate(trafficData: List<TrackedObject>?) {
try {
if (FunctionBuildConfig.isDrawIdentifyData) {
ThreadUtils.getSinglePool().execute { IdentifyDataDrawer.getInstance().renderAdasRecognizedResult(trafficData) }
ThreadUtils.getSinglePool().execute { IdentifyDataDrawerTest.getInstance().renderAdasRecognizedResult(trafficData) }
} else {
IdentifyDataDrawer.getInstance().clearOldMarker()
IdentifyDataDrawerTest.getInstance().clearOldMarker()
}
} catch (e: Exception) {
e.printStackTrace()