SLAM学习——使用ARUCO_marker进行AR投影

一、简介

1.1 目标

增强现实技术（Augmented Reality，简称 AR），是一种实时地计算摄影机影像的位置及角度并加上相应图像、视频、3D模型的技术，这种技术的目标是在屏幕上把虚拟世界套在现实世界并进行互动。

将三维模型投影到ARUCO marker上，并获取投影效果。

1.2 实现思路

1. 制作并识别ARUCO marker
2. 估计旋转矩阵和平移矩阵
3. 获取模型数据，并根据需要进行缩放和坐标调整
4. 通过projectPoints重投影将三维坐标转换到图像二维坐标
5. 显示图像

二、ARUCO marker

2.1 简介

ARUCO marker为汉明(海明)码的格子图，如下所示：

汉明码（Hamming Code）利用奇偶校验位的概念，通过在数据位后面增加一些比特，可以验证数据的有效性。利用一个以上的校验位，汉明码不仅可以验证数据是否有效，还能在数据出错的情况下指明错误位置。

2.2 原理解析

一个ArUco marker是一个二进制平方标记，它由一个宽的黑边和一个内部的二进制矩阵组成，内部的矩阵决定了它们的id。黑色的边界有利于快速检测到图像，二进制编码可以验证id，并且允许错误检测和矫正技术的应用。marker的大小决定了内部矩阵的大小。例如，一个4x4的marker由16bits组成。

上图为一个典型的ArUco marker，去除白色边框后为5X5的格子（黑色表示0，白色表示1），5X5的格子的外边缘为黑色。

黑白格子遵循下面的排列规则
$$\begin{array}{ccccc}p& d& p& d& p\\ p& d& p& d& p\\ p& d& p& d& p\\ p& d& p& d& p\\ p& d& p& d& p\end{array}$$

• p为校验位：1,3,5列
• d为数据位：2,4列，共10位能表示0-1023的数字

用数字01可表示为下面的排列，去除验位后可获得右侧排列，
$$\begin{array}{ccccc}0& 1& 0& 0& 1\\ 1& 0& 1& 1& 1\\ 1& 0& 0& 0& 0\\ 1& 0& 1& 1& 1\\ 0& 1& 0& 0& 1\end{array}\Rightarrow \begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\\ 0& 1\\ 1& 0\end{array}$$
将数据行行首尾相接后可获得该ArUco marker的id为582
$$1001000110\Rightarrow 582$$

2.3 ARUCO marker的生成

注意：此处采用opencv的aruco库实现，opencv版本需要3以上，此处使用的是OpenCV3.3.1，下同

1. 函数解析

void cv::aruco::drawMarker(const Ptr< Dictionary > & dictionary,
						   int 	id,
						   int 	sidePixels,
						   OutputArray 	img,
						   int 	borderBits = 1)	

• dictionary，包含marker对象的字典：

  cv::Ptr dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

  DICT_6X6_250为6x6的marker，该字典包含id为0-249的marker。

• id，返回marker的id，需要在设置的字典的范围内。

• sidePixels，返回图像的像素值。

• img，输出marker图像。

• borderBits，marker图像边缘向像素值。

2. 完整程序

#include <opencv2/aruco.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/highgui.hpp>

#include <fstream>
#include <iostream>
#include <sstream>

using namespace std;
using namespace cv;

void createArucoMarkers() {
  Mat outputMarker;
  int marker_nums = 10;

  // 6x6,id:0~249
  Ptr<aruco::Dictionary> markerDictionary =
      aruco::getPredefinedDictionary(aruco::DICT_6X6_250);

  for (int i = 0; i < marker_nums; i++) {
    // generate
    cv::aruco::drawMarker(markerDictionary, i, 200, outputMarker, 1);

    // save
    ostringstream convent;
    string imageName = "6X6_Marker_";
    convent << "marker/" << imageName << i << ".jpg";
    imwrite(convent.str(), outputMarker);
  }
}

int main(int argc, char *argv[]) {
  createArucoMarkers();
  return 0;
}

三、姿态估计

姿态估计（Pose estimation）在计算机视觉领域扮演着十分重要的角色：机器人导航、增强现实以及其它。这一过程的基础是找到现实世界和图像投影之间的对应点。

最为常用的方法是基于二进制平方的标记，这种Marker的主要便利之处在于，一个Marker提供了足够多的对应（四个角）来获取相机的信息。同样的，内部的二进制编码使得算法非常健壮，允许应用错误检测和校正技术的可能性。

此处的姿态估计采用PnP（Perspective-n-Point）的方法获得Marker相对相机的旋转矩阵和平移矩阵。

PnP（Perspective-n-Point）是求解3D到2D点对运动的方法。它描述了当我们知道n个3D空间点以及它们的投影位置时，如何估计相机所在的位姿。

