A New Monocular Pose Estimation Method for the Coplanar P4P Problem

Abstract

The Perspective of N Points (PNP) problem in optical measurement and machine vision is to estimate the pose parameters of an object by a monocular image. For traditional coplanar P4P methods, the four points must have known coordinates or be a rectangle’s four vertices. In this paper, a new method of the P4P problem for parallelogram objects without shape parameters is proposed to solve the objects’ pose and shape parameters based on the parallelogram vertices’ relative coordinate constraints. The correctness and effectiveness of the method have been verified through experiments. In addition to being used to estimate a parallelogram object’s pose and shape, the new method extends the conclusions of the coplanar P4P problem when the four points are a parallelogram’s four vertices.

1. Introduction

In optical measurement and machine vision, there is a kind of classical pose estimation problem named PNP (Perspective of N Points) problem, proposed by Fishler and Bolles [1]. The PNP problem can be described as how to estimate the pose parameters between object and camera if there are N points on an object and their corresponding projections in the image captured by the camera.

Because at least three points are needed to determine the full pose parameters of an object, if N ≤ 2, the PNP problem will have no determinate solution, while, if N ≥ 6 and the points are non-coplanar, the PNP problem can be easily solved by a direct linear transformation (DLT) algorithm. As a result, the P3P, P4P and P5P problems receive the most attention in the literature.

For the P4P problem, if the four points are non-coplanar, the P4P problem can be translated into a problem of solving a biquadratic equation with five groups of solutions at most [2,3,4]. Otherwise, a unique solution will exist if the four points are coplanar. Linear algorithms can solve not only the rotation matrix and the translation vector, but also any two intrinsic parameters of the camera, e.g., the equivalent focal lengths can be solved [5,6].

In addition, the non-coplanar P4P problem may have a unique solution if the points meet some spatial distribution restrictions [7]. If a PNP problem has a unique solution, linear algorithms can obtain the solution [8,9,10]. The PNP problems with multi-groups of solutions are usually solved by iterative algorithms with initial values [11,12], such as the Bundle Adjustment (BA) method [13,14]. In recent years, methods for PNP problems have been used in indoor positioning, navigation and monitoring [15,16,17,18].

In this paper, the coplanar P4P problem is studied when the four points are just the four vertices of a parallelogram. For traditional P4P algorithms, the four points’ coordinates must be known, or the four points must form a rectangle [19,20]. For the pose and shape estimation of parallelogram objects with unknown shape parameters, such as some parallelogram structures on buildings as shown in Figure 1 and the parallelogram formed by a walking man’s body standing at two positions (discussed at the end of this paper), the solution conditions for traditional P4P methods do not exist.

According to the projection relationships and the relative coordinate constraints of the parallelogram’s vertices, a new monocular pose estimation method is presented in this paper to solve the coplanar P4P problem of a parallelogram object without shape parameters. By the new method, a parallelogram object’s pose parameters (translation vectors and rotation angles relative to the camera) and shape parameters (the four vertices’ coordinates) can be obtained from one image of the object when one of the parallelogram’s side lengths is known. If there is no known scale information, a scale factor between the estimation results and the real values will exist.

2. Methodology

In the new method, the camera system will be taken as a reference system to simplify the expression of the projection relationships. Similarly, the number of unknown values will be reduced according to the relative coordinate constraints of the parallelogram’s four vertices. Then, a set of linear equations will be established and solved. After that, the coordinates of the four vertices can be obtained by using a side length of the parallelogram. With the coordinates of the four vertices, an object system can be established on the parallelogram and the coordinates of each vertex in the object system can be calculated. With the four vertices’ coordinates in the camera system and the object system respectively, the transformation parameters between the two systems can be obtained, including the position and attitude parameters of the object relative to the camera.

In general, the new method to estimate a parallelogram object’s pose parameters includes two steps: Step 1. To calculate the real coordinates of the parallelogram’s four vertices by taking the camera system as a reference system, and Step 2. To calculate the parallelogram’s pose parameters relative to the camera based on the four vertices’ known coordinates in the camera system and the object system, respectively.

2.1. To Calculate the Real Coordinates of the Parallelogram’s Four Vertices

