diff --git a/modules/mogo-module-service/src/main/java/com/mogo/module/service/MogoServices.java b/modules/mogo-module-service/src/main/java/com/mogo/module/service/MogoServices.java index 69a9d792ff..bdec9c50c0 100644 --- a/modules/mogo-module-service/src/main/java/com/mogo/module/service/MogoServices.java +++ b/modules/mogo-module-service/src/main/java/com/mogo/module/service/MogoServices.java @@ -4,6 +4,7 @@ import android.content.ComponentName; import android.content.Context; import android.content.Intent; import android.content.IntentFilter; +import android.graphics.BitmapFactory; import android.location.Location; import android.net.ConnectivityManager; import android.os.Handler; @@ -15,6 +16,7 @@ import android.view.MotionEvent; import androidx.annotation.NonNull; +import com.amap.api.maps.model.MarkerOptions; import com.mogo.commons.debug.DebugConfig; import com.mogo.commons.network.ParamsProvider; import com.mogo.commons.network.SubscribeImpl; @@ -27,6 +29,7 @@ import com.mogo.map.MogoLatLng; import com.mogo.map.listener.IMogoMapListener; import com.mogo.map.location.IMogoLocationListener; import com.mogo.map.location.MogoLocation; +import com.mogo.map.marker.MogoMarkerOptions; import com.mogo.map.navi.IMogoAimlessModeListener; import com.mogo.map.navi.IMogoCarLocationChangedListener2; import com.mogo.map.navi.IMogoNavi; @@ -34,6 +37,7 @@ import com.mogo.map.navi.IMogoNaviListener; import com.mogo.map.navi.MogoCongestionInfo; import com.mogo.map.navi.MogoTraffic; import com.mogo.map.uicontroller.IMogoMapUIController; +import com.mogo.module.common.MogoApisHandler; import com.mogo.module.common.MogoModule; import com.mogo.module.common.MogoModulePaths; import com.mogo.module.common.entity.CloudLocationInfo; @@ -452,6 +456,46 @@ public class MogoServices implements IMogoMapListener, Logger.i(TAG,"cloudLocationInfos size : " + cloudLocationInfos.size()); startSendCarLocationAndAdasRecognizedResult2Server(cloudLocationInfos); }); + + SimpleLocationCorrectStrategy.getInstance().setRecordLocationListener((history, correct,valid,err) -> { + // todo 打点 + Logger.d("SimpleCorrect","correct: "+correct); + Logger.d("SimpleCorrect","valid: "+valid); + Logger.d("SimpleCorrect","err: "+err); + ArrayList optionList = new ArrayList<>(); +// for (CloudLocationInfo h : history) { +// MogoMarkerOptions options = new MogoMarkerOptions(); +// options.position(LocationParseUtil.cloudLocationToMogoLatLng(h)); +// options.icon(BitmapFactory.decodeResource(context.getResources(), +// R.drawable.bg_map_marker_blue)); +// optionList.add(options); +// } + + for (CloudLocationInfo v : valid) { + MogoMarkerOptions options = new MogoMarkerOptions(); + options.position(LocationParseUtil.cloudLocationToMogoLatLng(v)); + options.icon(BitmapFactory.decodeResource(context.getResources(), + R.drawable.bg_map_marker_oragne)); + optionList.add(options); + } + + for (CloudLocationInfo c : correct) { + MogoMarkerOptions options = new MogoMarkerOptions(); + options.position(LocationParseUtil.cloudLocationToMogoLatLng(c)); + options.icon(BitmapFactory.decodeResource(context.getResources(), + R.drawable.bg_map_marker_green)); + optionList.add(options); + } + + for (CloudLocationInfo e : err) { + MogoMarkerOptions options = new MogoMarkerOptions(); + options.position(LocationParseUtil.cloudLocationToMogoLatLng(e)); + options.icon(BitmapFactory.decodeResource(context.getResources(), + R.drawable.bg_map_marker_red)); + optionList.add(options); + } + MogoApisHandler.getInstance().getApis().getMapServiceApi().getMarkerManager(context).addMarkers("MogoServices", optionList, false); + }); } private void initLocationServiceProcess( Context context ) { diff --git a/modules/mogo-module-service/src/main/java/com/mogo/module/service/utils/SimpleLocationCorrectStrategy.java b/modules/mogo-module-service/src/main/java/com/mogo/module/service/utils/SimpleLocationCorrectStrategy.java index cb2cf47455..57bf885f2e 100644 --- a/modules/mogo-module-service/src/main/java/com/mogo/module/service/utils/SimpleLocationCorrectStrategy.java +++ b/modules/mogo-module-service/src/main/java/com/mogo/module/service/utils/SimpleLocationCorrectStrategy.java @@ -3,6 +3,7 @@ package com.mogo.module.service.utils; import android.location.Location; import android.os.SystemClock; +import com.mogo.map.MogoLatLng; import com.mogo.module.common.MogoApisHandler; import com.mogo.module.common.entity.CloudLocationInfo; import com.mogo.utils.logger.Logger; @@ -33,17 +34,25 @@ public class SimpleLocationCorrectStrategy { return instance; } - private List tmpList = new ArrayList<>(); + private List historyList = new ArrayList<>(); + private List validList = new ArrayList<>(); + private List correctList = new ArrayList<>(); + private List errList = 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(recordLocation()) { + historyList.add(info); + } + if (lastLocation == null) { lastLocation = info; anchorTime = SystemClock.elapsedRealtime(); Logger.d(TAG, "第一条数据"); + if(recordLocation()) { + validList.add(lastLocation); + } return info; } if (lastLocation.equals(info)) { @@ -62,11 +71,18 @@ public class SimpleLocationCorrectStrategy { anchorTime = SystemClock.elapsedRealtime(); errCount = 0; Logger.d(TAG, "在范围内，为有效点"); + if(recordLocation()) { + validList.add(lastLocation); + } return info; } else { // 出现异常点 if (errCount >= ERR_COUNT_THRESHOLD) { // 出错次数超过阈值，认为本次出错点为正确点 + if(recordLocation()) { + errList.add(new CloudLocationInfo(lastLocation)); + correctList.add(info); + } lastLocation = info; anchorTime = SystemClock.elapsedRealtime(); errCount = 0; @@ -74,15 +90,24 @@ public class SimpleLocationCorrectStrategy { 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; + CloudLocationInfo nextInfo = new CloudLocationInfo(lastLocation); + MogoLatLng nextLatLon = computerThatLonLat(lastLocation.getLon(), + lastLocation.getLat(), lastLocation.getHeading(), targetDistance); + nextInfo.setLon(nextLatLon.lon); + nextInfo.setLat(nextLatLon.lat); + if(recordLocation()) { + errList.add(info); + correctList.add(nextInfo); + } + lastLocation = nextInfo; anchorTime = SystemClock.elapsedRealtime(); errCount++; Logger.d(TAG, "异常点纠偏 info: " + lastLocation); - return lastLocation; -// return nextInfo; +// return lastLocation; + if(recordLocation()) { + correctList.add(nextInfo); + } + return nextInfo; } } } catch (Exception e) { @@ -101,15 +126,24 @@ public class SimpleLocationCorrectStrategy { 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; + CloudLocationInfo nextInfo = new CloudLocationInfo(lastLocation); + MogoLatLng nextLatLon = computerThatLonLat(lastLocation.getLon(), + lastLocation.getLat(), lastLocation.getHeading(), targetDistance); + nextInfo.setLon(nextLatLon.lon); + nextInfo.setLat(nextLatLon.lat); + if(recordLocation()) { + errList.add(info); + correctList.add(nextInfo); + } + lastLocation = nextInfo; anchorTime = SystemClock.elapsedRealtime(); errCount++; Logger.d(TAG, "异常点纠偏 info: " + lastLocation); -// return nextInfo; - return lastLocation; + if(recordLocation()) { + correctList.add(nextInfo); + } +// return lastLocation; + return nextInfo; }catch (Exception e){ Logger.e(TAG, e, "纠偏异常"); e.printStackTrace(); @@ -123,13 +157,101 @@ public class SimpleLocationCorrectStrategy { 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(); + private RecordLocationListener recordLocationListener = null; + private boolean hasCallbackReocrd = false; + + public void setRecordLocationListener(RecordLocationListener recordLocationListener) { + this.recordLocationListener = recordLocationListener; + } + + private boolean recordLocation(){ + if (historyList.size() >= 100 && !hasCallbackReocrd && recordLocationListener != null) { + hasCallbackReocrd = true; + recordLocationListener.onRecordFinish(historyList, correctList,validList,correctList); } - return results[0]*1000; + return historyList.size() < 100; + } + + + /** + * 根据距离和角度计算下一个经纬度 + * 大地坐标系资料WGS-84 长半径a=6378137 短半径b=6356752.3142 扁率f=1/298.2572236 + */ + public MogoLatLng computerThatLonLat(double lon, double lat, double brng, double dist) { + + double alpha1 = rad(brng); + double sinAlpha1 = Math.sin(alpha1); + double cosAlpha1 = Math.cos(alpha1); + + // 扁率f=1/298.2572236 + double f = 1 / 298.2572236; + double tanU1 = (1 - f) * Math.tan(rad(lat)); + double cosU1 = 1 / Math.sqrt((1 + tanU1 * tanU1)); + double sinU1 = tanU1 * cosU1; + double sigma1 = Math.atan2(tanU1, cosAlpha1); + double sinAlpha = cosU1 * sinAlpha1; + double cosSqAlpha = 1 - sinAlpha * sinAlpha; + // 长半径a=6378137 + double a = 6378137; + // 短半径b=6356752.3142 + double b = 6356752.3142; + double uSq = cosSqAlpha * (a * a - b * b) / (b * b); + double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq))); + double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq))); + + double cos2SigmaM=0; + double sinSigma=0; + double cosSigma=0; + double sigma = dist / (b * A), sigmaP = 2 * Math.PI; + while (Math.abs(sigma - sigmaP) > 1e-12) { + cos2SigmaM = Math.cos(2 * sigma1 + sigma); + sinSigma = Math.sin(sigma); + cosSigma = Math.cos(sigma); + double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) + - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM))); + sigmaP = sigma; + sigma = dist / (b * A) + deltaSigma; + } + + double tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1; + double lat2 = Math.atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1, + (1 - f) * Math.sqrt(sinAlpha * sinAlpha + tmp * tmp)); + double lambda = Math.atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1); + double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha)); + double L = lambda - (1 - C) * f * sinAlpha + * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM))); + + // final bearing + double revAz = Math.atan2(sinAlpha, -tmp); + + System.out.println(revAz); + System.out.println(lon+deg(L)+","+deg(lat2)); + return new MogoLatLng(deg(lat2), lon + deg(L)); + } + + /** + * 度换成弧度 + * + * @param d + * 度 + * @return 弧度 + */ + private double rad(double d) { + return d * Math.PI / 180.0; + } + + /** + * 弧度换成度 + * + * @param x + * 弧度 + * @return 度 + */ + private double deg(double x) { + return x * 180 / Math.PI; + } + + public interface RecordLocationListener{ + void onRecordFinish(List history, List correct,List valid,List err); } }