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 dummySoldier: Boolean = false,
var dangerLevel: DangerLevelEnum = DangerLevelEnum.NO_DANGEROUS,
)
enum class DetectTargetEnum(var value: Int, var info: String) {
BACKGROUND(0, "Background"),
ANIMAL(1, "Animal"),
BOAT(2, "Boat"),
CAR(3, "Car"),
PERSON(4, "Pedestrian"),
RIDER(5, "Rider"),
VEHICLE(6, "Large Vehicle"),
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"),
TOWERTOP(17, "Tower Head"),
POWERBASE(18, "Power Base"),
}
enum class AiDetectSceneTypeEnum {
SCENE_TYPE_UNIVERSAL
}