はじめに

HDRカメラボードは明暗差の激しい環境でも明瞭に写真を撮影できます。このHDRカメラを使えばマーカーをいい感じに検出できるのでは？と思い、マーカー計測装置の作成にチャレンジしました。勉強のためマーカー検出アルゴリズムとか位置姿勢計算は可能な限り自作するようにしてみました。以下に成果物の全体像と動作の様子を示します。

ここにPOSTが表示されます

ここにPOSTが表示されます

ここにPOSTが表示されます

全体構成

以下に使用した主要パーツと全体構成を示します。

品名	用途
Spresense メインボード	主演算装置
Spresense HDRカメラボード	画像取得
Spresense Wi-Fi Add-onボード iS110B	デバッグ用
FEETECHサーボ STS3215	画角制御
FEETECHサーボ用インターフェイスボード FE-URT-1	前記サーボ制御
その辺に転がっていた2S LiPoバッテリー	前記サーボ駆動

機能

本作品の機能について４つに分けて説明します。

マーカー検出
　伝統的な手法を使用する。二値化→ラベリング処理→頂点検出＆マーカーパターンフィルタの流れ。
マーカー位置・姿勢の推定
　マーカー頂点の実寸法とカメラパラメーターを使用して、Perspective 3 Point 問題を解くことでカメラを原点としたマーカーの位置、姿勢を計算する。
ジンバル
　より広い範囲で計測するため、検知したマーカーがカメラの画角中央に来るようにジンバルを制御する。
映像伝送
　デバッグ用。通信速度がそこまで速くないので、二値化した画像を送信する。
ログ保存
　マーカー位置姿勢をSDカードにテキストで保存する。

マーカー検出

マーカー検出を以下の流れで行いました。

二値化

輝度が閾値を超えるか閾値以下か、という最も単純な手法で二値化しました。HDRのおかげなのか分かりませんが、明暗差が厳しめな環境でも良い感じになったと思います。

ラベリング処理

ラベリング処理は、二値化した画像の黒い部分(白い部分でもOK)のまとまった領域をグループ分けする処理のことです。画像を1ピクセルずつ読み込んで、隣り合ったピクセルが白いか黒いかを見て、黒いグループの仲間に入れてもらうかどうかを判断していくようなイメージです。実装については伝統的なラベリング処理の方法で実装しました。ただ、ルックアップテーブルを活用することでスキャン回数を削減する工夫は施しています。
以下に入力画像とその処理結果、ラベリング処理をpythonで実装したコードを示します。入力画像の黒い部分をラベリングするとグループごとに色分けされた処理結果画像が得られます。

このコードは以下のライブラリを使用して作成されています。

OpenCV (cv2): BSD 3-Clause License（https://github.com/opencv/opencv/blob/4.4.0/LICENSE）
NumPy (numpy): BSD License（https://numpy.org/doc/stable/license.html）
各ライブラリのライセンス条項については、それぞれの公式サイトをご参照ください。


ラベリング処理
import cv2
import numpy as np

def label_scan(label_num,label,lut,y,x):
    flg_update = 0
    label_buf = np.zeros(4)
    if x-1 >= 0 and label[y,x-1] != 0:#左
        label_buf[0] = lut[label[y,x-1]]
        flg_update = 1
    if y-1 >= 0 and label[y-1,x] != 0:#上
        label_buf[1] = lut[label[y-1,x]]
        flg_update = 1
    if x-1 >= 0 and y-1 >= 0 and label[y-1,x-1] != 0:#左上
        label_buf[2] = lut[label[y-1,x-1]]
        flg_update = 1
    if x+1 < cv_image.shape[1] and y-1>=0 and label[y-1,x+1] != 0:#右上
        label_buf[3] = lut[label[y-1,x+1]]
        flg_update = 1

    if flg_update == 0:
        label_num = label_num + 1
        label[y,x] = label_num
        return 0 ,int(label_num),label,lut
    else:
        label_min = label_num
        for i in range(len(label_buf)):
            if label_buf[i] != 0 and label_min > label_buf[i]:
                label_min = label_buf[i]
        if x-1 >= 0 and label[y,x-1] != 0:
            lut[label[y,x-1]] = label_min
        if y-1 >= 0 and label[y-1,x] != 0:
            lut[label[y-1,x]] = label_min
        if x-1 >= 0 and y-1 >= 0 and label[y-1,x-1] != 0:
            lut[label[y-1,x-1]] = label_min
        if x+1 < cv_image.shape[1] and y-1>=0 and label[y-1,x+1] != 0:
            lut[label[y-1,x+1]] = label_min
        label[y,x] = label_min

        return 1 ,int(label_num),label,lut
def label_rescan(label_num,label,lut,y,x):
    flg_update = 0
    label_buf = np.zeros(4)
    if x+1 < cv_image.shape[1] and label[y,x+1] != 0:#右
        label_buf[0] = lut[label[y,x+1]]
        flg_update = 1
    if y+1 < cv_image.shape[0] and label[y+1,x] != 0:#下
        label_buf[1] = lut[label[y+1,x]]
        flg_update = 1
    if x+1 < cv_image.shape[1] and y+1 < cv_image.shape[0] and label[y+1,x+1] != 0:#右下
        label_buf[2] = lut[label[y+1,x+1]]
        flg_update = 1
    if x-1 >= 0 and y+1<cv_image.shape[0] and label[y+1,x-1] != 0:#左下
        label_buf[3] = lut[label[y+1,x-1]]
        flg_update = 1

    if flg_update == 0:
        label_num = label_num + 1
        label[y,x] = label_num
        return 0 ,int(label_num),label,lut
    else:
        label_min = label_num
        for i in range(len(label_buf)):
            if label_buf[i] != 0 and label_min > label_buf[i]:
                label_min = label_buf[i]
        if x+1 < cv_image.shape[1] and label[y,x+1] != 0:
            lut[label[y,x+1]] = label_min
        if y+1 < cv_image.shape[0] and label[y+1,x] != 0:
            lut[label[y+1,x]] = label_min
        if x+1 < cv_image.shape[1] and y+1 < cv_image.shape[0] and label[y+1,x+1] != 0:#右下
            lut[label[y+1,x+1]] = label_min
        if x-1 >= 0 and y+1<cv_image.shape[0] and label[y+1,x-1] != 0:#左下
            lut[label[y+1,x-1]] = label_min

        label[y,x] = label_min

        return 1 ,int(label_num),label,lut
if __name__ == "__main__":

    color_map = np.zeros([15,3],dtype=np.uint8)
    color_map[0,:] = np.array([0,0,0])
    color_map[1,:] = np.array([0,155,255])
    color_map[2,:] = np.array([0,255,255])
    color_map[3,:] = np.array([155,255,255])
    color_map[4,:] = np.array([255,155,255])
    color_map[5,:] = np.array([255,155,155])
    color_map[6,:] = np.array([255,255,0])
    color_map[7,:] = np.array([255,155,0])
    color_map[8,:] = np.array([255,0,0])
    color_map[9,:] = np.array([255,90,90])
    color_map[10,:] = np.array([255,255,90])
    color_map[11,:] = np.array([0,255,90])
    color_map[12,:] = np.array([90,255,0])
    color_map[13,:] = np.array([0,90,255])
    color_map[14,:] = np.array([90,0,255])

    filename = "marker_test.bmp"
    cv_image = cv2.imread(filename)
    if cv_image.ndim == 3:
        cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR)
    cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
    cv_image_out = np.zeros([cv_image.shape[0],cv_image.shape[1],3],dtype=np.uint8)
    cv2.imshow("window",cv_image)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

    lut_g = np.zeros(cv_image.shape[0]*cv_image.shape[1],dtype=int)
    lut_g2 = np.zeros(cv_image.shape[0]*cv_image.shape[1],dtype=int)
    for i in range(cv_image.shape[0]*cv_image.shape[1]):
        lut_g[i] = i
        lut_g2[i] = i
    label_g = np.zeros([cv_image.shape[0],cv_image.shape[1]],dtype=int)
    label_counter: int= 0
    for v in range(cv_image.shape[0]):
        for h in range(cv_image.shape[1]):
            if cv_image[v,h] < 80:
                flg_update ,label_counter,label_g,lut_g = label_scan(label_counter,label_g,lut_g,v,h)
            print(label_counter)
    for v in range(cv_image.shape[0]):
        for h in range(cv_image.shape[1]): 
            x = cv_image.shape[1] - 1 - h
            y = cv_image.shape[0] - 1 - v
            if cv_image[y,x] < 80:
                flg_update ,label_counter,label_g,lut_g = label_rescan(label_counter,label_g,lut_g,y,x)
            print(label_counter)
    label_max = np.amax(label_g)
    label_cnt = 0
    label_last = 0
    for i in range(label_counter+1):
        if i != 0:
            if label_last < lut_g[i]:
                label_cnt = label_cnt + 1
            if label_last > lut_g[i]:
                lut_g2[i] = lut_g2[lut_g[i]]
            else:
                label_last = lut_g[i]
                lut_g2[i] = label_cnt
    print(lut_g[0:label_counter])
    print(lut_g2[0:label_counter])
    print("start")
    for h in range(cv_image.shape[1]):
        for v in range(cv_image.shape[0]):
            if label_g[v,h] == 0:
                cv_image_out[v,h,:] = color_map[0]
            else:
                cv_image_out[v,h,:] = color_map[lut_g2[label_g[v,h]]%14+1,:]
                print(lut_g2[label_g[v,h]])
    cv2.imshow("window",cv_image_out)
    cv2.waitKey(0)
    cv2.destroyAllWindows()
    cv2.imwrite("cv_image_out.bmp",cv_image_out)

頂点検出

上図のように、ラベリング領域されたマーカーのX,Y方向の最大・最小それぞれの位置を４つの頂点候補として設定します。つぎに、ラベリング領域を45度回転させた状態でもX,Y方向の最大・最小それぞれの位置を４つの頂点候補として設定し、２つ目の候補とします。これら２候補の頂点が成すそれぞれの四角形(上図の赤線で囲った部分)の面積に対するラベリング領域の実面積の比率(充填率)が設定値に近い方をマーカーの頂点と判断するようにしています。
上図の場合は、右側の方正しく頂点を検出できているということになります。

マーカーパターンフィルタ

下図のように左上頂点を基準に格子を切り、格子の交点の中で中央４点、外側４点の輝度値を抽出し、外側がすべて白色、中央３点が黒色、中央１点が白色の場合はマーカー検出と判断します。同時に中央４点のうち、どこが白色かでマーカーの回転パターン(４パターン)も検出します。

検出している様子を以下動画に示します。回転していたり、多少のノイズがあってもしっかり認識していることが確認できます。

ここにPOSTが表示されます

赤枠が検出したマーカー位置を示します。若干ずれているのは、赤枠表示位置にカメラのレンズ歪み補正を反映しているためです。

マーカー位置・姿勢の推定

以下の流れでマーカー位置・姿勢推定を行いました。

レンズ歪み係数の取得

レンズ歪みは以下の画像を見るとわかりますが、本来直線に写るべきものがレンズ特性によって湾曲して見えます。この歪み具合を数値化して湾曲したものをまっすぐ写るようにすることをレンズ歪み補正と呼ぶことにします。最近では、opencvで容易に補正した画像を得ることができます。しかし、opencvのcalibrateCamera関数で得られた補正係数を使用して、特定の画素位置単体での補正位置を得るためには繰り返し計算を実行する必要があります。これはcalibrateCamera関数が出力する補正係数が「補正後画像をどのように変形したら補正前画像になるか」を表していることに起因すると思われます。補正画像を生成する場合にはその方が高速に計算を済ませることができるのですが、今回は「補正前画像をどのように変形したら補正後画像になるか」を計算したいので繰り返し計算を回避するためにはcalibrateCamera関数の補正係数をそのまま使用することはできません。
　そこで、Spresense起動時に補正前画像のピクセル位置と補正後画像のピクセル位置を紐づけるマップを計算し、動作中はこのマップを参照することでレンズ歪みを補正するようにしました。
　opencvのカメラキャリブレーションに方法については公式のCamera Calibrationのページを参照してください。
　以下にキャリブレーションで得られた補正係数と使用したスクリプトを示します。

パラメータ	数値
$k_1$	21.7717354533507[-]
$k_2$	808.114465113412 [-]
$p_1$	0.00962848702938[-]
$p_2$	-0.02168948091617[-]
$k_3$	3990.29471654949 [-]
$k_4$	22.9632941293542[-]
$k_5$	784.599039778564 [-]
$k_6$	4676.72996735185 [-]

このコードは以下のライブラリを使用して作成されています。

glob: Python標準ライブラリ（Python Software Foundation Licenseに基づいて提供）
OpenCV (cv2): BSD 3-Clause License（https://github.com/opencv/opencv/blob/4.4.0/LICENSE）
NumPy (numpy): BSD License（https://numpy.org/doc/stable/license.html）
各ライブラリのライセンス条項については、それぞれの公式サイトをご参照ください。


カメラキャリブレーション
import numpy as np
import cv2
import glob

TMP_FOLDER_PATH = "./calib/"
MTX_PATH = TMP_FOLDER_PATH + "mtx.csv"
DIST_PATH = TMP_FOLDER_PATH + "dist.csv"
def main():
    calcCamera()

def calcCamera():
    square_size = 24.0 
    pattern_size = (6, 4) 
    pattern_points = np.zeros( (np.prod(pattern_size), 3), np.float32 )
    pattern_points[:,:2] = np.indices(pattern_size).T.reshape(-1, 2)
    pattern_points *= square_size
    obj_points = []
    img_points = []
 
    for fn in glob.glob(TMP_FOLDER_PATH + "img2/*"):
        print(fn)
        im = cv2.imread(fn)
        print(im.shape)
        im = cv2.cvtColor(im,cv2.COLOR_BGR2GRAY)
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10, 0.001)
        found, corner = cv2.findChessboardCorners(im, pattern_size)

        if found:
            corners2 = cv2.cornerSubPix(im, corner, (5,5), (-1,-1), criteria)
        if not found:
            continue
        img_points.append(corners2.reshape(-1, 2))
        obj_points.append(pattern_points)
 
    rms, K, D, r, t = cv2.calibrateCamera(obj_points, img_points, (im.shape[1],im.shape[0]),None,None,flags=cv2.CALIB_RATIONAL_MODEL)

    print ("RMS = ", rms)
    print ("K = \n", K)
    print ("d = ", D.ravel())

    saveCalibrationFile(K, D, MTX_PATH, DIST_PATH)
    for fn in glob.glob(TMP_FOLDER_PATH + "img2/*"):

        print(fn)
        im = cv2.imread(fn)
        newcameramtx, roi = cv2.getOptimalNewCameraMatrix(K, D, (im.shape[0],im.shape[1]), 1, (im.shape[0],im.shape[1]))
        dst = cv2.undistort(im, K, D, None, newcameramtx)
        print(fn.replace("./calib/img2","./calib/output"))
        cv2.imwrite(fn.replace("./calib/img2","./calib/output"),dst)

