a strange and confusing phenomenon with undistorting the depth and color image

[nonlinear1]

I add undistortion part of color image and depth image to get undistort color image and depth image. And, I use the undistort color image and depth to make a pointcloud file, which is pcd format. But the pointcloud file has many scattering points along the ray, which is emitted by depth camera. I am confusing this phenomenon. the pointcloud srceen shot is as follow: and the following is my codes:

include

include <k4a/k4a.h>

include <opencv2/core/core.hpp>

include <opencv2/highgui/highgui.hpp>

include <opencv2/imgproc/imgproc.hpp>

include

include <opencv2/calib3d/calib3d.hpp>

using namespace std; using namespace cv;

template Mat create_mat_from_buffer(T data, int width, int height, int channels = 1) { Mat mat(height, width, CV_MAKETYPE(DataType::type, channels)); memcpy(mat.data, data, width height channels sizeof(T)); return mat; }

static string get_serial(k4a_device_t device) { size_t serial_number_length = 0;

if (K4A_BUFFER_RESULT_TOO_SMALL != k4a_device_get_serialnum(device, NULL, &serial_number_length))
{
    cout << "Failed to get serial number length" << endl;
    k4a_device_close(device);
    exit(-1);
}

char *serial_number = new (std::nothrow) char[serial_number_length];
if (serial_number == NULL)
{
    cout << "Failed to allocate memory for serial number (" << serial_number_length << " bytes)" << endl;
    k4a_device_close(device);
    exit(-1);
}

if (K4A_BUFFER_RESULT_SUCCEEDED != k4a_device_get_serialnum(device, serial_number, &serial_number_length))
{
    cout << "Failed to get serial number" << endl;
    delete[] serial_number;
    serial_number = NULL;
    k4a_device_close(device);
    exit(-1);
}

string s(serial_number);
delete[] serial_number;
serial_number = NULL;
return s;

}

