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;
}
大萨达撒多
于 2023-04-21 10:51:41 首次发布
代码段主要处理识别到的TagDetected对象,根据ID码调整中心点坐标,更新旋转矩阵和平移向量,并依据像素系数和新TagSize计算新的Tag大小。当ID等于2时,中心点沿x轴正方向移动,否则沿负方向移动。
摘要由CSDN通过智能技术生成