def saveCalibrationFile(mtx, dist, mtx_path, dist_path):
    np.savetxt(mtx_path, mtx, delimiter =',',fmt="%0.14f")
    np.savetxt(dist_path, dist, delimiter =',',fmt="%0.14f")


if __name__ == '__main__':
    print("start")
    main()

以下に補正前画像、opencvで補正した画像画像を示します。

カメラ内部パラメータの取得

calibrateCamera関数によって前述のレンズ歪み係数と同時に内部パラメーターも得られるのでこの値を使用します。
以下に使用した内部パラメータを示します。

パラメータ	数値
x軸焦点距離 fx	251.838752352578 [pixel]
y軸焦点距離 fy	250.68958306935 [pixel]
x軸光学中心 cx	96.731883713966 [pixel]
y軸光学中心 cy	43.9976611433349 [pixel]

Perspective 3 Point 問題の計算

P3P(Perspective 3 Point) 問題はコンピュータビジョンやロボティクスの分野で扱われる幾何学的な問題の一つで、カメラの位置と向きを特定するために用いられます。この問題は、三次元空間に既知の3点と、カメラ画像上のその対応する2次元点が与えられたときに、カメラの位置（3次元座標）と向き（姿勢）を求める問題です。
P3P問題は19世紀後半から20世紀初頭あたりから研究されてきた分野で、解き方にはいくつか流派というか種類がありますが、今回はM.A. Fischler and R.C. Bolles, “Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography,”Commun. ACM, vol.24, no.6, pp.381–395, June 1981.の論文を参考に実装します。

地球固定座標系の原点を $O$ 、カメラ座標系の原点を $O^c$ とします。上付き文字に $c$ が付く場合はカメラ座標系基準のベクトルとし、なにもつかないときは地球固定座標系基準のベクトルとします。
まず、マーカーの頂点 $i \,$ $(i=1,2,3)$ について、カメラ光学中心からの位置を $P^c_i$ とすると、頂点の成す各辺の長さは以下の式で表されます。

$a=||P^c_2-P^c_3||$

$b=||P^c_1-P^c_3||$

$c=||P^c_1-P^c_2||$

焦点距離で正規化された画像上のマーカー頂点 $i \,$ $(i=1,2,3)$ について、画像上での位置を $u_i,v_i$ とすると、カメラ光学中心から画像上のマーカー頂点までのベクトルは以下のように表されます。

$q_i=\begin{bmatrix} u_i \\ v_i \\ 1\end {bmatrix} where\,i=1,2,3$

$P^c_i$ の大きさを $s_i$ 、単位ベクトルを $j_i$ としたとき、 $u_i,v_i$ を用いて $P^c_i$ を表現すると以下のようになります。

$P^c_i=s_ij_i=\frac{s_i}{\sqrt{u^2_i+v^2_i+1}}\begin{bmatrix} u_i \\ v_i \\ 1\end {bmatrix}where\,i=1,2,3$

$P^c_i$ のそれぞれが成す角について、 $\alpha,\beta,\gamma$ とすると以下のように表現できます。

$\cos{\alpha}=j_2 \cdot j_3$

$\cos{\beta}=j_1 \cdot j_3$

$\cos{\gamma}=j_1 \cdot j_2$

以上を踏まえて余弦定理の式を使うと以下の式が得られます。

$s^2_1+s^2_2-2s_1s_2\cos{\gamma}=c^2$

$s^2_1+s^2_3-2s_1s_3\cos{\beta}=b^2$

$s^2_2+s^2_3-2s_2s_3\cos{\alpha}=a^2$

ここで、 $s_2,s_3$ を $s_1$ に対する比で表現するため以下の $u,v$ を導入します。

$s_2=us_1$

$s_3=vs_1$

以上の式を駆使すると、以下の $u$ を変数とした四次方程式が得られます。

$B_4u^4+B_3u^3+B_2u^2+B_1u+B_0=0$

$B_4=4b^2c^2|cos{\alpha}-(a^2-b^2-c^2)$
$B_3=-4c^2(a^2+b^2-c^2)\cos{\alpha}\cos{\beta}-8b^2c^2\cos^2{\alpha}\cos{\gamma}+4(a^2-b^2-c^2)(a^2-b^2)\cos{\gamma}$
$B_2=4c^2(a^2-c^2)\cos^2{\beta}+8c^2(a^2+b^2)\cos{\alpha}\cos{\beta}\cos{\gamma}+4c^2(b^2-c^2)\cos^2{\alpha}-2(a^2-b^2-c^2)(a^2-b^2+c^2)-4(a^2-b^2)^2\cos^2{\gamma}$
$B_1=-8a^2c^2\cos^2{\beta}\cos{\gamma}-4c^2(b^2-c^2)\cos{\alpha}\cos{\beta}-4a^2c^2\cos{\alpha}\cos{\beta}+4(a^2-b^2)(a^2-b^2+c^2)\cos{\gamma}$
$B_0=4a^2c^2\cos^2{\beta}-(a^2-b^2+c^2)^2$
四次方程式なので、 $u$ の実数解は最大４つ得られます。
$v$ については、以下の式から求まります。

$v=-\frac{(a^2-b^2-c^2)u^2+2(b^2-a^2)\cos{\gamma}u+a^2-b^2+c^2}{2c^2(\cos{\alpha}u-\cos{\beta})}$

$s_1$ については、以下の式から求まり、 $s_2,s_3$ は $u,v,s_1$ から求まります。

$s_1=\sqrt{\frac{c^2+2s_1s_2\cos{\gamma}}{1+u^2}}$

よって、 $P^c_i$ は以下の式で求まります。

$P^c_i=s_ij_i=\frac{s_i}{\sqrt{u^2_i+v^2_i+1}}\begin{bmatrix} u_i \\ v_i \\ 1\end {bmatrix}where\,i=1,2,3$

次に、姿勢角について導出します。
$P_1 \to P_2$ の方向をY軸、 $P_1,P_2,P_3$ が成す平面の法線ベクトルをZ軸、Y、Z軸の成す法線ベクトルをX軸とし、 $P_1$ を原点とする座標系 $O^p$ を設定します。座標系 $O^c$ に対する $O^p$ の姿勢を表す回転行列 $R^c_p$ はXYZ軸の単位ベクトル $e^c_x,e^c_y,e^c_z$ で以下のように表現できます。

$e^c_y=\frac{P^c_2-P^c_1}{||P^c_2-P^c_1||}$

$e^c_z=\frac{(P^c_3-P^c_1)\times (P^c_2-P^c_1)}{||(P^c_3-P^c_1)\times (P^c_2-P^c_1)||}$

$e^c_x=\frac{e^c_y\times e^c_z}{||e^c_y\times e^c_z||}$

$R^c_p=\begin{bmatrix} e^c_x&e^c_y&e^c_z\end {bmatrix}=\begin{bmatrix} e^c_{xx}&e^c_{yx}&e^c_{zx} \\ e^c_{xy}&e^c_{yy}&e^c_{zy}\\ e^c_{xz}&e^c_{yz}&e^c_{zz}\end {bmatrix}$

同様に座標系 $O$ に対する $O^p$ の姿勢を表す回転行列 $R_p$ はXYZ軸の単位ベクトル $e_x,e_y,e_z$ で以下のように表現できます。

$e_y=\frac{P_2-P_1}{||P_2-P_1||}$

$e_z=\frac{(P_3-P_1)\times (P_2-P_1)}{||(P_3-P_1)\times (P_2-P_1)||}$

$e_x=\frac{e_y\times e_z}{||e_y\times e_z||}$

$R_p=\begin{bmatrix} e_x&e_y&e_z\end {bmatrix}=\begin{bmatrix} e_{xx}&e_{yx}&e_{zx} \\ e_{xy}&e_{yy}&e_{zy}\\ e_{xz}&e_{yz}&e_{zz}\end {bmatrix}$

つぎに、カメラ座標系 $O^c$ 基準のマーカーの位置 $P^c_{O^p}$ ・姿勢 $R^c_p$ を求めるにはどうすればよいでしょうか？実はすでに求まっていて、以下のように求まります。

$P^c_{O^p}=P^c_1=s_1j_1=\frac{s_1}{\sqrt{u^2_1+v^2_1+1}}\begin{bmatrix} u_1 \\ v_1 \\ 1\end {bmatrix}$

$R^c_p=\begin{bmatrix} e^c_x&e^c_y&e^c_z\end {bmatrix}=\begin{bmatrix} e^c_{xx}&e^c_{yx}&e^c_{zx} \\ e^c_{xy}&e^c_{yy}&e^c_{zy}\\ e^c_{xz}&e^c_{yz}&e^c_{zz}\end {bmatrix}$

この回転行列 $R^c_p$ をZYXオイラー角に変換すると以下のようになります。

$\theta_x = \arctan{(\frac{e^c_{zy}}{e^c_{zz}})}\qquad\,\,\,(\cos{\theta_y} \neq 0 )$

$\theta_x = 0\qquad\qquad\qquad\quad\,(\cos{\theta_y} = 0 )$

$\theta_y=\arcsin{(-e^c_{zx})}\qquad\qquad\qquad\quad$

$\theta_z = \arctan{(\frac{e^c_{yx}}{e^c_{xx}})}\qquad\, \, \,(\cos{\theta_y} \neq 0)$

$\theta_z = \arctan{(-\frac{e^c_{xy}}{e^c_{yy}})} \qquad(\cos{\theta_y} = 0)$

このあと、マーカー頂点を再投影するために、地球固定座標系 $O$ 基準のカメラの位置・姿勢を求めたいです。座標系 $O$ に対する $O^c$ の位置 $P_{O^c}$ ・姿勢 $R_c$ は以下のように表現できます。

$\vec{0}=R^c_pP^p_{O^c}+P^c_{O^p}$

$P^p_{O^c}={R^c_p}^T(\vec{0}-P^c_{O^p})$

$P_{O^c}=R_pP^p_{O^c}+P_{O^p}$

$P_{O^c}=R_p{R^c_p}^T(\vec{0}-P^c_{O^p})+P_{O^p}$

$R_c={R^c_p}^TR_p=\begin{bmatrix} m_{xx}&m_{yx}&m_{zx} \\ m_{xy}&m_{yy}&m_{zy}\\ m_{xz}&m_{yz}&m_{zz}\end {bmatrix}$

投影するのは簡単で、透視投影変換の式をそのまま計算すればよいです。簡単のため、地球固定座標系 $O$ の原点をマーカー基準 $O^p$ と同じということにして、投影する点を $P_4$ とすると、以下の式から再投影点 $u_4,v_4$ が求まります。

$s\begin{bmatrix} u_4 \\ v_4 \\ 1\end {bmatrix}=\begin{bmatrix} f_x &0&c_x\\ 0 &f_y&c_y\\ 1&0&0\end {bmatrix}\begin{bmatrix} m_{xx}&m_{yx}&m_{zx}& P_{O^cx}\\ m_{xy}&m_{yy}&m_{zy}& P_{O^cy}\\ m_{xz}&m_{yz}&m_{zz}& P_{O^cz}\end {bmatrix}\begin{bmatrix} P_{4x} \\ P_{4y} \\ P_{4z}\\ 1\end {bmatrix}$

P3Pを解くことで再投影点は最大４パターン計算されますが、これらを検出頂点位置と比較して差分の小さいパターンの位置・姿勢を計測します。以下の動画に動作の状況を示します。

ここにPOSTが表示されます

赤枠の右上に青と緑の点がちょろちょろしていますが、これが再投影点になります。
注意点というか、ハマったところですが、<Complex.h>を使用したコードをコンパイルしようとしたところ、すでにComplex.hが予約されていて、コンパイルできませんでした。リンクのgithubページのNoteにも記載がありますが、ライブラリ本体のフォルダ名を「CComplex」に変更し、ソースコード中の「Complex」と記載の箇所を「CComplex」に書き換えることで使用できるようになりました。

ジンバルを含む座標変換

グローバル座標系からのマーカー位置・姿勢が欲しいので、ジンバルの回転によってカメラ座標系の位置と姿勢が移動することを考慮する必要があります。
以下にジンバル座標系と座標変換の概要図を示します。

座標変換の流れの図で示す青色文字は設計値、赤文字は計測値、オレンジ文字は最終的に求めたい値です。
$p_p$ について、 $q_s$ はグローバル座標系原点をジンパル座標系を一致させることで $0$ とします。 $R_s$ はジンバルを構成するサーボ回転角によって与えられます。サーボはx軸回転角が $\phi$ ,y軸回転角が $\theta$ とし、z軸は $\psi$ ですが、z軸サーボがないので0になります。よって、 $R_s$ は

$R_s= R_z(0) \cdot R_y(\theta) \cdot R_x(\phi)$

となります。
$p^s_c$ は、ジンバル座標系に対して、カメラ座標系原点がどれだけオフセットしているかを示すものです。上図に示すジンバル座標系の概要図に示す通り、z軸方向に0.019mオフセットしているので、

$p^s_c=\begin{bmatrix} 0 \\ 0 \\ 0.019\end {bmatrix}$

となります。
$R^s_c$ は、ジンバル座標系に対するカメラ座標系の姿勢ですが、こちらは上図に示すジンバル座標系の概要図に示す通り、平行移動しているだけなので姿勢変化なしのため以下の通り単位行列 $E$ になります。

$R^s_c= R_z(0) \cdot R_y(0) \cdot R_x(0)=E$

$p^c_p$ は、前述のP3P問題で計算した $P^c_{O^p}$ のことであるため、 $P^c_{O^p}$ の計算値を参照します。
以上より、 $p_p$ は以下の通り計算できます。

$p_p=R_s(p^s_c+p^c_p)$

$R_p$ については、前述の $R_s$ とカメラ座標系に対するマーカー座標系の姿勢 $R^c_p$ を掛け算するだけです。
$R^c_p$ は前述のP3P問題で計算した計算値を参照します。
以上より、 $R_p$ は以下の通り計算できます。

$R_p=R_sR^c_p$

これでグローバル座標系から見たマーカーの位置・姿勢が計算できました。

性能評価

それでは計測した結果を見てみます。以下に冒頭で紹介した動画をもう一度示します。

ここにPOSTが表示されます

この映像を取得した際の計測環境、マーカー位置・姿勢のグラフを以下に示します。

見えづらいですが、青色プロットがX軸、赤色プロットがY軸、緑色プロットがZ軸のデータを示します。
計測値の更新周期は、画像の写り方次第ではありますが、平均して70ms程度でした。
精度についてですが、マーカー位置のX,Y軸については、安定しており、精度も悪くない結果が得られましたが、Z軸方向はばらつきが大きいように見えます。姿勢については、Z軸は安定しており、精度も悪くない結果が得られましたが、X,Y軸にばらつきが大きいように見えます。
コンピュータビジョンの専門家ではないので詳しい考察は省きますが、画像の解像度はQQVGAであるため、1pixelのズレが計算結果に大きな影響を与えた可能性も考えられました。

