如何从opencv solvepnp获取unity的相机位置

我无法将solvePnP的输出转换为Unity中的相机位置。在过去的几天里，我一直在浏览文档，阅读所有我能找到的关于它的问题，并尝试所有不同的方法，但我仍然被卡住了

这是我能做的。我在现实世界中有一个3D对象，具有已知的2D-3D共同响应点。我可以用这些和solvePNP来得到rvec，tvec。然后我可以绘制2D点，然后在这些点之上，我可以绘制用projectPoints找到的点。这些点排得很近

(success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix,
                                                              dist_coeffs, flags=cv2.cv2.SOLVEPNP_ITERATIVE)

print("Rotation Vector:\n" + str(rotation_vector))

print("Translation Vector:\n" + str(translation_vector))

(repro_points, jacobian) = cv2.projectPoints(model_points, rotation_vector,
                                                 translation_vector, camera_matrix, dist_coeffs)

original_img = cv2.imread(r"{}".format("og.jpg"))
for p in repro_points:
    cv2.circle(original_img, (int(p[0][0]), int(p[0][1])), 3, (255, 255, 255), -1)
    print(str(p[0][0]) + "-" + str(p[0][1]))

for p in image_points:
   cv2.circle(original_img, (int(p[0]), int(p[1])), 3, (0, 0, 255), -1)

cv2.imshow("og", original_img)
cv2.waitKey()

上面的代码将显示我的原始图像，图像点和复制点大致对齐。现在我从阅读中了解到，tvec和rvec不能直接统一使用，它们表示摄影机空间和对象空间之间的变换矩阵，因此可以使用以下公式转换两个空间之间的点

现在我想拍摄我的solvepnp给我的，rvec和tvec，并确定如何旋转和平移我的unity相机，使其与原始照片在同一位置对齐。在Unity中，我从面向对象的摄影机开始，并且两个摄影机都处于0,0,0。因此，为了让我的头脑保持清醒，我编写了另一个python脚本，试图将Rvec和Tvec转换为统一位置和旋转。这是基于我在opencv论坛上看到的。首先，我从罗德里格斯得到旋转矩阵，将其转置，然后交换第二行和第三行，以交换y和z，尽管我不知道这是否正确。然后旋转矩阵，最后求反，然后乘以tvec得到位置。但这一立场与现实世界不符

def main(argv):
print("Start")

rvec = np.array([0.11160132, -2.67532422, -0.55994949])
rvec = rvec.reshape(3,1)
print("RVEC")
print(rvec)

tvec = np.array([0.0896325, -0.14819345, -0.36882839])
tvec = tvec.reshape(3,1)
print("TVEC")
print(tvec)

rotmat,_ = cv2.Rodrigues(rvec)
print("Rotation Matrix:")
print(rotmat)

#trans_mat = cv2.hconcat((rotmat, tvec))
#print("Transformation Matrix:")
#print(trans_mat)

#transpose the transformation matrix
transposed_mat = np.transpose(rotmat)
print("Transposed Mat: ")
print(transposed_mat)

#swap rows 1 & 2
swap = np.empty([3, 3])
swap[0] = rotmat[0]
swap[1] = rotmat[2]
swap[2] = rotmat[1]

print("SWAP")
print(swap)

R = np.rot90(swap)
#this is supposed to be the rotation matrix for the camera
print("R:")
print(R)

#translation matrix
#they say negative matrix is 1's on diagonals do they mean idenity matrix
#negativematrix = np.identity(3)

position = np.matmul(-R, tvec);
print("Position: ")
print(position)

此代码的输出为：

Start
RVEC
[[ 0.11160132]
 [-2.67532422]
 [-0.55994949]]
TVEC
[[ 0.0896325 ]
 [-0.14819345]
 [-0.36882839]]
Rotation Matrix:
[[-0.91550667  0.00429232 -0.4022799 ]
 [-0.15739624  0.91641547  0.36797976]
 [ 0.37023502  0.40020526 -0.83830888]]
