Marker_SLAM

2020-03-20

Source

Edit

History

Aruco-1-基本介绍

Catalogue

1. 1. Aruco
1. 1.1. 1.1. 字典
2. 2. Marker的创建
3. 3. Marker的检测
1. 3.1. 3.1. 检测的函数cv::aruco::detectMarkers及参数
2. 3.2. 3.2. 代码
4. 4. 位姿估计
5. 5. 流程

1. Aruco

Aruco是一个开源的相机姿态估计库，已经嵌入到Opencv的Contribute包中。

简单来说，这是类似于二维码的Marker，长这样:

结构:

黑色的邊界有利於快速檢測到圖像，同时，黑色边界的旁边需要有白色的space(用于检测)
二進制編碼可以驗證id，並且允許錯誤檢測和矯正技術的應用。
marker的大小決定了內部矩陣的大小。例如，一個4x4的marker由16bits組成

功能：

检测这些Marker
标定相机参数
相机位姿估计(Oroku用于计算相机相对于某个Marker的位姿，Haruko用于多相机的定位)

1.1. 字典

markers的字典是在一個特殊應用中使用到的marker的集合

字典的大小就是组成字典所用到的Marker的数量
marker的大小就是这些Marker的尺寸(bits)

2. Marker的创建

3. Marker的检测

输入一张有Marker的图像，检测所有Marker，并返回Marker信息：

Marker的四个角点
Marker的ID

检测过程主要分为两步

预检测Marker
根据内部编码筛选是否Marker

下面是需要检测Marker的图片

3.1. 检测的函数`cv::aruco::detectMarkers`及参数

cv::Mat inputImage;
...
std::vector<int> markerIds;
std::vector<std::vector<cv::Point2f>> markerCorners, rejectedCandidates;
cv::Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
cv::aruco::detectMarkers(inputImage, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);

第一个参数是将要检测Marker的图像
第二个参数是字典对象，在这种情况下是一个预定义的字典（ DICT_6X6_250 ）
检测到的Marker存储在markerCorners和markerIds结构中
1. markerCorners是检测到的Marker的角点列表。对于每个Marker，其四个角以其原始顺序返回（从左上角开始顺时针）。因此，第一个角是左上角，然后是右上角，右下角和左下角
2. markerIds是markerIds中每个检测到的Marker的id列表。请注意，返回的markerCorners和markerIds向量具有相同的大小
第四个参数是DetectionParameters类型的对象。此对象包括可在检测过程中自定义的所有参数
最终参数rejectedCandidates是一个返回的Marker候选列表，即已找到的那些方格，但它们不提供有效的编码。每个候选者也由其四个角定义，其格式与markerCorners参数相同。此参数可以省略

3.2. 代码

#include <iostream>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>
#include <opencv2/highgui/highgui.hpp>

using namespace std;

int main()
{
    cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

    cv::Mat img=cv::imread("../../../marker.jpg");
    cv::Mat imageCopy;
    img.copyTo(imageCopy);

    std::vector<int> ids;
    std::vector<std::vector<cv::Point2f> > corners;

    cv::aruco::detectMarkers(img, dictionary, corners, ids);


    // if at least one marker detected
    if (ids.size() > 0)
        cv::aruco::drawDetectedMarkers(imageCopy, corners, ids);
    cv::imshow("out", imageCopy);
    cv::waitKey(0);

    return 0;
}

检测结果

4. 位姿估计

在检测到这些Marker之后，下一件你可能要做的事情就是从它们那里获得相机的姿势。

为了进行摄像机的姿态估计，你需要知道你的摄像机的校准参数。这些是相机矩阵和畸变系数。如果您不知道如何校准您的相机，您可以查看 calibrateCamera ()函数和 OpenCV 的校准教程。您还可以使用 ArUco 模块校准相机，如 ArUco 和 ChArUco。

最后，校准后得到的是相机矩阵\(K\): 一个由3x3元素组成的矩阵，其中包括焦距和相机中心坐标(又称内参数) ，以及畸变系数: 一个由5个或更多元素组成的矢量，用于模拟相机产生的畸变

当你用 ArUco Marker估计姿势时，你可以单独估计每个Marker的姿势。如果你想从一组Marker中估计一个姿势，使用 ArUco Boards (参见 ArUco Boards 的检测教程)。使用 ArUco 板代替单一Marker允许一些Marker被遮挡。

4.1. 原理

