apriltags2_ros应用实践——如何用apriltags二维码输出相机位姿与轨迹
前言
之前我们已经成功实现了在Realsense D435i的相机下，跑通apriltags2_ros，并发布一个可在RVIZ可视化的odometry topic，输出相机的位姿。接下来我们主要讨论如何利用这个位姿，将其转换到与VINS对齐的机器人坐标系下，并能够输出一个轨迹topic。
之前的博客：
apriltags2_ros应用实践——如何在realsense d435i上使用apriltags二维码实现定位
依旧放上源码，若对你的研究有所帮助，麻烦给个star~
https://github.com/ManiiXu/Apriltags2_VO
我们先看一下最后的效果
在旋转的时候发布的轨迹有抖动，但总体能够实现只通过拍摄Apriltag完成室内定位（位姿估计）。
坐标系转换
在apriltags2_ros输出位姿的过程中，一共存在2个坐标系：相机坐标系©和世界坐标系(w)。而其他有：Apriltag所在坐标系(tag)和机器人坐标系(b)。
其中世界坐标系，z轴方向与重力方向相反，x、y轴平行于地面，具体方向是由你决定的，根据你在tag.yaml设置的Apriltag图的位姿确定。
这里我希望x轴作为机器人运动的主要方向，便采用绿色所示的世界坐标系，并且Apriltag是如下图进行放置的。那么对应Apriltag的姿态分别为：
x: 0.0000, y: 0.0000, z: 0.0, qw: 0.707, qx: 0.0, qy: 0.0, qz: -0.707
而相机坐标系如图蓝色坐标轴，从相机角度看，其z轴垂直相机平面向前，x轴向右，y轴向下（x、y轴是跟像素平面的x、y轴方向对应的）。
同时，因为相机安装在机器人的前方，我希望机器人在平地上正常工作的时候，机器人坐标系的z轴和世界坐标系的z轴对齐，x轴和世界坐标系x轴对齐，这样输出的姿态角就没有俯仰角和横滚角，而偏航角也和实际机器人运动情况相同。因此选择的机器人坐标系如红色部分所示，即x轴垂直相机平面向前，z轴与重力方向相反。
（上述讨论的机器人坐标系对应在VINS-Mono即为Body Frame/IMU Frame，并将下面的 R c b R^b_c Rcb换成标定后的相机到IMU的外参 R c i R^i_c Rci）
至此我们已经讨论完这四个坐标系了。由于已经对Apriltag的位姿进行了设置，原版apriltags2_ros最后输出的其实是从我们定义的世界坐标系(w)到相机坐标系©的矩阵变换（ R w c , t w c R^c_w,t^c_w Rwc,twc），而我们最后要得到机器人位姿其实是从机器人坐标系(b)到世界坐标系(w)的变换（ R b w , t b w R^w_b,t^w_b Rbw,tbw）。
由 T b w = T c w T b c = [ R c w t c w 0 1 ] [ R b c t b c 0 1 ] = [ [ R w c ] T − [ R w c ] T t w c 0 1 ] [ R b c 0 0 1 ] T^w_b=T^w_c T^c_b= \left[ \begin{matrix} R^w_c & t^w_c \\ 0 & 1 \end{matrix} \right] \left[ \begin{matrix} R^c_b & t^c_b \\ 0 & 1 \end{matrix} \right]=\left[ \begin{matrix} [R^c_w]^T & -[R^c_w]^Tt^c_w \\ 0 & 1 \end{matrix} \right]\left[ \begin{matrix} R^c_b & 0 \\ 0 & 1 \end{matrix} \right] Tbw=TcwTbc=[Rcw0tcw1][Rbc0tbc1]=[[Rwc]T0−[Rwc]Ttwc1][Rbc001]
R b w = [ R w c ] T R b c ， t b w = − [ R w c ] T t w c R^w_b = [R^c_w]^T R^c_b，t^w_b= -[R^c_w]^Tt^c_w Rbw=[Rwc]TRbc，tbw=−[Rwc]Ttwc
对应在代码common_fuction.cpp中：
Eigen::Matrix4d TagDetector::getRelativeTransform(
std::vector<cv::Point3d > objectPoints,
std::vector<cv::Point2d > imagePoints,
double fx, double fy, double cx, double cy) const
{
cv::Mat rvec, tvec;
cv::Matx33d cameraMatrix(fx, 0, cx,
0, fy, cy,
0, 0, 1);
cv::Vec4f distCoeffs(0,0,0,0); // distortion coefficients
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
cv::Matx33d R;
cv::Rodrigues(rvec, R);
Eigen::Matrix3d wRo,tmpwRo;
tmpwRo << R(0,0), R(0,1), R(0,2), R(1,0), R(1,1), R(1,2), R(2,0), R(2,1), R(2,2);//R^c_w
Eigen::Vector3d t;
t << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);//t^c_w
t = - tmpwRo.transpose() * t;//t^w_b = -[R^c_w]^T * t^c_w
Eigen::Matrix3d rr;
rr << 0,0,1,-1,0,0,0,-1,0;//R^b_c
tmpwRo = rr * tmpwRo;//R^b_c * R^c_w
wRo = tmpwRo.transpose();//R^w_b = [R^c_w]^T * R^c_b = [R^b_c * R^c_w]^T
Eigen::Matrix4d T; // homogeneous transformation matrix
T.topLeftCorner(3, 3) = wRo;
T(0,3) = t[0];
T(1,3) = t[1];
T(2,3) = t[2];
T.row(3) << 0,0,0,1;
return T;
}
发布PATH
最后我们发布一个轨迹的topic，即把之前得到的位置信息（x,y,z）放进去：
//continuous_detector.h
ros::Publisher path_pubilsher;
nav_msgs::Path camera_path;
//continuous_detector.cpp
if (true) {
for (unsigned int i = 0; i < tag_detection_array.detections.size(); i++) {
nav_msgs::Odometry odometry;
odometry.header = tag_detection_array.detections[i].pose.header;
//odometry.header.frame_id = "my_bundle";
odometry.pose.pose = tag_detection_array.detections[i].pose.pose.pose;
odomtry_publisher_.publish(odometry);
//上面是本来就有的，下面开始添加：
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header = odometry.header;
//pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = tag_detection_array.detections[i].pose.pose.pose.position.x;
pose_stamped.pose.position.y = tag_detection_array.detections[i].pose.pose.pose.position.y;
pose_stamped.pose.position.z = tag_detection_array.detections[i].pose.pose.pose.position.z;
camera_path.header = pose_stamped.header;
//camera_path.header.frame_id = "world";
camera_path.poses.push_back(pose_stamped);
path_pubilsher.publish(camera_path);
}
}
这样便根据原版的apriltags2_ros，得到了我们所需要的机器人位姿和轨迹了。