优化策略

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<MogoMarkerOptions> 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 ) {

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<CloudLocationInfo> tmpList = new ArrayList<>();
+    private List<CloudLocationInfo> historyList = new ArrayList<>();
+    private List<CloudLocationInfo> validList = new ArrayList<>();
+    private List<CloudLocationInfo> correctList = new ArrayList<>();
+    private List<CloudLocationInfo> 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);
+                        CloudLocationInfo nextInfo = new CloudLocationInfo(lastLocation);
-//                        nextInfo.setLon(targetDistance * Math.sin(lastLocation.getHeading() * Math.PI / 180));
+                        MogoLatLng nextLatLon = computerThatLonLat(lastLocation.getLon(),
-//                        nextInfo.setLat(targetDistance * Math.cos(lastLocation.getHeading() * Math.PI / 180));
+                                lastLocation.getLat(), lastLocation.getHeading(), targetDistance);
-//                        lastLocation = nextInfo;
+                        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 lastLocation;
-//                        return nextInfo;
+                        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);
+                    CloudLocationInfo nextInfo = new CloudLocationInfo(lastLocation);
-//                    nextInfo.setLon(targetDistance * Math.sin(lastLocation.getHeading()));
+                    MogoLatLng nextLatLon = computerThatLonLat(lastLocation.getLon(),
-//                    nextInfo.setLat(targetDistance * Math.cos(lastLocation.getHeading()));
+                            lastLocation.getLat(), lastLocation.getHeading(), targetDistance);
-//                    lastLocation = nextInfo;
+                    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;
+                    if(recordLocation()) {
-                    return lastLocation;
+                        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) {
+    private RecordLocationListener recordLocationListener = null;
-        float[] results=new float[1];
+    private boolean hasCallbackReocrd = false;
-        try{
+
-            Location.distanceBetween(lastInfo.getLat(), lastInfo.getLon(), info.getLat(), info.getLon(), results);
+    public void setRecordLocationListener(RecordLocationListener recordLocationListener) {
-        }catch(Exception e){
+        this.recordLocationListener = recordLocationListener;
-            e.printStackTrace();
+    }
+
+    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<CloudLocationInfo> history, List<CloudLocationInfo> correct,List<CloudLocationInfo> valid,List<CloudLocationInfo> err);
     }
 }