How to calculate relative pose between two objects and put them in a transformation matrix

I have poses of two objects. They are in terms of x,y,z and roll, pitch and yaw (I get them in quaternion and then convert them to roll,pitch,yaw). What I need to do is I will record the relative pose between two objects, record them, so, when one of the object rotates, the other is translated and rotated exactly the same way. I am getting the pose from the centroid of the objects.

This video will illustrate what I intend to do.

https://drive.google.com/file/d/1NKtS9fv-FasloVwCKqYIAV1Uc2i9_PN0/view

I have visited the following site for help:

http://planning.cs.uiuc.edu/node102.html

Please help me, I am stuck for a long time, I need a direction, I am new in robotics and I do not have any background in computer vision.

@Vik, so I have implemented as the way you wanted but it is not working properly. I am working in ROS environment in python but I have given the logic part of the code. If you could take a look, then it would be very helpful. I got my matrices from here: http://planning.cs.uiuc.edu/node104.html

#This is object2
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.90
p.pose.position.y = 0.30
p.pose.position.z = 1.2
p.pose.orientation.x=0.0
p.pose.orientation.y=0.0
p.pose.orientation.z=0.0
p.pose.orientation.w=1.0
scene.add_box("object2", p, (0.1, 0.1, 0.2))

rospy.sleep(2)

quaternion1 =   (p.pose.orientation.x,p.pose.orientation.y,p.pose.orientation.z,p.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion1, axes='sxyz') # will provide result in x, y,z sequence
roll=euler[0]
pitch=euler[1]
yaw=euler[2]

C00=math.cos(yaw)*math.cos(pitch)
C01=math.cos(yaw)*math.sin(pitch)*math.sin(roll) - math.sin(yaw)*math.sin(roll)
C02=math.cos(yaw)*math.sin(pitch)*math.cos(roll) + math.sin(yaw)*math.sin(roll)
C03=p.pose.position.x
C10=math.sin(yaw)*math.cos(pitch)
C11=math.sin(yaw)*math.sin(pitch)*math.sin(roll) + math.cos(yaw)*math.cos(roll)
C12=math.sin(yaw)*math.sin(pitch)*math.cos(roll) -math.cos(yaw)*math.sin(roll)
C13=p.pose.position.y
C20= -math.sin(pitch)
C21=math.cos(pitch)*math.sin(roll)
C22=math.cos(pitch)*math.cos(roll)
C23=p.pose.position.z
C30=0
C31=0
C32=0
C33=1

obj2_mat=np.array([[C00, C01, C02, C03],[C10, C11, C12, C13],[C20, C21, C22, C23],[C30, C31, C32, C33]])

#This is object1
p1 = PoseStamped()
p1.header.frame_id = robot.get_planning_frame()
p1.pose.position.x = 0.9
p1.pose.position.y = 0.30
p1.pose.position.z = 0.7
p1.pose.orientation.x=0.0
p1.pose.orientation.y=0.0
p1.pose.orientation.z=0.0
p1.pose.orientation.w=1.0


scene.add_box("object1", p1, (0.1, 0.1, 0.5))


rospy.sleep(2)

quaternion2 = (p1.pose.orientation.x,p1.pose.orientation.y,p1.pose.orientation.z,p1.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion2, axes='sxyz')
roll=euler[0]
pitch=euler[1]
yaw=euler[2]

C00=math.cos(yaw)*math.cos(pitch)
C01=math.cos(yaw)*math.sin(pitch)*math.sin(roll) - math.sin(yaw)*math.sin(roll)
C02=math.cos(yaw)*math.sin(pitch)*math.cos(roll) + math.sin(yaw)*math.sin(roll)
C03=p1.pose.position.x
C10=math.sin(yaw)*math.cos(pitch)
C11=math.sin(yaw)*math.sin(pitch)*math.sin(roll) + math.cos(yaw)*math.cos(roll)
C12=math.sin(yaw)*math.sin(pitch)*math.cos(roll) -math.cos(yaw)*math.sin(roll)
C13=p1.pose.position.y
C20= -math.sin(pitch)
C21=math.cos(pitch)*math.sin(roll)
C22=math.cos(pitch)*math.cos(roll)
C23=p1.pose.position.z
C30=0
C31=0
C32=0
C33=1

obj1_mat=np.array([[C00, C01, C02, C03],[C10, C11, C12, C13],[C20, C21, C22, C23],[C30, C31, C32, C33]])

transformation_mat=np.dot(inv(obj2_mat), obj1_mat) #generating the transformation

rospy.sleep(10)

#This is object1 in second pose
p2 = PoseStamped()
p2.header.frame_id = robot.get_planning_frame()
p2.pose.position.x = 0.70
p2.pose.position.y = -0.9
p2.pose.position.z = 0.7
p2.pose.orientation.x=0.3826834
p2.pose.orientation.y=0.0
p2.pose.orientation.z=0.0
p2.pose.orientation.w=-0.9238795
scene.add_box("object1", p2, (0.1, 0.1, 0.5))

object_position_mat=np.array([[p2.pose.position.x],[p2.pose.position.y],[p2.pose.position.z],[1]]) # (x,y,z,1) position matrix for object1 in its second position

