Get coordinates of the projection point depending of the angles do the Camera

edit

So basically I have a Camera positioning coords (x1, y1, y1) as well as the angles of the direction the camera is facing, and a point in the same space that I want to show in screen (x2, y2, z3).

I have a camera Matrix (with the values of the future camera im going to use):

def get_camera_matrix(fovx, fovy, height, width):
## FOVX is the horizontal FOV angle of the camera
## FOVY is the vertical FOV angle of the camera
x = width / 2
y = height / 2
fx = x / math.tan(fovx)
fy = y / math.tan(fovy)
return np.array([[fx, 0, x],
                 [0, fy, y],
                 [0, 0, 1]])

I also have a rotationMatrix from the angles, given by:

def eulerAnglesToRotationMatrix(pitch, yaw, roll):
# Calculates Rotation Matrix given euler angles.
r_x = np.array([[1, 0, 0],
                [0, math.cos(roll), -math.sin(roll)],
                [0, math.sin(roll), math.cos(roll)]])
r_y = np.array([[math.cos(pitch), 0, math.sin(pitch)],
                [0, 1, 0],
                [-math.sin(pitch), 0, math.cos(pitch)]])
r_z = np.array([[math.cos(yaw), -math.sin(yaw), 0],
                [math.sin(yaw), math.cos(yaw), 0],
                [0, 0, 1]])
return np.dot(r_z, np.dot(r_y, r_x))

But now I dont seem to understand what Im missing to get the final (x, y) point on screen. I looked up: http://docs.opencv.org/2.4/modules/ca... But it I dont seem to understand where the angle of the camera enters. I want to move the angle of the camera and have it reflect on the position of the target.

This is what Im doing so far:

getPointInterestOnPixel(point_in_world,
       point_of_camera,
       eulerAnglesToRotationMatrix(roll, pitch, yaw),
       np.array([[0, 0, 0]]),
       eulerAnglesToRotationMatrix(-np.pi / 2, -np.pi / 2, -np.pi / 2),
       matrix)

def getPointInterestOnPixel(P_i_w, P_a_w, R_a_w, P_c_a, R_c_a, K):
       R_c_w = np.dot(R_a_w, R_c_a)
       #P_i_c = np.dot(R_c_w.T, (P_i_w.T - P_a_w.T - np.dot(R_a_w, P_c_a.T)))
       P_i_c = np.dot(R_c_w, (P_i_w.T - P_a_w.T))
       P_i_c_pixels = np.dot(K, P_i_c)
       P_i_c_pixels = np.divide(P_i_c_pixels, P_i_c_pixels[2])
       return P_i_c_pixels

Im going crazy, at this moment, I get the points in screen, they move, but if I do a 180 degree turn, the points turn upsidedown. I tried so many things its all getting so confused right now.

Please help? Thanks

EDIT: Used projectPoints as @Tetragramm sugested, but things got even wierder. All points are in the same place when they shouldnt, maybe Im missing something, kinda new to this. I just want to move my camera and have the points move as well in the correct place :(

points2d, jacobian = cv2.projectPoints(np.asarray(points3d),
    np.dot(hud.eulerAnglesToRotationMatrix(math.radians(pitch), math.radians(yaw), math.radians(roll)), hud.eulerAnglesToRotationMatrix(-np.pi / 2, -np.pi / 2,-np.pi / 2)),
    np.array([np.float32(0), np.float32(0), np.float32(0)]),
    hud.get_camera_matrix(self._fovx, self._fovy, height, width),
    None)

or if I use Rodrigues:

dst, jacobian = cv2.Rodrigues(np.array([np.float64(pitch),np.float64(yaw),np.float64(roll)]))

points2d, jacobian3 = cv2 ...