Cv::fisheye::undistortPoints() returns -nan

C++
1

I’m trying to undistort points using the fish-eye camera model. I got it working perfectly fine in python3 using

distorted = np.array([[list(target[0].pt)]], dtype=np.float32)
    new_xy = cv2.fisheye.undistortPoints(
        distorted,
        cam_cal.camera_matrix,
        cam_cal.distortion_coefficients,
        P=cam_cal.new_camera_matrix,
    )

Where “target” is a sequence of keypoints. Now I’m trying to do the equivalent in C++

cv::Point2f points_in = {800, 700};
cv::Point2f point_out;
std::vector<cv::Point2f> vec_in = {};
vec_in.push_back(points_in);
std::cout << vec_in << std::endl;
std::vector<cv::Point2f> vec_out = {};
cv::fisheye::undistortPoints(
    vec_in,
    vec_out,
    m_camera_matrix,
    m_dist_coeffs,
    cv::Mat::eye(3, 3, CV_64FC1),
    m_new_camera_matrix
);
std::cout << vec_out << std::endl;
point_out = vec_out[0];
vec_out.pop_back();

But the result is [-nan, -nan]. I also tried using cv::Mat instead of std::vector<cv::Point2f>but get the same result.

However, the following works and I get the expected results, so I really have no clue what I’m doing wrong.

m_new_camera_matrix = cv::Mat(3, 3, CV_64F);
    cv::fisheye::estimateNewCameraMatrixForUndistortRectify(
        m_camera_matrix,
        m_dist_coeffs,
        cv::Size(image_width, image_height),
        cv::Mat::eye(3, 3, CV_64F),
        m_new_camera_matrix
    );

    cv::fisheye::initUndistortRectifyMap(
        m_camera_matrix,
        m_dist_coeffs,
        cv::Mat::eye(3, 3, CV_64F),
        m_new_camera_matrix,
        cv::Size(image_width, image_height),
        CV_32FC1,
        m_map_1,
        m_map_2
    );
cv::remap(
        frame,
        output,
        m_map_1,
        m_map_2,
        cv::InterpolationFlags::INTER_LINEAR,
        cv::BorderTypes::BORDER_CONSTANT
    );

where

m_camera_matrix = 
[695.84766, 0, 823.49;
 0, 697.85657, 704.2644;
 0, 0, 1]
m_distortion_coefficients = 
[-0.00358979;
 0.01170594;
 -0.008970209999999999;
 0.00252217],
image_width = 1600
image_height = 1300
2

I found the problem. I was initializing the camera matrix and distortion coefficients in a function using arrays, unaware that the initializer for cv::Mat refers to the array pointer, rather than copying the data itself. So what happened is that these were pointing to the stack, which is then freed when exiting the scope, and the data would be corrupted causing undefined behavior. Initializing the matrices like

m_camera_matrix = (cv::Mat_<double>(3,3) << 
        f_x, 0.0, c_x,
        0.0, f_y, c_y,
        0.0, 0.0, 1.0);

solved the problem