sitf+LK+pnp 识别、跟踪图片，并求三维旋转角度（三） -----pnp 相机位姿求解

最新推荐文章于 2024-04-17 19:20:02 发布

多财多亿,凭亿近人

最新推荐文章于 2024-04-17 19:20:02 发布

阅读量906

点赞数

分类专栏：不带slam玩的AR

本文链接：https://blog.csdn.net/qq_38284951/article/details/113367919

版权

不带slam玩的AR 专栏收录该内容

4 篇文章 2 订阅

订阅专栏

上一篇说到单应性矩阵求解相机位姿不是特别准确，因为求解出来的旋转向量有四组解。这个时候我不知道怎么办，不过百度是个好东西，找到了pnp相机位姿求解很多文章，这里就推荐这篇。一开始看的云里雾里，什么点什么像素点，对应不起来啊，这个点要怎么找啊，完全就是懵b状态，也怪我没有仔细阅读。后来又去请教其他大佬，其他大佬都是用slam进阶方法去做的，跟我这个没多大关系。索性就按照自己的想法来吧，先去理解下pnp求解相机位姿是怎么做的，我们先去看下代码，opencv中自带pnp求解函数。转载请注明出处：https://blog.csdn.net/qq_38284951/article/details/113367919

def solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec=None, tvec=None, useExtrinsicGuess=None, flags=None)

objectPoints 是世界坐标点，就是我们需要去找的点。

imagePoints 是像素点，也就是和世界坐标点对应的像素坐标点。

cameraMatrix，distcoeffs ，以及后面几个参数，看过我上篇文章的应该知道这是相机参数，不懂的可以去看下我上篇文章。

那就剩下我要怎么取点的问题了，我要识别图像，以及返回我相机的位姿和角度，根据初中物理知识，运动是相对的，图不动，是不是表示我的点在世界中也是不动的，我能不能固定取几个点呢？pnp最少取4个点，那我就随便取四个点，这四个点的世界坐标怎么搞？简单，以图像的左顶点为原点建立世界坐标系，单位呢，就按照世界和像素之间的比例，比如1cm表示20个像素点，我取如图四个点坐标，因为是在平面那z就赋值为0吧：

这四个点世界坐标以及对应性的像素坐标，我这张图宽高分别是9.1cm和11.2cm，像素宽高是500*632 我这边取了整9和11（正方形是不是更好些，读者们可以测试下，我就不测了），世界坐标是根据像素坐标对应比例算的：

#世界坐标，根据像素坐标计算
objpos = np.float32([[0.9, 0.87, 0], [0.9, 10.44, 0], [8.1, 10.44, 0], [8.1,0.87, 0]]).reshape(-1, 1, 3)
#像素坐标
imgpos = np.float32([[50, 50], [50, 600], [450, 600], [450, 50]]).reshape(-1, 1, 2)

点就取好了，但是我pnp要的参数肯定不是这样的，世界坐标和对应的像素坐标放里面，那求解出来的相机位姿不是不会变化嘛，我要求变化的相机位姿呀，就比如上一张图我相机正对着，要是我相机旋转一下，那拍出来的图片不就像下面这样了嘛，然后我相机位姿还不变，但其实这个时候的z变了90度。

这个时候我有想起了上一篇的单应性矩阵啊，说到底，单应性矩阵不是就是上一张图和当前这张图之间的变化关系吗？那上一章的像素点坐标是不是应该就“乘以”单应性矩阵M得到当前像素点坐标了，单应性矩阵就是我第一篇中那些点的匹配关系。如下图：这张图得到的（x,y,z）三个角度结果应该是0,0,90。

那么我输入pnp的数据应该就是我标定好的世界坐标和经过单应性矩阵变化后的像素坐标。

#经过单应性矩阵变化后的像素坐标
dst_imgpos = cv2.perspectiveTransform(imgpos, M)
# pnp 求解
found, rvec, tvec = cv2.solvePnP(objpos, dst_imgpos, camera_parameters, None)

然后在根据pnp的解求出相机位姿和角度，整体代码如下：