int main(int argc, char **argv) {

cout << endl << "-------" << endl;
cout << "Openning Kinect for Azure ..." << endl;
cv::Mat imBGRA, imBGR, imD, im_transformed_color_image;
cv::Mat im_transformed_depth_image;

cv::Mat camera_matrix;
cv::Mat new_camera_matrix;
float factor{ 1.0 }; // scaling factor
cv::Mat cv_undistorted_color;
cv::Mat cv_undistorted_depth;
cv::Mat cv_depth_downscaled;
cv::Mat cv_color_downscaled;
cv::Mat map1;
cv::Mat map2;

int returnCode = 1;
k4a_device_t device = NULL;
const int32_t TIMEOUT_IN_MS = 1000;
k4a_transformation_t transformation = NULL;
int captureFrameCount;
k4a_capture_t capture = NULL;
k4a_image_t depth_image = NULL;
k4a_image_t color_image = NULL;
k4a_image_t ir_image = NULL;
k4a_image_t transformed_color_image = NULL;
k4a_image_t transformed_depth_image = NULL;
uint32_t device_count = k4a_device_get_installed_count();
bool continueornot = true;
if (device_count == 0)
{
    cout << "No K4A devices found" << endl;
    return 0;
}

if (K4A_RESULT_SUCCEEDED != k4a_device_open(K4A_DEVICE_DEFAULT, &device))
{
    cout << "No K4A devices found" << endl;
    if (device != NULL)
    {
        k4a_device_close(device);
    }
}
k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
config.synchronized_images_only = true;
config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
config.color_resolution = K4A_COLOR_RESOLUTION_720P;
//config.depth_format = K4A_IMAGE_FORMAT_DEPTH16;
config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
config.camera_fps = K4A_FRAMES_PER_SECOND_30;
k4a_device_set_color_control(device, K4A_COLOR_CONTROL_CONTRAST, K4A_COLOR_CONTROL_MODE_MANUAL, 7);
k4a_device_set_color_control(device, K4A_COLOR_CONTROL_SHARPNESS, K4A_COLOR_CONTROL_MODE_MANUAL, 4);

k4a_calibration_t calibration;
if (K4A_RESULT_SUCCEEDED !=
    k4a_device_get_calibration(device, config.depth_mode, config.color_resolution, &calibration))
{
    cout << "Failed to get calibration" << endl;
    return 0;
}
const int width = calibration.color_camera_calibration.resolution_width;
const int height = calibration.color_camera_calibration.resolution_height;
auto calib = calibration.depth_camera_calibration;

cout << "\n===== Device "  << ": " << get_serial(device) << " =====\n";
cout << "depth camera resolution width: " << calib.resolution_width << endl;
cout << "depth camera resolution height: " << calib.resolution_height << endl;
cout << "depth camera principal point x: " << calib.intrinsics.parameters.param.cx << endl;
cout << "depth camera principal point y: " << calib.intrinsics.parameters.param.cy << endl;
cout << "depth camera focal length x: " << calib.intrinsics.parameters.param.fx << endl;
cout << "depth camera focal length y: " << calib.intrinsics.parameters.param.fy << endl;
cout << "depth camera radial distortion coefficients:" << endl;
cout << "depth camera k1: " << calib.intrinsics.parameters.param.k1 << endl;
cout << "depth camera k2: " << calib.intrinsics.parameters.param.k2 << endl;
cout << "depth camera k3: " << calib.intrinsics.parameters.param.k3 << endl;
cout << "depth camera k4: " << calib.intrinsics.parameters.param.k4 << endl;
cout << "depth camera k5: " << calib.intrinsics.parameters.param.k5 << endl;
cout << "depth camera k6: " << calib.intrinsics.parameters.param.k6 << endl;
cout << "depth camera center of distortion in Z=1 plane, x: " << calib.intrinsics.parameters.param.codx << endl;
cout << "depth camera center of distortion in Z=1 plane, y: " << calib.intrinsics.parameters.param.cody << endl;
cout << "depth camera tangential distortion coefficient x: " << calib.intrinsics.parameters.param.p1 << endl;
cout << "depth camera tangential distortion coefficient y: " << calib.intrinsics.parameters.param.p2 << endl;
cout << "depth camera metric radius: " << calib.intrinsics.parameters.param.metric_radius << endl;

auto calib1 = calibration.color_camera_calibration;
cout << "color camera resolution width: " << calib1.resolution_width << endl;
cout << "color camera resolution height: " << calib1.resolution_height << endl;
cout << "color camera principal point x: " << calib1.intrinsics.parameters.param.cx << endl;
cout << "color camera principal point y: " << calib1.intrinsics.parameters.param.cy << endl;
cout << "color camera focal length x: " << calib1.intrinsics.parameters.param.fx << endl;
cout << "color camera focal length y: " << calib1.intrinsics.parameters.param.fy << endl;
cout << "color camera radial distortion coefficients:" << endl;
cout << "color camera k1: " << calib1.intrinsics.parameters.param.k1 << endl;
cout << "color camera k2: " << calib1.intrinsics.parameters.param.k2 << endl;
cout << "color camera k3: " << calib1.intrinsics.parameters.param.k3 << endl;
cout << "color camera k4: " << calib1.intrinsics.parameters.param.k4 << endl;
cout << "color camera k5: " << calib1.intrinsics.parameters.param.k5 << endl;
cout << "color camera k6: " << calib1.intrinsics.parameters.param.k6 << endl;
cout << "color camera center of distortion in Z=1 plane, x: " << calib1.intrinsics.parameters.param.codx << endl;
cout << "color camera center of distortion in Z=1 plane, y: " << calib1.intrinsics.parameters.param.cody << endl;
cout << "color camera tangential distortion coefficient x: " << calib1.intrinsics.parameters.param.p1 << endl;
cout << "color camera tangential distortion coefficient y: " << calib1.intrinsics.parameters.param.p2 << endl;
cout << "color camera metric radius: " << calib1.intrinsics.parameters.param.metric_radius << endl;

//init undistortion map
auto intrinsics = calibration.color_camera_calibration.intrinsics.parameters.param;
    cv_undistorted_color = cv::Mat::zeros(
  height/factor,
  width/factor,
  CV_8UC4);

cv_undistorted_depth = cv::Mat::zeros(
  height/factor,
  width/factor,
  CV_16U);

cv_depth_downscaled = cv::Mat::zeros(
  height/factor,
  width/factor,
    CV_16U);
cv_color_downscaled = cv::Mat::zeros(
  height/factor,
  width/factor,
  CV_8UC4);

std::vector<double> _camera_matrix = {
intrinsics.fx / factor,
0.f,
intrinsics.cx / factor,
0.f,
intrinsics.fy / factor,
intrinsics.cy / factor,
0.f,
0.f,
1.f};

 // Create cv matrices
camera_matrix = cv::Mat(3, 3, CV_64F, &_camera_matrix[0]);

std::vector<double> _dist_coeffs = { intrinsics.k1, intrinsics.k2, intrinsics.p1,
                                   intrinsics.p2, intrinsics.k3, intrinsics.k4,
                                   intrinsics.k5, intrinsics.k6 };

cv::Mat dist_coeffs = cv::Mat(8, 1, CV_64F, &_dist_coeffs[0]);
new_camera_matrix = cv::getOptimalNewCameraMatrix(
  camera_matrix,
  dist_coeffs,
  cv_depth_downscaled.size(),
  0,
  cv_depth_downscaled.size());

cv::Mat_<double> I = cv::Mat_<double>::eye(3, 3);

map1 = cv::Mat::zeros(cv_depth_downscaled.size(), CV_16SC2);
map2 = cv::Mat::zeros(cv_depth_downscaled.size(), CV_16UC1);
initUndistortRectifyMap(camera_matrix, dist_coeffs, I, new_camera_matrix, cv::Size(width / factor, height / factor),
                      map1.type(), map1, map2);

transformation = k4a_transformation_create(&calibration);

if (K4A_RESULT_SUCCEEDED != k4a_device_start_cameras(device, &config))
{
    cout << "Failed to start cameras" << endl;
    return 0;
}

vector<int> compression_params;
compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
compression_params.push_back(0);   
int count = 0;
for(; ; )
{
       std::ostringstream cnt;
   //string path = "Frames/";
   cnt << std::setw(6) << std::setfill('0') << count;
   std::string file =  "frame-" + cnt.str() + ".depth.png";
   std::string file1 = "frame-" + cnt.str() + ".color.png";
    // Get a capture
    switch (k4a_device_get_capture(device, &capture, TIMEOUT_IN_MS))
    {
    case K4A_WAIT_RESULT_SUCCEEDED:
    break;
    case K4A_WAIT_RESULT_TIMEOUT:
    cout << "Timed out waiting for a capture" << endl;
    return 0;
    case K4A_WAIT_RESULT_FAILED:
     cout << "Failed to read a capture" << endl;
    return 0;
    }

     // Get a depth image
    depth_image = k4a_capture_get_depth_image(capture);
    if (depth_image == 0)
    {
    cout <<"Failed to get depth image from capture" << endl;

    }

    int depth_image_width_pixels = k4a_image_get_width_pixels(depth_image);
    int depth_image_height_pixels = k4a_image_get_height_pixels(depth_image);
    imD = cv::Mat(depth_image_height_pixels, depth_image_width_pixels, CV_16UC1, (void*)k4a_image_get_buffer(depth_image));
        //ushort d = imD.ptr<ushort>(320)[320];
        //cout << "d = " << d << endl;
   // imshow("depth Image", imD);
    //waitKey(0);
    // Get a color image
    color_image = k4a_capture_get_color_image(capture);

    if (color_image == 0)
    {
    cout << "Failed to get color image from capture" << endl;

    }
    int color_image_width_pixels = k4a_image_get_width_pixels(color_image);
    int color_image_height_pixels = k4a_image_get_height_pixels(color_image);
    imBGRA = cv::Mat(color_image_height_pixels, color_image_width_pixels, CV_8UC4, (void*)k4a_image_get_buffer(color_image));

        cvtColor(imBGRA, imBGR, CV_BGRA2BGR);

       if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16,
                                             color_image_width_pixels,
                                             color_image_height_pixels,
                                             color_image_width_pixels * (int)sizeof(uint16_t),
                                             &transformed_depth_image))
       {
            cout << "Failed to create transformed depth image" << endl;
            return false;
       }
       if(K4A_RESULT_SUCCEEDED != k4a_transformation_depth_image_to_color_camera(transformation, depth_image, transformed_depth_image))
       {

            cout << "Failed to compute transformed depth image" << endl;
            return false;
       }

       cv::Mat frame;
       uint8_t *buffer = k4a_image_get_buffer(transformed_depth_image);
       uint16_t *depth_buffer = reinterpret_cast<uint16_t *>(buffer);
       create_mat_from_buffer<uint16_t>(depth_buffer, color_image_width_pixels, color_image_height_pixels).copyTo(frame);

       if(frame.empty())
       cout << "im_transformed_color_image is empty" << endl;

      cv::resize(
  frame,
  cv_depth_downscaled,
  cv_depth_downscaled.size(),
  CV_INTER_AREA);

      cv::resize(imBGRA,
  cv_color_downscaled,
  cv_color_downscaled.size(),
  INTER_LINEAR);//CV_INTER_AREA

      remap(cv_depth_downscaled, cv_undistorted_depth, map1, map2, cv::INTER_LINEAR, cv::BORDER_CONSTANT);
  remap(cv_color_downscaled, cv_undistorted_color, map1, map2, cv::INTER_LINEAR, cv::BORDER_CONSTANT);
      cv::cvtColor(cv_undistorted_color, cv_undistorted_color, CV_BGRA2RGB);

       imwrite(file, cv_undistorted_depth, compression_params);
       imwrite(file1, cv_undistorted_color, compression_params);

       k4a_image_release(depth_image); 
       k4a_image_release(color_image);

       k4a_image_release(transformed_depth_image); 
       k4a_capture_release(capture);
       ++count;

}

 return 0;

}

