apriltag_ros Homogeneous Transformation Matrix
Hi I want to get HTM , because i want to use HTM to compute camera coordinate in tag frame. Result of this code is incorrect, i think this HTM is incorrect , how can get correct HTM? thanks
def tag_callback(msg):
for i in msg.detections:
x = i.pose.pose.pose.position.x
y = i.pose.pose.pose.position.y
z = i.pose.pose.pose.position.z
orientation = [i.pose.pose.pose.orientation.x,i.pose.pose.pose.orientation.y,i.pose.pose.pose.orientation.z,i.pose.pose.pose.orientation.w]
H = tf.transformations.quaternion_matrix(orientation)
H = np.linalg.inv(R)
Pc = np.array([0,0,0,1])
H[0][3] = x
H[1][3] = y
H[2][3] = z
Pt = H.dot(Pc)
print(Pt)
if __name__ =='__main__':
rospy.init_node('localization')
sub = rospy.Subscriber('/tag_detections',AprilTagDetectionArray,tag_callback)
rospy.spin()
ros provides the
tf2andtf2_rospackages to work with homogeneous transforms. You can do the calculation manually of course, but you first have to learn the underlying assumptions made by ros. It's far less effort to use the already-debugged methods in the library. You can get the source code if you want to see how they do it.