(1)

Projection relationships of a parallelogram’s four vertices

As shown in Figure 2, a parallelogram’s four vertices are P₁ to P₄, where P₁P₂∥P₃P₄, |P₁P₂| = |P₃P₄| and P₁P₃∥P₂P₄, |P₁P₃| = |P₂P₄|. The camera system S-XYZ is taken as a reference system. The image plane is I. The corresponding projections of P₁ to P₄ are p₁ to p₄.

In the reference system, the coordinates of P_i are M_i = [X_Si Y_Si Z_Si]^T, where i = 1, 2, 3, 4. The image coordinates of the corresponding projections are m_i = [u_i v_i]^T. A pin-hole camera model is used. Because the camera system is taken as the reference system, the translation vector T_S is a zero vector, and the rotation matrix R_S is an identity matrix. Then, the projection relationships are

$${\lambda }_i{\tilde{m}}_i=K{\widetilde{{\rm M}}}_i\begin{array}{cc}& \left(i=1,2,3,4\right)\end{array}$$

(1)

where ${\tilde{m}}_i$ and ${\tilde{M}}_i$ are the augment matrices of m_i and M_i. λ_i is a scale factor. K is the camera’s known intrinsic matrix.

For a parallelogram, the coordinates of the four vertices meet the following relationship.

$$M_3-M_1=M_4-M_2$$

(2)

By denoting t = [t_X t_Y t_Z]^T = M₃ − M₁ = M₄ − M₂, the projection relationships are

