var euls: DoubleArray = DoubleArray(3)
var fov: DoubleArray = DoubleArray(2)
var msl: Double = 0.0
var robot_geo: DoubleArray = DoubleArray(3)
var point_org: DoubleArray = DoubleArray(3)
var camera_matrix: DoubleArray = DoubleArray(4)
var laserDis: Double = 0.0
private val trackListener = object : CommonCallbacks.KeyListener<DetectTrackNotifyBean> {
override fun onValueChange(
oldValue: DetectTrackNotifyBean?,
newValue: DetectTrackNotifyBean,
) {
newValue.objNum
newValue.infoList?.forEach {
it.startX
it.type
}
euls = getEul(DeviceManager.getFirstDroneDevice())
robot_geo = getRobotGeo(DeviceManager.getFirstDroneDevice())
point_org = getHomePoint(DeviceManager.getFirstDroneDevice())
msl = getMSL(DeviceManager.getFirstDroneDevice())
laserDis = getDis(DeviceManager.getFirstDroneDevice())
camera_matrix = getCameraMatrix(newValue.resolutionWidth, newValue.resolutionHeight)
newValue.infoList?.forEach { detectBean ->
val obj = UploadDetectBean.InputObjects()
obj.tracker_id = "${detectBean.objectId}"
obj.cls_id = detectBean.type
obj.bbox = Bbox().apply {
x = detectBean.startX
y = detectBean.startY
w = detectBean.width
h = detectBean.height
}
obj.timestamp = SystemClock.elapsedRealtime()
obj.pic = "None"
val gps = AIJni.getGpsFromScreenWithXY(
euls,
robot_geo,
point_org,
laserDis,
camera_matrix,
msl,
detectBean.startX.toDouble() + detectBean.width / 2f,
detectBean.startY.toDouble() + detectBean.height / 2f
)
obj.pos = GlobalPos().apply {
altitude = gps?.z?.toFloat() ?: 0f
latitude = gps?.x?.toFloat() ?: 0f
longitude = gps?.y?.toFloat() ?: 0f
}
obj.locked = false
objCnts.add(obj)
}
}
}
val key = KeyTools.createKey(AIServiceKey.KeyAiDetectTarget)
AutelLog.i(TAG, "register track listener")
DeviceManager.getDeviceManager().getFirstDroneDevice()?.getKeyManager()?.listen(key, trackListener)
private fun getEul(device: IAutelDroneDevice): DoubleArray {
val eul = DoubleArray(3)
device.getDeviceStateData().flightControlData.let {
eul[0] = it.gimbalAttitudeYaw.toDouble()
eul[1] = it.gimbalAttitudePitch.toDouble()
eul[2] = it.gimbalAttitudeRoll.toDouble()
}
return eul
}
private fun getRobotGeo(device: IAutelDroneDevice): DoubleArray {
val robot_geos = DoubleArray(3)
device.getDeviceStateData().flightControlData.let {
robot_geos[0] = it.droneLatitude
robot_geos[1] = it.droneLongitude
robot_geos[2] = it.altitudeMSL.toDouble()
}
return robot_geos
}
private fun getHomePoint(device: IAutelDroneDevice): DoubleArray {
val homePoints = DoubleArray(3)
device.getDeviceStateData().flightControlData.let {
homePoints[0] = it.homeLatitude
homePoints[1] = it.homeLongitude
homePoints[2] = 0.0
}
return homePoints
}
private fun getDis(device: IAutelDroneDevice): Double {
var laserDis = 0.0
device.getDeviceStateData().flightControlData.let {
laserDis = it.laserDistance.toDouble() / 100.0
}
return laserDis
}
private fun getMSL(device: IAutelDroneDevice): Double {
var msl = 0.0
device.getDeviceStateData().flightControlData.let {
msl = it.altitudeMSL.toDouble()
}
return msl
}
private fun getFovs(device: IAutelDroneDevice): DoubleArray {
val fov = DoubleArray(2)
device.getDeviceStateData().gimbalDataMap[device.getGimbalDeviceType()].let {
fov[0] = it?.cameraData?.cameraInfoList?.first()?.fovH?.toDouble() ?: 0.0
fov[1] = it?.cameraData?.cameraInfoList?.first()?.fovV?.toDouble() ?: 0.0
}
return fov
}
private fun getCameraMatrix(width: Int, height: Int): DoubleArray {
val fovs = getFovs(DeviceManager.getFirstDroneDevice())
return doubleArrayOf(
fovs[0], fovs[1], width.toDouble(), height.toDouble()
)
}
data class DetectTrackNotifyBean(
var resolutionWidth: Int = 0,
var resolutionHeight: Int = 0,
var objNum: Int = 0,
var infoList: List<DetectObjectBean>? = null,
)
data class DetectObjectBean(
var startX: Float = 0f,
var startY: Float = 0f,
var width: Float = 0f,
var height: Float = 0f,
var type: Int = 0,
var status: Int = 0,
var camouflage: Boolean = false,
var gun: Boolean = false,
var tacticalBackpack: Boolean = false,
var pose: PoseTypeEnum = PoseTypeEnum.STANDUP,
var actions: ActionEnum = ActionEnum.COMMENT,
var objectId: Int = 0,
var dummp_soldier: Boolean = false,
var danger_level: DangerLevelEnum = DangerLevelEnum.NO_DANGEROUS,
)
enum class DetectTargetEnum(var value: Int, var info: String) {
BACKGROUND(0, "背景"),
ANIMAL(1, "动物"),
BOAT(2, "船"),
CAR(3, "小车"),
PERSON(4, "行人"),
RIDER(5, "骑行者"),
VEHICLE(6, "大车"),
INSULATOR(10, "Insulated terminal"),
CROSSARMPOINT(11, "Cross arm end hanging point"),
WIREPOINT(12, "Wire end hanging point"),
EARTHWIREPOINT(13, "Ground wire hanging point"),
DAMPER(14, "Vibration hammer"),
POLE(15, "Electric pole"),
TOWER(16, "Tower body"),
TOWER_HEAD(17, "Tower head"),
TOWER_FOOT(18, "Taki"),
}