3.1 关键函数解析

1. 识别Marker

void cv::aruco::detectMarkers(InputArray 	image,
							  Dictionary 	dictionary,
							  OutputArrayOfArrays 	corners,
							  OutputArray 	ids,
							  DetectorParameters 	parameters = DetectorParameters(),
							  OutputArrayOfArrays 	rejectedImgPoints = noArray()
                              InputArray 	cameraMatrix = noArray(),
							  InputArray 	distCoeff = noArray())	

• image，待检测marker的图像。
• dictionary，字典对象，为需要识别的marker类型，与生成marker时设置的字典对象一致即可。
• corners，检测marker获得的包含所有marker四个角点的数组，数据类型为std::vector<std::vector<cv::Point2f> >，若检测到N个marker，则数组大小为Nx4，角点顺序为顺时针，第一个点为左上角。
• ids，检测marker获得的id，大小与corners一致，只能检测包含在字典内的id。
• parameters，marker检测所需参数，输入对象类型为 DetectionParameters。
• rejectedCandidates，返回被检测出来但不是有效编码的marker，每个返回的marker同样通过四个角点定义，数据格式与corners一致，该参数可忽略，参用于debug模式和refineDetectedMarkers()
• cameraMatrix，相机内参数，可省略。
• distCoeff，相机畸变参数，可省略。

2. 姿态估计

void cv::aruco::estimatePoseSingleMarkers(InputArrayOfArrays 	corners,
										  float 	markerLength,
										  InputArray 	cameraMatrix,
										  InputArray 	distCoeffs,
										  OutputArrayOfArrays 	rvecs,
										  OutputArrayOfArrays 	tvecs)	

• corners，识别的包含marker的角点的数组，为detectMarkers() 函数中corners参数的返回值。
• markerLength，marker的边长，单位为米。
• cameraMatrix，相机内参数，标定获得。
• distCoeffs，相机畸变参数，标定获得。
• rvecs，markers相对相机的旋转矩阵。
• tvecs，markers相对相机的平移矩阵。

markers的坐标系为：矩形中心为坐标原点，红色为X轴，绿色为Y轴，蓝色为Z轴，轴的指向为该轴的正方向。

3.2 程序实现

• 姿态估计

bool poseEstimation(Mat image, Vec3d &markerRvec, Vec3d &markerTvec, int detectID) {
  // detect marker
  vector<int> ids;
  vector<vector<Point2f>> corners;
  aruco::detectMarkers(image, dictionary, corners, ids);

  // if at least one marker detected
  if (ids.size() > 0) {
    // estimate Pose
    vector<Vec3d> rvecs, tvecs;
    aruco::estimatePoseSingleMarkers(corners, 0.05, cameraMatrix, distCoeffs,
                                     rvecs, tvecs);

    // screen out designed marker rvecs and tvecs
    for (int i = 0; i < ids.size(); i++) {
      if (ids[i] == detectID) {
        // save pose
        markerRvec = rvecs[i];
        markerTvec = tvecs[i];
        return true;
      } else
        return false;
    }
  } else
    return false;
}

四、显示模型

4.1 模型准备与读入

三维模型采用普林斯顿三维模型库的数据，该数据库的模型为.off格式，程序内部通过fstream读入。读入前需对文件进行修改处理：直接修改.off格式为.txt，同时删除首行的OFF。

• off文件简析

  Object File Format(off)文件通过描述物体表面的多边形来表示一个模型的几何结构

  格式为：

  OFF

  顶点数 面数 边数

  x y z

  x y z

  …

  n个顶点 顶点1的索引 顶点2的索引 … 顶点n的索引

  …

• 程序实现

  // model data
  vector<Point3f> pointData;
  vector<vector<int>> plantData;
 
  // read model
  fstream modelfile;
  modelfile.open("./marker/m100.txt");
  int pointSize, plantSize, lineSize;
  modelfile >> pointSize;
  modelfile >> plantSize;
  modelfile >> lineSize;

  // point data
  for (int i = 0; i < pointSize; i++) {
    Point3f pointTmp;
    modelfile >> pointTmp.x;
    modelfile >> pointTmp.y;
    modelfile >> pointTmp.z;

    // resize model
    pointTmp.x = pointTmp.x / 0.5 * markerLength - (markerLength / 2.0);
    pointTmp.y = pointTmp.y / 0.5 * markerLength - (markerLength / 2.0);
    pointTmp.z = pointTmp.z / 0.5 * markerLength - (markerLength / 2.0);
    pointData.push_back(pointTmp);
  }

  // plant data
  for (int i = 0; i < plantSize; i++) {
    vector<int> plantTmp;
    for (int j = 0; j < 4; j++) {
      int data;
      modelfile >> data;
      plantTmp.push_back(data);
    }
    plantData.push_back(plantTmp);
  }

