相机标定-kinectv1外参标定（epnp）

说明

1、kinectv1外参标定
2、使用Opencv的epnp函数实现
3、python编程

一、标定需求

根据kinectv1的彩色相机得到像素坐标（u，v），通过kinectv1的深度相机得到（u,v）对应相机坐标系中的Zc，然后根据求得的相机内外参数还原出该点的真实世界坐标。
如果我们没有深度信息，只能求出平面的世界坐标，实际的Z值是还原不出来的，看这一篇博客，里面包括相机标定的原理和标定涉及的各个坐标系的关系及实际意义

二、相机介绍

kinect相机介绍

深度信息的实际意义是点到相机光轴平面的垂直距离，即相机坐标系中的Zc值

三、内参标定

kinect相机内参标定
实际使用时用彩色的标定文件即可

四、标定方法

1、原理及实用相关文章

使用pnp的方法进行标定，pnp的标定原理网上非常多，这篇文章原理我觉得比较不错，这一篇的应用我觉得很好。
后一篇目录：
《相机位姿估计0：基本原理之如何解PNP问题》
《相机位姿估计1：根据四个特征点估计相机姿态》
《相机位姿估计1_1：OpenCV:solvePnP二次封装与性能测试》
《相机位姿估计2：[应用]实时位姿估计与三维重建相机姿态》
《相机位姿估计3：根据两幅图像的位姿估计结果求某点的世界坐标》
pnp求解为啥要4对坐标：最通俗的几何图像解释法

2、opencv中的pnp标定函数solvepnp

retval1, rvec1, tvec1=cv.solvePnP(world_point,pixel_point,K,distortion_coefficients)

该函数的参数依次输入世界坐标，像素坐标，内参矩阵，畸变矩阵，迭代次数，方法
最后一个参数有如下三种，博主用了迭代求解和epnp，两种方法均可以还原出世界坐标，差别不大。

五、标定实验

相机的高度在桌面上方1m的位置，光轴面与桌面具有一定的倾斜夹角，棋盘格每一个格子4cm。

1、点的选取

采用默认的方法时选取下图中的四个角为标定点。
采用epnp标定时，
第一个控制点选取下图中的淡蓝色点
第二个控制点选择蓝色的点
第三个控制点选取红色的点
第四个控制点选取绿色的点

世界坐标的方向，
垂直桌面向上为Z轴正方向
蓝色点到红色点为Y轴负方向
蓝色到绿色点为X轴正方向

2、epnp函数调用代码(python)

# encoding: utf-8
import cv2 as cv
import numpy as np
import yaml
import os
import io
with open("file/rgb_A22593W01131223A.yaml", 'rb') as f:   #写入正确的文件路径
    data = yaml.load(f)
    camera_matrix = data['camera_matrix']['data']
    distortion_coefficients = data['distortion_coefficients']['data']

distortion_coefficients=np.array(distortion_coefficients)


K= np.zeros([3, 3], dtype=float)
# K=np.mat(K)

K[0,0]=camera_matrix[0]
K[0,1]=camera_matrix[1]
K[0,2]=camera_matrix[2]

K[1,0]=camera_matrix[3]
K[1,1]=camera_matrix[4]
K[1,2]=camera_matrix[5]
#
K[2,0]=camera_matrix[6]
K[2,1]=camera_matrix[7]
K[2,2]=camera_matrix[8]



pixel_point= np.array([
    [328.0, 172.0],
    [158.0, 58.0],
    [137.0, 310.0],
    [508.0, 55.0]
                            ])
world_point= np.array([
                            [360.0, -280.0, 90.0],
                             [0.0, 0.0, 0.0],
                             [0.0, -520.0, 0.0],
                            [720.0, 0.0, 0.0]
                            ])

retval1, rvec1, tvec1=cv.solvePnP(world_point,pixel_point,K,distortion_coefficients, useExtrinsicGuess=1, flags=cv.SOLVEPNP_EPNP)
r1 = cv.Rodrigues(rvec1,jacobian=0)
print(r1[0])
print(tvec1)

3、结果分析

（1）像素坐标和实际坐标

pixel_point= np.array([
    [328.0, 172.0],
    [158.0, 58.0],
    [137.0, 310.0],
    [508.0, 55.0]
                            ])
world_point= np.array([
                            [360.0, -280.0, 90.0],
                             [0.0, 0.0, 0.0],
                             [0.0, -520.0, 0.0],
                            [720.0, 0.0, 0.0]
                            ])

（2）外参矩阵

[[ 9.98539408e-01  1.49732959e-02 -5.19119596e-02 -3.28108974e+02]
 [ 3.34250968e-03 -9.76109922e-01 -2.17251577e-01 -3.78811896e+02]
 [-5.39247510e-02  2.16760745e-01 -9.74734272e-01  1.18536981e+03]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]

（3）验算结果

四个控制点以及两个任意点的验证，相机自身得到的深度值进行了7厘米的补偿。
两个任意点坐标

720,-520, 0
200,-80,25

结果

[[ 3.59876941e+02 -5.67379331e-01 -7.67101745e+00  7.19528649e+02 7.25095153e+02  2.00069031e+02]
 [-2.80541796e+02  7.66719036e-01 -5.24430480e+02 -7.01423857e-01 -5.22356724e+02 -8.39731426e+01]
 [ 9.35158355e+01  1.39182581e+01 -6.04520752e+00  8.63555196e+00 -9.18937150e+00  2.29591386e+01]
 [ 1.00000000e+00  1.00000000e+00  1.00000000e+00  1.00000000e+00 1.00000000e+00  1.00000000e+00]]

误差在一厘米以内