rospy.sleep(2)

Final_position=np.dot(transformation_mat, object_position_mat) #Getting the new position of object2 by multiplying transformation matrix with position of object1


print "============ Generating plan 2"


#This is object2 in second pose
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = Final_position[0]
p.pose.position.y = Final_position[1]
p.pose.position.z = Final_position[2]
p.pose.orientation.x=p2.pose.orientation.x #Kept the same orientation values of object1 in second pose, did not do any calculation as it is logical
p.pose.orientation.y=p2.pose.orientation.y
p.pose.orientation.z=p2.pose.orientation.z
p.pose.orientation.w=p2.pose.orientation.w

scene.add_box("object2", p, (0.1, 0.1, 0.2))

Solution

Using the notation in http://www.ccs.neu.edu/home/rplatt/cs5335_fall2017/slides/homogeneous_transforms.pdf, let's say you have 2 Homogeneous Transformation matrices of the objects in the world frame $^{w}T_{o_1}$ and $^{w}T_{o_2}$ .
You can compose the 4x4 transformation matrices using $T = \begin{bmatrix} \mathbb{R} & \mathbb{X} \\ 0 & 1 \end{bmatrix}$ where R is the 3x3 rotation matrix and X is a 3x1 the translation vector.

What you are looking for is $^{o_1}T_{o_2}$ which is simply ${}^{o_1}T_{o_2} = {}^{o_1}T_w { } {}^wT_{o_2}$ where ${ }^{o_1}T_w = {}^{w}T_{o_1}^{-1}$

Once you transform object one in your desired way, you should be able to just apply $^{o_1}T_{o_2}$ and get the desired results.

Update:
If ${}^{o_2}\mathbb{X}$ refers to the coordinates of points (for examples object points like its corners) in object 2's local co-orindate system as ${}^{o_2}\mathbb{X}=\begin{bmatrix} {}^{o_2}x\\ {}^{o_2}y \\ {}^{o_2}z \end{bmatrix}$ . Then these points are given in the any arbitrary frame f as ${}^{f}\mathbb{X}={}^{f} T_{o_2} \begin{bmatrix} {}^{o_2}\mathbb{X}\\1 \end{bmatrix}$

Here is some code based on your code, hope I understand what you are trying to do:

from tf.transformations import euler_from_quaternion, quaternion_from_euler, \
                               quaternion_matrix, quaternion_from_matrix
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
from std_msgs.msg import Header
import numpy as np

def PoseStamped_2_mat(p):
    q = p.pose.orientation
    pos = p.pose.position
    T = quaternion_matrix([q.x,q.y,q.z,q.w])
    T[:3,3] = np.array([pos.x,pos.y,pos.z])
    return T

def Mat_2_posestamped(m,f_id="test"):
    q = quaternion_from_matrix(m)
    p = PoseStamped(header = Header(frame_id=f_id), #robot.get_planning_frame()
                    pose=Pose(position=Point(*m[:3,3]), 
                    orientation=Quaternion(*q)))
    return p

def T_inv(T_in):
    R_in = T_in[:3,:3]
    t_in = T_in[:3,[-1]]
    R_out = R_in.T
    t_out = -np.matmul(R_out,t_in)
    return np.vstack((np.hstack((R_out,t_out)),np.array([0, 0, 0, 1])))

#This is object2
p_o2 = PoseStamped(header = Header(frame_id="test"), #robot.get_planning_frame()
                   pose=Pose(position=Point(0.9,0.3,1.2), 
                   orientation=Quaternion(0,0,0,1)))

#scene.add_box("object2", p_o2, (0.1, 0.1, 0.2))
#rospy.sleep(2)
Tw2 = PoseStamped_2_mat(p_o2)

#This is object1
p_o1 = PoseStamped(header = Header(frame_id="test"), #robot.get_planning_frame()
                   pose=Pose(position=Point(0.9,0.3,0.7), 
                   orientation=Quaternion(0,0,0,1)))
#scene.add_box("object1", p_o1, (0.1, 0.1, 0.5))
#rospy.sleep(2)
Tw1 = PoseStamped_2_mat(p_o1)

T2w = T_inv(Tw2)
T21 = np.matmul(T2w, Tw1) 

#rospy.sleep(10)
#This is object1 in second pose
p_o2_prime = PoseStamped(header = Header(frame_id="test"), #robot.get_planning_frame()
                         pose=Pose(position=Point(0.7,-0.9,0.7), 
                         orientation=Quaternion(0.3826834,0,0,-0.9238795)))
#scene.add_box("object1", p_o2_prime, (0.1, 0.1, 0.5))
Tw2_prime = PoseStamped_2_mat(p_o2_prime)


Tw1_prime = np.matmul(Tw2_prime, T21)

#rospy.sleep(2)

print "============ Generating plan 2"

#This is object2 in second pose
p_o1_prime = Mat_2_posestamped(Tw1_prime, f_id="test") #  robot.get_planning_frame()
#scene.add_box("object2", p_o1_prime, (0.1, 0.1, 0.2))