4.2 重投影

projectPoints()可根据所给的三维坐标和已知的相机内外参数求解投影到图像坐标系上的二维坐标。

void projectPoints(InputArray objectPoints, 
                   InputArray rvec, 
                   InputArray tvec, 
                   InputArray cameraMatrix, 
                   InputArray distCoeffs, 
                   OutputArray imagePoints, 
                   OutputArray jacobian=noArray(), 
                   double aspectRatio=0 )

• objectPoints，三维坐标数组。
• rvec，旋转向量，通过estimatePoseSingleMarkers获得。
• tvec，平移向量，通过estimatePoseSingleMarkers获得。
• cameraMatrix，相机内参数。
• distCoeffs，相机畸变参数。
• imagePoints，返回图像坐标数组。
• jacobian，雅克比行列式，
• aspectRatio，相机传感器的感光单元有关的可选参数，如果设置为非0，则函数默认感光单元的dx/dy是固定的，会依此对雅可比矩阵进行调整。

4.3 绘制模型

4.3.1 绘制函数解析

1. 绘制坐标

void cv::aruco::drawAxis(InputOutputArray 	image,
						 InputArray 	cameraMatrix,
						 InputArray 	distCoeffs,
						 InputArray 	rvec,
						 InputArray 	tvec,
						 float 	length)	

• image ，是输入/输出图像，坐标将会绘制在该图像上（通常就是检测marker的那张图像）。

• cameraMatrix，相机内参数，标定获得。

• distCoeffs ，相机畸变参数，标定获得。

• rvec ，外参数，旋转向量。

• tvec ，外参数，平移向量。

• length，坐标轴的长度，单位为米。

2. 绘制轮廓

void drawContours(InputOutputArray image, 
                  InputArrayOfArrays contours, 
                  int contourIdx, 
                  const Scalar& color, 
                  int thickness=1, 
                  int lineType=LINE_8, 
                  InputArray hierarchy=noArray(), 
                  int maxLevel=INT_MAX, 
                  Point offset=Point() )

3. 绘制线

void line(InputOutputArray img, 
          Point pt1, 
          Point pt2, 
          const Scalar& color, 
          int thickness=1, 
          int lineType=LINE_8, 
          int shift=0 )

4. 绘制点

void circle(InputOutputArray img, 
            Point center, 
            int radius, 
            const Scalar& color, 
            int thickness=1, 
            int lineType=LINE_8, 
            int shift=0 )

4.3.2 程序实现

#include <opencv2/aruco.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp>

#include <fstream>
#include <iostream>
#include <sstream>
#include <vector>

#include <cv_bridge/cv_bridge.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>

using namespace std;
using namespace cv;

#define markerLength 0.05 // ar码实际边长，单位m
#define modelSize 1       //模型大小0-1

ros::Publisher image_pub;

// init data
Mat cameraMatrix, distCoeffs;
Ptr<cv::aruco::Dictionary> dictionary =
    cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

// model data
vector<Point3f> pointData;
vector<vector<int>> plantData;

// 绘制模型
Mat drawModel(Mat image, Vec3d rvec, Vec3d tvec, vector<Point3f> modelPoints,
              vector<vector<int>> modelPlants, bool judge) {
  Mat showImage;
  cvtColor(image, showImage, CV_GRAY2BGR);

  if (judge) {
    // draw axis
    aruco::drawAxis(showImage, cameraMatrix, distCoeffs, rvec, tvec, 0.05);

    // draw model
    // projectPoints
    std::vector<cv::Point2f> imagePoints;
    projectPoints(modelPoints, rvec, tvec, cameraMatrix, distCoeffs,
                  imagePoints);

    // draw plant
    vector<vector<Point>> plantPoints(modelPlants.size());
    vector<Point> tmpPoints(3);
    for (int i = 0; i < modelPlants.size(); i++) {
      tmpPoints[0] = imagePoints[modelPlants[i][1]];
      tmpPoints[1] = imagePoints[modelPlants[i][2]];
      tmpPoints[2] = imagePoints[modelPlants[i][3]];
      plantPoints[i] = tmpPoints;
    }
    for (int i = 0; i < plantPoints.size(); i++) {
      drawContours(showImage, plantPoints, i, Scalar(203, 192, 255), FILLED);
    }

    // draw line
    for (int i = 0; i < modelPlants.size(); i++) {
      line(showImage, imagePoints[modelPlants[i][1]],
           imagePoints[modelPlants[i][2]], Scalar(0, 0, 0), 1);
      line(showImage, imagePoints[modelPlants[i][2]],
           imagePoints[modelPlants[i][3]], Scalar(0, 0, 0), 1);
      line(showImage, imagePoints[modelPlants[i][3]],
           imagePoints[modelPlants[i][1]], Scalar(0, 0, 0), 1);
    }

    // draw point
    for (int i = 0; i < imagePoints.size(); i++) {
      circle(showImage, imagePoints[i], 1, Scalar(0, 0, 0), 1);
    }
  }
  return showImage;
}