def Pnp(M,cameraMatrix,coff_dis):
    objpos = np.float32([[0.9, 0.87, 0], [0.9, 10.44, 0], [8.1, 10.44, 0], [8.1,0.87, 0]]).reshape(-1, 1, 3)
    imgpos = np.float32([[50, 50], [50, 600], [450, 600], [450, 50]]).reshape(-1, 1, 2)

    dst_imgpos = cv2.perspectiveTransform(imgpos, M)
    # imgp.append(dst_imgpos)
    # pnp 求解
    found, rvec, tvec = cv2.solvePnP(objpos, dst_imgpos, cameraMatrix, None)

    rotM = cv2.Rodrigues(rvec)[0]
    camera_postion = -np.mat(rotM).T * np.mat(tvec)
    Ca = camera_postion.T
    # 计算相机坐标系的三轴旋转欧拉角，旋转后可以转出世界坐标系。旋转顺序z,y,x
    thetaZ = math.atan2(rotM[1, 0], rotM[0, 0]) * 180.0 / math.pi
    thetaY = math.atan2(-1.0 * rotM[2, 0], math.sqrt(rotM[2, 1] ** 2 + rotM[2, 2] ** 2)) * 180.0 / math.pi
    thetaX = math.atan2(rotM[2, 1], rotM[2, 2]) * 180.0 / math.pi
    # 相机坐标系下值
    x = tvec[0]
    y = tvec[1]
    z = tvec[2]

    # 进行三次旋转
    def RotateByZ(Cx, Cy, thetaZ):
        rz = thetaZ * math.pi / 180.0
        outX = math.cos(rz) * Cx - math.sin(rz) * Cy
        outY = math.sin(rz) * Cx + math.cos(rz) * Cy
        return outX, outY

    def RotateByY(Cx, Cz, thetaY):
        ry = thetaY * math.pi / 180.0
        outZ = math.cos(ry) * Cz - math.sin(ry) * Cx
        outX = math.sin(ry) * Cz + math.cos(ry) * Cx
        return outX, outZ

    def RotateByX(Cy, Cz, thetaX):
        rx = thetaX * math.pi / 180.0
        outY = math.cos(rx) * Cy - math.sin(rx) * Cz
        outZ = math.sin(rx) * Cy + math.cos(rx) * Cz
        return outY, outZ

    (x, y) = RotateByZ(x, y, -1.0 * thetaZ)
    (x, z) = RotateByY(x, z, -1.0 * thetaY)
    (y, z) = RotateByX(y, z, -1.0 * thetaX)
    Cx = x * -1
    Cy = y * -1
    Cz = z * -1
    # 输出相机位置
    camera_Location =[Cx, Cy, Cz]
    print("相机位姿",camera_Location)
    # 输出相机旋转角
    # print("相机旋转角度：",'[', thetaX, thetaY, -thetaZ, ']')
    angles = [thetaX, thetaY, -thetaZ]
    #result = [camera_Location, "旋转角度", angles]
    return angles

相机位姿 [array([4.93656876]), array([4.66336094]), array([-9.77590885])]
[-0.4368029884380946, 0.028180371725645432, 89.97804401799725]

这个是我得出的结果，因为我取整并且保留了两位小数。会存在误差，因为相机参数本身是通过张正友相机标定得到的，本身会产生误差，更别说我这里的相机参数是我自己随便写的，所以相机位姿并不准，点经过单应性矩阵计算也会存在误差。所以出现误差是可以理解的。x显示-0.4 y显示0.028 z是89.978 ，近似 0，0，90。再稍微调下相机参数就准确了。

下篇文章会介绍lk，为什么会用到lk以及sift怎么和lk结合，都会一一介绍。

sitf+LK+pnp 识别、跟踪图片，并求三维旋转角度（一） -----特征匹配

sitf+LK+pnp 识别、跟踪图片，并求三维旋转角度（二） -----单应性矩阵分解，求解角度

sitf+LK+pnp 识别、跟踪图片，并求三维旋转角度（四） -----LK光流跟踪