大疆無人機實現目標定位

大疆無人機回傳的實時視頻可以幫助飛友觀察發現美景，就稱為目標吧，你偏偏對目標很感興趣，迫切想知道目標的位置，最好是經緯度坐標。你查找大疆Document，欣喜地發現大疆提供了定位機體本身的定位方法（通過FlightController.setStateCallback重寫Update函數即可），不過并沒有定位目標的現成函數。
實際上，屏幕上的像素與實地坐標存在一一對應的關系，找到這個關系就能實現實地目標的定位。
建模條件：①地面為水平平面；②相機視角θ（大疆Z3為94度）；③飛機相對高度droneHeight，經緯度（droneLat，droneLng），機頭朝向droneHeading；④屏幕分辨率deviceWidth、deviceHeight；⑤屏幕以左上角為原點（0,0），向右向下為正；⑥相機垂直地面；⑦不考慮目標的高度和圖像形變。

則任意屏幕的像素點（x,y），對應的實地經緯度坐標為唯一的，假設為（xLng,yLat），從而可以用大疆SDK提供的函數在地圖中標定。
基于上述模型，得到從像素點到實地位置的計算過程：
1、根據飛機高度和相機視角，計算屏幕中的圖像對應實地的長度（Height1）和寬度（Width1）:
Width1=droneHeighttan(Θ/2)
Height1=deviceHeightWidth1/deviceWidth；
2、計算目標像素在屏幕中相對于屏幕中心點的歸一化位置（tmpX ,tmpY）,其中tmpX = droneHeightx2/Width1， tmpY = -y/x*tmpX;

3、根據飛機當前位置和機頭方向，解算實地的頂點坐標：
xLng=droneLng+(tmpXcos(droneHeading)- tmpYsin(droneHeading))M_TO_LAT;
yLat=droneLat+(tmpYcos(droneHeading)+ tmpX*.sin(droneHeading))*M_TO_LAT;
以上可以計算屏幕像素與實地經緯度的一一對應關系。
實現函數也很簡單：

private DJILatLng calcTargetLatLng(LocationCoordinate3D drone3D, PointF pointF,int droneHeading){
final double M_TO_LAT = 360.0/40000000;
        DJILatLng targetLatLng = new DJILatLng(0.0,0.0);
        double tmpX,tmpY;
        double Theta = -droneHeading*Math.PI/180;
        double Height =  drone3D.getAltitude();
        tmpX = Height*pointF.x*2/deviceWidth;
        tmpY = -pointF.y/pointF.x*tmpX;
        targetLatLng.longitude = drone3D.getLongitude() + (tmpX*Math.cos(Theta) - tmpY*Math.sin(Theta))*M_TO_LAT;
        targetLatLng.latitude  = drone3D.getLatitude() + (tmpY*Math.cos(Theta) + tmpX*Math.sin(Theta))*M_TO_LAT;

        return targetLatLng;
    }

參數說明：drone3D是飛機的高度、經度、緯度信息，droneHeading是機頭朝向，以上兩個參數均可以通過在OnCreate函數中，用FlightController.setStateCallback重寫Update函數得到；pointF是目標像素點的位置，通過點擊屏幕獲取。
如果需要，可以通過DJIMap.addMarker將目標標定到地圖中。測試效果，飛機在100米高度，有可能把機動車道標記到慢車道，這個誤差可以接受。