สอบเทียบกล้องและ IMU

ตัวอย่างที่ 1 วัดระยะบนลานจอดจากภาพเฉียง

มุมทั้งสี่ของลานจอดขนาด 10 × 20 m ปรากฏในภาพที่ตำแหน่งพิกเซลดังนี้ ต้องการระยะระหว่างสองจุดบนพื้นที่เห็นในภาพ (ข้อมูลจำลอง)

import numpy as np

img_pts = np.array([[412, 830], [1508, 812], [1320, 402], [590, 410.]])   # พิกเซล
ground = np.array([[0, 0], [10, 0], [10, 20], [0, 20.]])                 # เมตร

def dlt(src, dst):
    rows = []
    for (x, y), (u, v) in zip(src, dst):
        rows.append([-x, -y, -1, 0, 0, 0, u * x, u * y, u])
        rows.append([0, 0, 0, -x, -y, -1, v * x, v * y, v])
    _, _, vt = np.linalg.svd(np.array(rows))
    h = vt[-1].reshape(3, 3)
    return h / h[2, 2]

def to_ground(H, p):
    q = H @ np.r_[p, 1.0]
    return q[:2] / q[2]

H = dlt(img_pts, ground)
a, b = to_ground(H, [960, 700]), to_ground(H, [960, 450])
print("point A on ground (m):", np.round(a, 2))
print("point B on ground (m):", np.round(b, 2))
print(f"distance A-B: {np.linalg.norm(a - b):.2f} m, while in the image they are {700 - 450} px apart")
print("check corner:", np.round(to_ground(H, img_pts[2]), 3))

point A on ground (m): [4.98 4.3 ]
point B on ground (m): [ 5.03 16.97]
distance A-B: 12.67 m, while in the image they are 250 px apart
check corner: [10. 20.]

สองจุดห่างกันในภาพ 250 พิกเซล แต่บนพื้นห่างกันกว่า 12 m เพราะส่วนบนของภาพอยู่ไกลกล้องกว่า หนึ่งพิกเซลจึงแทนระยะบนพื้นมากกว่า homography ใช้ได้เฉพาะจุดบนระนาบเดียวกันเท่านั้น วัตถุที่มีความสูง เช่น เสา จะถูกฉายผิดตำแหน่ง

ตัวอย่างที่ 2 หาเวลาคลาดด้วย cross-correlation

อัตราหมุนจาก gyro กับอัตราหมุนที่ประมาณจากภาพ (สุ่มที่ 200 Hz) โดยสัญญาณจากภาพช้ากว่าจริง 35 ms (ข้อมูลจำลอง)

import numpy as np

fs = 200
t = np.arange(0, 10, 1 / fs)
gyro = np.sin(2 * np.pi * 0.7 * t) + 0.5 * np.sin(2 * np.pi * 1.9 * t + 1)
TRUE_OFFSET = 0.035                              # s
cam = np.interp(t - TRUE_OFFSET, t, gyro) + np.random.default_rng(1).normal(0, 0.05, t.size)

lags = np.arange(-40, 41)                        # ตัวอย่าง = ±200 ms
score = [np.dot(gyro[50:-50], np.roll(cam, -lag)[50:-50]) for lag in lags]
est = lags[int(np.argmax(score))] / fs
print(f"estimated offset {est * 1000:.0f} ms (true {TRUE_OFFSET * 1000:.0f} ms, resolution {1000 / fs:.0f} ms)")
rate = 60                                        # deg/s ขณะหมุนหัว
print(f"at {rate} deg/s an uncorrected offset of {TRUE_OFFSET * 1000:.0f} ms means {rate * TRUE_OFFSET:.1f} deg of heading error")

estimated offset 40 ms (true 35 ms, resolution 5 ms)
at 60 deg/s an uncorrected offset of 35 ms means 2.1 deg of heading error

ค่าที่ได้คลาดจากค่าจริงไม่เกินหนึ่งช่วงการสุ่ม ความละเอียดขึ้นกับอัตราการสุ่ม และปรับให้ละเอียดขึ้นได้ด้วยการประมาณระหว่างจุด แม้เวลาคลาดดูน้อย แต่ขณะหมุนหัวเร็ว ทิศที่คำนวณจะผิดหลายองศา ซึ่งมากพอให้ระบบนำทางด้วยภาพเสียความแม่นยำ