Revision history [back]

You have to find the homogenous 2d points explicitly. Check their z-value. If negative ignore them. Those are the points behind you.

In python it looks like this:

R, t - current 6dof pose of the camera proj_mat = np.dot(K, )

You have to find the homogenous 2d points explicitly. Check their z-value. If negative ignore them. Those are the points behind you.

In python it looks like this:

R, t - current 6dof pose of the camera
K - 3x3 Camera matrix
D - distortion coefficients
xyz - Nx3 3d points


proj_mat = np.dot(K, )
np.hstack((R, t[:, np.newaxis])))
# convert 3D points into homgenous points
xyz_hom = np.hstack((xyzs, np.ones((xyzs.shape[0], 1))))
 xy_hom = np.dot(proj_mat, xyzs_hom.T).T

# get 2d coordinates in image [pixels]
z = xy_hom[:, -1]
xy = xy_hom[:, :2] / np.tile(z[:, np.newaxis], (1, 2))

# undistort - has to be 1xNx2 structure
xy = cv2.undistortPoints(np.expand_dims(xy, axis=0), np.eye(3), D).squeeze()

# drop all points behind camera
xy = xy[z > 0]

You have to find the homogenous 2d points explicitly. Check their z-value. If negative ignore them. Those are the points behind you.

In python it looks like this:

R, t - current 6dof pose of the camera
K - 3x3 Camera matrix
D - distortion coefficients
xyz - Nx3 3d points


proj_mat = np.dot(K, np.hstack((R, t[:, np.newaxis])))
# convert 3D points into homgenous points
xyz_hom = np.hstack((xyzs, np.ones((xyzs.shape[0], 1))))

xy_hom = np.dot(proj_mat, xyzs_hom.T).T
xyz_hom.T).T

# get 2d coordinates in image [pixels]
z = xy_hom[:, -1]
xy = xy_hom[:, :2] / np.tile(z[:, np.newaxis], (1, 2))

# undistort - has to be 1xNx2 structure
xy = cv2.undistortPoints(np.expand_dims(xy, axis=0), np.eye(3), D).squeeze()

# drop all points behind camera
xy = xy[z > 0]