Calibration: Add functions to access intrinsic camera parameters

[johan12345]

Camera matrix and distortion coefficients in OpenCV-compatible format.
These values are already specific to the selected camera resolution (in contrast to those accessed through calibration_raw).

related to #35, #69

[codecov]

Codecov Report

❗ No coverage uploaded for pull request base (develop@54281b4). Click here to learn what that means.
The diff coverage is n/a.

@@            Coverage Diff             @@
##             develop     #113   +/-   ##
==========================================
  Coverage           ?   87.39%           
==========================================
  Files              ?       10           
  Lines              ?      682           
  Branches           ?        0           
==========================================
  Hits               ?      596           
  Misses             ?       86           
  Partials           ?        0           

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 54281b4...f63d978. Read the comment docs.

[lpasselin]

Thanks for the PR!

Can anyone else confirm the new functions work properly? (I will have access to my k4a later this week)

[ZdenekM]

I haven't tested the code yet, but it seems correct. I was using this conversion so far.

[pgrady3]

I've downloaded this pull request and can confirm that getting the color camera intrinsic and distortion parameters work. They allow undistorting with the OpenCV functions.

k4a = PyK4A(
    Config(
        color_resolution=pyk4a.ColorResolution.RES_1080P,
        depth_mode=pyk4a.DepthMode.WFOV_2X2BINNED,
        synchronized_images_only=False,
        camera_fps=pyk4a.FPS.FPS_15
    ),
)

k4a.start()

intrinsic = k4a.calibration.get_camera_matrix(pyk4a.calibration.CalibrationType.COLOR)
distortion = k4a.calibration.get_distortion_coefficients(pyk4a.calibration.CalibrationType.COLOR)

[wangmiaowei]

How to undistort the depth map? I just use opencv function but the results are bad
import numpy as np
import cv2
import open3d as o3d
w = 640
h = 576

intrinsic = np.array([[503.08432007, 0.,324.27612305],[ 0. ,503.16601562,330.65237427],
[ 0. ,0. ,1. ]])
dist_coeffs=np.array([ 2.06184745e-01,-1.28368318e-01,2.92664499e-05,2.75623443e-05,
-5.74961351e-03,5.43716311e-01,-1.30464926e-01,-3.45068611e-02])

newcameramatrix, _ = cv2.getOptimalNewCameraMatrix(
intrinsic, dist_coeffs, (w, h), 1, (w, h)
)
path1= "/home/SENSETIME/wangmiaowei/WMW_WORK/programs/2022_start/data_local_center/plant_3D/Pole/cylinder80mm1/depth/SN_1.png"
depth = cv2.imread(path1, cv2.IMREAD_UNCHANGED)
undistorted_image = cv2.undistort(
depth, intrinsic, dist_coeffs, None, newcameramatrix
)
depth=undistorted_image/1000
print('newcamera: ',newcameramatrix)
depth = o3d.geometry.Image(depth.astype('float32'))
cam = o3d.camera.PinholeCameraIntrinsic()
cam.intrinsic_matrix = intrinsic
pcd = o3d.geometry.PointCloud.create_from_depth_image(depth,cam)
o3d.visualization.draw_geometries([pcd])
o3d.io.write_point_cloud("/home/SENSETIME/wangmiaowei/WMW_WORK/programs/2022_start/data_local_center/plant_3D/Pole/cylinder80mm1/depth/SN_1.ply",pcd)