[PYTHON] Try adding fisheye lens distortion to the image

Commonplace http://www.sanko-shoko.net/note.php?id=wvb7

import numpy as np
import open3d as o3d
import cv2
import math

#Lattice length per grid
CONST_IMG_PER_WIDTH = 10
CONST_IMG_PER_HEIGHT = 10

#Number of grids
CONST_IMG_NUM_WIDTH = 100
CONST_IMG_NUM_HEIGHT = 100

RGB_WHITE = [255,255,255]
RGB_BLACK = [0,0,0]

def write_point_cloud(path,pcl):
    assert o3d.io.write_point_cloud(path, pcl),"error : fail to write pointcloud  = " + path

def exportPLYfromNumpy(nplist, colorList, outputPath):
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(nplist)
    pcd.colors = o3d.utility.Vector3dVector(colorList)
    write_point_cloud(outputPath, pcd)
    return pcd

def TransformPointI2C(uv,depth,K):
    X = float(float(uv[0] - K[0][2]) * depth / K[0][0])
    Y = float(float(uv[1] - K[1][2]) * depth / K[1][1])
    Z = depth
    CameraPos3D = np.array([[X], [Y], [Z]])
    return CameraPos3D

width_whole = (CONST_IMG_PER_WIDTH+1)*(CONST_IMG_NUM_WIDTH)+1
img_parallel_grid = []
img_parallel_grid.append([RGB_BLACK] * width_whole)
hrange = range((CONST_IMG_PER_HEIGHT+1)*(CONST_IMG_NUM_HEIGHT))

h_cnt=0
for h in hrange:
    h_cnt += 1
    if h_cnt == CONST_IMG_PER_HEIGHT+1:
        tmpimg = [RGB_BLACK] * width_whole
        h_cnt=0
    else:
        tmpimg = [RGB_WHITE] * width_whole

    for w in range(width_whole):
        if CONST_IMG_PER_WIDTH*w+w < len(tmpimg):
            tmpimg[CONST_IMG_PER_WIDTH*w+w] = RGB_BLACK
    img_parallel_grid.append(tmpimg)

img_parallel_grid=np.array(img_parallel_grid)
print(img_parallel_grid.shape)

print(img_parallel_grid)
cv2.imwrite('grid_img.png', img_parallel_grid)

print(img_parallel_grid.shape)
img_height = img_parallel_grid.shape[0]
img_width = img_parallel_grid.shape[1]
f = 10
ch = img_height/2
cw = img_width/2
K = np.array([[f,0,ch],
              [0,f,cw],
              [0,0,1]])

depth = 10 #Fixed value
pointcloud=[]
colorList=[]
for h in range(img_height):
    for w in range(img_width):
        if img_parallel_grid[h][w].tolist() == RGB_BLACK:
            XYZ = TransformPointI2C([h,w], depth , K)
            pointcloud.append(XYZ.tolist())
            colorList.append(RGB_BLACK)
pcd = exportPLYfromNumpy(pointcloud, colorList, 'grid_pcl.ply')

# http://www.sanko-shoko.net/note.php?id=wvb7
points = pcd.points
k1 = 0.0
k2 = 0.0
k3 = 0.0
k4 = 0.0

#Prepare an image of a pure white sky
img_disto = np.full(img_parallel_grid.shape,255)

pointcloud=[]
colorList=[]
for point in points:
    X = point[0]
    Y = point[1]
    Z = point[2]

    a = float(X/Z)
    b = float(Y/Z)
    r = math.sqrt(a**2 + b**2)
    tht = np.arctan(r)
    tht2 = tht**2
    tht4 = tht**4
    tht6 = tht**6
    tht8 = tht**8
    theta_d = tht * (1 + k1*tht2 + k2*tht4 + k3*tht6 + k4*tht8)

    u_d = (a*theta_d)/r
    v_d = (b*theta_d)/r

    u = (u_d-ch)/f
    v = (v_d-cw)/f

    pointcloud.append([u,v,f])
    colorList.append(RGB_BLACK)
pcd = exportPLYfromNumpy(pointcloud, colorList, 'disto_pcl.ply')

#
#     img_disto[u_d][v_d]=np.array(RGB_BLACK)
# cv2.imwrite('disto_img.png', img_disto)