大萨达撒多

int targetTagId0 = 0;
int targetTagId2 = 2;
int targetTagId4 = 4;

double moveDistance = 0.065;
double newTagSize = 0.8;
Eigen::Matrix<double, 3, 3> rotation;
Eigen::Matrix<double, 3, 1> translation;




    if(TagDetected_.id){//如果识别到的bushi  0 ID码
      // 获取该TagDetected_结构体中存储的像素系数和中心点坐标信息
      double PixelCoef = TagDetected_.PixelCoef;
      Point2d center = TagDetected_.Center;

      // 获取该TagDetected_结构体中存储的旋转矩阵和平移向量信息
      rotation = TagDetected_.Trans_C2T.matrix().block<3, 3>(0, 0);
      translation = TagDetected_.Trans_C2T.matrix().block<3, 1>(0, 3);
      // 计算新的tag码大小
      double tagSize = PixelCoef * newTagSize;

      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> R(pose.R->data);
      Eigen::Map<Eigen::Matrix<double, 3, 1>> t(pose.t->data);

      // 使用新的姿态矩阵来更新TagDetected_结构体中的旋转矩阵和平移向量信息
      TagDetected_.Trans_C2T.matrix().block<3, 3>(0, 0) = R;
      TagDetected_.Trans_C2T.matrix().block<3, 1>(0, 3) = t;

      Point2d newCenter;

      if (TagDetected_.id == targetTagId2)
        newCenter = Point2d(center.x + moveDistance, center.y);
      else
        newCenter = Point2d(center.x - moveDistance, center.y);

      TagDetected_.Center = newCenter;
    }