Output coordinates of cv2.triangulatePoints()

edit

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 ...