Some parts of the closed-form solution are achieved by translating, but not rotating, the end-effector. This pure translational motion can decompose the problem into linear forms according to the constraints of parallel laser beams.
Several orientations of the end-effector with the laser pointer are performed. Under the m-th orientation, the end-effector is translated, but not rotated, along several vectors, jm n, ’s, and the laser spot on the plane moves along corresponding vectors,
m n,
i ’s, as shown in Figure 2-3. In the end, the orientations of laser beams corresponding with different hand poses determine the geometric dimensions of the overall system.
Figure 2-3 Projection of parallel laser beams on a plane.
The step-by-step of this closed-form solution is clearly described in detail as follows.
1) Calculate the ray directions of all points:
Each projected laser spot is located in an image, and its direction from the camera is obtained by calculating its inverse projection. With the camera intrinsic parameters, for any point in an image, the ray direction x can then be derived accordingly. r 2) Find homogeneous matrixes, each having an unknown scale factor:
Suppose the end-effector moves along a direction (without rotation) that is a linear combination of N translating vectors (jm n, vectors). The laser spot will move along a vector that is the same linear combination of corresponding vectors (im n, vectors).
Specifically, under m-th rotation and k-th translation, the laser spot is at position
1
where w , n = 1~N are scales of a combination according to end-effector movement, n
and H is a homogeneous matrix . Let m h be the i-th row of i H. A solution for the homogeneous matrix can then be obtained by DLT method (Hartley and Zisserman [54]). That is
Q is ranked 3N+2. The solution is the eigenvector corresponding to the smallest H
eigenvalue of the matrix Q QTH H. According to (2-13), the solution has an unknown scalar factor λ, and
3) Find the normal vector of the plane w.r.t the camera:
Since all im n, ’s are on the same plane, the normal vector of the plane, ΠCn, is
orthogonal to these vectors, i.e., C TΠn i⋅ m n, =0 for n = 1~N, m = 1~M. The resulting if the ˆim n, ’s are not parallel. If the numbers of translating vectors in each orientation are identical, Equation (2-14) has N×M equations. The solution is the eigenvector corresponding to the smallest eigenvalue of the matrix Q QTn n and is normalized to a unit vector.
4) Find laser directions w.r.t. the camera:
As indicated by Figure 2-3, the vector jm n, projects onto vector im n, along the laser beam direction um. Figure 2-4 shows the relationships by placing the vectors together on the plane.
Figure 2-4 Intersection of a laser beam and a plane.
Therefore,
After attaching the coordinate systems,
, , ,
C C C C C B
m n m⋅ m n m B⋅ ⋅ m n
i = G j = G R j . (2-17)
Let a transformation matrix be defined as
1 1
Equation (2-17) can be rewritten as
, 1
The N translating vectors in m-th orientation form
1 1 (2-20) are obtained from previous procedure ((2-13)). The solution is calculated by least squares method and is rearranged into a 3×3 matrix, which is proportional to
ˆm
G with a scalar 1λm. This matrix is denoted as Gm =Gˆm λm . According to (2-20), at least three non-coplanar robot translations are required to obtain a unique solution.
The rotation term can be eliminated algebraically by multiplying Gm with its transpose. Let um =um /
(
λm muT ⋅ Πn)
. According to (2-16) and (2-17), Each element on the left side equal to relative element on the right side forms a quadratic equation of three arguments that are three elements of um. Since the matrixm Tm
G G is symmetric, it contains six quadratic equations and their coefficients are composed of the elements of the normal vector Πn . The solution of these equations is um. Normalizing um to a unit vector gives the laser direction um at the m-th orientation in the camera frame.
5) Find the rotation matrix between the camera and robot:
Since rotation between the robot base frame and the camera frame is unique, all vectors at different hand orientations are subject to this relation. From (2-18),
, ,
ˆTa m =CB C Ta m
g R g for a = 1~3 and m = 1~M. Rotation CBR represented by a unit quaternion is applied to obtain a linear form of this equation (Horn[55]). The quaternion is generally defined as q q q= 0+ 1i+q2j+q3k and can also be written in a vector q =
[
q0 q q1 2 q3]
T. Let vˆ [0= gˆ]T and v=[0 Cg , ]Tˆ = ⊗ ⊗ -1
v q v q (2-22)
where ⊗ represents quaternion multiplication and q is the inverse of the -1 quaternion. Equation (2-22) is equal to
The equation for all vectors rotating from camera frame to robot base frame is,
1,1 1,1
The solution is the eigenvector corresponding to the smallest eigenvalue of the matrix
Tq q
Q Q and is normalized to satisfy CB T Bq qC =1. The rotation matrix from the camera coordinate system to the robot base coordinate system is then obtained from
2 2
The laser emitter is rigidly attached on the end-effector and the laser beam direction is EΛu relative to the end-effector frame. The laser beam direction at the m-th hand orientation in the camera frame is
C C B E
m B E m Λ
u = R R u. (2-26)
Hence, the M orientations lead to an equation,
1 1
The solution can be calculated by least square method.
7) Find the scale factors of the homogeneous matrix and the translations of the laser and the camera:
The laser beam is emitted from its origin in hand to the position on the plane. At m-th orientation and k-th translation, the laser is at position ΛBpm,k = BER tmEΛ +E m kBt ,
After transforming the laser position from the robot base frame to the camera frame, the laser beam from the robot side to the plane is solution is calculated by least square method.
8) Find the plane w.r.t. the camera frame:
Since the plane normal vector is already obtained in the third step, the position of the plane can be calculated. A better estimation can be obtained by averaging the results computed in the previous steps. The plane is
(
M K1 1 ,)
in the camera frame.