Hi there I am trying to convert two 2D points center1 = (x1, y1), center2 = (x2,y2) from a stereo camera into 3D world coordinates. First I calculate the center of a detected object in both pictures (left and right), afterwards this points are triangulated with the LeftProjection and RightProjection from cv2.StereoRectify()
It looks like this:
P1 = calibration["leftProjection"] #read camera matrix
P2 = calibration["rightProjection"]
center1 = cX1, cY1 #x,y coordinates of an object in the pictures
center2 = cX2, cY2
arr = np.array([])
arr = cv2.triangulatePoints(P1, P2, center1, center2, arr)
arrCar = arr/arr[3] # from homogeneous to cartesian coordinates.
arrCar = np.delete(arrCar, (3), axis=0)
print(center1)
print(center2)
print(arrCar)
center1 and center2 are calculated with the remaped pictures from calibration like below..
fixedLeft = cv2.remap(leftFrame, leftMapX, leftMapY, REMAP_INTERPOLATION)
fixedRight = cv2.remap(rightFrame, rightMapX, rightMapY, REMAP_INTERPOLATION)
My purpose is to find distance and angle of an the object from the camera. (the goal is to follow an object autonomous..)
My output looks like this:
(818, 556) #center1 (left image) #camera resolution = (1920,1080)
(351, 555) #center2
[[ 0.34404608] #x
[ 1.01526877] #y
[38.3707673 ]] #z
In my opinion the origin (0,0,z) should be in the center of the left image (1920/2 = 960 and 1080/2 = 560) but x and y are approcimately 0 at a (800,500) pixel value..
In addition the z=38.37 is computed with an object distance of 1m from the camera... which I don´t know if I just can multiply this value with a weight to get 1m. Because of the fact that z is changing linear...
I would appreciate any help..
The whole code you can find here:
CAMERA_WIDTH = 1920
CAMERA_HEIGHT = 1080
left = cv2.VideoCapture(0)
right = cv2.VideoCapture(1)
#User set camera
left.set(3, CAMERA_WIDTH) # Set resolution width
left.set(4, CAMERA_HEIGHT) # Set resolution height
right.set(3, CAMERA_WIDTH) # Set resolution width
right.set(4, CAMERA_HEIGHT) # Set resolution hight
REMAP_INTERPOLATION = cv2.INTER_LINEAR
#read parameters
rootDir = './'
outputFile = rootDir + 'output/calibration.npz'
calibration = np.load(outputFile, allow_pickle=False)
imageSize = tuple(calibration["imageSize"])
leftMapX = calibration["leftMapX"]
leftMapY = calibration["leftMapY"]
leftROI = tuple(calibration["leftROI"])
rightMapX = calibration["rightMapX"]
rightMapY = calibration["rightMapY"]
rightROI = tuple(calibration["rightROI"])
R = calibration["rotationMatrix"]
T = calibration["translationVector"]
P1 = calibration["leftProjection"]
P2 = calibration["rightProjection"]
global cX2
global cX1,D
cX1=0
cX2=0;D=0
count=0
print("Ready!")
while(1):
if not left.grab() or not right.grab():
print("No more frames")
break
ret, leftFrame = left.retrieve()
leftHeight, leftWidth = leftFrame.shape[:2]
ret, rightFrame = right.retrieve()
rightHeight, rightWidth = rightFrame.shape[:2]
if (leftWidth, leftHeight) != imageSize:
print("Left camera has different size than the calibration data")
#break
if (rightWidth, rightHeight) != imageSize:
print("Right camera has different size than the calibration data")
#break
#cv2.imshow('left', leftFrame)
fixedLeft = cv2.remap(leftFrame, leftMapX, leftMapY, REMAP_INTERPOLATION)
fixedRight = cv2.remap(rightFrame, rightMapX, rightMapY, REMAP_INTERPOLATION)
image_blur1 = cv2.GaussianBlur(fixedLeft, (7, 7), 0)
image_blur2 = cv2.GaussianBlur(fixedRight, (7, 7), 0)
#HSV format of image
image_blur_hsv1 = cv2.cvtColor(image_blur1, cv2.COLOR_BGR2RGB)
image_blur_hsv2 = cv2.cvtColor(image_blur2, cv2.COLOR_BGR2RGB)
#colour grading for cone
max_orange = np.array([245,136,99])
min_orange = np.array([150,45,10])
max_white= np.array([225,224,228])
min_white=np.array([119,115,136])
#colour masks, using only orange
mask11 = cv2.inRange(image_blur_hsv1,min_orange,max_orange)
mask12 = cv2.inRange(image_blur_hsv1,min_white,max_white)
mask21 = cv2.inRange(image_blur_hsv2,min_orange,max_orange)
mask22 = cv2.inRange(image_blur_hsv2,min_white,max_white)
mask1=mask11+mask12
mask2=mask21+mask22
#canny edges
edged1 = cv2.Canny(mask11, 50, 100)
edged1 = cv2.dilate(edged1, None, iterations=1)
edged1 = cv2.erode(edged1, None, iterations=1)
edged2 = cv2.Canny(mask21, 50, 100)
edged2 = cv2.dilate(edged2, None, iterations=1)
edged2 = cv2.erode(edged2, None, iterations=1)
#contour plays
_,contours1,_ = cv2.findContours(edged1, cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
_,contours2,_ = cv2.findContours(edged2, cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
if (len(contours1) and len(contours2))!=0:
c1 = max(contours1, key = cv2.contourArea)
M1=cv2.moments(c1)
if M1["m00"] != 0:
cX1=int(M1['m10']/M1["m00"])
cY1=int(M1["m01"]/M1["m00"])
cv2.drawContours(fixedLeft,c1, -1, (0, 255, 0), 2)
cv2.circle(fixedLeft, (cX1, cY1), 7, (255, 255, 255), -1)
cv2.putText(fixedLeft, "center", (cX1 - 20, cY1 - 20),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
else:
cX1,cY1=0,0
c2 = max(contours2, key = cv2.contourArea)
M2=cv2.moments(c2)
if M2["m00"] != 0:
cX2=int(M2['m10']/M2["m00"])
cY2=int(M2["m01"]/M2["m00"])
cv2.drawContours(fixedRight,c2, -1, (0, 255, 0), 2)
cv2.circle(fixedRight, (cX2, cY2), 7, (255, 255, 255), -1)
cv2.putText(fixedRight, "center", (cX2 - 20, cY2 - 20),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
else:
cX2,cY2=0,0
else:
print("0")
center1 = cX1, cY1
center2 = cX2, cY2
arr = np.array([])
arr = cv2.triangulatePoints(P1, P2, center1, center2, arr)
arrCar = arr/arr[3]
arrCar = np.delete(arrCar, (3), axis=0)
print(center1)
print(center2)
print(arrCar)
cv2.imshow('left', cv2.resize(fixedLeft, (960, 540)))
cv2.imshow('right', cv2.resize(fixedRight, (960, 540)))
if cv2.waitKey(10)==27:
break
count=count+1