1

With cv2.solvePnP I try to do pose a estimation in pyvista, which is a python wrapper for vtk. The results of solvePnP seem wrong to me, i.e. the resulting translation and rotation. For simplicity I try to "undo" a translation of the camera. I expect the inverse of the translation to be the result of solvePnP.

enter image description here The translation is just

Translation = np.array([[  1.,   0.,   0., 1000.],
                        [  0.,   1.,   0.,   0.],
                        [  0.,   0.,   1.,   0.],
                        [  0.,   0.,   0.,   1.]])

i.e. a shift along one axis. But the resulting rvec,tvec are

 rvec = array([ 0., 0., -3.142]),
 tvec = array([ 707.107, 408.248, 8882.736])

The resulting translation and rotation seem nonsensical to me. Since the translation does no rotation, I expect that only tvec has non zero entries to undo the translation in the opposite direction. Concretely, I expect tvec= [-1000,0,0] and rvec=[0,0,0]. If I then apply this (wrong) result to the camera, all points disappear completely.

Here is What I do:

import cv2
import pyvista as pv
from pyvista import examples
import pyvistaqt
from vtk import vtkMatrix4x4, vtkMatrix3x3, vtkTransform
from vtk.util.numpy_support import vtk_to_numpy  
import numpy as np

np.set_printoptions(suppress=True,precision=3)

def getCamMatrix():
    
    narray = np.eye(4)
    vmatrix = plotter.camera.GetModelViewTransformMatrix()
    vmatrix.DeepCopy(narray.ravel(), vmatrix)
    return narray    

def toVTK(m,n =4):
    if n == 4:
        newMatrixVTK = vtkMatrix4x4()
    else:
        newMatrixVTK = vtkMatrix3x3()    
    for i in range(n):
        for j in range(n):
            newMatrixVTK.SetElement(i,j, m[i,j])            
    return newMatrixVTK

def applyMatrixToCam(newMatrix):
    global plotter
    newMatrixVTK = toVTK(newMatrix)    
    transform = vtkTransform()
    transform.SetMatrix(newMatrixVTK)
    transform.Update()
    plotter.camera.ApplyTransform(transform)
    pass

print("Setting up points in world coordinates")
Points =  np.array([[ 2918.972,  -887.573,   416.331,1],
        [ 2338.002,  -702.07 ,  1039.864,1],
        [ 1458.473,  -707.246,  1005.19,1 ],
        [ 1219.4  ,  -890.161,   377.004,1],
        [ 1318.727, -1017.829,  -156.537,1],
        [ 2529.132, -1026.888,  -169.222,1]])


pMesh = pv.PolyData(Points[:,:3]) # vtk object to hold the six points


plotter = pyvistaqt.BackgroundPlotter() # setting up the plotting function
plotter.enable_trackball_style()
plotter.add_mesh(pMesh)



print("Transforming from World to Image Coordinates")
# Rotating the points towards a camera at the origin, i.e. applying the default camera transform
projected = (getCamMatrix() @ Points.T)[:3,:].T 

print("store original image points")
image_points = projected.copy()[:,:2]
print("Applying the perspective transform, i.e. division by the Z-coordinate")
image_points /= projected[:,-1].reshape(-1,1) 


print("Setting up a simple translation of the camera position")
Translation = np.array([[  1.,   0.,   0., 1000.],
                        [  0.,   1.,   0.,   0.],
                        [  0.,   0.,   1.,   0.],
                        [  0.,   0.,   0.,   1.]])

applyMatrixToCam(Translation)


print("Apply the new Camera Matrix to the six points")
projected_shift = (getCamMatrix() @ Points.T)[:3,:].T

retval, rvec, tvec = cv2.solvePnP(projected_shift, np.array(image_points), np.eye(3), None, None, None, False, cv2.SOLVEPNP_EPNP)

R = cv2.Rodrigues(rvec)[0]
extrinsicReal = np.vstack([np.hstack([R.T, -R.T@tvec]), [0,0,0,1]])
applyMatrixToCam(extrinsicReal)
dba
  • 325
  • 1
  • 6
  • 16
  • There's several peculiarities in your snippet, but putting that aside for the moment, it's not clear to me which part is causing you trouble. Is it the translation part? Is it the final pose estimation? As someone who has no idea about pose estimation: why are those two vectors "nonsensical"? What do you expect instead? Can you pin down the step which gives the wrong result? Crucially, pinning down the matrices/arrays before `solvePnP` is called would probably help a lot in debugging this. – Andras Deak -- Слава Україні Jun 08 '22 at 18:16
  • Hi Andras, I have added more comments to the text and code. I expected that `solvePnP` will yield the inverse of the transformation matrix that I applied. – dba Jun 08 '22 at 19:04
  • 1
    OK, even having looked at https://docs.opencv.org/4.6.0/d5/d1f/calib3d_solvePnP.html it's clear I'm missing too much domain knowledge to tackle this, sorry. My only recommendation is to add more general tags to your question, in particular [tag:python] and [tag:opencv] because a lot more people watch those broad tags. I'm not even sure why we have a tag as specific as [tag:opencv-solvepnp]... but if we have it we might as well use it. – Andras Deak -- Слава Україні Jun 08 '22 at 20:08

0 Answers0