映像伝送

Spresense Wi-Fi Add-onボード iS110Bを使用してUDP通信でノートPCに二値化画像とマーカー頂点位置を送信しております。前項で示した動画は送信された画像をキャプチャしたものになります。位置姿勢データは、SpresenseからノートPCに送信しておりませんが、ノートPC側でマーカー頂点位置から位置姿勢を計算し、動画上に描画しております。この数値がSpresenseで計算したものと一致するようにデバッグを行いました。

ログ保存

SDカードにテキストでタイムスタンプ、位置、姿勢を保存しています。前項グラフに表示したデータはこの保存データを使用して作成しております。

Spresense ソースコード

メインコア

このプログラムは以下のライブラリを使用して作成されています。

<MP.h>``<Camera.h>``<SDHCI.h>``<File.h>:
- GNU Lesser General Public License (LGPL) v2.1 （https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html）を参照
- 詳細: https://developer.sony.com/develop/spresense/
- このライブラリの著作権: Copyright 2019 Sony Semiconductor Solutions Corporation.
<Complex.h>:
- MIT License
- 詳細: https://github.com/RobTillaart/Complex
- このライブラリの著作権: Copyright (c) 2013-2024 Rob Tillaart
  各ライブラリのライセンス条項については、それぞれの公式サイトをご参照ください。

メイン処理


main.ino
#ifdef SUBCORE
#error "Core selection is wrong!!"
#endif

#include "init.h"
static void led_onoff(int num, bool stat) {
  switch (num) {
    case 0:
      digitalWrite(LED0, stat);
      break;
    case 1:
      digitalWrite(LED1, stat);
      break;
    case 2:
      digitalWrite(LED2, stat);
      break;
    case 3:
      digitalWrite(LED3, stat);
      break;
  }
}
void IntervalSet(float interupt_time) {  //タイマー初期化
  attachTimerInterrupt(timer_1, uint64_t(interupt_time * 1000000));
}

uint64_t timer_1() {  //タイマー関数
  t = t + dt;
  cnt0 = cnt0 + 1;
  cnt1 = cnt1 + 1;
  cnt2 = cnt2 + 1;
  cnt3 = cnt3 + 1;
  cnt4 = cnt4 + 1;
  if (cnt1 > (uint16_t)(cnt1_time / dt)) {  //TBD
    cnt1_flag = !cnt1_flag;
    cnt1 = 0;
  }
  if (cnt2 > (uint16_t)(cnt2_time / dt)) {  //UDP送信タイミングカウンタ
    cnt2_flag = HIGH;
    cnt2 = 0;
  }
  if (cnt3 > (uint16_t)(cnt3_time / dt)) {  //TBD
    cnt3_flag = HIGH;
    cnt3 = 0;
  }
  if (cnt4 > (uint16_t)(cnt4_time / dt)) {  //TBD
    cnt4_flag = HIGH;
    cnt4 = 0;
  }
  return uint64_t(dt * 1000000);
}


static void led_effect(void) {
  static int cur = 0;
  int i;
  static bool direction = true;  // which way to go
  for (i = -1; i < 5; i++) {
    if (i == cur) {
      led_onoff(i, true);
      if (direction)
        led_onoff(i - 1, false);
      else
        led_onoff(i + 1, false);
    }
  }
  if (direction) {  // 0 -> 1 -> 2 -> 3
    if (++cur > 4)
      direction = false;
  } else {
    if (--cur < -1)
      direction = true;
  }
}

/**
 * Print error message
 */

void printError(enum CamErr err) {
  Serial.print("Error: ");
  switch (err) {
    case CAM_ERR_NO_DEVICE:
      Serial.println("No Device");
      break;
    case CAM_ERR_ILLEGAL_DEVERR:
      Serial.println("Illegal device error");
      break;
    case CAM_ERR_ALREADY_INITIALIZED:
      Serial.println("Already initialized");
      break;
    case CAM_ERR_NOT_INITIALIZED:
      Serial.println("Not initialized");
      break;
    case CAM_ERR_NOT_STILL_INITIALIZED:
      Serial.println("Still picture not initialized");
      break;
    case CAM_ERR_CANT_CREATE_THREAD:
      Serial.println("Failed to create thread");
      break;
    case CAM_ERR_INVALID_PARAM:
      Serial.println("Invalid parameter");
      break;
    case CAM_ERR_NO_MEMORY:
      Serial.println("No memory");
      break;
    case CAM_ERR_USR_INUSED:
      Serial.println("Buffer already in use");
      break;
    case CAM_ERR_NOT_PERMITTED:
      Serial.println("Operation not permitted");
      break;
    default:
      break;
  }
}

/**
 * Callback from Camera library when video frame is captured.
 */

void CamCB(CamImage img) {
  /* Check the img instance is available or not. */
  if (img.isAvailable()) {
    int8_t msgid = 10;
    if (f_read == 1) {
      img.convertPixFormat(CAM_IMAGE_PIX_FMT_GRAY);
      frame_num = frame_num + 1;
      memcpy(image_data, img.getImgBuff(), img.getImgSize());
      gimbal_x = st.ReadPos(10);
      gimbal_y = st.ReadPos(11);
      f_read = 0;
      led_effect();
    }
  } else {
    Serial.println("Failed to get video stream image");
  }
}
void mot_ctrl(uint16_t x, uint16_t y) {
  float eangle_x = atan2((float)(x - image_h / 2.0f), mtx_coeff[0]) / M_PI * 180.0f;
  float x_deg = (float)gimbal_x / 4095.0f * 360.0f - 180.0f;
  if (abs(eangle_x) < 0.0f) {
    eangle_x = 0.0f;
  }
  float x_speed = abs(eangle_x);
  short xpos = (short)((x_deg + eangle_x + 180.0f) / 360.0f * 4095.0f);
  if (xpos > 4095) {
    xpos = 4095;
  }
  if (xpos < 0) {
    xpos = 0;
  }

  float eangle_y = atan2((float)(y - image_v / 2.0f), mtx_coeff[1]) / M_PI * 180.0f;
  float y_deg = (float)gimbal_y / 4095.0f * 360.0f - 180.0f;
  if (abs(eangle_y) < 0.0f) {
    eangle_y = 0.0f;
  }
  float y_speed = abs(eangle_y);
  short ypos = (short)((y_deg - eangle_y + 180.0f) / 360.0f * 4095.0f);
  if (ypos > (short)((30.0f + 180.0f) / 360.0f * 4095.0f)) {
    ypos = (short)((30.0f + 180.0f) / 360.0f * 4095.0f);
  }
  if (ypos < (short)((-30.0f + 180.0f) / 360.0f * 4095.0f)) {
    ypos = (short)((-30.0f + 180.0f) / 360.0f * 4095.0f);
  }
  byte ID[2];
  s16 Position[2];
  u16 Speed[2];
  byte ACC[2];
  ID[0] = 10;
  ID[1] = 11;
  Position[0] = xpos;
  Position[1] = ypos;
  Speed[0] = (uint16_t)(x_speed * 500.0f);
  Speed[1] = (uint16_t)(y_speed * 500.0f);
  ACC[0] = 60;
  ACC[1] = 60;
  st.SyncWritePosEx(ID, 2, Position, Speed, ACC);
}
void read_cycle() {
  if (f_read == 0) {
    binarize(th_binarize);
    if (marker_detect()) {
      calc_3dpos();
      mot_ctrl((mkpos[0] + mkpos[2] + mkpos[4] + mkpos[6]) / 4, (mkpos[1] + mkpos[3] + mkpos[5] + mkpos[7]) / 4);
      char buffer[300];
      snprintf(buffer, 300, "%ld,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f", cnt0, mktc[0], mktc[1], mktc[2], mkrpyc[0], mkrpyc[1], mkrpyc[2], mkrotc[0], mkrotc[1], mkrotc[2], mkrotc[3], mkrotc[4], mkrotc[5], mkrotc[6], mkrotc[7], mkrotc[8]);
      myFile.println(buffer);
    }
    if (packet.status == 0) {
      int8_t msgid = 10;
      memcpy(packet.data_buf, image_bdata, sizeof(image_bdata));
      memcpy(packet.mkpos, mkpos, sizeof(mkpos));
      packet.data_size = sizeof(image_bdata);
      packet.status = 1;
      subcore = 1;
      MP.Send(msgid, &packet, subcore);
    }
    f_read = 1;
  }
}

