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

阅读量：4082 次

发布时间：2019-05-25

本文共 12856 字，大约阅读时间需要 42 分钟。

转载自：

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

2020-06-06 15:24:03 556

分类专栏：

最后发布:2020-06-06 15:24:03首次发布:2020-06-06 15:24:03

本文链接：

版权

一、简介

1.1 目标

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

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

1.2 实现思路

制作并识别ARUCO marker

估计旋转矩阵和平移矩阵

获取模型数据，并根据需要进行缩放和坐标调整

通过projectPoints重投影将三维坐标转换到图像二维坐标

显示图像

二、ARUCO marker

2.1 简介

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

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

2.2 原理解析

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

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

黑白格子遵循下面的排列规则

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 \begin{matrix} 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{matrix} pppppdddddpppppdddddppppp

p为校验位：1,3,5列

d为数据位：2,4列，共10位能表示0-1023的数字

用数字01可表示为下面的排列，去除验位后可获得右侧排列，

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 ⇒ 1 0 0 1 0 0 0 1 1 0 \begin{matrix} 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{matrix}\Rightarrow \begin{matrix} 1 & 0 \\ 0 & 1 \\ 0 & 0 \\ 0 & 1 \\ 1 & 0 \\ \end{matrix} 0111010001010100101011011⇒1000101010

将数据行行首尾相接后可获得该ArUco marker的id为582

1001000110 ⇒ 582 10 01 00 01 10\Rightarrow582 1001000110⇒582

2.3 ARUCO marker的生成

注意：此处采用opencv的aruco库实现，opencv版本需要3以上，此处使用的是OpenCV3.3.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图像边缘向像素值。

完整程序

#include 
   
    #include 
    
     #include 
     
      #include 
      
       #include 
       
        #include 
        
         using namespace std;using namespace cv;void createArucoMarkers() { Mat outputMarker; int marker_nums = 10; // 6x6,id:0~249 Ptr
         
           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模式和

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
   
     ids;  vector
    
     
      > corners;  aruco::detectMarkers(image, dictionary, corners, ids);  // if at least one marker detected  if (ids.size() > 0) {    // estimate Pose    vector
      
        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
   
     pointData;  vector
    
     
      > 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
      
        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 
   
    #include 
    
     #include 
     
      #include 
      
       #include 
       
        #include 
        
         #include 
         
          #include 
          
           #include 
           
            #include 
            
             #include 
             
              #include 
              
               #include 
               
                using namespace std;using namespace cv;#define markerLength 0.05 // ar码实际边长，单位m#define modelSize 1 //模型大小0-1ros::Publisher image_pub;// init dataMat cameraMatrix, distCoeffs;Ptr
                
                  dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);// model datavector
                 
                   pointData;vector
                  
                   
                    > plantData;// 绘制模型Mat drawModel(Mat image, Vec3d rvec, Vec3d tvec, vector
                    
                      modelPoints, vector
                     
                      
                       > 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
                       
                         imagePoints; projectPoints(modelPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints); // draw plant vector
                        
                         
                          > plantPoints(modelPlants.size()); vector
                          
                            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
                           
                             ids; vector
                            
                             
                              > corners; aruco::detectMarkers(imageCopy, dictionary, corners, ids); // if at least one marker detected if (ids.size() > 0) { // aruco::drawDetectedMarkers(showImage, corners, ids); vector
                              
                                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
                               
                                ("/camera/image", 1, imageCb); image_pub = nh.advertise
                                
                                 ("/camera/aruco", 1); // init parms vector
                                 
                                   cameraMatrixData, distCoeffsData; ros::param::get("/camera_matrix/data", cameraMatrixData); ros::param::get("/distortion_coefficients/data", distCoeffsData); Mat cameraMatrixTmp = (Mat_
                                  
                                   (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_
                                   
                                    (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
                                    
                                      plantTmp; for (int j = 0; j < 4; j++) { int data; modelfile >> data; plantTmp.push_back(data); } plantData.push_back(plantTmp); } ros::spin(); return 0;}