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

添加经纬度纠偏逻辑，但是仅简单甄别问题点，无法纠正

Browse Source
This commit is contained in:
tongchenfei
2020-12-01 14:50:04 +08:00
parent 2eaf96533d
commit 0dc442100b
5 changed files with 233 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
modules/mogo-module-service/src/main/java/com/mogo/module/service/MogoServices.java
View File

@@ -55,6 +55,8 @@ import com.mogo.module.service.refresh.AutoRefreshStrategy;
import com.mogo.module.service.refresh.CustomRefreshStrategy;
import com.mogo.module.service.refresh.RefreshObject;
import com.mogo.module.service.strategy.CarIconDisplayStrategy;
import com.mogo.module.service.utils.LocationParseUtil;
import com.mogo.module.service.utils.SimpleLocationCorrectStrategy;
import com.mogo.module.service.vrmode.VrModeController;
import com.mogo.module.service.websocket.LocationResult;
import com.mogo.module.service.websocket.OnePerSecondSendContent;
@@ -465,22 +467,29 @@ public class MogoServices implements IMogoMapListener,
}
private void startSendCarLocationAndAdasRecognizedResult2Server(List<CloudLocationInfo> cloudLocationInfo) {
Location lastCarLocation = mLastCarLocation;
// Location lastCarLocation = mLastCarLocation;
CloudLocationInfo lastInfo = LocationParseUtil.locationToCloudLocation(mLastCarLocation);
// 如果数组内容不为空，就用数组最后一个值
if (cloudLocationInfo != null && !cloudLocationInfo.isEmpty()) {
lastInfo = cloudLocationInfo.get(cloudLocationInfo.size() - 1);
}
LocationResult locationResult = null;
if ( lastCarLocation != null ) {
if ( lastInfo != null ) {
// 定位点预测纠偏
lastInfo = SimpleLocationCorrectStrategy.getInstance().correct(lastInfo);
if (lastInfo == null) {
return;
}
locationResult = new LocationResult();
locationResult.lastCoordinate = new CloudLocationInfo();
locationResult.lastCoordinate.setAlt( lastCarLocation.getAltitude() );
locationResult.lastCoordinate.setHeading( lastCarLocation.getBearing() );
locationResult.lastCoordinate.setLat( lastCarLocation.getLatitude() );
locationResult.lastCoordinate.setLon( lastCarLocation.getLongitude() );
locationResult.lastCoordinate.setSatelliteTime( lastCarLocation.getTime() );
locationResult.lastCoordinate.setSystemTime( System.currentTimeMillis() );
locationResult.lastCoordinate.setSpeed( lastCarLocation.getSpeed() );
locationResult.lastCoordinate = lastInfo;
locationResult.coordinates = new ArrayList<>();
locationResult.sn = com.mogo.commons.network.Utils.getSn();
locationResult.mortonCode = MortonCode.wrapEncodeMorton( locationResult.lastCoordinate.getLon(), locationResult.lastCoordinate.getLat() );
locationResult.coordinates.addAll( cloudLocationInfo );
locationResult.mortonCode = MortonCode.wrapEncodeMorton( lastInfo.getLon(), lastInfo.getLat() );
if(cloudLocationInfo == null){
locationResult.coordinates.addAll(new ArrayList<>());
}else {
locationResult.coordinates.addAll(cloudLocationInfo);
}
}
List< ADASRecognizedResult > recognizedResults = MarkerServiceHandler.getADASController().getLastADASRecognizedResult();
OnePerSecondSendContent content = new OnePerSecondSendContent();

3
modules/mogo-module-service/src/main/java/com/mogo/module/service/location/MogoRTKLocation.java
View File

@@ -60,8 +60,7 @@ public class MogoRTKLocation {
private void sendLocationData() {
if (rtkLocationListener != null) {
Logger.d(TAG, "sendLocationData size : " + cacheList.size());
List list = new ArrayList();
list.addAll(cacheList);
List<CloudLocationInfo> list = new ArrayList<>(cacheList);
rtkLocationListener.onLocationChanged(list);
}
if (cacheList != null && cacheList.size() > 0) {

40
modules/mogo-module-service/src/main/java/com/mogo/module/service/utils/LocationParseUtil.java Normal file
View File

@@ -0,0 +1,40 @@
package com.mogo.module.service.utils;
import android.location.Location;
import com.mogo.map.MogoLatLng;
import com.mogo.module.common.entity.CloudLocationInfo;
/**
* 定位数据类型转换工具
*
* @author tongchenfei
*/
public class LocationParseUtil {
/**
* 从Location 转 CloudLocationInfo
* @param info 待转数据
* @return 转后数据
*/
public static CloudLocationInfo locationToCloudLocation(Location info) {
if (info == null) {
return null;
}
CloudLocationInfo cloud = new CloudLocationInfo();
cloud.setLat(info.getLatitude());
cloud.setLon(info.getLongitude());
cloud.setAlt(info.getAltitude());
cloud.setHeading(info.getBearing());
cloud.setSpeed(info.getSpeed());
cloud.setSatelliteTime(info.getTime());
cloud.setSystemTime(System.currentTimeMillis());
return cloud;
}
public static MogoLatLng cloudLocationToMogoLatLng(CloudLocationInfo info) {
if (info == null) {
return null;
}
return new MogoLatLng(info.getLat(), info.getLon());
}
}

135
modules/mogo-module-service/src/main/java/com/mogo/module/service/utils/SimpleLocationCorrectStrategy.java Normal file
View File

@@ -0,0 +1,135 @@
package com.mogo.module.service.utils;
import android.location.Location;
import android.os.SystemClock;
import com.mogo.module.common.MogoApisHandler;
import com.mogo.module.common.entity.CloudLocationInfo;
import com.mogo.utils.logger.Logger;
import java.util.ArrayList;
import java.util.List;
/**
* 定位预测纠错策略
*
* @author tongchenfei
*/
public class SimpleLocationCorrectStrategy {
private static final String TAG = "SimpleLocationCorrectStrategy";
private static final int ERR_COUNT_THRESHOLD = 3;
/**
* 目标距离误差是10米，就是在原目标距离基础上增加10米
*/
private static final float TARGET_DISTANCE_DEVIATION = 10;
private CloudLocationInfo lastLocation = null;
private long anchorTime;
private int errCount;
private static SimpleLocationCorrectStrategy instance = new SimpleLocationCorrectStrategy();
public static SimpleLocationCorrectStrategy getInstance(){
return instance;
}
private List<CloudLocationInfo> tmpList = new ArrayList<>();
public CloudLocationInfo correct(CloudLocationInfo info) {
Logger.d(TAG, "info: " + info.print());
Logger.d("tmpList", "tmpList: " + tmpList);
if(isLocationValid(info)) {
tmpList.add(info);
if (lastLocation == null) {
lastLocation = info;
anchorTime = SystemClock.elapsedRealtime();
Logger.d(TAG, "第一条数据");
return info;
}
if (lastLocation.equals(info)) {
Logger.d(TAG, "相同坐标点==");
return info;
}
try {
float targetDistance =
(float) (lastLocation.getSpeed() * (SystemClock.elapsedRealtime() - anchorTime) / 1000) + TARGET_DISTANCE_DEVIATION;
float distance = MogoApisHandler.getInstance().getApis().getMapServiceApi().getMapUIController().calculateLineDistance(LocationParseUtil.cloudLocationToMogoLatLng(lastLocation), LocationParseUtil.cloudLocationToMogoLatLng(info));
Logger.d(TAG,
"准备计算{ lastInfo: " + lastLocation.print() + " info: " + info.print() + " targetDistance: " + targetDistance + " distance : " + distance + "}");
if (distance <= targetDistance) {
// 新的定位点在目标距离范围内，认为此数据有效
lastLocation = info;
anchorTime = SystemClock.elapsedRealtime();
errCount = 0;
Logger.d(TAG, "在范围内，为有效点");
return info;
} else {
// 出现异常点
if (errCount >= ERR_COUNT_THRESHOLD) {
// 出错次数超过阈值，认为本次出错点为正确点
lastLocation = info;
anchorTime = SystemClock.elapsedRealtime();
errCount = 0;
Logger.d(TAG, "出错次数超限，异常点变有效点");
return info;
} else {
// 按照上一个点的方向和速度，计算下一个点的位置，下一个点除坐标点外，其余数据与上一个点相同
// CloudLocationInfo nextInfo = new CloudLocationInfo(lastLocation);
// nextInfo.setLon(targetDistance * Math.sin(lastLocation.getHeading() * Math.PI / 180));
// nextInfo.setLat(targetDistance * Math.cos(lastLocation.getHeading() * Math.PI / 180));
// lastLocation = nextInfo;
anchorTime = SystemClock.elapsedRealtime();
errCount++;
Logger.d(TAG, "异常点纠偏 info: " + lastLocation);
return lastLocation;
// return nextInfo;
}
}
} catch (Exception e) {
Logger.e(TAG, e, "纠偏异常");
e.printStackTrace();
}
}else{
Logger.d(TAG, "定位点异常");
if (lastLocation == null) {
return null;
}else{
try {
float targetDistance =
(float) (lastLocation.getSpeed() * (SystemClock.elapsedRealtime() - anchorTime) / 1000) + TARGET_DISTANCE_DEVIATION;
float distance = MogoApisHandler.getInstance().getApis().getMapServiceApi().getMapUIController().calculateLineDistance(LocationParseUtil.cloudLocationToMogoLatLng(lastLocation), LocationParseUtil.cloudLocationToMogoLatLng(info));
Logger.d(TAG,
"异常定位点\n准备计算{ lastInfo: " + lastLocation.print() + " info: " + info.print() + " targetDistance: " + targetDistance + " distance : " + distance + "}");
// 按照上一个点的方向和速度，计算下一个点的位置，下一个点除坐标点外，其余数据与上一个点相同
// CloudLocationInfo nextInfo = new CloudLocationInfo(lastLocation);
// nextInfo.setLon(targetDistance * Math.sin(lastLocation.getHeading()));
// nextInfo.setLat(targetDistance * Math.cos(lastLocation.getHeading()));
// lastLocation = nextInfo;
anchorTime = SystemClock.elapsedRealtime();
errCount++;
Logger.d(TAG, "异常点纠偏 info: " + lastLocation);
// return nextInfo;
return lastLocation;
}catch (Exception e){
Logger.e(TAG, e, "纠偏异常");
e.printStackTrace();
}
}
}
return null;
}
private boolean isLocationValid(CloudLocationInfo info) {
return info.getLat() != 0 && info.getLon() != 0;
}
private float getDistance(CloudLocationInfo lastInfo, CloudLocationInfo info) {
float[] results=new float[1];
try{
Location.distanceBetween(lastInfo.getLat(), lastInfo.getLon(), info.getLat(), info.getLon(), results);
}catch(Exception e){
e.printStackTrace();
}
return results[0]*1000;
}
}