3D object orientation based on plane normal in OpenGL

Hi all

I am having a problem in 3D to 2D object orientation. So, I am working on an AR app in Python using OpenGL. I can estimate the depth of the scene and calculate the surface normals using other methods. I also have the camera calibration details+rotation and translation.

GOAL: I want to project a 3D model like a 3D wine bottle onto one of the planar surfaces. The surface can be horizontal or vertical. And, note that the object has to be projected perpendicular to surface. So the idea is that I choose a point by clicking on the surface and the bottle will be projected there.

Please see the attached images (sample). It shows what I have as input and the other ones are the expected outputs.
[ATTACH=CONFIG]2961[/ATTACH]
[ATTACH=CONFIG]2962[/ATTACH]
[ATTACH=CONFIG]2963[/ATTACH]

I just started to use opengl and managed to get up to a certain point. Atm I can click and locate the object but the result look ridiculous (image below) and I don’t know how to rotate the object based on surface normal.
[ATTACH=CONFIG]2964[/ATTACH]
Sorry for the mistakes in the code. I don’t know what I did correct and what is wrong.

Inputs of the following code: [ATTACH=CONFIG]2965[/ATTACH] Camera details

from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
import pygame, pygame.image
from pygame.locals import *
import pickle
import numpy as np
from objloader import *

glutInit()
width, height = 807, 605


def setup():
    """ Setup window and pygame environment. """
    pygame.init()
    window = pygame.display.set_mode((width, height), OPENGL | DOUBLEBUF)
    pygame.display.set_caption('Asset Int...')


def draw_background(imname):
    """ Draw background image using a quad. """

    # load background image (should be a .bmp) to OpenGL texture
    bg_image = pygame.image.load(imname).convert()
    bg_data = pygame.image.tostring(bg_image, "RGBX", 1)

    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)

    glEnable(GL_TEXTURE_2D)

    # bind the texture
    glBindTexture(GL_TEXTURE_2D, glGenTextures(1))
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, \
                 0, GL_RGBA, GL_UNSIGNED_BYTE, bg_data)
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, \
                    GL_NEAREST)
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, \
                    GL_NEAREST)

    # create quad to fill the whole window
    glBegin(GL_QUADS)
    glTexCoord2f(0.0, 0.0)
    glVertex3f(-1.0, -1.0, -1.0)
    glTexCoord2f(1.0, 0.0)
    glVertex3f(1.0, -1.0, -1.0)
    glTexCoord2f(1.0, 1.0)
    glVertex3f(1.0, 1.0, -1.0)
    glTexCoord2f(0.0, 1.0)
    glVertex3f(-1.0, 1.0, -1.0)
    glEnd()

    # clear the texture
    glDeleteTextures(1)
    glDisable(GL_TEXTURE_2D)


def set_projection_from_camera(K):
    """  Set view from a camera calibration matrix. """

    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()

    fx = K[0, 0]
    fy = K[1, 1]
    fovy = 2 * np.arctan(0.6 * height / fy) * 180 / np.pi
    aspect = (width * fy) / (height * fx)

    # define the near and far clipping planes
    near = 0.1
    far = 100.0

    # set perspective
    gluPerspective(fovy, aspect, near, far)
    glViewport(0, 0, width, height)


def set_modelview_from_camera(Rt):
    """  Set the model view matrix from camera pose. """

    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()

    # rotate teapot 90 deg around x-axis so that z-axis is up
    Rx = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]])

    # set rotation to best approximation
    R = Rt[:, :3]
    U, S, V = np.linalg.svd(R)
    R = np.dot(U, V)
    R[0, :] = -R[0, :]  # change sign of x-axis

    # set translation
    t = Rt[:, 3]

    # setup 4*4 model view matrix
    M = np.eye(4)
    M[:3, :3] = np.dot(R, Rx)

    M[:3, 3] = t

    # transpose and flatten to get column order
    M = M.T
    m = M.flatten()

    # replace model view with the new matrix
    glLoadMatrixf(m)


def draw_teapot(size, angle=[0, 0, 0], pos=[0, 0, 0]):
    """ Draw a red teapot at the origin """
    glEnable(GL_LIGHTING)
    glEnable(GL_LIGHT0)
    glEnable(GL_DEPTH_TEST)
    glClear(GL_DEPTH_BUFFER_BIT)

    # draw red teapot
    glMaterialfv(GL_FRONT, GL_AMBIENT, [0, 0, 0, 0.0])
    glMaterialfv(GL_FRONT, GL_DIFFUSE, [0.5, 0.0, 0.0, 0.0])
    glMaterialfv(GL_FRONT, GL_SPECULAR, [0.7, 0.6, 0.6, 0.0])
    glMaterialf(GL_FRONT, GL_SHININESS, 0.25 * 128.0)

    # Translation
    glTranslatef(pos[0], pos[1], pos[2])

    # Rotation
    glRotatef(angle[0], 1, 0, 0)
    glRotatef(angle[1], 0, 1, 0)
    glRotatef(angle[2], 0, 0, 1)

    #glutWireTeapot(size)
    glutSolidTeapot(size)
    glDisable(GL_LIGHTING)
    # glutSwapBuffers()


def normalize_v3(arr):
    ''' Normalize a numpy array of 3 component vectors shape=(n,3) '''
    lens = np.sqrt( arr[0]**2 + arr[1]**2 + arr[2]**2 )
    arr[0] /= lens
    arr[1] /= lens
    arr[2] /= lens
    return arr


# load camera data
with open('ar_camera_2.pkl', 'r') as f:
    K = pickle.load(f)
    Rt = pickle.load(f)

setup()
draw_background('IMG-2082_res.JPG')
set_projection_from_camera(K)
set_modelview_from_camera(Rt)
scale = 0.06
degrees=[0, 0, 0]
translate_unit = [0,0,0]

while True:
    event = pygame.event.poll()
    if event.type in (QUIT, KEYDOWN):
        break

    elif event.type == pygame.MOUSEBUTTONDOWN:

        pos = pygame.mouse.get_pos()
        pos = np.array(pos)
        pos[1] = height - pos[1]
        pos = tuple(pos)

        model_view = np.array(glGetDoublev(GL_MODELVIEW_MATRIX))
        proj = np.array(glGetDoublev(GL_PROJECTION_MATRIX))
        view = np.array(glGetIntegerv(GL_VIEWPORT))

        pointCenter = gluUnProject(pos[0], pos[1], 0.1865, model_view, proj, view)
        pointUp = gluUnProject(pos[0], pos[1] - 15, 0.1765, model_view, proj, view)
        pointLeft = gluUnProject(pos[0] - 15, pos[1], 0.1765, model_view, proj, view)

        centerUpVec = normalize_v3(np.subtract(pointCenter, pointUp))
        centerLeftVec = normalize_v3(np.subtract(pointCenter, pointLeft))
        normalVec = normalize_v3(np.cross(centerUpVec, centerLeftVec))

        upVec = (0.0, 1.0, 0.0)
        xAxis = normalize_v3(np.cross(upVec, normalVec))
        yAxis = normalize_v3(np.cross(xAxis, upVec))

        draw_teapot(scale, degrees, pointCenter)

    pygame.display.flip()