void initCamera() {
  CamErr err;
  Serial.println("Prepare camera");
  err = theCamera.begin(2,
                        CAM_VIDEO_FPS_30,
                        image_h,
                        image_v,
                        CAM_IMAGE_PIX_FMT_YUV422);
  if (err != CAM_ERR_SUCCESS) {
    printError(err);
  }
  // カメラストリームを受信したら CamCBを実行する
  Serial.println("Start streaming");
  err = theCamera.startStreaming(true, CamCB);
  if (err != CAM_ERR_SUCCESS) {
    printError(err);
  }
  // ホワイトバランスの設定
  Serial.println("Set Auto white balance parameter");
  err = theCamera.setAutoWhiteBalanceMode(CAM_WHITE_BALANCE_AUTO);
  if (err != CAM_ERR_SUCCESS) {
    printError(err);
  }
  // ISOの設定
  Serial.println("Set ISO Sensitivity");
  err = theCamera.setAutoISOSensitivity(true);
  if (err != CAM_ERR_SUCCESS) {
    printError(err);
  }

  // HDRの設定
  Serial.println("Set HDR");
  err = theCamera.setHDR(CAM_HDR_MODE_ON);
  if (err != CAM_ERR_SUCCESS) {
    printError(err);
  }

  err = theCamera.setAutoExposure(true);
  if (err != CAM_ERR_SUCCESS) {
    printError(err);
  }
  // int exposure = 10;
  // err = theCamera.setAbsoluteExposure(exposure);
  // if (err != CAM_ERR_SUCCESS) {
  //   printError(err);
  // }
}
void save_cycle() {
  if (cnt2_flag) {
    myFile.close();
    myFile = SD.open("dir/test.txt", FILE_WRITE);
    cnt2_flag = 0;
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(CONSOLE_BAUDRATE);
  Serial2.begin(CONSOLE_BAUDRATE, SERIAL_8N1);
  st.pSerial = &Serial2;
  // init the SD card
  if (!SD.begin()) {
    Serial.println("Card failed, or not present");
    // don't do anything more:
    while (1)
      ;
  }
  SD.mkdir("dir/");
  myFile = SD.open("dir/test.txt", FILE_WRITE);
  myFile.println("marker tracking");
  /* Close the file */
  myFile.close();
  //subcore1起動
  subcore = 1;
  int ret = 0;
  ret = MP.begin(subcore);
  if (ret < 0) {
    printf("MP.begin(%d) error = %d\n", subcore, ret);
  }
  //初期設定時のコア間通信はブロッキング。後でポーリングに再設定する。
  MP.RecvTimeout(MP_RECV_BLOCKING);

  calc_undistmap();

  //subcore1準備完了待ち
  int8_t msgid;
  void* adder;
  subcore = 1;
  MP.Recv(&msgid, &adder, subcore);
  MP.Send(msgid, &adder, subcore);  //subcore1にmaincore準備完了送信

  memset(&packet, 0, sizeof(packet));  //送信用構造体初期化

  MP.RecvTimeout(MP_RECV_POLLING);  //コア間通信をポーリングに変更

  initCamera();

  affin_rot45();

  IntervalSet(dt);  //サイクル開始
}

void loop() {
  save_cycle();
  read_cycle();
}

画像処理関係


cv.ino
void binarize(uint8_t t) {
  int i;
  /* 画像を２値化 */
  memset(image_bdata, 0, sizeof(image_bdata));
  for (i = 0; i < sizeof(image_data); i++) {
    int byteIndex = i / 8;  // 現在のuint8_tのインデックス
    int bitIndex = i % 8;   // 現在のビット位置
    if (image_data[i] > t) {
      image_bdata[byteIndex] |= (1 << bitIndex);  // 指定のビットを1にする
    }
  }
}


void affin_rot45() {
  int x, y;
  for (y = 0; y < image_v; y++) {
    for (x = 0; x < image_h; x++) {
      image_rot45table_x[y * image_h + x] = (float)x * rangle45deg - (float)y * rangle45deg;
      image_rot45table_y[y * image_h + x] = (float)x * rangle45deg + (float)y * rangle45deg;
    }
  }
}

void calc_undistmap() {
  int x, y;
  for (y = 0; y < image_v; y++) {
    for (x = 0; x < image_h; x++) {
      float x1 = (x - mtx_coeff[2]) / mtx_coeff[0];
      float y1 = (y - mtx_coeff[3]) / mtx_coeff[1];
      float x2 = x1;
      float y2 = y1;
      float dx = 0.0f;
      float dy = 0.0f;
      float r = sqrt(x2 * x2 + y2 * y2);
      float eps = 0.1f;
      for (int i = 0; i < 100; i++) {
        float x3 = x2;
        float y3 = y2;
        r = 0.9f * r + 0.1f * sqrt(x2 * x2 + y2 * y2);
        float fu = 1.0f + dist_coeff[0] * pow(r, 2.0f) + dist_coeff[1] * pow(r, 4.0f) + dist_coeff[4] * pow(r, 6.0f);
        float fl = 1.0f + dist_coeff[5] * pow(r, 2.0f) + dist_coeff[6] * pow(r, 4.0f) + dist_coeff[7] * pow(r, 6.0f);
        dx = dist_coeff[2] * 2 * x1 * y1 + dist_coeff[3] * (pow(r, 2.0f) + 2 * pow(x1, 2.0f));
        dy = dist_coeff[3] * 2 * x1 * y1 + dist_coeff[2] * (pow(r, 2.0f) + 2 * pow(y1, 2.0f));
        x2 = (x1 - dx) * fl / fu;
        y2 = (y1 - dy) * fl / fu;
        if (sqrt(pow(x2 - x3, 2.0f) + pow(y2 - y3, 2.0f)) < eps) {
          break;
        }
      }
      undistmap_x[y * image_h + x] = (uint16_t)(x2 * mtx_coeff[0] + mtx_coeff[2]);
      undistmap_y[y * image_h + x] = (uint16_t)(y2 * mtx_coeff[1] + mtx_coeff[3]);
    }
  }
}

uint16_t search_8neighbors(uint16_t num, uint16_t x, uint16_t y) {
  uint16_t nlabel[4] = { 0, 0, 0, 0 };
  uint8_t flg_update = 0;

  if (y - 1 >= 0 && label[(y - 1) * image_h + x] != 0) {  //上
    nlabel[0] = lut[label[(y - 1) * image_h + x]];
    flg_update = 1;
  }
  if (x - 1 >= 0 && label[y * image_h + (x - 1)] != 0) {  //左
    nlabel[1] = lut[label[y * image_h + (x - 1)]];
    flg_update = 1;
  }
  if (y - 1 >= 0 && x - 1 >= 0 && label[(y - 1) * image_h + (x - 1)] != 0) {  //左上
    nlabel[2] = lut[label[(y - 1) * image_h + (x - 1)]];
    flg_update = 1;
  }
  if (y - 1 >= 0 && x + 1 < image_h && label[(y - 1) * image_h + (x + 1)] != 0) {  //右上
    nlabel[3] = lut[label[(y - 1) * image_h + (x + 1)]];
    flg_update = 1;
  }

  if (flg_update == 0) {
    num = num + 1;
    label[y * image_h + x] = num;
    return num;
  } else {
    uint16_t min = image_h * image_v;
    for (uint16_t i = 0; i < 4; i++) {
      if (nlabel[i] != 0 && nlabel[i] < min) {
        min = nlabel[i];
      }
    }
    for (uint16_t i = 0; i < 4; i++) {
      if (nlabel[i] != 0) {
        lut[nlabel[i]] = min;
      }
    }
    // num = min;
    label[y * image_h + x] = min;
    return num;
  }
  return num;
}
uint16_t research_8neighbors(uint16_t num, uint16_t x, uint16_t y) {
  uint16_t nlabel[4] = { 0, 0, 0, 0 };
  uint8_t flg_update = 0;

  if (y + 1 < image_v && label[(y + 1) * image_h + x] != 0) {  //下
    nlabel[0] = lut[label[(y + 1) * image_h + x]];
    flg_update = 1;
  }
  if (x + 1 < image_h && label[y * image_h + (x + 1)] != 0) {  //右
    nlabel[1] = lut[label[y * image_h + (x + 1)]];
    flg_update = 1;
  }
  if (y + 1 < image_v && x - 1 >= 0 && label[(y + 1) * image_h + (x - 1)] != 0) {  //左下
    nlabel[2] = lut[label[(y + 1) * image_h + (x - 1)]];
    flg_update = 1;
  }
  if (y + 1 < image_v && x + 1 < image_h && label[(y + 1) * image_h + (x + 1)] != 0) {  //右下
    nlabel[3] = lut[label[(y + 1) * image_h + (x + 1)]];
    flg_update = 1;
  }

  if (flg_update == 0) {
    num = num + 1;
    label[y * image_h + x] = num;
    return num;
  } else {
    uint16_t min = image_h * image_v;
    for (uint16_t i = 0; i < 4; i++) {
      if (nlabel[i] != 0 && nlabel[i] < min) {
        min = nlabel[i];
      }
    }
    for (uint16_t i = 0; i < 4; i++) {
      if (nlabel[i] != 0) {
        lut[nlabel[i]] = min;
      }
    }
    // num = min;
    label[y * image_h + x] = min;
    return num;
  }
  return num;
}
uint16_t labeling() {
  uint16_t x, y, num;
  uint16_t count = 0;
  uint16_t cycle_count;

  memset(label, 0, sizeof(label));
  memset(label_area, 0, sizeof(label_area));

  memset(lmap, 0, sizeof(lmap));
  for (uint16_t i = 0; i < image_h * image_v; i++) {
    lut[i] = i;
  }
  for (y = 0; y < image_v; y++) {
    for (x = 0; x < image_h; x++) {
      if (image_data[y * image_h + x] < th_binarize) {
        num = search_8neighbors(num, x, y);
      }
    }
  }
  for (y = image_v - 1; y <= 0; y--) {
    for (x = image_h - 1; x <= 0; x--) {
      if (image_data[y * image_h + x] < th_binarize) {
        num = research_8neighbors(num, x, y);
      }
    }
  }
  if (num > 0) {
    uint16_t label_lest = 0;
    cycle_count = 0;
    for (uint16_t i = 1; i < num + 1; i++) {
      if (label_lest < lut[i]) {
        cycle_count = cycle_count + 1;
      }
      if (label_lest > lut[i]) {
        lmap[i] = lmap[lut[i]];
      } else {
        label_lest = lut[i];
        lmap[i] = cycle_count;
      }
    }
    for (y = 0; y < image_v; y++) {
      for (x = 0; x < image_h; x++) {
        label[y * image_h + x] = lmap[label[y * image_h + x]];
        label_area[label[y * image_h + x]] = label_area[label[y * image_h + x]] + 1;
      }
    }
    return cycle_count;
  } else return 0;
}
void calc_3dpos() {
  uint16_t mkpos_d[8];
  calc_distortion(mkpos[0], mkpos[1], &mkpos_d[0], &mkpos_d[1]);
  calc_distortion(mkpos[2], mkpos[3], &mkpos_d[2], &mkpos_d[3]);
  calc_distortion(mkpos[4], mkpos[5], &mkpos_d[4], &mkpos_d[5]);
  calc_distortion(mkpos[6], mkpos[7], &mkpos_d[6], &mkpos_d[7]);
  float mkpos_d2[6];
  float marker_pos2[9];
  for (uint16_t j = 0; j < 6; j++) {
    mkpos_d2[j] = (float)mkpos_d[j];
  }
  for (uint16_t j = 0; j < 9; j++) {
    marker_pos2[j] = marker_pos[j];
  }
  float mkrpyc_b[4][3][3];
  float mktc_b[4][3];
  float mkrpy_b[4][3][3];
  float mkt_b[4][3];
  bool fans[4];
  calcSimpleP3P(mkpos_d2, marker_pos2, mktc_b, mkrpyc_b, mkt_b, mkrpy_b, fans);
  float marker_posa[3] = { marker_pos[9], marker_pos[10], marker_pos[11] };
  float uv_err_min = 99999.9f;
  uint8_t ians = 0;
  for (uint16_t j = 0; j < 4; j++) {
    if (fans[j]) {
      float uv_ans[2];
      float mkt_b2[3] = { mkt_b[j][0], mkt_b[j][1], mkt_b[j][2] };
      float mkrpy_b2[3][3];
      float uv_err;
      mkrpy_b2[0][0] = mkrpy_b[j][0][0];
      mkrpy_b2[0][1] = mkrpy_b[j][0][1];
      mkrpy_b2[0][2] = mkrpy_b[j][0][2];
      mkrpy_b2[1][0] = mkrpy_b[j][1][0];
      mkrpy_b2[1][1] = mkrpy_b[j][1][1];
      mkrpy_b2[1][2] = mkrpy_b[j][1][2];
      mkrpy_b2[2][0] = mkrpy_b[j][2][0];
      mkrpy_b2[2][1] = mkrpy_b[j][2][1];
      mkrpy_b2[2][2] = mkrpy_b[j][2][2];
      calcProjection(marker_posa, mkrpy_b2, mkt_b2, uv_ans);
      uv_err = pow(uv_ans[0] - (float)mkpos_d[6], 2) + pow(uv_ans[1] - (float)mkpos_d[7], 2);

      if (uv_err_min > uv_err) {
        uv_err_min = uv_err;
        ians = j;
      }
    }
  }

  float mkrpyc_b1[3][3];
  float mkrpyc_b2[3][3];
  float mkrpyc_b2_T[3][3];
  mkrpyc_b1[0][0] = mkrpyc_b[ians][0][0];
  mkrpyc_b1[0][1] = mkrpyc_b[ians][0][1];
  mkrpyc_b1[0][2] = mkrpyc_b[ians][0][2];
  mkrpyc_b1[1][0] = mkrpyc_b[ians][1][0];
  mkrpyc_b1[1][1] = mkrpyc_b[ians][1][1];
  mkrpyc_b1[1][2] = mkrpyc_b[ians][1][2];
  mkrpyc_b1[2][0] = mkrpyc_b[ians][2][0];
  mkrpyc_b1[2][1] = mkrpyc_b[ians][2][1];
  mkrpyc_b1[2][2] = mkrpyc_b[ians][2][2];

  float gimbal_rot[3][3];
  float gimbal_rot_T[3][3];
  float gimbal_pos0[3];
  float gimbal_pos1[3];
  float gxyz[3];
  gxyz[0] = (float)gimbal_y / 4095.0f * 2.0f * M_PI - M_PI;
  gxyz[1] = (float)gimbal_x / 4095.0f * 2.0f * M_PI - M_PI;
  gxyz[2] = 0.0f;
  rpy2rot(gxyz, gimbal_rot);  //x,y,z

  gimbal_pos0[0] = cam_offset[0] + mktc_b[ians][0];
  gimbal_pos0[1] = cam_offset[1] + mktc_b[ians][1];
  gimbal_pos0[2] = cam_offset[2] + mktc_b[ians][2];
  vdot3x1(gimbal_rot, gimbal_pos0, mktc);
  vdot3x3(gimbal_rot, mkrpyc_b1, mkrpyc_b2);
  rot2rpy(mkrpyc_b2, mkrpyc);
  mkrotc[0] = mkrpyc_b2[0][0];
  mkrotc[1] = mkrpyc_b2[0][1];
  mkrotc[2] = mkrpyc_b2[0][2];
  mkrotc[3] = mkrpyc_b2[1][0];
  mkrotc[4] = mkrpyc_b2[1][1];
  mkrotc[5] = mkrpyc_b2[1][2];
  mkrotc[6] = mkrpyc_b2[2][0];
  mkrotc[7] = mkrpyc_b2[2][1];
  mkrotc[8] = mkrpyc_b2[2][2];

  Serial.print(mktc[0], 6);
  Serial.print(",");
  Serial.print(mktc[1], 6);
  Serial.print(",");
  Serial.print(mktc[2], 6);
  Serial.print(",");
  Serial.print(mkrpyc[0] / M_PI * 180.0f, 6);
  Serial.print(",");
  Serial.print(mkrpyc[1] / M_PI * 180.0f, 6);
  Serial.print(",");
  Serial.print(mkrpyc[2] / M_PI * 180.0f, 6);
  Serial.println("");
}

uint16_t marker_detect() {
  uint16_t i, j, k, x, y, number, marker_num;
  uint16_t xmin, xmax, ymin, ymax, area;
  float xminr, xmaxr, yminr, ymaxr;
  uint16_t pos0[4][2], pos1[4][2], pos2[4][2];
  float w, h, ratio, fill_rate;
  float w2, h2;
  marker_num = 0;
  number = labeling();
  if (number > 0) {
    for (i = 1; i <= number; i++) {
      area = label_area[i];
      if (area > th_area) {
        xmin = image_h - 1;
        ymin = image_v - 1;
        xmax = 0;
        ymax = 0;

        xminr = image_rot45table_x[0 * image_h + (image_h - 1)];
        yminr = image_rot45table_y[(image_v - 1) * image_h + (image_h - 1)];
        xmaxr = image_rot45table_x[(image_v - 1) * image_h + 0];
        ymaxr = image_rot45table_y[0 * image_h + 0];

        for (j = 0; j < 4; j++) {
          for (k = 0; k < 2; k++) {
            pos0[j][k] = 0;
            pos1[j][k] = 0;
            pos2[j][k] = 0;
          }
        }

        for (y = 0; y < image_v; y++) {
          for (x = 0; x < image_h; x++) {
            if (label[y * image_h + x] == i) {
              if (x < xmin) {
                xmin = x;
                pos1[0][0] = x;
                pos1[0][1] = y;
              }
              if (x > xmax) {
                xmax = x;
                pos1[1][0] = x;
                pos1[1][1] = y;
              }
              if (y < ymin) {
                ymin = y;
                pos1[2][0] = x;
                pos1[2][1] = y;
              }
              if (y > ymax) {
                ymax = y;
                pos1[3][0] = x;
                pos1[3][1] = y;
              }

              if (image_rot45table_x[y * image_h + x] < xminr) {
                xminr = image_rot45table_x[y * image_h + x];
                pos2[0][0] = x;
                pos2[0][1] = y;
              }
              if (image_rot45table_x[y * image_h + x] > xmaxr) {
                xmaxr = image_rot45table_x[y * image_h + x];
                pos2[1][0] = x;
                pos2[1][1] = y;
              }
              if (image_rot45table_y[y * image_h + x] < yminr) {
                yminr = image_rot45table_y[y * image_h + x];
                pos2[2][0] = x;
                pos2[2][1] = y;
              }
              if (image_rot45table_y[y * image_h + x] > ymaxr) {
                ymaxr = image_rot45table_y[y * image_h + x];
                pos2[3][0] = x;
                pos2[3][1] = y;
              }
            }
          }
        }

        //頂点から面積算出
        float area_p2 = 0.0;
        float area_p1 = 0.0;
        float area_ratio = 0.0;

        area_p1 = 0.5 * (pos1[0][0] - pos1[2][0]) * (pos1[0][1] + pos1[2][1]);
        area_p1 = area_p1 + 0.5 * (pos1[2][0] - pos1[1][0]) * (pos1[2][1] + pos1[1][1]);
        area_p1 = area_p1 + 0.5 * (pos1[1][0] - pos1[3][0]) * (pos1[1][1] + pos1[3][1]);
        area_p1 = area_p1 + 0.5 * (pos1[3][0] - pos1[0][0]) * (pos1[3][1] + pos1[0][1]);
        area_p1 = abs(area_p1);

        area_p2 = 0.5 * (pos2[0][0] - pos2[2][0]) * (pos2[0][1] + pos2[2][1]);
        area_p2 = area_p2 + 0.5 * (pos2[2][0] - pos2[1][0]) * (pos2[2][1] + pos2[1][1]);
        area_p2 = area_p2 + 0.5 * (pos2[1][0] - pos2[3][0]) * (pos2[1][1] + pos2[3][1]);
        area_p2 = area_p2 + 0.5 * (pos2[3][0] - pos2[0][0]) * (pos2[3][1] + pos2[0][1]);
        area_p2 = abs(area_p2);

        if (area_p1 < area_p2) {
          area_ratio = area_p2 / area;
          for (j = 0; j < 4; j++) {
            for (k = 0; k < 2; k++) {
              pos0[j][k] = pos2[j][k];
            }
          }
        } else {
          area_ratio = area_p1 / area;
          for (j = 0; j < 4; j++) {
            for (k = 0; k < 2; k++) {
              pos0[j][k] = pos1[j][k];
            }
          }
        }
        if (th_area_ratio_max > area_ratio && th_area_ratio_min < area_ratio) {
          /* 数値を昇順にソート */
          int tmpx;
          int tmpy;
          for (j = 0; j < 4; ++j) {
            for (k = j + 1; k < 4; ++k) {
              if (pos0[j][0] > pos0[k][0]) {
                tmpx = pos0[j][0];
                tmpy = pos0[j][1];
                pos0[j][0] = pos0[k][0];
                pos0[j][1] = pos0[k][1];
                pos0[k][0] = tmpx;
                pos0[k][1] = tmpy;
              }
            }
          }

          if (pos0[0][1] > pos0[1][1]) {
            tmpx = pos0[1][0];
            tmpy = pos0[1][1];
            pos0[1][0] = pos0[0][0];
            pos0[1][1] = pos0[0][1];
            pos0[0][0] = tmpx;
            pos0[0][1] = tmpy;
          }
          if (pos0[3][1] > pos0[2][1]) {
            tmpx = pos0[2][0];
            tmpy = pos0[2][1];
            pos0[2][0] = pos0[3][0];
            pos0[2][1] = pos0[3][1];
            pos0[3][0] = tmpx;
            pos0[3][1] = tmpy;
          }
          float v01[2];
          float v01A[2];
          float v01B[2];
          float v32[2];
          float v32A[2];
          float v32B[2];
          float v03[2];
          float v03A[2];
          float v03B[2];
          float v12[2];
          float v12A[2];
          float v12B[2];
          int tv[8][2];
          v01[0] = (float)pos0[1][0] - (float)pos0[0][0];
          v01[1] = (float)pos0[1][1] - (float)pos0[0][1];
          v01A[0] = marker_ratioA * ((float)pos0[1][0] - (float)pos0[0][0]);
          v01A[1] = marker_ratioA * ((float)pos0[1][1] - (float)pos0[0][1]);
          v01B[0] = marker_ratioB * ((float)pos0[1][0] - (float)pos0[0][0]);
          v01B[1] = marker_ratioB * ((float)pos0[1][1] - (float)pos0[0][1]);

          v03[0] = (float)pos0[3][0] - (float)pos0[0][0];
          v03[1] = (float)pos0[3][1] - (float)pos0[0][1];
          v03A[0] = marker_ratioA * ((float)pos0[3][0] - (float)pos0[0][0]);
          v03A[1] = marker_ratioA * ((float)pos0[3][1] - (float)pos0[0][1]);
          v03B[0] = marker_ratioB * ((float)pos0[3][0] - (float)pos0[0][0]);
          v03B[1] = marker_ratioB * ((float)pos0[3][1] - (float)pos0[0][1]);

          v12[0] = (float)pos0[2][0] - (float)pos0[1][0];
          v12[1] = (float)pos0[2][1] - (float)pos0[1][1];
          v12A[0] = marker_ratioA * ((float)pos0[2][0] - (float)pos0[1][0]) + v01[0];
          v12A[1] = marker_ratioA * ((float)pos0[2][1] - (float)pos0[1][1]) + v01[1];
          v12B[0] = marker_ratioB * ((float)pos0[2][0] - (float)pos0[1][0]) + v01[0];
          v12B[1] = marker_ratioB * ((float)pos0[2][1] - (float)pos0[1][1]) + v01[1];

          v32[0] = (float)pos0[2][0] - (float)pos0[3][0];
          v32[1] = (float)pos0[2][1] - (float)pos0[3][1];
          v32A[0] = marker_ratioA * ((float)pos0[2][0] - (float)pos0[3][0]) + v03[0];
          v32A[1] = marker_ratioA * ((float)pos0[2][1] - (float)pos0[3][1]) + v03[1];
          v32B[0] = marker_ratioB * ((float)pos0[2][0] - (float)pos0[3][0]) + v03[0];
          v32B[1] = marker_ratioB * ((float)pos0[2][1] - (float)pos0[3][1]) + v03[1];

          tv[0][0] = int((v12A[0] - v03A[0]) * marker_ratioA + v03A[0] + (float)pos0[0][0]);
          tv[0][1] = int((v12A[1] - v03A[1]) * marker_ratioA + v03A[1] + (float)pos0[0][1]);
          tv[1][0] = int((v12A[0] - v03A[0]) * marker_ratioB + v03A[0] + (float)pos0[0][0]);
          tv[1][1] = int((v12A[1] - v03A[1]) * marker_ratioB + v03A[1] + (float)pos0[0][1]);
          tv[2][0] = int((v12B[0] - v03B[0]) * marker_ratioB + v03B[0] + (float)pos0[0][0]);
          tv[2][1] = int((v12B[1] - v03B[1]) * marker_ratioB + v03B[1] + (float)pos0[0][1]);
          tv[3][0] = int((v12B[0] - v03B[0]) * marker_ratioA + v03B[0] + (float)pos0[0][0]);
          tv[3][1] = int((v12B[1] - v03B[1]) * marker_ratioA + v03B[1] + (float)pos0[0][1]);

          tv[4][0] = int(-(v32A[0] - v01A[0]) * marker_ratioC + v01A[0] + (float)pos0[0][0]);
          tv[4][1] = int(-(v32A[1] - v01A[1]) * marker_ratioC + v01A[1] + (float)pos0[0][1]);
          tv[5][0] = int((v12A[0] - v03A[0]) * (1.0 + marker_ratioC) + v03A[0] + (float)pos0[0][0]);
          tv[5][1] = int((v12A[1] - v03A[1]) * (1.0 + marker_ratioC) + v03A[1] + (float)pos0[0][1]);
          tv[6][0] = int((v32B[0] - v01B[0]) * (1.0 + marker_ratioC) + v01B[0] + (float)pos0[0][0]);
          tv[6][1] = int((v32B[1] - v01B[1]) * (1.0 + marker_ratioC) + v01B[1] + (float)pos0[0][1]);
          tv[7][0] = int(-(v12B[0] - v03B[0]) * marker_ratioC + v03B[0] + (float)pos0[0][0]);
          tv[7][1] = int(-(v12B[1] - v03B[1]) * marker_ratioC + v03B[1] + (float)pos0[0][1]);

          uint8_t check_range = 0;
          bool tp[8];
          uint8_t rot_pat = 0;
          for (j = 0; j < 8; j++) {
            if (tv[j][0] >= 0 && tv[j][0] < image_h && tv[j][1] >= 0 && tv[j][1] < image_v) {
              check_range = check_range + 1;
            }
          }
          if (check_range == 8) {
            for (j = 0; j < 8; j++) {
              if (image_data[(uint16_t)tv[j][1] * image_h + (uint16_t)tv[j][0]] > th_binarize) {
                tp[j] = 1;
              } else {
                tp[j] = 0;
              }
            }
            if (tp[0] == 1 && tp[1] == 0 && tp[2] == 0 && tp[3] == 0) {
              rot_pat = 1;
            }
            if (tp[0] == 0 && tp[1] == 1 && tp[2] == 0 && tp[3] == 0) {
              rot_pat = 2;
            }
            if (tp[0] == 0 && tp[1] == 0 && tp[2] == 1 && tp[3] == 0) {
              rot_pat = 3;
            }
            if (tp[0] == 0 && tp[1] == 0 && tp[2] == 0 && tp[3] == 1) {
              rot_pat = 4;
            }
            for (j = 4; j < 8; j++) {
              if (tp[j] == 0) {
                rot_pat = 0;
              }
            }
            if (rot_pat > 0) {

              if (rot_pat == 1) {
                mkpos[0 + 8 * marker_num] = pos0[0][0];
                mkpos[1 + 8 * marker_num] = pos0[0][1];
                mkpos[2 + 8 * marker_num] = pos0[1][0];
                mkpos[3 + 8 * marker_num] = pos0[1][1];
                mkpos[4 + 8 * marker_num] = pos0[2][0];
                mkpos[5 + 8 * marker_num] = pos0[2][1];
                mkpos[6 + 8 * marker_num] = pos0[3][0];
                mkpos[7 + 8 * marker_num] = pos0[3][1];
              } else if (rot_pat == 2) {
                mkpos[0 + 8 * marker_num] = pos0[1][0];
                mkpos[1 + 8 * marker_num] = pos0[1][1];
                mkpos[2 + 8 * marker_num] = pos0[2][0];
                mkpos[3 + 8 * marker_num] = pos0[2][1];
                mkpos[4 + 8 * marker_num] = pos0[3][0];
                mkpos[5 + 8 * marker_num] = pos0[3][1];
                mkpos[6 + 8 * marker_num] = pos0[0][0];
                mkpos[7 + 8 * marker_num] = pos0[0][1];
              } else if (rot_pat == 3) {
                mkpos[0 + 8 * marker_num] = pos0[2][0];
                mkpos[1 + 8 * marker_num] = pos0[2][1];
                mkpos[2 + 8 * marker_num] = pos0[3][0];
                mkpos[3 + 8 * marker_num] = pos0[3][1];
                mkpos[4 + 8 * marker_num] = pos0[0][0];
                mkpos[5 + 8 * marker_num] = pos0[0][1];
                mkpos[6 + 8 * marker_num] = pos0[1][0];
                mkpos[7 + 8 * marker_num] = pos0[1][1];
              } else if (rot_pat == 4) {
                mkpos[0 + 8 * marker_num] = pos0[3][0];
                mkpos[1 + 8 * marker_num] = pos0[3][1];
                mkpos[2 + 8 * marker_num] = pos0[0][0];
                mkpos[3 + 8 * marker_num] = pos0[0][1];
                mkpos[4 + 8 * marker_num] = pos0[1][0];
                mkpos[5 + 8 * marker_num] = pos0[1][1];
                mkpos[6 + 8 * marker_num] = pos0[2][0];
                mkpos[7 + 8 * marker_num] = pos0[2][1];
              }

              marker_num = marker_num + 1;
              if (marker_num >= marker_max) {
                return marker_num;
              }
            }
          }
        }
      }
    }
  }
  return marker_num;
}

void calc_distortion(uint16_t px, uint16_t py, uint16_t *dx, uint16_t *dy) {
  *dx = undistmap_x[py * image_h + px];
  *dy = undistmap_y[py * image_h + px];
}

void calcProjection(float pos[3], float crot[3][3], float cpos[3], float uv_ans[2]) {
  float fx = mtx_coeff[0];
  float fy = mtx_coeff[1];
  float cx = mtx_coeff[2];
  float cy = mtx_coeff[3];
  float mtx[3][3];
  float uv[3];
  float XYZ[4] = { pos[0], pos[1], pos[2], 1.0f };
  float KT[3][4];
  float KTXYZ[3];
  for (uint8_t i = 0; i < 3; i++) {
    for (uint8_t j = 0; j < 3; j++) {
      KT[i][j] = crot[i][j];
      mtx[i][j] = 0.0f;
    }
  }
  KT[0][3] = cpos[0];
  KT[1][3] = cpos[1];
  KT[2][3] = cpos[2];
  mtx[0][0] = fx;
  mtx[1][1] = fy;
  mtx[0][2] = cx;
  mtx[1][2] = cy;
  mtx[2][2] = 1.0f;
  vdot4x1(KT, XYZ, KTXYZ);
  vdot3x1(mtx, KTXYZ, uv);

  if (abs(uv[2]) < 0.0001f) {
    uv_ans[0] = 0.0f;
    uv_ans[1] = 0.0f;
  } else {
    uv_ans[0] = uv[0] / uv[2];
    uv_ans[1] = uv[1] / uv[2];
  }
}
void solveQuartic(float a4, float a3, float a2, float a1, float a0, float ans[4], float ians[4]) {
  Complex b1s(0.0, 0.0);
  Complex b2s(0.0, 0.0);
  Complex b4s(0.0, 0.0);

  Complex b1(0.0, 0.0);
  Complex b2(0.0, 0.0);
  Complex b3(0.0, 0.0);
  Complex b4(0.0, 0.0);
  Complex b5(0.0, 0.0);
  Complex b6(0.0, 0.0);
  Complex b7(0.0, 0.0);
  Complex b8(0.0, 0.0);

  Complex x1(0.0, 0.0);
  Complex x2(0.0, 0.0);
  Complex x3(0.0, 0.0);
  Complex x4(0.0, 0.0);

  b3 = -pow(a3, 3.0) / pow(a4, 3.0) + 4.0 * a2 * a3 / pow(a4, 2.0) - 8.0 * a1 / a4;
  b5 = pow(a2, 2) - 3.0 * a1 * a3 + 12.0 * a0 * a4;
  b6 = 2.0 * pow(a2, 3) - 9.0 * a1 * a2 * a3 + 27.0 * a0 * pow(a3, 2) + 27.0 * pow(a1, 2) * a4 - 72.0 * a0 * a2 * a4;
  b7 = b5.c_pow(3.0) * (-4.0) + b6.c_pow(2.0);
  b8 = b6 + b7.c_sqrt();
  b4 = b5 / b8.c_pow(1.0 / 3.0) / 3.0 / a4 * pow(2, (1.0 / 3.0)) + b8.c_pow(1.0 / 3.0) / 3.0 / pow(2, (1.0 / 3.0)) / a4 - 2.0 * a2 / 3.0 / a4 + pow(a3, 2.0) / 4.0 / pow(a4, 2.0);

  b4s = b4.c_sqrt();

  b1 = b3 / b4s / 4.0 - b5 / b8.c_pow(1.0 / 3.0) / 3.0 / a4 * pow(2, (1.0 / 3.0)) - b8.c_pow(1.0 / 3.0) / 3.0 / pow(2, (1.0 / 3.0)) / a4 - 4.0 * a2 / 3.0 / a4 + pow(a3, 2.0) / 2.0 / pow(a4, 2.0);
  b2 = b3 / b4s / (-4.0) - b5 / b8.c_pow(1.0 / 3.0) / 3.0 / a4 * pow(2, (1.0 / 3.0)) - b8.c_pow(1.0 / 3.0) / 3.0 / pow(2, (1.0 / 3.0)) / a4 - 4.0 * a2 / 3.0 / a4 + pow(a3, 2.0) / 2.0 / pow(a4, 2.0);
  b1s = b1.c_sqrt();
  b2s = b2.c_sqrt();
  x1 = -b2s / 2.0 - b4s / 2.0 - a3 / 4.0 / a4;
  x2 = b2s / 2.0 - b4s / 2.0 - a3 / 4.0 / a4;
  x3 = -b1s / 2.0 + b4s / 2.0 - a3 / 4.0 / a4;
  x4 = b1s / 2.0 + b4s / 2.0 - a3 / 4.0 / a4;
  ans[0] = x1.real();
  ans[1] = x2.real();
  ans[2] = x3.real();
  ans[3] = x4.real();
  ians[0] = x1.imag();
  ians[1] = x2.imag();
  ians[2] = x3.imag();
  ians[3] = x4.imag();
}

uint8_t calcSimpleP3P(float mpixel[6], float mpos[9], float tcans[4][3], float rpycans[4][3][3], float tans[4][3], float rpyans[4][3][3], bool fans[4]) {
  memset(fans, 0, 4);
  float pos[3][3];
  pos[0][0] = mpos[0];
  pos[0][1] = mpos[1];
  pos[0][2] = mpos[2];

  pos[1][0] = mpos[3];
  pos[1][1] = mpos[4];
  pos[1][2] = mpos[5];

  pos[2][0] = mpos[6];
  pos[2][1] = mpos[7];
  pos[2][2] = mpos[8];
  float pixel[3][2];
  pixel[0][0] = mpixel[0];
  pixel[0][1] = mpixel[1];

  pixel[1][0] = mpixel[2];
  pixel[1][1] = mpixel[3];

  pixel[2][0] = mpixel[4];
  pixel[2][1] = mpixel[5];
  float fx = mtx_coeff[0];
  float fy = mtx_coeff[1];
  float Cu = mtx_coeff[2];
  float Cv = mtx_coeff[3];
  float q[3][3];
  for (uint16_t i = 0; i < 3; i++) {
    q[i][0] = (pixel[i][0] - Cu) / fx;
    q[i][1] = (pixel[i][1] - Cv) / fy;
    q[i][2] = 1.0f;
    float norm = sqrt(pow(q[i][0], 2.0f) + pow(q[i][1], 2.0f) + pow(q[i][2], 2.0f));
    q[i][0] = q[i][0] / norm;
    q[i][1] = q[i][1] / norm;
    q[i][2] = q[i][2] / norm;
  }
  float a = sqrt(pow(pos[0][0] - pos[1][0], 2.0f) + pow(pos[0][1] - pos[1][1], 2.0f) + pow(pos[0][2] - pos[1][2], 2.0f));
  float b = sqrt(pow(pos[1][0] - pos[2][0], 2.0f) + pow(pos[1][1] - pos[2][1], 2.0f) + pow(pos[1][2] - pos[2][2], 2.0f));
  float c = sqrt(pow(pos[2][0] - pos[0][0], 2.0f) + pow(pos[2][1] - pos[0][1], 2.0f) + pow(pos[2][2] - pos[0][2], 2.0f));

  float ca = q[0][0] * q[1][0] + q[0][1] * q[1][1] + q[0][2] * q[1][2];
  float cb = q[1][0] * q[2][0] + q[1][1] * q[2][1] + q[1][2] * q[2][2];
  float cc = q[2][0] * q[0][0] + q[2][1] * q[0][1] + q[2][2] * q[0][2];
  float ca2 = ca * ca;
  float cb2 = cb * cb;
  float cc2 = cc * cc;
  float b4 = 4.0f * pow(b, 2.0f) * pow(c, 2.0f) * ca2 - pow((pow(a, 2.0f) - pow(b, 2.0f) - pow(c, 2.0f)), 2.0f);
  float b3 = -4.0f * pow(c, 2.0f) * (pow(a, 2.0f) + pow(b, 2.0f) - pow(c, 2.0f)) * ca * cb - 8.0f * pow(b, 2.0f) * pow(c, 2.0f) * ca2 * cc + 4.0f * (pow(a, 2.0f) - pow(b, 2.0f) - pow(c, 2.0f)) * (pow(a, 2.0f) - pow(b, 2.0f)) * cc;
  float b2 = 4.0f * pow(c, 2.0f) * (pow(a, 2.0f) - pow(c, 2.0f)) * cb2 + 8.0f * pow(c, 2.0f) * (pow(a, 2.0f) + pow(b, 2.0f)) * ca * cb * cc + 4.0f * pow(c, 2.0f) * (pow(b, 2.0f) - pow(c, 2.0f)) * ca2 - 2.0f * (pow(a, 2.0f) - pow(b, 2.0f) - pow(c, 2.0f)) * (pow(a, 2.0f) - pow(b, 2.0f) + pow(c, 2.0f)) - 4.0f * pow((pow(a, 2.0f) - pow(b, 2.0f)), 2.0f) * cc2;
  float b1 = -8.0f * pow(a, 2.0f) * pow(c, 2.0f) * cb2 * cc - 4.0f * pow(c, 2.0f) * (pow(b, 2.0f) - pow(c, 2.0f)) * ca * cb - 4.0f * pow(a, 2.0f) * pow(c, 2.0f) * ca * cb + 4.0f * (pow(a, 2.0f) - pow(b, 2.0f)) * (pow(a, 2.0f) - pow(b, 2.0f) + pow(c, 2.0f)) * cc;
  float b0 = 4.0f * pow(a, 2.0f) * pow(c, 2.0f) * cb2 - pow((pow(a, 2.0f) - pow(b, 2.0f) + pow(c, 2.0f)), 2.0f);
  float ans[4] = { 0.0f, 0.0f, 0.0f, 0.0f };
  float ians[4] = { 0.0f, 0.0f, 0.0f, 0.0f };
  uint16_t cnt_ans = 0;
  solveQuartic(b4, b3, b2, b1, b0, ans, ians);
  for (uint16_t i = 0; i < 4; i++) {
    // if (abs(ians[i]) < 0.000001f) {
    float u = ans[i];
    float v = -((pow(a, 2.0f) - pow(b, 2.0f) - pow(c, 2.0f)) * pow(u, 2.0f) + 2.0f * (pow(b, 2.0f) - pow(a, 2.0f)) * cc * u + (pow(a, 2.0f) - pow(b, 2.0f) + pow(c, 2.0f))) / (2.0f * pow(c, 2.0f) * (ca * u - cb));
    float x = a * a / (u * u + v * v - 2.0f * u * v * ca);
    float y = b * b / (1 + v * v - 2.0f * v * cb);
    float z = c * c / (1 + u * u - 2.0f * u * cc);

    if (x > 0.0f) {
      float s1 = sqrt(x);
      float s2 = u * s1;
      float s3 = v * s1;
      float P1[3] = { q[0][0] * s2, q[0][1] * s2, q[0][2] * s2 };
      float P2[3] = { q[1][0] * s3, q[1][1] * s3, q[1][2] * s3 };
      float P3[3] = { q[2][0] * s1, q[2][1] * s1, q[2][2] * s1 };

      float P12[3] = { P2[0] - P1[0], P2[1] - P1[1], P2[2] - P1[2] };
      float P13[3] = { P3[0] - P1[0], P3[1] - P1[1], P3[2] - P1[2] };
      float P23[3] = { P3[0] - P2[0], P3[1] - P2[1], P3[2] - P2[2] };
      float xv[3];
      float yv[3];
      float zv[3];
      vcross(P13, P12, zv);
      vnorm(zv);
      yv[0] = P12[0];
      yv[1] = P12[1];
      yv[2] = P12[2];
      vnorm(yv);
      vcross(yv, zv, xv);
      vnorm(xv);
      float Rotc[3][3];
      Rotc[0][0] = xv[0];
      Rotc[1][0] = xv[1];
      Rotc[2][0] = xv[2];
      Rotc[0][1] = yv[0];
      Rotc[1][1] = yv[1];
      Rotc[2][1] = yv[2];
      Rotc[0][2] = zv[0];
      Rotc[1][2] = zv[1];
      Rotc[2][2] = zv[2];

      float P12b[3] = { pos[1][0] - pos[0][0], pos[1][1] - pos[0][1], pos[1][2] - pos[0][2] };
      float P13b[3] = { pos[2][0] - pos[0][0], pos[2][1] - pos[0][1], pos[2][2] - pos[0][2] };
      float xb[3];
      float yb[3];
      float zb[3];
      vcross(P13b, P12b, zb);
      vnorm(zb);
      yb[0] = P12b[0];
      yb[1] = P12b[1];
      yb[2] = P12b[2];
      vnorm(yb);
      vcross(yb, zb, xb);
      vnorm(xb);

      float Rota[3][3];
      Rota[0][0] = xb[0];
      Rota[1][0] = xb[1];
      Rota[2][0] = xb[2];
      Rota[0][1] = yb[0];
      Rota[1][1] = yb[1];
      Rota[2][1] = yb[2];
      Rota[0][2] = zb[0];
      Rota[1][2] = zb[1];
      Rota[2][2] = zb[2];
      float Rota_T[3][3];
      float Rotc_T[3][3];
      vtrans(Rota, Rota_T);
      vtrans(Rotc, Rotc_T);

      float rc[3][3];
      float rc2[3][3];
      float pos2[3];
      vdot3x3(Rota, Rotc_T, rc);
      vdot3x3(Rotc_T, Rota, rc2);

      vdot3x1(rc, P1, pos2);
      tans[cnt_ans][0] = -pos2[0] + pos[0][0];
      tans[cnt_ans][1] = -pos2[1] + pos[0][1];
      tans[cnt_ans][2] = -pos2[2] + pos[0][2];
      tcans[cnt_ans][0] = P1[0];
      tcans[cnt_ans][1] = P1[1];
      tcans[cnt_ans][2] = P1[2];

      rpyans[cnt_ans][0][0] = rc2[0][0];
      rpyans[cnt_ans][0][1] = rc2[0][1];
      rpyans[cnt_ans][0][2] = rc2[0][2];
      rpyans[cnt_ans][1][0] = rc2[1][0];
      rpyans[cnt_ans][1][1] = rc2[1][1];
      rpyans[cnt_ans][1][2] = rc2[1][2];
      rpyans[cnt_ans][2][0] = rc2[2][0];
      rpyans[cnt_ans][2][1] = rc2[2][1];
      rpyans[cnt_ans][2][2] = rc2[2][2];

      rpycans[cnt_ans][0][0] = Rotc[0][0];
      rpycans[cnt_ans][0][1] = Rotc[0][1];
      rpycans[cnt_ans][0][2] = Rotc[0][2];
      rpycans[cnt_ans][1][0] = Rotc[1][0];
      rpycans[cnt_ans][1][1] = Rotc[1][1];
      rpycans[cnt_ans][1][2] = Rotc[1][2];
      rpycans[cnt_ans][2][0] = Rotc[2][0];
      rpycans[cnt_ans][2][1] = Rotc[2][1];
      rpycans[cnt_ans][2][2] = Rotc[2][2];
      fans[cnt_ans] = 1;
      cnt_ans = cnt_ans + 1;
    }
    // }
  }
  return 0;
}
void rpy2rot(float a[3], float rot[3][3]) {
  rot[0][0] = cos(a[1]) * cos(a[2]);
  rot[1][0] = cos(a[1]) * sin(a[2]);
  rot[2][0] = -sin(a[1]);

  rot[0][1] = sin(a[0]) * sin(a[1]) * cos(a[2]) - cos(a[0]) * sin(a[2]);
  rot[1][1] = sin(a[0]) * sin(a[1]) * sin(a[2]) + cos(a[0]) * cos(a[2]);
  rot[2][1] = sin(a[0]) * cos(a[1]);

  rot[0][2] = cos(a[0]) * sin(a[1]) * cos(a[2]) + sin(a[0]) * sin(a[2]);
  rot[1][2] = cos(a[0]) * sin(a[1]) * sin(a[2]) - sin(a[0]) * cos(a[2]);
  rot[2][2] = cos(a[0]) * cos(a[1]);
}
void rot2rpy(float rot[3][3], float a[3]) {
  a[2] = 0.0f;
  a[1] = asin(-rot[2][0]);
  a[0] = 0.0f;
  if (abs(rot[2][0]) != 1.0f) {
    a[2] = atan2(rot[1][0], rot[0][0]);
    a[0] = atan2(rot[2][1], rot[2][2]);
  }
  if (rot[2][0] == 1.0f) {
    a[0] = 0.0f;
    a[2] = atan2(rot[0][1], rot[1][1]);
  }
  if (rot[2][0] == -1.0f) {
    a[0] = 0.0f;
    a[2] = atan2(rot[0][1], rot[1][1]);
  }
  if (a[0] > M_PI) {
    a[0] = a[0] - 2.0f * M_PI;
  }
  if (a[0] < -M_PI) {
    a[0] = a[0] + 2.0f * M_PI;
  }

  if (a[1] > M_PI) {
    a[1] = a[1] - 2.0f * M_PI;
  }
  if (a[1] < -M_PI) {
    a[1] = a[1] + 2.0f * M_PI;
  }

  if (a[2] > M_PI) {
    a[2] = a[2] - 2.0f * M_PI;
  }
  if (a[2] < -M_PI) {
    a[2] = a[2] + 2.0f * M_PI;
  }
}
void vcross(float a[3], float b[3], float c[3]) {
  c[0] = a[1] * b[2] - a[2] * b[1];
  c[1] = a[2] * b[0] - a[0] * b[2];
  c[2] = a[0] * b[1] - a[1] * b[0];
}
void vnorm(float a[3]) {
  float norm = sqrt(pow(a[0], 2.0f) + pow(a[1], 2.0f) + pow(a[2], 2.0f));
  a[0] = a[0] / norm;
  a[1] = a[1] / norm;
  a[2] = a[2] / norm;
}

void vtrans(float a[3][3], float b[3][3]) {
  b[0][0] = a[0][0];
  b[0][1] = a[1][0];
  b[0][2] = a[2][0];
  b[1][0] = a[0][1];
  b[1][1] = a[1][1];
  b[1][2] = a[2][1];
  b[2][0] = a[0][2];
  b[2][1] = a[1][2];
  b[2][2] = a[2][2];
}

void vdot3x3(float a[3][3], float b[3][3], float c[3][3]) {
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      c[i][j] = 0;
      for (int k = 0; k < 3; k++) {
        c[i][j] += a[i][k] * b[k][j];
      }
    }
  }
}

