Calculate the normal equation for a checkerboard found through pose estimation-CodePudding

I am working on a laserscanning project. For this, I am following this After calculating/finding the intrinsic camera matrix with OpenCv (Python 3)

cv.findChessboardCorners()

cv.cornerSubPix()

following this tutorial by opencv.org, I am using the

cv.solvePnP()

function to get the rotational and translational vector of the planes. With the following code, I am calculating now the normal and the signed distance from the camera center following the authors formula:

normalvector = rotationmatrix * [[0][0][1]]
distance = (normalvector * -1)T * translational vector

(T = vector got transposed)

import numpy as np
import cv2 as cv

with np.load("vectorsRT.npz", "r") as file:
    rvecs, tvecs = [file[i] for i in ("rotationVectors", "translationVectors")]

rot_matrix = cv.Rodrigues(rvecs)[0]
mult_matrix = np.array([[0],[0],[1]])

normal_vector = np.matmul(rot_matrix, mult_matrix)
normal_vector_neg = normal_vector * -1
normal_vector_neg_transposed = np.transpose(normal_vector_neg)

signed_distance = np.matmul(normal_vector_neg_transposed, tvecs)

print("normal_vector:", normal_vector)    
print("signed_distance:", signed_distance)

With this, I am getting now a normal vector and a distance from the camera center, eg.: normal_vector:

[[-0.0604253 ]
 [ 0.16923283]
 [ 0.98372203]]

signed_distance:

[[[-51.67514398]]]

from blender I am getting the following normal

[[-0.00117647]
 [-0.00333335]
 [-0.01939508]]

The normals are more or less equivalent, however, how is the signed distance now converted to a support vector of plane in the world coordinate system, so I can get the full plane equation?

CodePudding user response：

I think I found the answer: Following this post, I introduced a scale factor s for the signed distance, to convert the distance into a real world distance (in mm).

calc_distance_camera_plane = signed_distance * s

This scale factor is calculated through dividing a known distance from a plane (e.g.: the chessboard is on Z = 0 in the world coordinate frame, with the camera beeing fixed at a postion, e.g.: Z = 25 mm, looking downwards) by the signed_distance of that exact image

scale_factor = dist_camera_world_orign / signed_distance

With that scale factor, one can the transformed each signed_distance to a real world distance (camera-plane) and also get the real world distance (world orign-plane)

 dist_world_orign_plane = dist_camera_world_orign - calc_distance_camera_plane

The plane equation I looked for is then given by the calculated normal and a point with

(0, 0, dist_world_orign_plane)

Beeing that I have the exact values of the orignal planes (images were calculated with blender), I could compare the calculated distances of the camera_plane with the real distances (real distance/calculated distance):

n = 10
mean difference: 1.005
standard deviation: 0.0281

I think this is a good starting point for further testing. Hope this answer can help others too, cheers