Given two image-points from two different positions of a camera, we want to calculate the 3D coordinate that this point is located at in the world. This is highly related to Structure from Motion (sfm) approaches, where we try to create a dense 3d point-cloud using consecutive images (of course this will only work if the 3D worldpoint in question is not moving itself, or if we know it’s movement):

In this post I will present two things: the first is how to project a point from known 3d-coordinates to the image plain (2d-coordinates) using the camera-matrix and external transformations (rotation and translation). The second part shows how to use the Direct Linear Transform (DLT) method described in Hartley & Zisserman’s Multiple View Geometry for linear triangulation (this is the method implemented in OpenCV’s triangulatepoints(), so we will use this).

```
%matplotlib inline
"""
Test projection and triangulation functions of OpenCV
- Given a camera calibration:
- define two camera poses (rotation and translation)
- compute the projection matrix for each pose
- define some point in 3d
- project it to image coordinates
- Given two camera poses and two projected points:
- use OpenCVs triangulatePoints
(from: HartleyZisserman, Multiple View Geometry, DLT-Linear triangulation)
to retrieve the original 3D points
Axis semantics:
x - right
y - down
z - forward
"""
import math
import cv2
import numpy as np
import matplotlib.pyplot as plt
```

```
# camera constants
fl = [1.487847159024568555e+03, 1.489275578039685570e+03]
pp = [9.200624828553764019e+02, 5.759643410906321606e+02]
pitch=0.0
yaw = 0.0
roll = 0.0
```

```
def eulerAnglesToRotationMatrix( theta) :
R_x = np.array([[1, 0, 0 ],
[0, math.cos(theta[0]), -math.sin(theta[0]) ],
[0, math.sin(theta[0]), math.cos(theta[0]) ]
])
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ],
[0, 1, 0 ],
[-math.sin(theta[1]), 0, math.cos(theta[1]) ]
])
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
[math.sin(theta[2]), math.cos(theta[2]), 0],
[0, 0, 1]
])
R = np.dot(R_z, np.dot( R_y, R_x ))
return R
```

```
# lets leave rotation and the camera matrix unchanged
R = eulerAnglesToRotationMatrix([0,0,0]) # Rotation matrix:
K = np.array([
[fl[0], 0., pp[0]],
[0., fl[1], pp[1]],
[0., 0., 1.]
]) # Kamera matrix:
# define pose 0
T0 = np.array([0,0,0]) # Translation vector
RT0 = np.zeros((3,4)) # combined Rotation/Translation matrix
RT0[:3,:3] = R
RT0[:3, 3] = T0
P0 = np.dot(K, RT0) # Projection matrix
# define pose 1
T1 = np.array([0,0,2.])
RT1 = np.zeros((3,4))
RT1[:3,:3] = R
RT1[:3, 3] = -T1
P1 = np.dot(K, RT1)
```

```
wcsp = np.array([2.0, 0.5, 15., 1.]) # define an arbitrary 3D point in world coordinates
icsp0 = np.dot(P0, wcsp) # project this point using the first camera pose
icsp0 = icsp0/icsp0[-1] # normalize as we are in homogenuous coordinates
icsp1 = np.dot(P1, wcsp)
icsp1 = icsp1/icsp1[-1]
# plot the projection
fig = plt.figure()
plt.xlim((0,1920))
plt.ylim((0,1080))
plt.plot(icsp0[0], icsp0[1], 'ko')
plt.plot(icsp1[0], icsp1[1], 'ro')
plt.axvline(pp[0], alpha=0.2)
plt.axhline(pp[1], alpha=0.2)
plt.show()
```

```
res = cv2.triangulatePoints(P0, P1, icsp0[:2], icsp1[:2]) # now try to get those 3d-positions back using triangulation
print "Abracadabra, are these the coordinates you put in the hat?"
print res[:3]/res[-1] # again, normalize b/c of homogenuous coordinates
```

## Leave a Reply