Different ways to estimate relative pose during visual servoing

In order to obtain relative pose (pdHp) between current and desired pattern pose (cHp and cHpd) in camera frame, we can:

Method 1: Find rigid transformation between two set of pattern points (X_p and X_pd) in camera frame directly using a findRigidTransform() function described here.

Method 2: Calculate cHp and cHpd using the findRigidTransform() as the pattern points in local coordinate is known. The relative pose pdHp is calculated as:

pdHp = inv(cHpd) * cHp

Method 3: Calculate cHp and cHpd using the cv::solvePnP() function which finds an object pose from 3D-2D point correspondence. The relative pose is then calculated same as method 2.

According to the results, Method 1 is least stable and accurate as the estimated relative pose varies a lot and very different from the other two methods.
Method 2 and 3 have similar results in which Method 3 seems better since the rotation part is closer to Identity when the relative pose should be the same.

Summary: Method 3 is the best for the time being.

Relative trajectory playback from different starting pose

1. Given a trajectory in the global frame, a relative trajectory (H_rela) can be generated as:

H_rela = inv(H_1) * H_i

Since the relative transformation is regard to local frame (first pose in the trajectory). H_rela(1) is Identity

2. For a new starting pose in the global frame H_start, a new trajectory begins at the starting pose is generated as:

H_new = H_start * H_rela

where H_new is in the global frame.

Angular velocity and Skew Symmetric Matrices

A matrix S is said to be skew symmetric if and only if: S^T + S = 0.
Thus S contains only three independent entries and every 3 × 3 skew symmetric matrix has the form:

If a = (a_x, a_y, a_z)' is a 3-vector, the skew symmetric matrix S(a) can be defined as:

Important properties of the matrix S(a):

• Linearity: for any vector a and b belonging to R^3 and scalars alpha and beta:
• S(alpha*a + beta*b) = alpha*S(a) + beta*S(b)
• Calculation of cross project: for any vector p = (p_x, p_y, p_z)':
• S(a)*p = a.cross(p)
• For an orthogonal matrix (such as rotation) R in SO(3), and a,b are vector in R^3:
• R(a.cross(b)) = (R*a).cross(R*b)
• For R in SO(3) and a vector a belongs to R^3, we have:
• 
• the deviation is:
  

• Computing the derivative of the rotation matrix R is equivalent to a matrix multiplication by a skew symmetric matrix S, that is:
• 

Velocity of a point on a rotating rigid body which is moving with a linear velocity is derived in:

http://en.wikipedia.org/wiki/Angular_velocity#Rigid_body_considerations

Reference:
Skew symmetric matrix