相机的姿势是指从Marker坐标系到相机坐标系的3d转换。它是通过旋转和平移向量指定的(主要使用solvePnP()函数)。

4.2. 核心函数`cv::aruco::estimatePoseSingleMarkers()`

cv::Mat cameraMatrix, distCoeffs;
...
std::vector<cv::Vec3d> rvecs, tvecs;
cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs);

第一个参数markerCorners是由detectMarkers()函数返回的marker的4个角点的坐标vector
第二个参数是marker的尺寸，单位是米或其他，估计出来的姿态的平移量将会与这个尺度保持一致，如果size使用(米)，那么估计出来的平移量也是(米)
第三个参数是相机的内参矩阵
第四个参数是相机的畸变参数
rvecs,tvecs是输出的位姿估计(旋转量，平移量)，需要注意的是，旋转量是轴角表示的旋转，需要使用罗德里格斯公式转换为旋转矩阵R

这个函数所假定的Marker坐标系原点是Marker的中心，z 轴指向外面，如下图所示。轴色对应为

x: 红色
y: 绿色
z: 蓝色

模块提供了一个绘制轴的功能，如上图所示，因此可以检查位姿估计，可视化代码如下:

inputImage.copyTo(outputImage);
for (int i = 0; i < rvecs.size(); ++i) {
    auto rvec = rvecs[i];
    auto tvec = tvecs[i];
    cv::aruco::drawAxis(outputImage, cameraMatrix, distCoeffs, rvec, tvec, 0.1);
}

函数的最后一个参数: (这里填了0.1) 指的是绘制的坐标轴的长度，与上面位姿估计的尺度保持一致

4.3. 代码

#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

#include <opencv2/aruco/charuco.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d.hpp>

using namespace std;

int main()
{
    // 内参不知道哪个老哥整出来的
    double fx, fy, cx, cy, k1, k2, k3, p1, p2;
    fx = 955.8925;
    fy = 955.4439;
    cx = 296.9006;
    cy = 215.9074;
    k1 = -0.1523;
    k2 = 0.7722;
    k3 = 0;
    p1 = 0;
    p2 = 0;
    // 内参矩阵
    cv::Mat cameraMatrix = (cv::Mat_<float>(3, 3) <<
        fx, 0.0, cx,
        0.0, fy, cy,
        0.0, 0.0, 1.0);
    // 畸变矩阵
    cv::Mat distCoeffs = (cv::Mat_<float>(5, 1) << k1, k2, p1, p2, k3);

    // 字典读取
    cv::Ptr<cv::aruco::Dictionary> dictionary =
    cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

    cv::Mat image, imageCopy;
    image = cv::imread("../../../marker.jpg");
    image.copyTo(imageCopy);
    vector<int> ids;
    vector<vector<cv::Point2f>> corners;

    // 检测Marker
    cv::aruco::detectMarkers(image, dictionary, corners, ids);

    if (ids.size() > 0) {
        // 绘制检测边框
        //cv::aruco::drawDetectedMarkers(imageCopy, corners, ids);

        // 估计相机位姿(相对于每一个marker)
        std::vector<cv::Vec3d> rvecs, tvecs;
        cv::aruco::estimatePoseSingleMarkers(corners, 0.055, cameraMatrix, distCoeffs, rvecs, tvecs);

        // draw axis for each marker
        for (int i = 0; i < ids.size(); i++)
        {
            if(ids[i]!=23)
                continue;
            /// 得到的位姿估计是：从Marker坐标系到相机坐标系的
            cv::Mat R;
            cv::Rodrigues(rvecs[i],R);
            cout << "ID :" << ids[i] << endl;
            cout << "R_{camera<---marker} :" << R << endl;
            cout << "t_{camera<---marker} :" << tvecs[i] << endl;
            cout << endl;
            cv::aruco::drawAxis(imageCopy, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);

            Eigen::Matrix3d R_eigen;
            cv::cv2eigen(R,R_eigen);
            Eigen::Vector3d zyx_Euler_fromR=R_eigen.eulerAngles(0,1,2);//Eigen中使用右乘的顺序,因此ZYX对应的是012,实际上这个编号跟乘法的顺序一致就可以了(从左向又右看的顺序)
            cout<< "zyx euler from Rotation \n[输出顺序为:x,y,z]:\n"<<(180)/(M_PI)*zyx_Euler_fromR.transpose()<<endl;
        }
    }
    cv::imshow("out", imageCopy);
    cv::waitKey(0);

    return 0;
}

实际运行效果