opencv的solvePnPRansac函数在python和c++中计算结果不一致
部署模型的时候遇到的问题，opencv的solvePnPRansac函数在python和c++中计算结果不一致，仅仅2d关键点差了一个像素，其他参数完全一致，计算得到的位姿却差这么多
c++代码
cv::Mat my_pnp(cv::Mat points3d, cv::Mat points2d, cv::Mat camera_mat){
cv::Mat dist_coeffs = cv::Mat::zeros(8, 1, CV_64FC1);
int method = cv::SOLVEPNP_EPNP;
if(!points3d.isContinuous()){
cv::Mat new_points_3d = points3d.clone();
points3d = new_points_3d;
}
if(!points2d.isContinuous()){
cv::Mat new_points_2d = points2d.clone();
points2d = new_points_2d;
}
//points2d.convertTo(points2d, CV_64FC1);
cv::Mat rvec(3, 1, CV_64FC1);
cv::Mat tvec(3, 1, CV_64FC1);
cv::solvePnPRansac(points3d, points2d, camera_mat, dist_coeffs, rvec, tvec, false, 100, 8.0, 0.99, cv::noArray(),method);
cv::Rodrigues(rvec, rvec);
cv::Mat result;
cv::hconcat(rvec, tvec, result);
return result;
}
double kpt_2d = {178,15，
48,85,
135,132,
135,85,
31,1,
82,18,
16,72,
2,59,
54,113,
66,136,
88,91,
55,52,
61,43,
52,61,
115,133,
24,16};
cv::Mat points2d = cv::Mat(16, 2, CV_64FC1, kpt_2d)
cv::Mat get_pose(cv::Mat points2d){
double kpt_3d[] = { 0, -190.16952, 969.358209,
1, -215, -756,
619, -237, -374,
324, -246, -83,
-619 -237, -374,
-324, -246, -83,
-187, -200, -920,
-367, -200, -962,
187, -200, -920,
367, -200, -962,
173, -246, -543,
-173, -246, -543,
-170, -560, -782,
170, -560, -782,
543, -240, -557,
-543, -240, -557 };
cv::Mat points3d = cv::Mat(16, 3, CV_64FC1, kpt_3d);
double K[] = {1920, 0.00000000e+00, static_cast<double>(232),
0.00000000e+00, 1920, static_cast<double>(35),
0.00000000e+00, 0.00000000e+00, 1.00000000e+00};
cv::Mat camera_mat = cv::Mat(3, 3, CV_64FC1, K);
cv::Mat pose = my_pnp(points3d, points2d, camera_mat);
return pose;
}
python代码
kpt = np.array([[178., 15.],
[ 49., 85.],
[135., 132.],
[135., 85.],
[ 31., 1.],
[ 82., 18.],
[ 16., 72.],
[ 2., 59.],
[ 54., 113.],
[ 66., 136.],
[ 88., 91.],
[ 55., 52.],
[ 61., 43.],
[ 52., 61.],
[115., 133.],
[ 24., 16.]], dtype="double")
def my_pnp(points_3d, points_2d, camera_matrix, method=cv2.SOLVEPNP_EPNP):
dist_coeffs = np.zeros(shape=[8, 1], dtype='float64')
points_2d = points_2d.squeeze(dim=0).numpy()
assert points_3d.shape[0] == points_2d.shape[0]
#method = cv2.SOLVEPNP_EPNP
if method == cv2.SOLVEPNP_EPNP:
points_3d = np.expand_dims(points_3d, 0)
points_2d = np.expand_dims(points_2d, 0)
points_2d = np.ascontiguousarray(points_2d.astype(np.float64))
points_3d = np.ascontiguousarray(points_3d.astype(np.float64))
camera_matrix = camera_matrix.astype(np.float64)
_, R_exp, t, _ = cv2.solvePnPRansac(points_3d,
points_2d,
camera_matrix,
dist_coeffs,
flags=method)
R, _ = cv2.Rodrigues(R_exp)
return np.concatenate([R, t], axis=-1)
def get_pose(kpt):
kpt_3d = np.array([ [969.358209, 0, 190.16952],
[-756, 1, 215],
[-374, 619, 237],
[-83, 324, 246],
[-374, -619, 237],
[-83, -324, 246],
[-920, -187, 200],
[-962, -367, 200],
[-920, 187, 200],
[-962, 367, 200],
[-543, 173, 246],
[-543, -173, 246],
[-782, -170, 560],
[-782, 170, 560],
[-557, 543, 240],
[-557, -543, 240],
], dtype="double") # cm
CH = np.array([[-0., 1., 0.],
[ 0., 0., -1.],
[ 1., 0., 0.]])
kpt_3d = np.dot(kpt_3d, CH.T)
K = np.array([[1920, 0.00000000e+00, 232],
[0.00000000e+00, 1920, 35],
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]], dtype="double")
pose_pred = my_pnp(kpt_3d, kpt, K)
return pose_pred
c++结果
python结果
你这里是cpp和python在my_pnp输入的points3d参数对比，从这里就可以看出来你的输入已经不一样了，结果肯定不相等啊。
我看了下你的代码，你少了一个逗号。。。。
所以你的这里的长度为47，Mat创建16X3长度越界了
这还是我拷贝到python下面去reshape报错长度不对发现的。。。
改完之后结果一样了
在my_pnp函数中第三个参数中，为啥python对它有个数据类型的转换？ 参考 https://blog.csdn.net/callinglove/article/details/128564916 看看是否对你有所帮助。
- 给你找了一篇非常好的博客，你可以看看是否有帮助，链接：利用python-opencv中solvepnp函数解决位姿定位定向问题。
- 除此之外, 这篇博客: c++ opencv cv::solvePnPRansac 的使用和注意事项中的 文章目录 部分也许能够解决你的问题, 你可以仔细阅读以下内容或跳转源博客中阅读:
该回答引用GPT
出现该问题的原因可能是两个平台（Python和C++）的算法实现在一些方面存在微小差异，导致计算结果也略有不同。可以尝试进行以下操作来解决：
- 确保使用的OpenCV版本相同，并且在两个平台上都安装了必要的库和依赖项。
- 检查在Python和C++代码中对于输入数据类型的转换是否正确，例如，在C++代码中将输入的二维点转换成双精度浮点数格式时，可能需要先检查它是否已经是连续的。
- 检查在计算旋转向量时是否对 Rodrigues 函数的输入参数进行了正确的设置。在 Python 中，这可能需要使用“for s in range(3): rvecs[0][s] = rvec[s][0]”来转置矩阵；而在 C++ 中，Rodrigues 函数可能会自动处理这个过程。
- 确保两个平台上的算法具有相同的参数设置，例如 RANSAC 最大迭代次数、阈值和置信度等。
- 可以尝试使用一些其他的 OpenCV 函数来替换 solvePnPRansac 函数，如 solvePnP 函数来进行比较，查看其计算结果是否一致。