3D to 2D Points using cv::projectPoints

edit

Here is code to do it from "scratch". GLM is a header-only library available from https://glm.g-truc.net

Just be mentally prepared for the code to return screen coordinates that are outside of the view port, when the world position is not within the corresponding frustum.

#include <glm/vec2.hpp>
#include <glm/vec3.hpp>
#include <glm/vec4.hpp>
#include <glm/mat4x4.hpp>
#include <glm/gtc/matrix_transform.hpp>
using namespace glm;

#include <iostream>
using namespace std;



vec2 world_to_screen(const vec3 world, const mat4& MVP, const int viewport_width, const int viewport_height)
{
    const vec4 world4(world.x, world.y, world.z, 1);
    vec4 screen4(MVP * world4);

    // Take care of possible division by zero
    if (screen4.w == 0)
        screen4.w = 1;

    vec2 screen2;
    screen2.x = round(screen4.x / screen4.w * viewport_width / 2 + viewport_width / 2);
    screen2.y = round(screen4.y / screen4.w * viewport_height / 2 + viewport_height / 2);

    return screen2;
}



int main(void)
{
    const float y_field_of_view_degrees = 45.0f;
    const float near_plane = 0.01f;
    const float far_plane = 1000.0f;
    const vec3 camera_pos(0, 0, -1);
    const vec3 look_at_pos(0, 0, 0);

    const vec3 camera_vector = normalize(camera_pos - look_at_pos);

    // Be flexible enough to take into account
    // an arbitrary camera_pos and look_at_pos
    vec3 side_vector;

    if (camera_vector == vec3(0, 1, 0))
        side_vector = normalize(cross(vec3(0, 0, 1), camera_vector));
    else
        side_vector = normalize(cross(vec3(0, 1, 0), camera_vector));

    const vec3 up_vector = cross(camera_vector, side_vector);

    const vec3 world_pos(3, 1, -10);
    const int viewport_width = 800;
    const int viewport_height = 600;
    const float aspect_ratio = static_cast<float>(viewport_width) / static_cast<float>(viewport_height);

    mat4 projection_mat = perspective(
        radians(y_field_of_view_degrees),
        aspect_ratio,
        near_plane,
        far_plane
    );

    mat4 view_mat = lookAt(
        camera_pos,
        look_at_pos,
        up_vector
    );

    mat4 model_mat(1.0f); // Identity matrix

    mat4 mvp_mat = projection_mat * view_mat * model_mat;
    vec2 screen_pos = world_to_screen(world_pos, mvp_mat, viewport_width, viewport_height);

    cout << screen_pos.x << ' ' << screen_pos.y << endl;

    return 0;
}

If you're interested, here is a custom projection matrix code that I wrote before I started to use GLM:

void get_perspective_matrix(float fovy, float aspect, float znear, float zfar, float (&mat)[16])
{
    const float pi = 4.0f*atanf(1.0);

    // Convert fovy to radians, then divide by 2
    float       f = 1.0f / tan(fovy/360.0f*pi);

    mat[0] = f/aspect; mat[4] = 0; mat[8] = 0;                              mat[12] = 0;
    mat[1] = 0;        mat[5] = f; mat[9] = 0;                              mat[13] = 0;
    mat[2] = 0;        mat[6] = 0; mat[10] = (zfar + znear)/(znear - zfar); mat[14] = (2.0f*zfar*znear)/(znear - zfar);
    mat[3] = 0;        mat[7] = 0; mat[11] = -1;                            mat[15] = 0;
}

You can see that f plays a major role in the projection matrix. I believe that this is analogous to focal length. Regardless, GLM is the way to go! Basically, I implemented my own matrix library, but GLM was so much better.

As for screen coordinates lying outside of the view port, there are 8 possible regions where it could lie. Check out this screen shot from a game I'm making, illustrating what I mean (look for the yellow arrow along the bottom edge of the view port, which indicates the presence of an enemy game ... (more)