aruco - pose estimation 姿態辨識

這裡實作使用 Aruco Marker 的姿態辨識

---

 前置作業:

1. 相機校正 opencv - camera calibration 相機校正

2. 印出二維碼 aruco - print out opencv aruco markers 印出二維碼

---

程式碼:

#include <iostream>
#include "opencv2\opencv.hpp"
#include "opencv2\aruco.hpp"
#include <direct.h>
#include <string>

using namespace std;
using namespace cv;

Mat load_distortion(string fileName)
{
 Mat distortion(5, 1, CV_64F);

 ifstream inStream(fileName);
 if (inStream)
 {
  cout << "Load " << fileName << "..." << endl;
  int rows = 5;
  int cols = 1;

  for (int r = 0; r < rows; r++)
  {
   for (int c = 0; c < cols; c++)
   {
    double read = 0.0f;
    inStream >> read;
    distortion.at<double>(r, c) = read;
   }
  }

 }

 return distortion;
}

Mat load_intrinsic(string fileName)
{
 Mat intrinsic(3, 3, CV_64F);

 ifstream inStream(fileName);
 if (inStream)
 {
  cout << "Load " << fileName << "..." << endl;
  int rows = 3;
  int cols = 3;

  for (int r = 0; r < rows; r++)
  {
   for (int c = 0; c < cols; c++)
   {
    double read = 0.0f;
    inStream >> read;
    intrinsic.at<double>(r, c) = read;
   }
  }

 }

 return intrinsic;
}

int startWebcam(Mat intrinsic, Mat distortion)
{
 VideoCapture webcam(2);

 if (!webcam.isOpened())
 {
  cout << "CANNOT open webcam" << endl;
  return 0;
 }

 vector<Vec3d> rvec, tvec; //旋轉矩陣及位移矩陣
 vector<int> marker_ids;    //二維碼的編號
 vector<vector<Point2f>> marker_corners; //二維碼的角點
 vector<vector<Point2f>> rejected_case;  //被拒絕的人

 //把二維碼庫讀進來
 Ptr<aruco::Dictionary> markerDic = aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME::DICT_4X4_50);
 
 Mat frame; //視訊鏡頭照到的每張影像
 

 while (true)
 {
  if (!webcam.read(frame)) //讀影像進來
  {
   cout << "CANNOT read webcam" << endl;
   break;
  }

  aruco::detectMarkers(frame, markerDic, marker_corners, marker_ids);
  aruco::estimatePoseSingleMarkers(marker_corners, 0.1016f, intrinsic, distortion, rvec, tvec);
  aruco::drawDetectedMarkers(frame, marker_corners, marker_ids);
  
  for (int index_marker = 0; index_marker < marker_ids.size(); index_marker++)
  {
   aruco::drawAxis(frame, intrinsic, distortion, rvec[index_marker], tvec[index_marker], 0.1);
   cout << index_marker << endl;
  }

  imshow("frame", frame); //顯示原始影像
  waitKey(22);
 }
}

int main()
{
 Mat intrinsic(3, 3, CV_64F);
 Mat distortion(5, 1, CV_64F);
 
 //載入intrinsic和distortion
 intrinsic = load_intrinsic(".//calibration//intrinsic.txt");  
 distortion = load_distortion(".//calibration//distortion.txt");
 
 cout << "intrinsic: " << intrinsic << endl;
 cout << "distortion: " << distortion << endl;
 
 startWebcam(intrinsic, distortion);

 system("pause");
 return 0;
}

---

運行結果:

---