// 状态估计
bool poseEstimation(Mat image, Vec3d &markerRvec, Vec3d &markerTvec) {
  Mat imageCopy = image.clone();

  // detect marker
  vector<int> ids;
  vector<vector<Point2f>> corners;
  aruco::detectMarkers(imageCopy, dictionary, corners, ids);

  // if at least one marker detected
  if (ids.size() > 0) {
    // aruco::drawDetectedMarkers(showImage, corners, ids);
    vector<Vec3d> rvecs, tvecs;
    aruco::estimatePoseSingleMarkers(corners, 0.05, cameraMatrix, distCoeffs,
                                     rvecs, tvecs);

    // screen out designed marker rvecs and tvecs
    for (int i = 0; i < ids.size(); i++) {
      if (ids[i] == 2) {
        markerRvec = rvecs[i];
        markerTvec = tvecs[i];
        return true;
      } else
        return false;
    }
  } else
    return false;
}

// 回调函数
void imageCb(const sensor_msgs::ImageConstPtr &msg) {
  // get image
  cv_bridge::CvImagePtr cv_ptr =
      cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8);
  Mat cameraImage = cv_ptr->image;

  // estimate pose
   Vec3d markerRvec, markerTvec;
   bool judge = poseEstimation(cameraImage, markerRvec, markerTvec);

  // draw model
   Mat showImage = drawModel(cameraImage, markerRvec, markerTvec, pointData,
                            plantData, judge);

  // pub result
  cv_bridge::CvImage out_msg;
  out_msg.encoding = sensor_msgs::image_encodings::BGR8;
  out_msg.image = showImage;
  image_pub.publish(out_msg.toImageMsg());
}

int main(int argc, char *argv[]) {
  // init ros
  ros::init(argc, argv, "aruco");
  ros::NodeHandle nh;
  ros::Subscriber image_sub =
      nh.subscribe<sensor_msgs::Image>("/camera/image", 1, imageCb);
  image_pub = nh.advertise<sensor_msgs::Image>("/camera/aruco", 1);

  // init parms
  vector<double> cameraMatrixData, distCoeffsData;
  ros::param::get("/camera_matrix/data", cameraMatrixData);
  ros::param::get("/distortion_coefficients/data", distCoeffsData);

  Mat cameraMatrixTmp =
      (Mat_<double>(3, 3) << cameraMatrixData[0], cameraMatrixData[1],
       cameraMatrixData[2], cameraMatrixData[3], cameraMatrixData[4],
       cameraMatrixData[5], cameraMatrixData[6], cameraMatrixData[7],
       cameraMatrixData[8]);
  cameraMatrix = cameraMatrixTmp;

  Mat distCoeffsTmp =
      (Mat_<double>(1, 5) << distCoeffsData[0], distCoeffsData[1],
       distCoeffsData[2], distCoeffsData[3], distCoeffsData[4]);
  distCoeffs = distCoeffsTmp;

  // read model
  fstream modelfile;
  modelfile.open("./marker/m100.txt");
  int pointSize, plantSize, lineSize;
  modelfile >> pointSize;
  modelfile >> plantSize;
  modelfile >> lineSize;

  for (int i = 0; i < pointSize; i++) {
    Point3f pointTmp;
    modelfile >> pointTmp.x;
    modelfile >> pointTmp.y;
    modelfile >> pointTmp.z;

    // resize model
    pointTmp.x = pointTmp.x / 0.5 * markerLength - (markerLength / 2.0);
    pointTmp.y = pointTmp.y / 0.5 * markerLength - (markerLength / 2.0);
    pointTmp.z = pointTmp.z / 0.5 * markerLength - (markerLength / 2.0);
    pointData.push_back(pointTmp);
  }

  for (int i = 0; i < plantSize; i++) {
    vector<int> plantTmp;
    for (int j = 0; j < 4; j++) {
      int data;
      modelfile >> data;
      plantTmp.push_back(data);
    }
    plantData.push_back(plantTmp);
  }

  ros::spin();
  return 0;
}

4.4 实现效果

参考

• aruco

  ARUCO marker的解释

  [OpenCV] aruco Markers识别

  tutorial_aruco_detection

• 程序参考

  https://github.com/fdcl-gwu/aruco-markers

  https://github.com/jeradesign/ar-challenge

• 原理参考

  《视觉SLAM十四讲》——第7讲 视觉里程计1

• 模型数据

  off文件格式(Object File Format)

  常见的三维点云数据下载链接

  普林斯顿三维模型库