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

wait to test

Browse Source
This commit is contained in:
zhongchao
2022-06-02 11:40:05 +08:00
parent 1a48c29951
commit d45f528aa9
5 changed files with 344 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -0,0 +1,33 @@
package com.mogo.eagle.core.function.map;
import android.util.Log;
import java.util.Vector;
public class CircleQueue {
private final Vector<ObjQueue> objQueue;
private final int maxSize;
public CircleQueue(int maxSize) {
this.maxSize = maxSize;
objQueue = new Vector<>(maxSize);
}
public void addQueue(ObjQueue obj) {
if(objQueue.size() == maxSize){
objQueue.remove(0);
}
objQueue.add(obj);
}
public void deleteObj(ObjQueue obj) {
objQueue.remove(obj);
}
public void show() {
for (ObjQueue queue : objQueue) {
Log.i("CircleQueue","show : " + queue.getType());
}
}
}

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

@@ -1,15 +1,13 @@
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.utilcode.mogo.AppIdentityModeUtils;
import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger;
import com.mogo.map.MogoMap;
import com.mogo.map.MogoMarkerManager;
import com.mogo.module.common.MogoApisHandler;
import com.mogo.module.common.constants.AdasRecognizedType;
@@ -126,8 +124,9 @@ public class IdentifyDataDrawer {
//首次过来的数据不添加,首次未添加的感知物在调用完绘制方法后再塞入cache map
MessagePad.TrackedObject cacheData = mMarkersCaches.get(uuid);
if (cacheData != null) {
if (data.getSpeed() < 0.5) {
data = data.toBuilder().setHeading(cacheData.getHeading()).setLongitude(cacheData.getLongitude()).setLatitude(cacheData.getLatitude()).build();
double limitSpeed = AppIdentityModeUtils.isBus(FunctionBuildConfig.appIdentityMode) ? 0.7 : 0.5;
if (data.getSpeed() < limitSpeed) {
data.toBuilder().setHeading(cacheData.getHeading()).setLongitude(cacheData.getLongitude()).setLatitude(cacheData.getLatitude()).build();
}
mFilterTrafficData.add(data);
//更新已存在的感知物体数据

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

@@ -0,0 +1,213 @@
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.utilcode.mogo.logger.CallerLogger;
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 IdentifyDataDrawerTrack {
private static final String TAG = "IdentifyDataDrawer";
protected final Context mContext;
private static volatile IdentifyDataDrawerTrack sInstance;
/**
* 上一帧数据的缓存
*/
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<>();
/**
* 过滤后的数据集合
*/
private final ArrayList<MessagePad.TrackedObject> mFilterTrafficData = new ArrayList<>();
private IdentifyDataDrawerTrack() {
mContext = AbsMogoApplication.getApp();
}
public static IdentifyDataDrawerTrack getInstance() {
if (sInstance == null) {
synchronized (IdentifyDataDrawerTrack.class) {
if (sInstance == null) {
sInstance = new IdentifyDataDrawerTrack();
}
}
}
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;
}
String uuid = "" + data.getUuid();
//首次过来的数据不添加,首次未添加的感知物在调用完绘制方法后再塞入cache map
MessagePad.TrackedObject cacheData = mMarkersCaches.get(uuid);
if (cacheData != null) { //todo 代码不要推到云端，本地测试即可！！！！！！！！！！
MessagePad.TrackedObject correctData;
correctData = kalmanCorrectData(data);
mFilterTrafficData.add(correctData);
//更新已存在的感知物体数据
mMarkersCaches.put(uuid, correctData);
} else {
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;
double[] lonLat = kf.filter(data.getLongitude(), data.getLatitude());
algoCache.put(uuid, kf);
MessagePad.TrackedObject cacheTrackObj = mMarkersCaches.get(uuid);
assert cacheTrackObj != null;
//todo 代码不要推到云端，本地测试即可！！！！！！！！！！
if (data.getSpeed() >= 1.5 && (data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TA_CHE.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_MOTO.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BICYCLE.getType())) {
double kalH = MogoMap.getInstance().getMogoMap().getUIController().getAngle(cacheTrackObj.getLongitude(), cacheTrackObj.getLatitude(), lonLat[0], lonLat[1]);
if ((cacheTrackObj.getHeading() / 360 > 0.75 && data.getHeading() / 360 < 0.25) || (cacheTrackObj.getHeading() / 360 < 0.25 && data.getHeading() / 360 > 0.75)) {
return data;
}
if (Math.abs(cacheTrackObj.getHeading() - data.getHeading()) > 165 && Math.abs(cacheTrackObj.getHeading() - data.getHeading()) < 195) {
return data;
}
if (Math.abs(cacheTrackObj.getHeading() - data.getHeading()) > 40 && (Math.abs(kalH - cacheTrackObj.getHeading())) > 40) {
if (data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TA_CHE.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK.getType()) {
CallerLogger.INSTANCE.d(M_HMI + "type : " + data.getType(), " uuid : " + uuid + " , cacheH : " + cacheTrackObj.getHeading() + " , origin H : " + data.getHeading() + " , kalH H : " + kalH + " , 使用卡尔曼" + " ， speed : " + data.getSpeed());
}
return data.toBuilder().setHeading(kalH).build();
}
if (Math.abs(cacheTrackObj.getHeading() - data.getHeading()) > 40 && (Math.abs(kalH - data.getHeading())) > 40) { //存在前后帧误差
double correct = Math.abs(cacheTrackObj.getHeading() - kalH) - Math.abs(data.getHeading() - kalH) > 0 ? data.getHeading() : cacheTrackObj.getHeading();
if (data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TA_CHE.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK.getType()) {
CallerLogger.INSTANCE.d(M_HMI + "type : " + data.getType(), " uuid : " + uuid + " , cacheH : " + cacheTrackObj.getHeading() + " , origin H : " + data.getHeading() + " , correct H : " + correct + " , 使用 : " + (Math.abs(cacheTrackObj.getHeading() - kalH) - Math.abs(data.getHeading() - kalH) > 0 ? "当前帧" : "缓存帧" + " ， speed : " + data.getSpeed()));
}
return data.toBuilder().setHeading(correct).build();
}
} else if (data.getSpeed() <= 0.5) {
double roadAngle = MogoMap.getInstance().getMogoMap().getUIController().getRoadAngle(cacheTrackObj.getLongitude(), cacheTrackObj.getLatitude(), -1);
double result = roadAngle != 0 ? roadAngle : cacheTrackObj.getHeading();
CallerLogger.INSTANCE.d(M_HMI + "type : " + data.getType()," uuid : " + uuid + " , <0.5 result : " + result);
return data.toBuilder().setHeading(cacheTrackObj.getHeading()).setLongitude(cacheTrackObj.getLongitude()).setLatitude(cacheTrackObj.getLatitude()).build();
} else {
return data;
}
} else {
algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005));
return data;
}
return data;
}
private double getAngle(double lat_a, double lng_a, double lat_b, double lng_b) {
double y = Math.sin(lng_b - lng_a) * Math.cos(lat_b);
double x = Math.cos(lat_a) * Math.sin(lat_b) - Math.sin(lat_a) * Math.cos(lat_b) * Math.cos(lng_b - lng_a);
double bearing = Math.atan2(y, x);
bearing = Math.toDegrees(bearing);
if (bearing < 0) {
bearing = bearing + 360;
}
return 360 - bearing;
}
/**
* 清除旧的 marker 数据
*/
public void clearOldMarker() {
for (String uuid : trafficDataUuidList) {
MogoMarkerManager.getInstance(mContext)
.removeMarker(uuid);
}
trafficDataUuidList.clear();
}
}

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

@@ -0,0 +1,47 @@
package com.mogo.eagle.core.function.map;
public class ObjQueue {
private double heading;
private double speed;
private int type;
public ObjQueue(double heading, double speed, int type) {
this.heading = heading;
this.speed = speed;
this.type = type;
}
public double getHeading() {
return heading;
}
public void setHeading(double heading) {
this.heading = heading;
}
public double getSpeed() {
return speed;
}
public void setSpeed(double speed) {
this.speed = speed;
}
public int getType() {
return type;
}
public void setType(int type) {
this.type = type;
}
@Override
public String toString() {
return "ObjQueue{" +
"heading=" + heading +
", speed=" + speed +
", type=" + type +
'}';
}
}

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

@@ -0,0 +1,47 @@
package com.mogo.eagle.core.function.map;
public class TrackObj {
private final CircleQueue circleQueue = new CircleQueue(10);
private final int[] observationType = new int[3]; //类型变化的观测数组
private long recentlyTime;
private double headingDelta; //航向角德尔塔
private double speedDelta; //速度德尔塔
private double typeWeight; //类型权重
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;
}
public void updateObj() {
}
}