使用Aruco二维码实现定位

首先使用cv::aruco::estimatePoseSingleMarkers()函数后得到两个很重要的数据revc和tevc，分别是旋转向量和平移向量。通过这两个数据就可以得到相机在世界坐标系下的坐标。此处需要了解solvePnP的想关知识。

solvePnP求解的r和t是将世界坐标系下的点变换到相机坐标系所需要的旋转和平移

rvec - 输出的旋转向量。使坐标点从世界坐标系旋转到相机坐标系
tvec - 输出的平移向量。使坐标点从世界坐标系平移到相机坐标系

想要明白为什么这样计算需要弄明白什么是缸体变换、也需要看一下像素、图像、相机、世界坐标系变换相关内容。

以下的计算过程的原理为什么是这样的需要参照：https://www.cnblogs.com/singlex/p/6037020.html

1、现在假设已经拿到了revc和tevc两个数值，首先需要使用罗德里旋转将旋转向量变成旋转矩阵。

      double rm[9];
      RoteM = cv::Mat(3, 3, CV_64FC1, rm);
      Rodrigues(rvec, RoteM); //罗德里旋转得到旋转矩阵
      double r11 = RoteM.ptr<double>(0)[0];
      double r12 = RoteM.ptr<double>(0)[1];
      double r13 = RoteM.ptr<double>(0)[2];
      double r21 = RoteM.ptr<double>(1)[0];
      double r22 = RoteM.ptr<double>(1)[1];
      double r23 = RoteM.ptr<double>(1)[2];
      double r31 = RoteM.ptr<double>(2)[0];
      double r32 = RoteM.ptr<double>(2)[1];
      double r33 = RoteM.ptr<double>(2)[2];

2、此时便得到了旋转矩阵，下一步需要根据旋转矩阵计算每一个轴的旋转角度：

double thetaz = atan2(r21, r11) / CV_PI * 180;
double thetay = atan2(-1 * r31, sqrt(r32 * r32 + r33 * r33)) / CV_PI * 180;
double thetax = atan2(r32, r33) / CV_PI * 180;

此时thetaz、thetay、thetax就是分别绕z、y、x的旋转角度。

此上就是旋转矩阵和欧拉角的转换，参考此博客：https://blog.csdn.net/darlingqiang/article/details/80829666

3、此时已经有了旋转角度，那每个轴上量是什么呢？那就需要使用平移向量了

double tx = tvec.ptr<double>(0)[0];
double ty = tvec.ptr<double>(0)[1];
double tz = tvec.ptr<double>(0)[2];

 

CodeRotateByZ(x, y, -1 * thetaz, x, y);
CodeRotateByY(x, z, -1 * thetay, x, z);
CodeRotateByX(y, z, -1 * thetax, y, z);

void CodeRotateByZ(double x, double y, double thetaz, double &outx, double &outy)
{
  double x1 = x; //将变量拷贝一次，保证&x == &outx这种情况下也能计算正确
  double y1 = y;
  double rz = thetaz * CV_PI / 180;
  outx = cos(rz) * x1 - sin(rz) * y1;
  outy = sin(rz) * x1 + cos(rz) * y1;
}

void CodeRotateByY(double x, double z, double thetay, double &outx, double &outz)
{
  double x1 = x;
  double z1 = z;
  double ry = thetay * CV_PI / 180;
  outx = cos(ry) * x1 + sin(ry) * z1;
  outz = cos(ry) * z1 - sin(ry) * x1;
}
void CodeRotateByX(double y, double z, double thetax, double &outy, double &outz)
{
  double y1 = y; //将变量拷贝一次，保证&y == &y这种情况下也能计算正确
  double z1 = z;
  double rx = thetax * CV_PI / 180;
  outy = cos(rx) * y1 - sin(rx) * z1;
  outz = cos(rx) * z1 + sin(rx) * y1;
}

4、此时变得到了相机在世界坐标系下的坐标，不过还需要一个计算即反向的平移：

x = x * -1;
y = y * -1;
z = z * -1;

此时得到的（x,y,z）就是相机在以二维码为原点的世界坐标系下的坐标了。