void vdot3x1(float a[3][3], float b[3], float c[3]) {
  for (int i = 0; i < 3; i++) {
    c[i] = 0.0f;
    for (int k = 0; k < 3; k++) {
      c[i] += a[i][k] * b[k];
    }
  }
}
void vdot4x1(float a[3][4], float b[4], float c[3]) {
  for (int i = 0; i < 3; i++) {
    c[i] = 0.0f;
    for (int k = 0; k < 4; k++) {
      c[i] += a[i][k] * b[k];
    }
  }
}

変数宣言、設定値等


init.h
#include <MP.h>
#include <SDHCI.h>
#include <File.h>
#include <Camera.h>
#include "CComplex.h"
#include <SCServo.h>


#define CONSOLE_BAUDRATE 1000000
#define image_h CAM_IMGSIZE_QQVGA_H
#define image_v CAM_IMGSIZE_QQVGA_V
#define M_PI 3.141592653f
#define marker_max 2
uint8_t image_data[image_h * image_v];
uint8_t image_bdata[int(image_h * image_v / 8)];
uint8_t th_binarize = 70;
uint16_t label[image_h * image_v];
uint16_t label_area[image_h * image_v];
uint16_t lut[image_h * image_v];

uint16_t lmap[image_h * image_v];
float image_rot45table_x[image_h * image_v];
float image_rot45table_y[image_h * image_v];
float rangle45deg = 0.70710678;