Could anyone know why? Thanks a lot

[jasjuang]

In your remap you are using INTER_LINEAR, which will cause weird artifacts at depth discontinuity. You have to use INTER_NEAREST when you warp or interpolated depth, or if you care about the depth quality you can do some sort of depth discontinuity detection where at non depth discontinuity use INTER_LINEAR and depth discontinuity use INTER_NEAREST.

[nonlinear1]

I will try thank you very much

[nonlinear1]

@jasjuang Thank you for your help, you are right!

[SeanStephensenAIRM]

@nonlinear1 @jasjuang Commenting here because my issue seems to be identical. Please let me know if this is bad form and I should start a fresh issue instead. I'm post processing k4a images in python and running into the same error. I'm reading into python:

• 24 bit, 3 channel color image, uint8 dtype

• 16 bit 1 channel depth image, uint16 dtype, which is already transformed into color reference

• the Kinect camera's intrinsic and distortion data

The images are reading in correctly, and generating a point cloud from the raw images works perfectly fine. They I apply the following code:

h, w = color_im.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(matrix_in, dist_vector, (w, h), 1, (w, h))
x, y, w, h = roi
mapx, mapy = cv2.initUndistortRectifyMap(matrix_in, dist_vector, None, newcameramtx, (w, h), 5)
depth_undistorted = cv2.remap(depth_im, mapx, mapy, interpolation=cv2.INTER_NEAREST)
color_undistorted = cv2.remap(color_im, mapx, mapy, interpolation=cv2.INTER_LINEAR)
color_cropped = color_undistorted[y:y+h, x:x+w]
depth_cropped = depth_undistorted[y:y+h, x:x+w]

