ArUco估计位姿原理

ArUco使用

PnP

【OpenCV/aruco】校准相机(Camera Calibration) Demo

--------------------------------------------------------------------------------------------------------------------------------

使用opencv的aruco库进行位姿估计

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <cv.h>
#include <iostream>
#include <highgui.h>
#include <opencv2/aruco.hpp>
#include <stdio.h>

using namespace cv;
using namespace std;

int main(int argc, char *argv[])

{
//内参与畸变矩阵，笔者在前面的博客已经给出求解方法，有需要的可以找找看看
    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;

  Mat cameraMatrix = (cv::Mat_<float>(3, 3) <<
        fx, 0.0, cx,
        0.0, fy, cy,
        0.0, 0.0, 1.0);
   Mat distCoeffs = (cv::Mat_<float>(5, 1) << k1, k2, p1, p2, k3);
   cv::VideoCapture inputVideo;
   inputVideo.open(0);
   cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

   while (inputVideo.grab()) {
       cv::Mat image, imageCopy;
       inputVideo.retrieve(image);//抓取视频中的一张照片
       image.copyTo(imageCopy);
       std::vector<int> ids;
       std::vector<std::vector<cv::Point2f>> corners;
       cv::aruco::detectMarkers(image, dictionary, corners, ids);//检测靶标
       // if at least one marker detected
       if (ids.size() > 0) {
           cv::aruco::drawDetectedMarkers(imageCopy, corners, ids);//绘制检测到的靶标的框
           std::vector<cv::Vec3d> rvecs, tvecs;
           cv::aruco::estimatePoseSingleMarkers(corners, 0.055, cameraMatrix, distCoeffs, rvecs, tvecs);//求解旋转矩阵rvecs和平移矩阵tvecs
           //cout<<"R :"<<rvecs[0]<<endl;
           cout<<"T :"<<tvecs[0]<<endl;
           // draw axis for each marker
           for(int i=0; i<ids.size(); i++)
               cv::aruco::drawAxis(imageCopy, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
       }
       cv::imshow("out", imageCopy);
       cv::waitKey(50);
       //if (key == 27)1
       // break;
   }
return 0;
}

--------------------------------------------------------------------------------------------------------------------------------

三维坐标变换——旋转矩阵与旋转向量互转

Dlib姿态估计——旋转矩阵与欧拉角互转