package com.flightfeather.uav.common.location
|
|
import com.flightfeather.uav.domain.entity.BaseRealTimeData
|
import java.math.BigDecimal
|
import kotlin.math.abs
|
import kotlin.math.atan
|
import kotlin.math.cos
|
import kotlin.math.sin
|
|
/**
|
* 走航轨迹分割分类
|
* @date 2024/7/3
|
* @author feiyu02
|
*/
|
object TrackSegment {
|
|
// 坐标点间最小距离，单位米
|
private const val MIN_DISTANCE = 6
|
|
// 两条直线夹角为90度时，认为垂直。实际情况中，角度允许有一定偏差，允许偏差角度
|
private const val VERTICAL_OFFSET_DEG = 45
|
|
/**
|
* 按照道路对走航轨迹进行分割
|
* 此处简化认为当连续的前后两段轨迹趋向于一条直线（平行）时，即为在同一条道路上；当其更趋向于垂直时，即认为车辆进行了转向，进入了新的道路
|
* 具体的计算方法为：
|
* 1. 求出每个坐标点相对于前一个坐标点在经纬度坐标系下偏转的角度（当前后两个坐标点的距离过近时，直接认为其同属于同一路段）；
|
* 2. 依次比较前后两个偏转角度的差值，当差值角度在[45°, 135°]或[225°, 315°]范围内时，认为车辆进行了转向，进入新的道路，否则依旧处于同一道路上
|
*/
|
fun segmentWithRoad(data: List<BaseRealTimeData>): List<List<BaseRealTimeData>> {
|
val records = mutableListOf<MutableList<BaseRealTimeData>>()
|
if (data.isEmpty()) return records
|
// 记录上一组接近平行的坐标点的偏转角度
|
val lastDegList = mutableListOf<Double>()
|
// 记录上一组距离接近的坐标点
|
val closeList = mutableListOf<BaseRealTimeData>()
|
records.add(mutableListOf())
|
data.forEachIndexed { i, d ->
|
// if (records.size == 23) {
|
// println(records.size)
|
// }
|
var isSame = false
|
if (i > 0) {
|
// 前一个有效监测点
|
var lastData = data[i - 1]
|
// 确保两点坐标合法
|
if ((lastData.longitude != null && lastData.longitude != BigDecimal.ZERO)
|
&& (lastData.latitude != null && lastData.latitude != BigDecimal.ZERO)
|
&& (d.longitude != null && d.longitude != BigDecimal.ZERO)
|
&& (d.latitude != null && d.latitude != BigDecimal.ZERO)
|
) {
|
// 计算两点距离，过近时直接认为是同一个点，同一路段
|
var distance = CoordinateUtil.calculateDistance(
|
lastData.longitude!!.toDouble(), lastData.latitude!!.toDouble(),
|
d.longitude!!.toDouble(), d.latitude!!.toDouble(),
|
)
|
// 距离过近时, 将距离替换为当前点和近点集合中的第一个点的距离
|
if (distance < MIN_DISTANCE && closeList.isNotEmpty()) {
|
// 如果已经有距离过近的点集合，则还需要和第一个点进行距离判断，
|
// 解决当车辆行驶速度过低时，连续点的距离都过近导致都判定为同一点的问题
|
val firstCloseData = closeList[0]
|
// lastData = closeList.toList().avg()
|
distance = CoordinateUtil.calculateDistance(
|
firstCloseData.longitude!!.toDouble(), firstCloseData.latitude!!.toDouble(),
|
d.longitude!!.toDouble(), d.latitude!!.toDouble())
|
}
|
|
if (distance >= MIN_DISTANCE) {
|
val deg = CoordinateUtil.getAngle(
|
lastData.longitude!!.toDouble(), lastData.latitude!!.toDouble(),
|
d.longitude!!.toDouble(), d.latitude!!.toDouble(),
|
)
|
isSame = if (lastDegList.isNotEmpty()) {
|
var bool = true
|
|
|
// // 出现角度接近垂直状态的次数
|
// var unSameCount = 0
|
// // 比较当前方位角和上一组每个方位角的差值是否都处于范围内
|
// for (lastDeg in lastDegList) {
|
// val diffDeg = abs(deg - lastDeg)
|
// if (diffDeg in (90.0 - VERTICAL_OFFSET_DEG)..(90.0 + VERTICAL_OFFSET_DEG)
|
// || diffDeg in (270.0 - VERTICAL_OFFSET_DEG)..(270.0 + VERTICAL_OFFSET_DEG)
|
// ) {
|
// unSameCount++
|
// }
|
// }
|
// // 当接近垂直的角度超过上一组平行角度的一半时，认为从该点轨迹转弯（消除个别坐标点由于定位误差导致的错误影响）
|
// bool = unSameCount < (lastDegList.size / 3 + 1)
|
|
val avgDeg = avgDegree(lastDegList)
|
val diffDeg = abs(deg - avgDeg)
|
if (diffDeg in (90.0 - VERTICAL_OFFSET_DEG)..(90.0 + VERTICAL_OFFSET_DEG)
|
|| diffDeg in (270.0 - VERTICAL_OFFSET_DEG)..(270.0 + VERTICAL_OFFSET_DEG)
|
) {
|
bool = false
|
}
|
|
// 当出现转弯点时，清空历史角度，并且舍弃转弯点相对于前一个点的角度（解决一种极端情况，当连续出现转弯点时，当前坐标点会被单独分割为一段）
|
if (!bool) {
|
lastDegList.clear()
|
} else {
|
lastDegList.add(deg)
|
}
|
bool
|
} else {
|
// 当坐标点形成有效路径时，记录为上一个坐标点
|
lastDegList.add(deg)
|
true
|
}
|
closeList.clear()
|
} else {
|
closeList.add(d)
|
isSame = true
|
}
|
}
|
// 否则认为同一路段
|
else {
|
isSame = true
|
}
|
} else {
|
isSame = true
|
}
|
|
if (!isSame)
|
records.add(mutableListOf())
|
records.last().add(d)
|
}
|
|
return records
|
}
|
|
/**
|
* 求转向角度的均值
|
*/
|
private fun avgDegree(degList: List<Double>): Double {
|
if (degList.isEmpty()) return .0
|
//采用单位矢量法求取均值
|
var u = .0//东西方位分量总和
|
var v = .0//南北方位分量总和
|
var c = 0//数据计数
|
|
degList.forEach {
|
val r = Math.toRadians(it)
|
u += sin(r)
|
v += cos(r)
|
c++
|
}
|
|
val avgU = u / c
|
val avgV = v / c
|
var a = atan(avgU / avgV)
|
a = Math.toDegrees(a)
|
/**
|
* avgU>0;avgV>0: 真实角度处于第一象限，修正值为+0°
|
* avgU>0;avgV<0: 真实角度处于第二象限，修正值为+180°
|
* avgU<0;avgV<0: 真实角度处于第三象限，修正值为+180°
|
* avgU<0;avgV>0: 真实角度处于第四象限，修正值为+360°
|
*/
|
a += if (avgV > 0) {
|
if (avgU > 0) 0 else 360
|
} else {
|
180
|
}
|
|
return a
|
}
|
}
|
