相机和检测-03-去畸变、3D坐标重建、3D立方体重建

1. 去畸变，利用remap()或者undistort()函数

// 初始化去畸变纠正变换Map
Mat map1, map2;
initUndistortRectifyMap(intrinsic_matrix, distortion_coeffs, Mat(),
                        intrinsic_matrix, image_size, CV_16SC2, map1, map2);

FileStorage fs("./calib_chessboard.yml", FileStorage::READ);
cv::Size imageSize;
fs["imageSize"] >> imageSize;

Mat cameraMatrix, distCoeffs;
fs["cameraMatrix"] >> cameraMatrix;
fs["distCoeffs"] >> distCoeffs;

std::cout << "imageSize: " << imageSize << std::endl;
std::cout << "cameraMatrix: \n" << cameraMatrix << std::endl;
std::cout << "distCoeffs: " << distCoeffs << std::endl;


const Mat image0 = cv::imread("./data/image_02.jpg", FileStorage::READ);
Mat image;
//initUndistortRectifyMap(intrinsic_matrix, distortion_coeffs, Mat(),intrinsic_matrix, image_size, CV_16SC2, map1, map2);
//remap(image0, image, map1, map2,cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar());
//经常使用下面这个简单版的畸变校正方法
undistort(image0, image, cameraMatrix, distCoeffs);

2. 3D坐标系的重建

• 根据物体坐标点和像素坐标点估算物体姿态，并建立坐标系

1. 加载相机内参矩阵、畸变系数
2. 加载图片
3. 查找每个图片的角点
4. 查找角点亚像素
5. 计算对象姿态solvePnpRansac
6. 投影3D点到图像平面
7. 在图片上坐标系并显示图片
   根据对象点和角点列表，查找旋转向量和平移向量cv2.solvePnPRansac(objp, corners2, mtx, dist),然后获得映射矩阵imagePoints, jacobian = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)，得到imagePoint后绘制坐标系

示例代码如下：

"""
根据物体坐标点和像素坐标点估算物体姿态，并建立坐标系

1. 加载相机内参矩阵、畸变系数
2. 加载图片
3. 查找每个图片的角点
4. 查找角点亚像素
5. 计算对象姿态solvePnpRansac
6. 投影3D点到图像平面
7. 在图片上坐标系并显示图片
"""
import numpy as np
import cv2
import glob


def draw(img, corners, imgpts):
# corners角点坐标
    corner = tuple(corners[0].ravel())
    img = cv2.line(img, corner, tuple(imgpts[0].ravel()), (255,0,0), 5)
    img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0,255,0), 5)
    img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (0,0,255), 5)
    return img

if __name__ == '__main__':
    fs = cv2.FileStorage("./calibration_in_params.yml", cv2.FILE_STORAGE_READ)
    mtx = fs.getNode("cameraMatrix").mat()
    dist = fs.getNode("distCoeffs").mat()

    print(mtx)
    print(dist)
    fs.release()

    objp = np.zeros((6 * 9, 3), np.float32)
    //所有元素的前两个，赋值，生成6*9大小的矩阵
    objp[:, :2] = np.mgrid[0:6, 0:9].T.reshape(-1, 2)


    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

    """
    [[ 3.  0.  0.]
     [ 0.  3.  0.]
     [ 0.  0. -3.]]
    """
    #确定坐标点，以绘制坐标轴
    axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3)

    for fname in glob.glob('image_*.jpg'):
        img = cv2.imread(fname)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        ret, corners = cv2.findChessboardCorners(gray, (6, 9), None)

        if ret:
            corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

            # 根据对象点和角点列表，查找旋转向量和平移向量
            retval, rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist)
            # 将3D点投影到图像平面
            # axis 为所需要的float类型的3D点列表
            # 输出图像点和雅克比矩阵
            imagePoints, jacobian = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)

            img = draw(img, corners2, imagePoints)
            cv2.imshow('img', img)
            k = cv2.waitKey(0) & 0xff
            if k == 's':
                cv2.imwrite(fname[:6] + '.png', img)

    cv2.destroyAllWindows()

3. 3维重建立方体

在上面的基础上，绘制一个立方体

# 将地板绘制成绿色，参数2为多个轮廓的列表，故需要多套一层[]
img = cv2.drawContours(img, [imgpts[:4]],-1,(0,255,0),-1)

print("-------------------")
# 柱子绘制成蓝色, 参数必须是tuple类型
# 0 1 2 3
# 4 5 6 7
for i,j in zip(range(4),range(4,8)):
    print("{}.{} -> {}.{}".format(i, tuple(imgpts[i]), j,tuple(imgpts[j])))
    img = cv2.line(img, tuple(imgpts[i]), tuple(imgpts[j]),(255,0,0),3)

# 顶部框用红色
img = cv2.drawContours(img, [imgpts[4:]],-1,(0,0,255),3)