flutter-sport/lib/utils/path_smooth_tool.dart

import 'dart:core';
import 'dart:math';

import 'package:amap_flutter_base/amap_flutter_base.dart';

class PathSmoothTool {
  int mIntensity = 3;
  double mThreshhold = 0.3;
  double mNoiseThreshhold = 10;

  // PathSmoothTool({this.mIntensity = 3, this.mThreshhold = 0.3, this.mNoiseThreshhold = 10});

  /**
   * 轨迹平滑优化
   * @param originlist 原始轨迹list,list.size大于2
   * @return 优化后轨迹list
   */
  List<LatLng>? pathOptimize(List<LatLng> originlist) {
    List<LatLng>? list = removeNoisePoint(originlist); //去噪
    List<LatLng> afterList = kalmanFilterPath(list, mIntensity); //滤波
    List<LatLng>? pathoptimizeList = reducerVerticalThreshold(afterList, mThreshhold); //抽稀
//        Log.i("MY","originlist: "+originlist.length);
//        Log.i("MY","list: "+list.length);
//        Log.i("MY","afterList: "+afterList.length);
//        Log.i("MY","pathoptimizeList: "+pathoptimizeList.length);
    return pathoptimizeList;
  }

  /**
   * 轨迹去噪，删除垂距大于20m的点
   * @param originlist 原始轨迹list,list.size大于2
   * @return
   */
  List<LatLng>? removeNoisePoint(List<LatLng> originlist) {
    return reduceNoisePoint(originlist, mNoiseThreshhold);
  }

  /********************************************************************************************************/
  /**
   * 轨迹线路滤波
   * @param originlist 原始轨迹list,list.size大于2
   * @param intensity 滤波强度（1—5）
   * @return
   */
  List<LatLng> kalmanFilterPath(List<LatLng>? originlist, int intensity) {
    List<LatLng> kalmanFilterList = [];
    if (originlist == null || originlist.length <= 2) return kalmanFilterList;
    initial(); //初始化滤波参数
    LatLng? latLng;
    LatLng lastLoc = originlist[0];
    kalmanFilterList.add(lastLoc);
    for (int i = 1; i < originlist.length; i++) {
      LatLng curLoc = originlist[i];
      latLng = kalmanFilterPoint(lastLoc, curLoc, intensity);
      if (latLng != null) {
        kalmanFilterList.add(latLng);
        lastLoc = latLng;
      }
    }
    return kalmanFilterList;
  }

  /**
   * 单点滤波
   * @param lastLoc 上次定位点坐标
   * @param curLoc 本次定位点坐标
   * @param intensity 滤波强度（1—5）
   * @return 滤波后本次定位点坐标值
   */
  LatLng? kalmanFilterPoint(LatLng? lastLoc, LatLng? curLoc, int intensity) {
    if (pdelt_x == null || pdelt_y == null) {
      initial();
    }
    LatLng? kalmanLatlng;
    if (lastLoc == null || curLoc == null) {
      return curLoc;
    }
    int _intensity = intensity;
    if (intensity < 1) {
      _intensity = 1;
    } else if (intensity > 10) {
      _intensity = 10;
    }
    for (int j = 0; j < _intensity; j++) {
      kalmanLatlng = kalmanFilter(lastLoc.longitude, curLoc!.longitude, lastLoc.latitude, curLoc.latitude);
      curLoc = kalmanLatlng;
    }
    return kalmanLatlng;
  }

  /***************************卡尔曼滤波开始********************************/
  late double lastLocation_x; //上次位置
  late double currentLocation_x; //这次位置
  late double lastLocation_y; //上次位置
  late double currentLocation_y; //这次位置
  late double estimate_x; //修正后数据
  late double estimate_y; //修正后数据
  double? pdelt_x; //自预估偏差
  double? pdelt_y; //自预估偏差
  late double mdelt_x; //上次模型偏差
  late double mdelt_y; //上次模型偏差
  late double gauss_x; //高斯噪音偏差
  late double gauss_y; //高斯噪音偏差
  late double kalmanGain_x; //卡尔曼增益
  late double kalmanGain_y; //卡尔曼增益

  double m_R = 0;
  double m_Q = 0;

  //初始模型
  void initial() {
    pdelt_x = 0.001;
    pdelt_y = 0.001;
//        mdelt_x = 0;
//        mdelt_y = 0;
    mdelt_x = 5.698402909980532E-4;
    mdelt_y = 5.698402909980532E-4;
  }

  LatLng kalmanFilter(double oldValue_x, double value_x, double oldValue_y, double value_y) {
    lastLocation_x = oldValue_x;
    currentLocation_x = value_x;
    // print("pdelt_x $pdelt_x mdelt_x $mdelt_x");
    gauss_x = sqrt(pdelt_x! * pdelt_x! + mdelt_x * mdelt_x) + m_Q; //计算高斯噪音偏差
    kalmanGain_x = sqrt((gauss_x * gauss_x) / (gauss_x * gauss_x + pdelt_x! * pdelt_x!)) + m_R; //计算卡尔曼增益
    estimate_x = kalmanGain_x * (currentLocation_x - lastLocation_x) + lastLocation_x; //修正定位点
    mdelt_x = sqrt((1 - kalmanGain_x) * gauss_x * gauss_x); //修正模型偏差

    lastLocation_y = oldValue_y;
    currentLocation_y = value_y;
    gauss_y = sqrt(pdelt_y! * pdelt_y! + mdelt_y * mdelt_y) + m_Q; //计算高斯噪音偏差
    kalmanGain_y = sqrt((gauss_y * gauss_y) / (gauss_y * gauss_y + pdelt_y! * pdelt_y!)) + m_R; //计算卡尔曼增益
    estimate_y = kalmanGain_y * (currentLocation_y - lastLocation_y) + lastLocation_y; //修正定位点
    mdelt_y = sqrt((1 - kalmanGain_y) * gauss_y * gauss_y); //修正模型偏差

    return LatLng(estimate_y, estimate_x);
  }