color_converted = o3d.geometry.Image(color_cropped)
depth_converted = o3d.geometry.Image(depth_cropped)
combined_im = o3d.geometry.RGBDImage.create_from_color_and_depth(color_converted, depth_converted, depth_scale=1000.0, depth_trunc=1.0, convert_rgb_to_intensity=False)
combined_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(combined_im, matrix_in)

The color image comes out fine, and the depth image is hard to check since it's 16 bit. Here is the raw color image (left) and the undistorted color image (right)

Now, here's a comparison of the "raw" point cloud vs the undistorted point cloud. Obviously something gone wrong, and I get the same "pyramidal" shape to the point cloud that @nonlinear1 seemed to encounter:

As you can see, I'm already specifying NEAREST interpolation for depth undistortion, and still running into the problem. Any other ideas?

Edit: Solved. My process was fine, but apparently:

"Open3d has a bug where it does not always handle memory correctly when it is passed a "view" into a numpy array (created by the index slicing) instead of a real array of contiguous memory. It can cause memory to get shuffled around which could explain the wild values"

Fixed with:

…
color_copy = copy.deepcopy(color_cropped)
depth_copy = copy.deepcopy(depth_cropped)
color_converted = o3d.geometry.Image(color_copy)
depth_converted = o3d.geometry.Image(depth_copy)
...

Hopefully anyone else using open3d for PCD generation can find this useful.