opencv的solvePnPRansac函数在python和c++中计算结果不一致

Question

部署模型的时候遇到的问题，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结果

Answer 1

你这里是cpp和python在my_pnp输入的points3d参数对比，从这里就可以看出来你的输入已经不一样了，结果肯定不相等啊。
我看了下你的代码，你少了一个逗号。。。。

所以你的这里的长度为47，Mat创建16X3长度越界了

这还是我拷贝到python下面去reshape报错长度不对发现的。。。
改完之后结果一样了