uint16_t mkpos[8 * marker_max];
float mkrpyc[3] = { 0.0f, 0.0f, 0.0f };
float mkrotc[9];
float mktc[3] = { 0.0f, 0.0f, 0.0f };

float cam_offset[3] = { 0.0f, 0.0f, 0.019f };

SDClass SD;  /**< SDClass object */
File myFile; /**< File object */
SMS_STS st;
int gimbal_x = 0;
int gimbal_y = 0;
uint16_t undistmap_x[image_h * image_v];
uint16_t undistmap_y[image_h * image_v];
float dist_coeff[8] = { 21.7717354533507,
                        808.114465113412,
                        0.00962848702938,
                        -0.02168948091617,
                        3990.29471654949,
                        22.9632941293542,
                        784.599039778564,
                        4676.72996735185 };

float mtx_coeff[4] = { 251.838752352578, 250.68958306935, 96.731883713966, 43.9976611433349 };
float marker_size = 0.05f;  //[m]
float marker_pos[12] = {
  0, 0, 0,
  0, marker_size, 0,
  marker_size, marker_size, 0,
  marker_size, 0, 0
};

float marker_ratioA = 0.35;
float marker_ratioB = 0.65;
float marker_ratioC = 0.1;
uint16_t th_area = 100;
float th_area_ratio_max = 1.3;
float th_area_ratio_min = 1.0;