Transposed Mat: 
[[-0.91550667 -0.15739624  0.37023502]
 [ 0.00429232  0.91641547  0.40020526]
 [-0.4022799   0.36797976 -0.83830888]]
SWAP
[[-0.91550667  0.00429232 -0.4022799 ]
 [ 0.37023502  0.40020526 -0.83830888]
 [-0.15739624  0.91641547  0.36797976]]
R:
[[-0.4022799  -0.83830888  0.36797976]
 [ 0.00429232  0.40020526  0.91641547]
 [-0.91550667  0.37023502 -0.15739624]]
Position: 
[[0.04754685]
 [0.39692311]
 [0.07887335]]

如果我在这里交换y和z，我可以看到它很接近，但它仍然不对

为了得到旋转，我一直在做以下工作。我还试着从y轴减去180，因为在统一中，我的相机和物体面对面。但这也不适合我

rotation_mat, jacobian = cv2.Rodrigues(rotation_vector)

pose_mat = cv2.hconcat((rotation_mat, translation_vector))

tr = -np.matrix(rotation_mat).T * np.matrix(translation_vector)
print("TR_TR")
print(tr)

_, _, _, _, _, _, euler_angles = cv2.decomposeProjectionMatrix(pose_mat)

print("Euler:")
print(euler_angles)

---

今天早上我感觉很好，当时我得到了重新安排的积分，但现在我感觉自己陷入了困境。感谢您的帮助。谢谢。

我在为Unity编写AR应用程序时遇到了类似的问题。我记得我也花了好几天的时间，直到我明白过来。我用C++的OpenCV编写了一个DLL，它从摄像头获取图像，检测到图像中的对象并找到它的姿势。用C#Unity编写的前端将调用DLL函数并相应地更新3D模型的位置和方向

C++和统一代码的简化版本为：

void getCurrentPose(float* outR, float* outT)
{
    cv::Mat Rvec;
    cv::Mat Tvec;

    cv::solvePnP(g_modelPoints, g_imagePoints, g_cameraMatrix, g_distortionParams, Rvec, Tvec, false, cv::SOLVEPNP_ITERATIVE);

    cv::Matx33d R;
    cv::Rodrigues(Rvec, R);
    
    cv::Point3d T;
    T.x = Tvec.at<double>(0, 0);
    T.y = Tvec.at<double>(1, 0);
    T.z = Tvec.at<double>(2, 0);

    // Uncomment to return the camera transformation instead of model transformation
/*  const cv::Matx33d Rinv = R.t();
    const cv::Point3d Tinv = -R.t() * T;
    
    R = Rinv;
    T = Tinv;
*/

    for(int i = 0; i < 9; i++)
    {
        outR[i] = (float)R.val[i];
    }
    
    outT[0] = (float)T.x;
    outT[1] = (float)T.y;
    outT[2] = (float)T.z;
}

---

在Python中移植并尝试应该很容易。由于您想要相机转换，您可能应该取消对C++代码中注释行的注释。我不知道这个代码是否适用于您的案例，但也许值得一试。不幸的是，我没有安装Unity来尝试其他东西，因此我无法提出任何其他建议。

感谢您抽出时间与我们分享。我试过这个，但我看到我的模型在我的相机后面。我不知道为什么。我会继续找的。再次感谢你的帮助。

public class Vision : MonoBehaviour {
    GameObject model = null;

    void Start() {
        model = GameObject.Find("Model");
    }

    void Update() {
        float[] r = new float[9];
        float[] t = new float[3];
        
        dll_getCurrentPose(r, t); // Get object pose from DLL
    
        Matrix4x4 R = new Matrix4x4();

        R.SetRow(0, new Vector4(r[0], r[1], r[2], 0));
        R.SetRow(1, new Vector4(r[3], r[4], r[5], 0));
        R.SetRow(2, new Vector4(r[6], r[7], r[8], 0));
        R.SetRow(3, new Vector4(0, 0, 0, 1));

        Quaternion Q = R.rotation;

        model.transform.SetPositionAndRotation(
            new Vector3(t[0], -t[1], t[2]),
            new Quaternion(-Q.x, Q.y, -Q.z, Q.w));
    }
}