实战 | 巧用位姿解算实现单目相机测距

1、角点检测函数bool findChessboardCorners(InputArray image,       Size patternSize,      OutputArray corners,       int flags=CALIB_CB_ADAPTIVE_THRESH+CALIB_CB_NORMALIZE_IMAGE       ); // image：传入拍摄的棋盘图Mat图像，必须是8位的灰度或者彩色图像// patternSize：每个棋盘图上内角点的行列数，一般情况下，行列数不要相同，便于后续标定程序识别标定板的方向；// corners：用于存储检测到的内角点图像坐标位置，一般用元素是Point2f的向量来表示：vector<Point2f> image_points_buf;// flage：用于定义棋盘图上内角点查找的不同处理方式，有默认值。

2、提取亚像素角点信息专门用来获取棋盘图上内角点的精确位置，降低相机标定偏差，还可以使用cornerSubPix函数bool find4QuadCornerSubpix(InputArray img,       InputOutputArray corners,       Size region_size);// img：输入的Mat矩阵，最好是8位灰度图像，检测效率更高// corners：初始的角点坐标向量，同时作为亚像素坐标位置的输出vector<Point2f> iamgePointsBuf；// region_size：角点搜索窗口的尺寸

3、相机标定double calibrateCamera( InputArrayOfArrays objectPoints,          InputArrayOfArrays imagePoints,      Size imageSize,      CV_OUT InputOutputArray cameraMatrix,      CV_OUT InputOutputArray distCoeffs,      OutputArrayOfArrays rvecs,       OutputArrayOfArrays tvecs,      int flags=0,       TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, DBL_EPSILON)      );  // objectPoints：世界坐标系中的三维点，三维坐标点的向量的向量vector<vector<Point3f>> object_points// imagePoints：每一个内角点对应的图像坐标点，vector<vector<Point2f>> image_points_seq形式// imageSize：图像的像素尺寸大小（列数=cols，行数=rows）（宽度=width，高度=height）// cameraMatrix：相机的3*3内参矩阵，Mat cameraMatrix=Mat(3,3,CV_32FC1,Scalar::all(0));// distCoeffs：1*5畸变矩阵，Mat distCoeffs=Mat(1,5,CV_32FC1,Scalar::all(0))// rvecs：旋转向量，输入一个Mat类型的vector，即vector<Mat>rvecs;// tvecs：位移向量，和rvecs一样，应该为vector<Mat> tvecs；// flags：标定时所采用的算法// criteria：最优迭代终止条件设定

void solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags = CV_ITERATIVE)Parameters:objectPoints - 世界坐标系下的控制点的坐标，vector<Point3f>的数据类型在这里可以使用imagePoints - 在图像坐标系下对应的控制点的坐标。vector<Point2f>在这里可以使用cameraMatrix - 相机的内参矩阵distCoeffs - 相机的畸变系数以上两个参数通过相机标定可以得到。相机的内参数的标定参见：http://www.cnblogs.com/star91/p/6012425.htmlrvec - 输出的旋转向量。使坐标点从世界坐标系旋转到相机坐标系tvec - 输出的平移向量。使坐标点从世界坐标系平移到相机坐标系flags - 默认使用CV_ITERATIV迭代法

# -*-coding:utf-8-*-import cv2import numpy as np
class PnPDistance():    def __init__(self, args):        self.category = args.classes_names        self.cam = np.array([1.5880436204354560e+03, 0., 960., 0., 1.5880436204354560e+03,                        600., 0., 0., 1.], dtype=np.float64).reshape((3, 3))        self.distortion = np.array([-1.8580303062080919e-01, 5.7927645928450899e-01,                               5.5271164249844681e-03, -1.2684978794253729e-04,                               -5.6884229185639223e-01], dtype=np.float64).reshape((1, 5))
        self.obj_true_size = [[180, 180], [250, 250], [170, 45], [100, 150], [250, 180],                  [100, 150], [2000, 1000], [170, 45], [200, 100],                  [110, 48], [110, 48], [110, 48], [110, 48],                  [110, 48], [40, 105], [40, 105], [40, 105], [80, 30],                  [80, 30], [80, 30], [80, 30], [80, 30], [80, 30], [80, 30], [80, 30], [80, 30], [70, 35], [45, 45]]
    def get_distance(self, obj_dict):        """        :param bbox: x,y,w,h        :param category: category        :return:        """        # boxes = []        # categories = []        out_dist = []        out_dist_size = []        for keys, items in obj_dict.items():            left, top, right, bottom = items['bbox']            bbox = [left, top, right-left, bottom-top]            category = self.category.index(items['label'].split(' ')[0])            # boxes.append(bbox)            # categories.append(category)
            obj_pw, obj_ph = self.obj_true_size[category]            if obj_pw < obj_ph:                obj_pw = obj_ph            else:                obj_ph = obj_pw            obj_p = np.array([(0, 0, 0),                              (obj_pw, 0, 0),                              (obj_pw, obj_ph, 0),                              (0, obj_ph, 0)],                             dtype=np.float64).reshape((1, 4, 3))            img_rect_w, img_rect_h = bbox[2], bbox[3]            if img_rect_w < img_rect_h:                img_rect_w = img_rect_h            else:                img_rect_h = img_rect_w            img_points = np.array([(bbox[0], bbox[1]),                                   (bbox[0] + img_rect_w, bbox[1]),                                   (bbox[0] + img_rect_w, bbox[1] + img_rect_h),                                   (bbox[0] + img_rect_w, bbox[1])],                                  dtype=np.float64).reshape((1, 4, 2))
            ret_val, r_vec, t_vec = cv2.solvePnP(obj_p, img_points, self.cam, self.distortion,                                                 useExtrinsicGuess=False,                                                 flags=cv2.SOLVEPNP_AP3P)            out_dist.append([round(t_vec[0][0] / 100.0, 1), round(t_vec[2][0] / 100.0, 1)])            out_dist_size.append(self.obj_true_size[category])
        return out_dist, out_dist_size