unsigned long frame_num = 0;
uint8_t f_read = 0;



struct Core0Packet {
  volatile int status = 0; /* 0:ready, 1:busy */
  uint32_t data_size = 0;
  uint16_t mkpos[8 * marker_max];
  uint8_t data_buf[65536];
};
Core0Packet packet;
int subcore = 1;

//タイマー---------------------------------------------
float t = 0.0;
float dt = 0.001;        //s
unsigned long cnt0 = 0;  //時間カウンタ
uint16_t cnt1 = 0;       //カウンタ１
uint16_t cnt2 = 0;       //カウンタ２
uint16_t cnt3 = 0;       //カウンタ３
uint16_t cnt4 = 0;       //カウンタ４
boolean cnt1_flag = HIGH;
boolean cnt2_flag = LOW;
boolean cnt3_flag = LOW;
boolean cnt4_flag = LOW;
float cnt1_time = 1.0;   //割り込み周期[sec]//TBD
float cnt2_time = 1.0;   //割り込み周期[sec]//UDP送信タイミングカウンタ
float cnt3_time = 0.01;  //割り込み周期[sec]//TBD
float cnt4_time = 0.01;  //割り込み周期[sec]//TBD

サブコア1

<TelitWiFi.h>:
- GNU Lesser General Public License (LGPL) v2.1 （https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html）を参照
- 詳細: https://github.com/jittermaster/GS2200-WiFi
- このライブラリの著作権: Copyright 2019 Norikazu Goto.
<MP.h>:
- GNU Lesser General Public License (LGPL) v2.1 （https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html）を参照
- 詳細: https://developer.sony.com/develop/spresense/
- このライブラリの著作権: Copyright 2019 Sony Semiconductor Solutions Corporation.

各ライブラリのライセンス条項については、それぞれの公式サイトをご参照ください。


sub1_udp.ino
#if (SUBCORE != 1)
#error "Core selection is wrong!!"
#endif
#include <TelitWiFi.h>
#include <MP.h>

#define AP_SSID "**********"
#define PASSPHRASE "**********"
#define UDPSRVR_IP "192.168.***.***"//ノートPC側のIPアドレス
#define UDPSRVR_PORT "10001"
#define LocalPort "10001"

#define CONSOLE_BAUDRATE 1000000
#define marker_max 2
//コア間通信------------------------------------------
int8_t msgid = 0;         //メッセージID
struct Core0Packet {      //画像データ用構造体
  volatile int status;    //img_buに格納された画像が既にsubcore1が受信したものの時は0、そうでない時は1
  uint32_t img_size = 0;  //画像サイズ
  uint16_t mkpos[8 * marker_max];
  uint8_t img_buf[65536];
};
Core0Packet *packet;
uint16_t mkpos[8 * marker_max];
int subcore = 2;  //送信先コア番号

//UDP通信---------------------------------------------
TelitWiFi gs2200;
TWIFI_Params gsparams;
char server_cid = 0;                        //
const uint16_t UDP_PACKET_SIZE = 1460;      //パケット最大データ量
uint8_t UDP_Data[UDP_PACKET_SIZE] = { 0 };  //送信データ格納用

bool send_flg = 0;          //送信用画像データがmaincoreから受信されたとき1、UDPで画像送信終わった時0
uint8_t sendcycle_num = 0;  //複数パケットに分割して送信するときのパケット数（今何パケット目？）
uint32_t sended_bytes = 0;  //送信した完了した（送信しなくてもよい）データ数

//タイマー---------------------------------------------
float t = 0.0;
float dt = 0.001;        //s
unsigned long cnt0 = 0;  //時間カウンタ
uint16_t cnt1 = 0;       //カウンタ１
uint16_t cnt2 = 0;       //カウンタ２
uint16_t cnt3 = 0;       //カウンタ３
uint16_t cnt4 = 0;       //カウンタ４
boolean cnt1_flag = HIGH;
boolean cnt2_flag = LOW;
boolean cnt3_flag = LOW;
boolean cnt4_flag = LOW;
float cnt1_time = 1.0;   //割り込み周期[sec]//TBD
float cnt2_time = 0.05;  //割り込み周期[sec]//UDP送信タイミングカウンタ
float cnt3_time = 0.01;  //割り込み周期[sec]//TBD
float cnt4_time = 0.01;  //割り込み周期[sec]//TBD



void IntervalSet(float interupt_time) {  //タイマー初期化
  attachTimerInterrupt(timer_1, uint64_t(interupt_time * 1000000));
}

uint64_t timer_1() {  //タイマー関数
  t = t + dt;
  cnt0 = cnt0 + 1;
  cnt1 = cnt1 + 1;
  cnt2 = cnt2 + 1;
  cnt3 = cnt3 + 1;
  cnt4 = cnt4 + 1;
  if (cnt1 > (uint16_t)(cnt1_time / dt)) {  //TBD
    cnt1_flag = !cnt1_flag;
    cnt1 = 0;
  }
  if (cnt2 > (uint16_t)(cnt2_time / dt)) {  //UDP送信タイミングカウンタ
    cnt2_flag = HIGH;
    cnt2 = 0;
  }
  if (cnt3 > (uint16_t)(cnt3_time / dt)) {  //TBD
    cnt3_flag = HIGH;
    cnt3 = 0;
  }
  if (cnt4 > (uint16_t)(cnt4_time / dt)) {  //TBD
    cnt4_flag = HIGH;
    cnt4 = 0;
  }
  return uint64_t(dt * 1000000);
}


void send_cycle() {  //UDP送信関数
  if (cnt2_flag) {
    //maincoreから画像受信
    int ret;
    ret = MP.Recv(&msgid, &packet);

    //画像受信完了したとき
    if (ret > 0) {
      //画像データ受信完了フラグを立てる
      send_flg = 1;
    }

    if (send_flg) {                                 //画像情報が受信出来たとき
      if (packet->status == 1) {                    //画像データが最新のものかを確認
        uint32_t send_bytes = UDP_PACKET_SIZE - 1;  //フッタバイト分減算
        if (sendcycle_num == 0) {                   //初回パケットの時、mavlinkデータを送信する
          send_bytes = sizeof(mkpos);
          memcpy(UDP_Data, &packet->mkpos, send_bytes);
          UDP_Data[send_bytes] = sendcycle_num;                //データ末尾にパケット番号を付与
          gs2200.write(server_cid, UDP_Data, send_bytes + 1);  //UDP送信
          sendcycle_num = sendcycle_num + 1;                   //パケット番号カウントアップ
        } else {
          if (send_bytes > packet->img_size - sended_bytes) {
            send_bytes = packet->img_size - sended_bytes;
            if (send_bytes < 0) {
              sended_bytes = 0;    //画像データアドレスリセット
              sendcycle_num = 0;   //パケット番号リセット
              packet->status = 0;  //画像データ送信済み
              send_flg = 0;
            } else {
              memcpy(UDP_Data, &packet->img_buf[sended_bytes], send_bytes);  //データ格納
              UDP_Data[send_bytes] = sendcycle_num;                          //データ末尾にパケット番号を付与
              gs2200.write(server_cid, UDP_Data, send_bytes + 1);            //UDP送信
              sended_bytes = 0;                                              //画像データアドレスリセット
              sendcycle_num = 0;                                             //パケット番号リセット
              packet->status = 0;                                            //画像データ送信済み
              send_flg = 0;
            }

          } else {
            memcpy(UDP_Data, &packet->img_buf[sended_bytes], send_bytes);  //データ格納
            UDP_Data[send_bytes] = sendcycle_num;                          //データ末尾にパケット番号を付与
            gs2200.write(server_cid, UDP_Data, sizeof(UDP_Data));          //UDP送信
            sended_bytes = sended_bytes + send_bytes;                      //送信データ分だけ画像データアドレスカウントアップ
            sendcycle_num = sendcycle_num + 1;                             //パケット番号カウントアップ
          }
        }
      }
    }
    cnt2_flag = LOW;
  }
}

void setup() {
  Serial.begin(CONSOLE_BAUDRATE);

  MP.begin();
  MP.RecvTimeout(MP_RECV_BLOCKING);

  /* Initialize AT Command Library Buffer */
  AtCmd_Init();
  /* Initialize SPI access of GS2200 */
  Init_GS2200_SPI_type(iS110B_TypeC);
  /* Initialize AT Command Library Buffer */
  gsparams.mode = ATCMD_MODE_STATION;
  gsparams.psave = ATCMD_PSAVE_ALWAYS_ON;
  if (gs2200.begin(gsparams)) {
    ConsoleLog("GS2200 Initilization Fails");
    while (1) {}
  }
  /* GS2200 Association to AP */
  if (gs2200.activate_station(AP_SSID, PASSPHRASE)) {
    ConsoleLog("Association Fails");
    while (1) {}
  }

  bool served = false;
  while (1) {
    if (!served) {
      ConsoleLog("Start UDP Client");
      // Create UDP Client
      server_cid = gs2200.connectUDP(UDPSRVR_IP, UDPSRVR_PORT, LocalPort);
      ConsolePrintf("server_cid: %d \r\n", server_cid);
      if (server_cid == ATCMD_INVALID_CID) {
        continue;
      }
      served = true;
      break;
    } else {
      ConsoleLog("Start to send UDP Data");
      // Prepare for the next chunck of incoming data
      WiFi_InitESCBuffer();
    }
  }
  void *adder;
  MP.Send(msgid, &adder);           //準備完了をメインコアに通知
  MP.Recv(&msgid, &adder);          //メインコアの準備完了通知を受信
  MP.RecvTimeout(MP_RECV_POLLING);  //コア間通信ブロッキングからポーリングへ

  IntervalSet(dt);  //送信サイクル開始
}

void loop() {

  send_cycle();
}

受信データ表示スクリプト（ノートPCとかで動作）

このコードは以下のライブラリを使用して作成されています。

socket, math, struct: Python標準ライブラリ（Python Software Foundation Licenseに基づいて提供）
OpenCV (cv2): BSD 3-Clause License（https://github.com/opencv/opencv/blob/4.4.0/LICENSE）
NumPy (numpy): BSD License（https://numpy.org/doc/stable/license.html）
各ライブラリのライセンス条項については、それぞれの公式サイトをご参照ください。


udp.py
import socket
import cv2 #openCV
import numpy as np
import math
from struct import *
M_SIZE = 65536#バッファサイズ
host = '192.168.xxx.xxx'
port = 10001
serv_address = (host, port)


dist_data = np.loadtxt("calib/dist.csv", delimiter=',')
mtx_data = np.loadtxt("calib/mtx.csv", delimiter=',')
marker_3dpos = np.zeros([4,3])
marker_3dpos[0,:] = np.array([ 0   , 0   ,0])
marker_3dpos[1,:] = np.array([ 0   , 0.05,0])
marker_3dpos[2,:] = np.array([ 0.05, 0.05,0])
marker_3dpos[3,:] = np.array([ 0.05, 0   ,0])


def calc_undistmap(dist_data,mtx_data,w,h):
    alp = 0.1
    ita_num = 100
    x_undist = np.zeros([h,w])
    y_undist = np.zeros([h,w])
    for j in range(w):
        for k in range(h):
            x = (j - mtx_data[0,2])/mtx_data[0,0]
            y = (k - mtx_data[1,2])/mtx_data[1,1]
            xe = x
            ye = y
            dx = 0
            dy = 0
            r = np.sqrt(xe**2 + ye**2)
            eps = 0.1

            for i in range(ita_num):
                xe_b = xe
                ye_b = ye
                r = (1-alp) * r + alp * np.sqrt(xe**2 + ye**2)
                fu = 1 + dist_data[0]*r**2 + dist_data[1]*r**4 + dist_data[4]*r**6
                fl = 1 + dist_data[5]*r**2 + dist_data[6]*r**4 + dist_data[7]*r**6
                dx = dist_data[2]*2*x*y + dist_data[3]*(r**2+2*x**2)
                dy = dist_data[3]*2*x*y + dist_data[2]*(r**2+2*y**2)
                xe = (x - dx)*fl/fu
                ye = (y - dy)*fl/fu
                if np.sqrt((xe-xe_b)**2+(ye-ye_b)**2) < eps:
                    break
            # print([xe,ye])
            x_undist[k,j] = xe*mtx_data[0,0] + mtx_data[0,2]
            y_undist[k,j] = ye*mtx_data[1,1] + mtx_data[1,2]
    return x_undist ,y_undist

def rpy2rot(rpy):
    yaw = rpy[2]
    pitch = rpy[1]
    roll = rpy[0]
    Rx = np.array([[1,0,0],[0,np.cos(roll),-np.sin(roll)],[0,np.sin(roll),np.cos(roll)]])
    Ry = np.array([[np.cos(pitch),0,np.sin(pitch)],[0,1,0],[-np.sin(pitch),0,np.cos(pitch)]])
    Rz = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),np.cos(yaw),0],[0,0,1]])
    return Rz@Ry@Rx
def rot2rpy(R):
    yaw = 0
    pitch = np.arcsin(-R[2,0])
    roll = 0
    if np.abs(R[2,0]) != 1:
        yaw = np.arctan2(R[1,0],R[0,0])
        roll = np.arctan2(R[2,1],R[2,2])
    if R[2,0] == 1:
        roll = 0
        yaw = np.arctan2(R[0,1],R[1,1])
    if R[2,0] == -1:
        roll = 0
        yaw = np.arctan2(R[0,1],R[1,1])
    if roll > np.pi:
        roll = roll - 2*np.pi
    if roll < -np.pi:
        roll = roll + 2*np.pi

    if pitch > np.pi:
        pitch = pitch - 2*np.pi
    if pitch < -np.pi:
        pitch = pitch + 2*np.pi

    if yaw > np.pi:
        yaw = yaw - 2*np.pi
    if yaw < -np.pi:
        yaw = yaw + 2*np.pi
    rpy = np.array([roll,pitch,yaw])
    return rpy