  /***************************卡尔曼滤波结束**********************************/

  /***************************抽稀算法*************************************/
  List<LatLng>? reducerVerticalThreshold(List<LatLng>? inPoints, double threshHold) {
    if (inPoints == null) {
      return null;
    }
    if (inPoints.length <= 2) {
      return inPoints;
    }
    List<LatLng> ret = [];
    for (int i = 0; i < inPoints.length; i++) {
      LatLng? pre = getLastLocation(ret);
      LatLng cur = inPoints[i];
      if (pre == null || i == inPoints.length - 1) {
        ret.add(cur);
        continue;
      }
      LatLng next = inPoints[i + 1];
      double distance = calculateDistanceFromPoint(cur, pre, next);
      if (distance > threshHold) {
        ret.add(cur);
      }
    }
    return ret;
  }

  static LatLng? getLastLocation(List<LatLng>? oneGraspList) {
    if (oneGraspList == null || oneGraspList.length == 0) {
      return null;
    }
    int locListSize = oneGraspList.length;
    LatLng lastLocation = oneGraspList[locListSize - 1];
    return lastLocation;
  }

  /**
   * 计算当前点到线的垂线距离
   * @param p 当前点
   * @param lineBegin 线的起点
   * @param lineEnd 线的终点
   *
   */

  /***************************抽稀算法结束*********************************/

  List<LatLng>? reduceNoisePoint(List<LatLng>? inPoints, double threshHold) {
    if (inPoints == null) {
      return null;
    }
    if (inPoints.length <= 2) {
      return inPoints;
    }
    List<LatLng> ret = [];
    for (int i = 0; i < inPoints.length; i++) {
      LatLng? pre = getLastLocation(ret);
      LatLng cur = inPoints[i];
      if (pre == null || i == inPoints.length - 1) {
        ret.add(cur);
        continue;
      }
      LatLng next = inPoints[i + 1];
      double distance = calculateDistanceFromPoint(cur, pre, next);
      if (distance < threshHold) {
        ret.add(cur);
      }
    }
    return ret;
  }
}

double calculateDistanceFromPoint(LatLng p, LatLng lineBegin, LatLng lineEnd) {
  double A = p.longitude - lineBegin.longitude;
  double B = p.latitude - lineBegin.latitude;
  double C = lineEnd.longitude - lineBegin.longitude;
  double D = lineEnd.latitude - lineBegin.latitude;

  double dot = A * C + B * D;
  double len_sq = C * C + D * D;
  double param = dot / len_sq;

  double xx, yy;

  if (param < 0 || (lineBegin.longitude == lineEnd.longitude && lineBegin.latitude == lineEnd.latitude)) {
    xx = lineBegin.longitude;
    yy = lineBegin.latitude;
//            return -1;
  } else if (param > 1) {
    xx = lineEnd.longitude;
    yy = lineEnd.latitude;
//            return -1;
  } else {
    xx = lineBegin.longitude + param * C;
    yy = lineBegin.latitude + param * D;
  }
  return calculateLineDistance(p.latitude, p.longitude, yy, xx);
}

double calculateDistanceFromPointLatLng(LatLng l1, LatLng l2, LatLng l3) {
  return calculateDistanceFromPoint(l1, l2, l3);
}

double calculateLatLngDistance(LatLng var0, LatLng var1) {
  if (var0 != null && var1 != null) {
    return calculateLineDistance(var0.latitude, var0.longitude, var1.latitude, var1.longitude);
  }
  return 0.0;
}

double calculateLineDistance(double _lat0, double _lon0, double _lat1, double _lon1) {
  try {
    double var2 = _lon0;
    double var4 = _lat0;
    double var6 = _lon1;
    double var8 = _lat1;
    var2 *= 0.01745329251994329;
    var4 *= 0.01745329251994329;
    var6 *= 0.01745329251994329;
    var8 *= 0.01745329251994329;
    double var10 = sin(var2);
    double var12 = sin(var4);
    double var14 = cos(var2);
    double var16 = cos(var4);
    double var18 = sin(var6);
    double var20 = sin(var8);
    double var22 = cos(var6);
    double var24 = cos(var8);
    List<double> var28 = List.filled(3, 0);
    List<double> var29 = List.filled(3, 0);
    var28[0] = var16 * var14;
    var28[1] = var16 * var10;
    var28[2] = var12;
    var29[0] = var24 * var22;
    var29[1] = var24 * var18;
    var29[2] = var20;
    return asin(sqrt(
                (var28[0] - var29[0]) * (var28[0] - var29[0]) + (var28[1] - var29[1]) * (var28[1] - var29[1]) + (var28[2] - var29[2]) * (var28[2] - var29[2])) /
            2.0) *
        1.27420015798544E7;
  } catch (Throwable) {}
  return 0.0;
}