1+ import cv2
2+ import depthai as dai
3+ import numpy as np
4+
5+ camRes = dai .ColorCameraProperties .SensorResolution .THE_1080_P
6+ camSocket = dai .CameraBoardSocket .RGB
7+ ispScale = (1 ,2 )
8+
9+ def getMesh (calibData , ispSize ):
10+ M1 = np .array (calibData .getCameraIntrinsics (camSocket , ispSize [0 ], ispSize [1 ]))
11+ d1 = np .array (calibData .getDistortionCoefficients (camSocket ))
12+ R1 = np .identity (3 )
13+ mapX , mapY = cv2 .initUndistortRectifyMap (M1 , d1 , R1 , M1 , ispSize , cv2 .CV_32FC1 )
14+
15+ meshCellSize = 16
16+ mesh0 = []
17+ # Creates subsampled mesh which will be loaded on to device to undistort the image
18+ for y in range (mapX .shape [0 ] + 1 ): # iterating over height of the image
19+ if y % meshCellSize == 0 :
20+ rowLeft = []
21+ for x in range (mapX .shape [1 ]): # iterating over width of the image
22+ if x % meshCellSize == 0 :
23+ if y == mapX .shape [0 ] and x == mapX .shape [1 ]:
24+ rowLeft .append (mapX [y - 1 , x - 1 ])
25+ rowLeft .append (mapY [y - 1 , x - 1 ])
26+ elif y == mapX .shape [0 ]:
27+ rowLeft .append (mapX [y - 1 , x ])
28+ rowLeft .append (mapY [y - 1 , x ])
29+ elif x == mapX .shape [1 ]:
30+ rowLeft .append (mapX [y , x - 1 ])
31+ rowLeft .append (mapY [y , x - 1 ])
32+ else :
33+ rowLeft .append (mapX [y , x ])
34+ rowLeft .append (mapY [y , x ])
35+ if (mapX .shape [1 ] % meshCellSize ) % 2 != 0 :
36+ rowLeft .append (0 )
37+ rowLeft .append (0 )
38+
39+ mesh0 .append (rowLeft )
40+
41+ mesh0 = np .array (mesh0 )
42+ meshWidth = mesh0 .shape [1 ] // 2
43+ meshHeight = mesh0 .shape [0 ]
44+ mesh0 .resize (meshWidth * meshHeight , 2 )
45+
46+ mesh = list (map (tuple , mesh0 ))
47+
48+ return mesh , meshWidth , meshHeight
49+
50+ def create_pipeline (calibData ):
51+ pipeline = dai .Pipeline ()
52+
53+ cam = pipeline .create (dai .node .ColorCamera )
54+ cam .setIspScale (ispScale )
55+ cam .setBoardSocket (camSocket )
56+ cam .setResolution (camRes )
57+
58+ manip = pipeline .create (dai .node .ImageManip )
59+ mesh , meshWidth , meshHeight = getMesh (calibData , cam .getIspSize ())
60+ manip .setWarpMesh (mesh , meshWidth , meshHeight )
61+ manip .setMaxOutputFrameSize (cam .getIspWidth () * cam .getIspHeight () * 3 // 2 )
62+ cam .isp .link (manip .inputImage )
63+
64+ cam_xout = pipeline .create (dai .node .XLinkOut )
65+ cam_xout .setStreamName ("Undistorted" )
66+ manip .out .link (cam_xout .input )
67+
68+ dist_xout = pipeline .create (dai .node .XLinkOut )
69+ dist_xout .setStreamName ("Distorted" )
70+ cam .isp .link (dist_xout .input )
71+
72+ return pipeline
73+
74+ with dai .Device () as device :
75+
76+ calibData = device .readCalibration ()
77+ pipeline = create_pipeline (calibData )
78+ device .startPipeline (pipeline )
79+
80+ queues = [device .getOutputQueue (name , 4 , False ) for name in ['Undistorted' , 'Distorted' ]]
81+
82+ while True :
83+ for q in queues :
84+ frame = q .get ().getCvFrame ()
85+ cv2 .imshow (q .getName (), frame )
86+
87+ if cv2 .waitKey (1 ) == ord ('q' ):
88+ break
0 commit comments