def calc_distortion(undist_x,undist_y,point_data):
    point_data_ans = point_data.copy()
    for i in range(len(point_data)):
        point_data_ans[i,0] = undist_x[int(point_data[i,1]),int(point_data[i,0])]
        point_data_ans[i,1] = undist_y[int(point_data[i,1]),int(point_data[i,0])]
    return point_data_ans
    
def calcProjection(pos,cam_pos,cam_rot,mtx):
    if pos.ndim > 1:
        p_pixel = np.zeros([len(pos),2],dtype=int)
        pos_buf = np.zeros([len(pos),3])
    else:
        p_pixel = np.zeros([1,2],dtype=int)
        pos_buf = np.zeros([1,3]) 
    pos_buf[0,:] = pos
    tx = cam_pos[0]
    ty = cam_pos[1]
    tz = cam_pos[2]
    RT = np.array([[cam_rot[0,0],cam_rot[0,1],cam_rot[0,2],tx],[cam_rot[1,0],cam_rot[1,1],cam_rot[1,2],ty],[cam_rot[2,0],cam_rot[2,1],cam_rot[2,2],tz]])
    for i in range(len(pos_buf)):
        XYZ = np.array([pos_buf[i,0],pos_buf[i,1],pos_buf[i,2],1])
        uv = mtx@RT@XYZ.T
        if uv[2] == 0:
            return p_pixel
        p_pixel[i,0] = uv[0]/uv[2]
        p_pixel[i,1] = uv[1]/uv[2]
    return p_pixel
def calcSimpleP3P(pixel,pos,mtx):

    fx = mtx[0,0]
    fy = mtx[1,1]
    Cu = mtx[0,2]
    Cv = mtx[1,2]
    q = np.zeros([len(pixel),3])
    for i in range(len(pixel)):
        q[i,0] = (pixel[i,0] - Cu)/fx
        q[i,1] = (pixel[i,1] - Cv)/fy
        q[i,2] = 1
        q[i,:] = q[i,:]/np.linalg.norm(q[i,:])
    a = np.linalg.norm(pos[0,:]-pos[1,:])
    b = np.linalg.norm(pos[1,:]-pos[2,:])
    c = np.linalg.norm(pos[2,:]-pos[0,:])
    ca = q[0,:]@q[1,:]
    cb = q[1,:]@q[2,:]
    cc = q[2,:]@q[0,:]
    ca2 = ca*ca
    cb2 = cb*cb
    cc2 = cc*cc
    B4 = 4*b**2*c**2*ca2 - (a**2-b**2-c**2)**2
    B3 = -4*c**2*(a**2+b**2-c**2)*ca*cb - 8*b**2*c**2*ca2*cc + 4*(a**2-b**2-c**2)*(a**2-b**2)*cc
    B2 = 4*c**2*(a**2-c**2)*cb2 + 8*c**2*(a**2+b**2)*ca*cb*cc + 4*c**2*(b**2-c**2)*ca2 - 2*(a**2-b**2-c**2)*(a**2-b**2+c**2) - 4*(a**2-b**2)**2*cc2
    B1 = -8*a**2*c**2*cb2*cc - 4*c**2*(b**2-c**2)*ca*cb - 4*a**2*c**2*ca*cb + 4*(a**2-b**2)*(a**2-b**2+c**2)*cc
    B0 = 4*a**2*c**2*cb2 - (a**2-b**2+c**2)**2

    s = SolveQuarticEquation(B4,B3,B2,B1,B0)

    # print(s[:].real)
    cam_pos = np.zeros([4,3],dtype="float32")
    cam_rot = np.zeros([4,3,3],dtype="float32")
    f_cam_pos = np.zeros([4,1])
    ans_cnt = 0

    # print(len(s))
    for i in range(len(s)):
        if np.abs(s[i].imag) < 0.00000001:
            u = s[i].real
            v = -((a**2-b**2-c**2)*u**2+2*(b**2-a**2)*cc*u+(a**2-b**2+c**2))/(2*c**2*(ca*u-cb))
            x = a * a / (u * u + v * v - 2.0 * u * v * ca)
            y = b * b / (1 + v * v - 2.0 * v * cb)
            z = c * c / (1 + u * u - 2.0 * u * cc)
            if (x > 0.0):
                s1 = math.sqrt(x)
                s2 = u * s1
                s3 = v * s1
                P1 = q[0,:]*s2
                P2 = q[1,:]*s3
                P3 = q[2,:]*s1
                P12 = P2-P1
                P13 = P3-P1
                P23 = P3-P2
                zv = np.array(np.cross(P12,P13),dtype="float32")
                zv = zv/np.linalg.norm(zv)
                xv = np.array(P12,dtype="float32")
                xv = xv/np.linalg.norm(xv)
                yv = np.array(np.cross(xv,zv),dtype="float32")
                yv = yv/np.linalg.norm(yv)
                Rotc = np.zeros([3,3])
                Rotc[:,0] = xv
                Rotc[:,1] = yv
                Rotc[:,2] = zv
                P12b = pos[1,:]-pos[0,:]
                P13b = pos[2,:]-pos[0,:]
                zb = np.array(np.cross(P12b,P13b),dtype="float32")
                zb = zb/np.linalg.norm(zb)
                xb = np.array(P12b,dtype="float32")
                xb = xb/np.linalg.norm(xb)
                yb = np.array(np.cross(xb,zb),dtype="float32")
                yb = yb/np.linalg.norm(yb)
                Rota = np.zeros([3,3])
                Rota[:,0] = xb
                Rota[:,1] = yb
                Rota[:,2] = zb

                R = Rotc@Rota.T
                t = (P1.T-Rotc@Rota.T@pos[0,:].T)

                cam_pos[ans_cnt ,:] = t.copy().T
                cam_rot[ans_cnt,:,:]= R.copy()
                ans_cnt = ans_cnt + 1

    return cam_pos , cam_rot , f_cam_pos


def SolveCubicEquation(a,b,c,d):
    if a == 0.0:
        print("Error:a = 0.0\n")
        print("This equation is NOT Cubic.\n")
        return 0
    else:
        A = b/a
        B = c/a
        C = d/a
        p = B-A*A/3.0
        q = 2.0*A*A*A/27.0-A*B/3.0+C
        D = q*q/4.0+p*p*p/27.0
        if D < 0.0:
            x = np.zeros(3)
            theta = np.arctan2(np.sqrt(-D),-q*0.5)
            x[0] = 2.0*np.sqrt(-p/3.0)*np.cos(theta/3.0)-A/3.0
            x[1] = 2.0*np.sqrt(-p/3.0)*np.cos((theta+2.0*np.pi)/3.0)-A/3.0
            x[2] = 2.0*np.sqrt(-p/3.0)*np.cos((theta+4.0*np.pi)/3.0)-A/3.0
        else:
            x = np.zeros(3,dtype=complex)
            u = Cuberoot(-q*0.5+np.sqrt(D))
            v = Cuberoot(-q*0.5-np.sqrt(D))
            x[0] = u+v-A/3.0
            x[1] = complex(-0.5*(u+v)-A/3.0,np.sqrt(3.0)*0.5*(u-v))
            x[2] = complex(-0.5*(u+v)-A/3.0,-np.sqrt(3.0)*0.5*(u-v))
        return x

def SolveQuarticEquation(a,b,c,d,e):
    if a == 0.0:
        print("Error:a = 0.0\n")
        print("This equation is NOT Quartic.\n")
        return 0
    else:
        x = np.zeros(4,dtype=complex)
        A = b/a
        B = c/a
        C = d/a
        D = e/a
        p = -6.0*(A/4.0)**2.0+B
        q = 8.0*(A/4.0)**3.0-2.0*B*A/4.0+C
        r = -3.0*(A/4.0)**4.0+B*(A/4.0)**2.0-C*A/4.0+D
        t_temp = SolveCubicEquation(1.0,-p,-4.0*r,4.0*p*r-q*q)
        t = t_temp[0].real
        m = Squreroot(t-p)
        x[0] = (-m+Squreroot(-t-p+2.0*q/m))*0.5-A/4.0
        x[1] = (-m-Squreroot(-t-p+2.0*q/m))*0.5-A/4.0
        x[2] = (m+Squreroot(-t-p-2.0*q/m))*0.5-A/4.0
        x[3] = (m-Squreroot(-t-p-2.0*q/m))*0.5-A/4.0
        return x

def Cuberoot(x):
    if x > 0.0:
        return (x)**(1.0/3.0)
    else:
        return -(-x)**(1.0/3.0)
def Squreroot(x):
    r = np.sqrt(x.real*x.real+x.imag*x.imag)
    theta = np.arctan2(x.imag,x.real)
    if x.imag == 0.0:
        if x.real > 0.0:
            y = np.sqrt(r)
        else:
            y = complex(0,np.sqrt(r))
    else:
        if theta < 0.0:
            theta = theta + 2.0*np.pi
        y = complex(np.sqrt(r)*np.cos(theta*0.5),np.sqrt(r)*np.sin(theta*0.5))
    return y

def main():

    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind(serv_address)
    frame_num = 0
    struct_format = "HHHHHHHHHHHHHHHH"
    unpack_data = []

    bytesimg = bytearray()
    img_h = 160
    img_v = 120
    data_img = np.zeros([img_v,img_h],dtype=np.uint8)
    undist_x,undist_y = calc_undistmap(dist_data,mtx_data,img_h,img_v)
    frame_rate = 15 # フレームレート
    size = (img_h*4, img_v*4) # 動画の画面サイズ
    fourcc = cv2.VideoWriter_fourcc('m', 'p', '4', 'v')
    video = cv2.VideoWriter('video.mp4', fourcc, frame_rate, size) 
    print(len(bytesimg))

    while True:

        # ③Serverからのmessageを受付開始
        rx_meesage, addr = sock.recvfrom(M_SIZE)
        frame_num = rx_meesage[-1]
        if frame_num == 0 and len(bytesimg) > 0:
            if len(bytesimg) == 2400:
                data_img = np.zeros([img_v,img_h,3],dtype=np.uint8)
                for i in range(len(bytesimg)*8):
                    byte_index = i // 8  # 現在のuint8_tのインデックス
                    bit_index = i % 8    # 現在のビット位置

                    if (bytesimg[byte_index] & (1 << bit_index)) != 0 : # ビットをチェック
                        data_img[int((i - i%img_h)/img_h),i%img_h,0] = np.uint8(255)
                        data_img[int((i - i%img_h)/img_h),i%img_h,1] = np.uint8(255)
                        data_img[int((i - i%img_h)/img_h),i%img_h,2] = np.uint8(255)

                if len(unpack_data)>0:
                    marker_pos = np.zeros([4,2],dtype=int)
                    marker_pos[0,0] = unpack_data[0]
                    marker_pos[0,1] = unpack_data[1]
                    marker_pos[1,0] = unpack_data[2]
                    marker_pos[1,1] = unpack_data[3]
                    marker_pos[2,0] = unpack_data[4]
                    marker_pos[2,1] = unpack_data[5]
                    marker_pos[3,0] = unpack_data[6]
                    marker_pos[3,1] = unpack_data[7]

                    marker_posd = calc_distortion(undist_x,undist_y,marker_pos)
                    cam_pos1 , cam_rot1 , f_cam_pos = calcSimpleP3P(marker_posd[0:3,:],marker_3dpos[0:3,:],mtx_data)

                    uv1 = calcProjection(marker_3dpos[3,:],cam_pos1[0,:],cam_rot1[0,:],mtx_data)
                    uv2 = calcProjection(marker_3dpos[3,:],cam_pos1[1,:],cam_rot1[1,:],mtx_data)
                    uv1err = (uv1[0,0] - marker_posd[3,0])**2 + (uv1[0,1] - marker_posd[3,1])**2
                    uv2err = (uv2[0,0] - marker_posd[3,0])**2 + (uv2[0,1] - marker_posd[3,1])**2
                    
                    if uv1err < uv2err:
                        mkpos = cam_pos1[0,:]
                        mkrpy = rot2rpy(cam_rot1[0,:,:])*180/np.pi
                    else:
                        mkpos = cam_pos1[1,:]
                        mkrpy = rot2rpy(cam_rot1[1,:,:])*180/np.pi
                    cv2.circle(data_img,center=uv1[0,:], radius=1,color=(0, 255, 0),thickness=1,lineType=cv2.LINE_4,shift=0)
                    cv2.circle(data_img,center=uv2[0,:], radius=1,color=(255, 0, 0),thickness=1,lineType=cv2.LINE_4,shift=0)
                    cv2.line(data_img, marker_posd[0,:], marker_posd[1,:], (0, 0, 255), thickness=1, lineType=cv2.LINE_AA)
                    cv2.line(data_img, marker_posd[1,:], marker_posd[2,:], (0, 0, 255), thickness=1, lineType=cv2.LINE_AA)
                    cv2.line(data_img, marker_posd[2,:], marker_posd[3,:], (0, 0, 255), thickness=1, lineType=cv2.LINE_AA)
                    cv2.line(data_img, marker_posd[3,:], marker_posd[0,:], (0, 0, 255), thickness=1, lineType=cv2.LINE_AA)
                    font = cv2.FONT_HERSHEY_SIMPLEX

                    data_img=cv2.resize(data_img,None,fx=4,fy=4,interpolation=cv2.INTER_CUBIC)
                    cv2.putText(data_img,text="xyz[m]   :" +  "{:8.3f}".format(mkpos[0])+ "," + "{:8.3f}".format(mkpos[1]) + "," + "{:8.3f}".format(mkpos[2]),org=(10,20), fontFace=font,fontScale=0.5,color=(0,0,0),thickness=1,lineType=cv2.LINE_AA)
                    cv2.putText(data_img,text="rpy[deg.]:" + "{:8.3f}".format(mkrpy[0]) + "," + "{:8.3f}".format(mkrpy[1]) + "," + "{:8.3f}".format(mkrpy[2]),org=(10,40), fontFace=font,fontScale=0.5,color=(0,0,0),thickness=1,lineType=cv2.LINE_AA)

                    cv2.imshow('window title', data_img)
                    video.write(data_img)

            bytesimg = bytearray()

        if frame_num == 0:
            bytesbuf = bytearray()
            bytesbuf = rx_meesage[:-1]
            unpack_data = unpack(struct_format, bytesbuf)
        else:
            bytesbuf = bytearray()
            bytesbuf = rx_meesage[:-1]
            bytesimg = bytesimg + bytesbuf

        if cv2.waitKey(1) & 0xFF == ord('q'):
            sock.close()
            cv2.destroyAllWindows()

            break

if __name__ == '__main__':
    main()

コメント無し