apriltag_ros 坐标转换

 

 

 

将机载端相机坐标系下的表示 fly_p  转换为tag坐标系下的向量tag_p

 

#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
void QNode::tag_callback(apriltag_ros::AprilTagDetectionArray data) {
  if (data.detections.size() > 0) {
    imgcnt = 4;

    dx = data.detections[0].pose.pose.pose.position.x;
    dy = data.detections[0].pose.pose.pose.position.y;
    dz = data.detections[0].pose.pose.pose.position.z;
    tf2::Vector3 v_cam(dx, dy, dz);
    const tf2::Quaternion midq(data.detections[0].pose.pose.pose.orientation.x,
                               data.detections[0].pose.pose.pose.orientation.y,
                               data.detections[0].pose.pose.pose.orientation.z,
                               data.detections[0].pose.pose.pose.orientation.w);

    // q.setRPY(0,0,1.0);
    imgyaw = tf2::getYaw(midq);
    tw_yaw = imgyaw;
    printf("imgyaw %.2f\n", imgyaw * 57);

    tf2::Matrix3x3 m2(midq.inverse());
    //tf2::Matrix3x3 m2(midq);
    tf2::Vector3 v_tag = m2 * v_cam;

    std::cout << __LINE__ << "[" << dx << "," << dy << "," << dz << "]\n";

    printf("Vcam:X=%f,%f,%f\n", v_cam.getX(), v_cam.getY(), v_cam.getZ());
    printf("v_tag:X=%f,%f,%f\n", v_tag.getX(), v_tag.getY(), v_tag.getZ());
    dz = -v_tag.getY(); // forward
    //  dx =
  }
}

    def tag_callback(self,data):
        # print  len(data.detections )
        # useid =-1
        if len(data.detections )>0:
            q = data.detections[0].pose.pose.pose.orientation
            q = [q.x,q.y,q.z,q.w]
            m = quaternion_matrix(q)
            m = m.transpose()
            fly_p =np.array( [data.detections[0].pose.pose.pose.position.x, data.detections[0].pose.pose.pose.position.y,data.detections[0].pose.pose.pose.position.z,0])
            tag_p = np.matmul(m,fly_p)

            self.fly_cmd.linear.x = tag_p[0]
            self.fly_cmd.linear.y = tag_p[1]
            self.fly_cmd.linear.z = tag_p[2]
            self.pub.publish(self.fly_cmd)

from pymavlink import mavutil,mavwp
import rospy
import time
import sys
from math import *
from tf.transformations import *
import numpy as np
# global self.master
#import asyncio
# Create the connection
# self.master = mavutil.mavlink_connection('/dev/ttyUSB1',baud=57600)
import rospy
from sensor_msgs.msg import Joy
from apriltag_ros.msg import AprilTagDetectionArray
from geometry_msgs.msg import Twist,PoseStamped,Point 
class  demo:
    def tag_callback(self,data):
        # print  len(data.detections )
        # useid =-1
        if len(data.detections )>0:
            q = data.detections[0].pose.pose.pose.orientation
            q = [q.x,q.y,q.z,q.w]
            m = quaternion_matrix(q)
            m = m.transpose()
            fly_p =np.array( [data.detections[0].pose.pose.pose.position.x, data.detections[0].pose.pose.pose.position.y,data.detections[0].pose.pose.pose.position.z,0])
            tag_p = np.matmul(m,fly_p)

            self.fly_cmd.linear.x = tag_p[0]
            self.fly_cmd.linear.y = tag_p[1]
            self.fly_cmd.linear.z = tag_p[2]
            self.pub.publish(self.fly_cmd)
    def listener(self):

        # In ROS, nodes are uniquely named. If two nodes with the same
        # name are launched, the previous one is kicked off. The
        # anonymous=True flag means that rospy will choose a unique
        # name for our 'listener' node so that multiple listeners can
        # run simultaneously.
        rospy.init_node('listener', anonymous=True)
        # self.master.mav.rc_channels_override_send(self.master.target_system, self.master.target_component,
        #         1500, 1500, 1500, 1500, 1500, 1500, 1000, 1000,1000,1000 )
        self.change_mode(mode='GUIDED')
        self.fly_cmd = Twist()

        self.arm(1)
        self.takeoff(self.tkof_height)
        print 'take off---',self.tkof_height
        time.sleep(8)
        rospy.Subscriber("/tag_detections", AprilTagDetectionArray, self.tag_callback, queue_size=1)
        self.pub = rospy.Publisher('/tag_frame', Twist, queue_size=10)

将tag坐标系下的向量tag_p 转换为机载端相机坐标系下的表示 fly_p.

和上文类似

    def tag_callback(self,data):
        # print  len(data.detections )
        # useid =-1
        if len(data.detections )>0:
            q = data.detections[0].pose.pose.pose.orientation
            q = [q.x,q.y,q.z,q.w]
            m = quaternion_matrix(q)
            tag_p =np.array( [1,2,3,0])
            fly_p = np.matmul(m,tag_p)

    def tag_callback(self,data):
        # print  len(data.detections )
        # useid =-1
        if len(data.detections )>0:
            q = data.detections[0].pose.pose.pose.orientation
            q = [q.x,q.y,q.z,q.w]
            m = quaternion_matrix(q)
            tag_p =np.array( [1,2,3,0])
            fly_p = np.matmul(m,tag_p)