相机标定实验
相机标定
文章目录
- 相机标定
- 1 ROS标定
- 1.1安装标定程序
- 1.2 下载标定板
- 1.3 标定
- 1.4 标定结果
- 2 Kalibr相机标定
- 2.1 下载官方提供的标定板
- 2.2 自定义标定板
- 2.3 cam数据录制
- 2.4 标定
- 2.5 输出结果
- 3 MATLAB标定
- 3.1 打开工具
- 3.2 添加标定板图片
- 3.3 设置标定参数
- 3.4 生成标定结果
- 3.5 ���定结果
1 ROS标定
这里使用了OpenCV camera calibration
1.1安装标定程序
sudo apt install ros-$ROS_DISTRO-camera-calibration # https://github.com/ros-perception/image_pipeline/tree/melodic源码安装
选择对应的ROS版本,新建一个工作空间,把camera-calibration拖进去编译即可。
1.2 下载标定板
可以自己生成生成标定板网站,也可以直接用这个
1.3 标定
- 打开相机标定节点并指定话题
rosrun camera_calibration cameracalibrator.py --size 9x6 --square 0.02 image:=/camera2/camera/color/image_raw
–size 9x6 表示棋盘格按照角点(两个黑色块相接的地方)为9列6行
–square 0.02 表示每个黑白块的边长为2cm
image:=/camera/rgb/image_raw 设置接收的rgb图像话题
更多参数参考:http://wiki.ros.org/camera_calibration
这里还有个报错,显示找不到cameracalibrator这个可执行文件,因为这个文件是从windows拖到车里的,所以这些python文件没有可执行权限,所以运行失败
chmode +x xxx.py # 添加执行权限
- calibration file应该类似如下
image_width: 640 image_height: 480 # The camera name is fixed. The color camera is rgb_camera, the depth/IR camera name is ir_camera camera_name: rgb_camera camera_matrix: rows: 3 cols: 3 data: [517.301, 0, 326.785, 0, 519.291, 244.563, 0, 0, 1] distortion_model: plumb_bob distortion_coefficients: rows: 1 cols: 5 data: [-0.41527, 0.31874, -0.00197, 0.00071, 0] rectification_matrix: rows: 3 cols: 3 data: [0.999973, 0.00612598, -0.00406652, -0.00610201, 0.999964, 0.00588094, 0.0041024, -0.00585596, 0.999974 ] projection_matrix: rows: 3 cols: 4 data: [517.301, 0, 326.785, -25.3167, 0, 519.291, 244.563, 0.282065, 0, 0, 1, 0.0777703]
1.4 标定结果
生成压缩包/tmp/calibrationdata.tar.gz',其中的 ost.yaml就是标定结果。
[image] width:800 height:600 [narrow_stereo] camera matrix 375.192406 0.000000 400.541492 0.000000 376.253860 317.513124 0.000000 0.000000 1.000000 distortion 0.131676 -0.094047 0.004399 -0.004491 0.000000 rectification 1.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 1.000000 projection 786.365845 0.000000 -8.585413 0.000000 0.000000 862.199036 -9.969647 0.000000 0.000000 0.000000 1.000000 0.000000
2 Kalibr相机标定
Kalibr安装参考官网,下载好依赖即可,安装过程可能会出现内存不足情况,且编译时间很久。
2.1 下载官方提供的标定板
标定板下载–Aprilgrid 6x6 0.8x0.8 m (A0 page)
标定前, 注意测量格子的尺寸信息填入yaml文件
target_type: 'aprilgrid' #gridtype tagCols: 6 #number of apriltags---6列 tagRows: 6 #number of apriltags---6行 tagSize: 0.088 # size of apriltag, edge to edge [m]--a tagSpacing: 0.3 #ratio of space between tags to tagSize--a/b codeOffset: 0 #code offset for the first tag in the aprilboard
2.2 自定义标定板
可以使用下面命令自定义一个Aprilgrid标定板(前提安装好kalibr)
rosrun kalibr kalibr_create_target_pdf --type apriltag --nx [NUM_COLS] --ny [NUM_ROWS] --tsize [TAG_WIDTH_M] --tspace [TAG_SPACING_PERCENT]
报错:kalibr_create_target_pdf: command not found,因为执行ROS命令没有加rosrun kalibr
# 这样子实际大小在一张A4左右 rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.025 --tspace 0.3
参考
2.3 cam数据录制
将ros topic的频率降低到4hz左右进行采集
ROS 提供了改变 topic 发布频率的节点throttle, 指令如下
# 使用方法rosrun topic_tools throttle messages [outtopic] rosrun topic_tools throttle messages /mynteye/left/image_raw 4.0 /left rosrun topic_tools throttle messages /mynteye/right/image_raw 4.0 /right
录制数据
rosbag record -O stereo_calibra.bag /left /right-
2.4 标定
官网给了这个示例,可以先跑下这个看看对不对
source ros_ws/kalibr/devel/setup.bash # 径向和切向畸变 rosrun kalibr kalibr_calibrate_cameras --bag stereo_calibra.bag --topics /left /right --models pinhole-radtan pinhole-radtan --target april_6x6_80x80cm_A0.yaml # 或者采用 pinhole-equi 模型,对畸变大的相机效果不错----全景畸变 rosrun kalibr kalibr_calibrate_cameras --bag stereo_calibra.bag --topics /left /right --models pinhole-equi pinhole-equi --target april_6x6_80x80cm_A0.yaml
该工具必须提供以下输入:
- –bag filename.bag
包含数据的 ROS 包
- –topics TOPIC_0 … TOPIC_N
包中所有相机主题的列表。匹配 --models 的顺序
- –models MODEL_0 … MODEL_N
要安装的相机/畸变模型列表。与 --topics 的顺序匹配(请参阅支持的型号)
- –target target.yaml
校准目标配置(请参阅校准目标)
关于支持的相机型号模型
Kalibr支持以下投影模型
- pinhole camera model (pinhole)1-针孔相机模型
(intrinsics vector: [fu fv pu pv])
- omnidirectional camera model (omni)2-全向相机模型
(intrinsics vector: [xi fu fv pu pv])
- double sphere camera model (ds)3
(intrinsics vector: [xi alpha fu fv pu pv])
- extended unified camera model (eucm)4
(intrinsics vector: [alpha beta fu fv pu pv])
The intrinsics vector contains all parameters for the model:
- fu, fv: focal-length焦距
- pu, pv: principal point主点
- xi: mirror parameter (only omni)镜像参数
- xi, alpha: double sphere model parameters (only ds)双球模型参数
- alpha, beta: extended unified model parameters (only eucm)
Distortion models畸变模型
- radial-tangential (radtan)*-径向切向畸变
(distortion_coeffs: [k1 k2 r1 r2])
- equidistant (equi)**
(distortion_coeffs: [k1 k2 k3 k4])
- fov (fov)5
(distortion_coeffs: [w])
- none (none)
(distortion_coeffs: [])
请注意,主题 (–topics) 和相机/畸变模型 (–model) 的顺序必须匹配并确定输出中的内部相机编号。
可以使用以下方式运行校准:
kalibr_calibrate_cameras --bag [filename.bag] --topics [TOPIC_0 ... TOPIC_N] --models [MODEL_0 ... MODEL_N] --target [target.yaml]
由于对焦距的初始猜测错误,在处理前几张图像后,优化可能会出现偏差。在这种情况下,只需尝试重新启动校准,因为初始猜测是基于随机选择的图像。
使用 help 参数可以获得有关选项的更多信息:
kalibr_calibrate_cameras --h
示例包文件的示例命令(在此处下载):
# 就是要给出配置文件yaml和bag的路径! rosrun kalibr kalibr_calibrate_cameras \ --target april_6x6.yaml \ --models pinhole-radtan pinhole-radtan \ --topics /cam0/image_raw /cam1/image_raw \ --bag cam_april.bag \ --bag-freq 10.0
2.5 输出结果
大车没电了,下次补下
3 MATLAB标定
参考
3.1 打开工具
-
命令行窗口输入 cameraCalibrator
-
或在APP中找到图像处理和计算机视觉,点击Camera Calibrator
3.2 添加标定板图片
若标定图为棋盘格:
更改参数,因为这里是20mm的标定板
3.3 设置标定参数
CAMERA MODEL 栏中
-
Camera Model:选择 Standard
-
Options:
-
选择 2 Coefficients,3 Coefficients 一般用于鱼眼相机,工业相机一般不选择此项
-
勾选 Tangential Distortion,用以计算切向畸变
-
不勾选 Skew,若勾选,标定相机内参结果将出现参数 s ,即内参的第一行是 [fx, s, u0] ,这将会与我们使用的 OpenCV 进行测距的参数不同
然后点击校准
3.4 生成标定结果
拖拽红线,让误差尽可能小,抛弃哪些大于红线的图像
最后输出标定结果
3.5 标定结果
Camera Intrinsics # 相机内参 IntrinsicMatrix: [3×3 double] 焦距 FocalLength: [fx、fy] PrincipalPoint: [cx、cy] Skew: 0 # 倾斜系数,描述了图像的x轴和y轴之间的角度偏差,一般情况下为0 RadialDistortion: [k1、k2] #径向畸变参数 TangentialDistortion: [p1、p2] # 切向畸变参数, ImageSize: [600 800] Camera Extrinsics # 相机外参 RotationMatrices: [3×3×52 double] TranslationVectors: [52×3 double] Accuracy of Estimation MeanReprojectionError: xxx # 平均重投影误差
最终校准结果
Camera Intrinsics IntrinsicMatrix: [3×3 double] FocalLength: [371.4645 372.0877] PrincipalPoint: [402.1875 313.9287] Skew: 0 RadialDistortion: [0.1160 -0.0816] TangentialDistortion: [0.0026 -0.0053] ImageSize: [600 800] Camera Extrinsics RotationMatrices: [3×3×19 double] TranslationVectors: [19×3 double] Accuracy of Estimation MeanReprojectionError: 0.1345 ReprojectionErrors: [54×2×19 double] ReprojectedPoints: [54×2×19 double] Calibration Settings NumPatterns: 19 WorldPoints: [54×2 double] WorldUnits: 'millimeters' EstimateSkew: 0 NumRadialDistortionCoefficients: 2 EstimateTangentialDistortion: 1 estimationErrors = cameraCalibrationErrors - 属性: IntrinsicsErrors: [1×1 intrinsicsEstimationErrors] ExtrinsicsErrors: [1×1 extrinsicsEstimationErrors]
-
-
-
- radial-tangential (radtan)*-径向切向畸变
- pinhole camera model (pinhole)1-针孔相机模型
- –bag filename.bag
- calibration file应该类似如下
- 打开相机标定节点并指定话题