$$\{\begin{array}{c}{\lambda }_1{\tilde{m}}_1=K{\tilde{M}}_1\\ {\lambda }_2{\tilde{m}}_2=K{\tilde{M}}_2\\ {\lambda }_3{\tilde{m}}_3=K\left({\tilde{M}}_1+\tilde{t}\right)\\ {\lambda }_4{\tilde{m}}_4=K\left({\tilde{M}}_2+\tilde{t}\right)\end{array}$$

(3)

where $\tilde{\mathbf{t}}$ is the augment matrix of t.

(2)

Solution of the parallelogram vertices’ coordinates in the camera system

Because the object is in front of the camera, the Z coordinates of the four vertices are positive. Then, eight intermediate variables can be defined as follows.

$$\{\begin{array}{l}{g}_0=\frac{X_{S1}}{Z_{S1}},g_1=\frac{Y_{S1}}{Z_{S1}},g_2=\frac{X_{S2}}{Z_{S1}},g_3=\frac{Y_{S2}}{Z_{S1}},g_4=\frac{Z_{S2}}{Z_{S1}}\hfill \\ g_5=\frac{t_X}{Z_{S1}},g_6=\frac{t_Y}{Z_{S1}},g_7=\frac{t_Z}{Z_{S1}}\hfill \end{array}$$

(4)

The scale factor λ_i can be eliminated from Equation (3) and the equations can be expressed as

$$\{\begin{array}{ccc}\hfill \left(u_1-u_0\right)& =& \alpha g_0+\gamma g_1\hfill \\ \hfill \left(v_1-v_0\right)& =& \beta g_1\hfill \\ \hfill \left(u_2-u_0\right)g_4& =& \alpha g_2+\gamma g_3\hfill \\ \hfill \left(v_2-v_0\right)g_4& =& \beta g_3\hfill \\ \hfill \left(u_3-u_0\right)\left(1+g_7\right)& =& \alpha \left(g_0+g_5\right)+\gamma \left(g_1+g_6\right)\hfill \\ \hfill \left(v_3-v_0\right)\left(1+g_7\right)& =& \beta \left(g_1+g_6\right)\hfill \\ \hfill \left(u_4-u_0\right)\left(g_4+g_7\right)& =& \alpha \left(g_2+g_5\right)+\gamma \left(g_3+g_6\right)\hfill \\ \hfill \left(v_4-v_0\right)\left(g_4+g_7\right)& =& \beta \left(g_3+g_6\right)\hfill \end{array}$$

(5)

where α, β and γ are constant coefficients combined by the camera’s intrinsic parameters. Thus, a set of linear equations of the intermediate variables are obtained. Moreover, the intermediate variables can be obtained analytically.

By setting the length of P₁P₂ to L, the following expression can be obtained based on the definition of the intermediate variables.

$$Z_{S1}=L/\sqrt{{\left(g_2-g_0\right)}^2+{\left(g_3-g_1\right)}^2+{\left(g_4-1\right)}^2}$$

(6)

Then, the coordinates of the four vertices in the camera system are obtained.

$$\{\begin{array}{l}{M}_1={\left[\begin{array}{ccc}{g}_0{Z}_{S1}& g_1{Z}_{S1}& Z_{S1}\end{array}\right]}^T\\ M_2={\left[\begin{array}{ccc}{g}_2{Z}_{S1}& g_3{Z}_{S1}& g_4{Z}_{S1}\end{array}\right]}^T\\ M_3=M_1+t\\ M_4=M_2+t\end{array},\begin{array}{ccc}& \mathrm{with}& t={\left[\begin{array}{ccc}{g}_5{Z}_{S1}& g_6{Z}_{S1}& g_7{Z}_{S1}\end{array}\right]}^T\end{array}$$

(7)

If L is precisely known, the real coordinates of the parallelogram’s four vertices in the camera system can be obtained. In fact, if a distance between any two of the four vertices is known, a unique solution will exist for such a P4P problem, and if no scale information is known, a scale factor will exist between the vertices’ real coordinates and the estimation results.

2.2. To Calculate the Parallelogram’s Pose Parameters Relative to the Camera

As shown in Figure 3, the coordinates of the parallelogram’s four vertices in the camera system S-X_SY_SZ_S have been obtained. Then, their coordinates in the parallelogram’s object system B-X_BY_BZ_B can be obtained based on the definition of the object system. The object’s pose parameters relative to the camera, the rotation matrix R_B and the translation vector T_B, that

$$\left[\begin{array}{c}{X}_{Si}\\ Y_{Si}\\ Z_{Si}\end{array}\right]=R_B\left[\begin{array}{c}{X}_{Pi}\\ Y_{Pi}\\ Z_{Pi}\end{array}\right]+T_B\begin{array}{cc}& i=1,2,3,4\end{array}$$

(8)

can be calculated by coordinate transformation from the four vertices’ coordinates in the two systems, respectively.

Here, if no scale information is known, an unknown scale factor will exist between the estimation results of the object’s position and the real values. While the estimation results of the object’s attitude angles are independent of any scale information.

Additionally, the parallelogram object’s shape parameters can also be obtained after all the vertices’ positions are obtained.

3. Experimental Results

In the experiments, a parallelogram object is used. The object’s vertices’ coordinates in the object coordinate system are P₁ (−70 mm, 120 mm, 0), P₂ (−70 mm, −60 mm, 0), P₃ (70 mm, 60 mm, 0) and P₄ (70 mm, −120 mm, 0), as shown in Figure 4.

A camera with known intrinsic parameters is used to capture images of the object. The proposed method is used to estimate the parallelogram object’s pose parameters relative to the camera and the coordinates of the four vertices. As a comparison, a traditional iterative algorithm is used to estimate the object’s pose parameters. The “iterative algorithm” is actually the bundle adjustment method, a kind of iterative correction and optimization algorithm from initial values, which is famous in photogrammetry and computer vision for its performance of high accuracy. For the traditional method, the parallelogram vertices’ coordinates are taken as known, only the parallelogram’s pose parameters are to be solved.

One of the experimental images is shown in Figure 5, where the white crosses show the extracting positions of the parallelogram vertices.

The estimation results in experiments are shown in

Table 1. Shape and pose estimation results of a parallelogram object in experiments.

No.	Methods	Pose estimation Results						Difference between the Pose Results Estimated by the Two Methods						Mean Errors of the Vertices’ Coordinates (mm)
		Translation Vector(mm)			Rotation Angles (°)			Translation Vector (mm)			Rotation Angles (°)
		TBX	TBY	TBZ	ABX	ABY	ABZ	TBX	TBY	TBZ	ABX	ABY	ABZ
1	New P4P	20.4	−7.1	1981.9	−0.8	10.0	0.9	−0.1	0.2	3.6	−0.7	2.3	0.2	0.4
	Old P4P	20.5	−7.3	1978.3	−0.1	7.7	0.7
2	New P4P	−5.1	−7.4	1916.8	2.0	−39.3	−4.0	−0.6	−0.5	4.2	1.4	0.8	0.1	0.9
	Old P4P	−4.5	−6.9	1912.6	0.6	−40.1	−4.1
3	New P4P	269.4	5.6	2048.2	3.0	10.4	0.1	0.1	0.1	−2.7	−0.8	−1.8	−0.1	0.6
	Old P4P	269.3	5.5	2050.9	3.8	12.2	0.2
4	New P4P	323.9	19.0	2176.6	45.3	32.3	18.1	−0.4	−0.3	−3.2	0.6	−0.4	0.3	0.6
	Old P4P	323.3	19.3	2179.8	44.7	32.7	17.8

which consists of:

(1)

The translation vectors (T_BX, T_BY, T_BZ) and rotation angles (A_BX, A_BY, A_BZ) between the object and the camera estimated by the new method (noted by New P4P in the table).

(2)

The translation vectors and rotation angles estimated by the traditional method (noted by Old P4P in the table) taking all of the vertices as known.

(3)

The difference between the pose estimation results by the two methods, respectively.

(4)

The mean errors of the four vertices’ coordinates in the object system estimated by the new method in each set of experiment.

In the experiments, one of the side lengths of the parallelogram is taken as known and all of the vertex coordinates are taken as unknown for the new method. While, for the traditional method, the parallelogram vertices’ coordinates are known. Relatively, the new method solves the pose parameters linearly from fewer conditions, so its accuracy is undoubtedly inferior to bundle adjustment. The new method’s advantage is achieving pose estimation under less known conditions than traditional methods require. Comparing the pose estimation results by the new method with that by the traditional method shows the correctness and effectiveness of the presented new method.

In terms of estimating the parallelogram vertices’ coordinates, under above conditions, the new method can do it but traditional methods cannot. In Table 1, the mean errors of the vertices’ coordinates are calculated by

$$E_B=\frac{{\displaystyle \sum _{i=1}^4\sqrt{{\left(X_{Bi}-{\tilde{X}}_{Bi}\right)}^2+{\left(Y_{Bi}-{\tilde{Y}}_{Bi}\right)}^2}}}{4}$$

(9)

where, $\left(X_{Bi},Y_{Bi}\right)$ and $\left({\tilde{X}}_{Bi},{\tilde{Y}}_{Bi}\right)$ are the estimation results and true values of the ith vertex’ coordinates, respectively.

4. Conclusions and Discussion

A new method to estimate a parallelogram object’s pose and shape parameters from its single image is presented. If there is no scale information, a scale factor will exist between the estimation results and the real values of the object’s position and the vertices’ coordinates. The four points need not to be a rectangle’s four vertices and their coordinates need not to be known. In fact, it is also a method to calibrate a camera’s extrinsic parameters by a parallelogram reference object.

At the same time, the new method expands the conclusions of the P4P problem by obtaining a new conclusion regarding the coplanar P4P problem when the four points are a parallelogram’s four vertices. In other words, If the four points are a parallelogram’s four vertices and the distance between two of them is known, a unique solution will exist for the P4P problem, and if no scale information is known, a scale factor will exist between the real position and the estimation result.

The new method is verified by real data experiments. In the experiments, the pose and shape parameters results estimated by the new method accord well with that estimated by a traditional bundle adjustment method which takes all of the vertices as known. For most general applications, the results of the new estimation method will be practicable.

As previously mentioned, many parallelogram objects exist in real scenes, including some buildings and parallelogram patterns on buildings, etc. Especially, by considering two images in a monocular image sequence of a placidly walking human or other placidly moving objects, a parallelogram object can be obtained, as shown in Figure 6. The human’s medial axes in the two images are used as a pair of opposite sides of a parallelogram. The human’s stature can be used as the known side distance. Then, the position and attitude parameters of the placidly walking human can be estimated by our new method.