学习单目视觉定姿原理

问题和思考

自己采集了一些数据，然后找网上的代码实现了单目视觉定姿。只是把基本流程过了一遍，了解以下单目视觉定姿是什么，没有处理细节问题。如果是想要开始视觉导航学习，或者像我一样，单纯是闲下来玩点不一样的东西，可以看看我的学习过程。

但是在玩单目视觉导航的过程中，发现了一些问题： - 相机和IMU的时间同步问题（数据量大，传输存储都比较费时，同步不好做） - 相机数据处理/传输问题，一般的MCU或者DSP可以正常处理图像数据吗？ - 分辨率和帧率问题，分辨率高时，定姿数据处理结果会好一些，更加准确而且稳定，低分辨率更加适合实时数据处理，但是处理结果会差一些 - 我目前是1s采集一次，视觉数据1Hz的频率可能无法应用于实际系统，还是计算能力有限和实时性需要之间的矛盾 - 单目视觉定姿的误差比较随机，有时候会存在零偏，可能是光源变化引起的，误差方差的影响因素有点多，后期数据融合可能会有点麻烦

既然用手机都标定好了，那就把图像采集和我之前写的SensorService，结合起来，这样就多一组导航数据，说不定以后在手机上实现一个IMU/GNSS/地磁/气压/视觉导航的融合导航系统。数据采集这块完成了一个Demo，问题还是不少了的，一个人开发、调试太费时间了，等有空了再完善吧。项目主页

视觉定姿基本原理

原理非常简单，网上的资料可以很快了解底层的基本原理，公式、示意图什么的就没有必要再列出来了。

单目视觉定姿是指利用单目摄像机获取的图像信息，通过计算机视觉算法，实现对目标物体的三维空间位置和姿态的测量。单目视觉定姿的基本原理是通过摄像机成像原理，将三维空间中的物体映射到二维图像平面上，然后通过对图像中的特征点进行提取、匹配、跟踪等操作，计算出物体的三维空间位置和姿态。单目视觉定姿的具体实现方法有很多，包括基于特征点的方法、基于深度学习的方法等。如果您需要更多关于单目视觉定姿的详细信息，可以参考以下链接：1 2 3。 --- From New Bing

视觉定姿编程实现

如果有编程基础（会C/C++、懂python基础），那就直接开启AI编程，基本上半个小时就把程序的实现逻辑写完。

New Bing给的代码示例：

import cv2
import numpy as np

# Load the image
img = cv2.imread('image.jpg')

# Convert the image to grayscale
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

# Detect the keypoints using SIFT
sift = cv2.SIFT_create()
kp = sift.detect(gray, None)

# Draw the keypoints on the image
img_kp = cv2.drawKeypoints(img, kp, None)

# Display the image
cv2.imshow('Image', img_kp)
cv2.waitKey(0)
cv2.destroyAllWindows()

虽然直接运行可能会有各种问题，但是网上找找问题的答案，就能把程序调试好，顺便还可以多了解一些openCV编程的基础。

软件和设备

Matlab
python
Iriun Webcam（Win/Android）
Android手机（顶级全面屏手机Mix2s）

图像数据采集程序（要安装webcam）：

N = 60;
cam = webcam(2);        % 连接相机（2为手机摄像头）
cam.Resolution = cam.AvailableResolutions{end};  % 设置最大分辨率
imgs = cell(N,1);
timebar(1,N);
for idx = 1:N
    % Acquire a single image.
    rgbImage = snapshot(cam);
    imgs{idx} = rgbImage;
    imshow(imgs{idx});
    pause(0.2);
    timebar;
%     imwrite(imgs{idx}, ['Clbt/',num2str(idx),'.jpg']);    % 标定文件夹
    imwrite(imgs{idx}, ['test/',num2str(idx),'.jpg']);      % 测试文件夹
end
clear('cam');

相机标定

相机标定的目的是建立相机成像几何模型并矫正透镜畸变1。相机的内参矩阵反应了相机自身的属性，各个相机是不一样的，需要标定才能知道这些参数1。标定相机后，可以校正透镜畸变，生成矫正后的图像2。相机内参矩阵是一个3x3的矩阵，它包含了相机的焦距、光心、像素大小等信息3。相机内参矩阵的标定可以通过拍摄棋盘格等方式进行3。更多关于相机标定的知识可以参考以下链接： - 相机内参详解 - 相机内参标定究竟标了什么？相机内参外参保姆级教程 - 相机标定：张正友标定原理 --- From New Bing

相机标定程序：

import numpy as np
import cv2
import glob

# 设置棋盘格的尺寸
chessboard_size = (7, 6)

# 设置世界坐标系中的三维点
objp = np.zeros((np.prod(chessboard_size), 3), dtype=np.float32)
objp[:, :2] = np.indices(chessboard_size).T.reshape(-1, 2)

# 初始化世界坐标系和相机坐标系的3D点数组
objpoints = []  # 在世界坐标系中的3D点
imgpoints = []  # 在图像平面上的2D点

# 读取棋盘格图片
images = glob.glob('Clbt\*.jpg')
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    cv2.imshow('img', gray)
    # 寻找棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)

    # 如果找到足够的角点，将其添加到对象点和图像点数组中
    if ret:
        objpoints.append(objp)
        imgpoints.append(corners)

        # 绘制并显示角点
        cv2.drawChessboardCorners(img, chessboard_size, corners, ret)
        cv2.imshow('img', img)
        cv2.waitKey(500)

cv2.destroyAllWindows()

# 校准相机
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

print("相机内参矩阵：")
print(mtx)
print("畸变系数：")
print(dist)

# Mix2s相机内参
K = np.array([[900,   0,   640],
              [  0,   900, 360],
              [  0,   0,    1 ]])
Dist = np.array([ 0.4178, -2.3712,  0.001202,  0.001795,  3.9300])

数据处理结果

前后两幅图片得到的姿态是姿态变化量，可以认为是旋转矢量，要得到手机相对于初始时刻的姿态，还需要进行姿态更新。