プログラム関連の個人的メモ

OpenCV arucoマーカの認識結果であるカメラとマーカ間の
位置関係を使用してOpenGlでCGを描画する。

• OpenCVのカメラ内部パラメータをOpenGLの投影行列に変換
• 撮影画像をOpenGLで描画
• 認識結果を使ってOpenGLでCGを描画

OpenCVのキャリブレーションで出てきた内部パラメータを使って、
OpenGLの投影行列(gl_perspectiveでつくるやつ)を作る必要がある。

まあ、調べたらやり方は出てきます。
http://kgeorge.github.io/2014/03/08/calculating-op...

_m[16]がGLで使う投影行列
_intrinsicがCVの内部パラメータ

void InitGLProjectionMatrix(cv::Mat _intrinsic, GLfloat _m[]){
  double focalmax = 2000.0, focalmin = 10;
  double width = window_width, height = window_height;

  double fx = _intrinsic.at<double>(0, 0), fy = _intrinsic.at<double>(1, 1);
  double cx = _intrinsic.at<double>(0, 2), cy = height - _intrinsic.at<double>(1, 2);

  cv::Mat mP = cv::Mat::zeros(4, 4, CV_64FC1);
  mP.at<double>(0, 0) = (2.0 * fx / (width - 1));
  mP.at<double>(0, 1) = 0.0;
  mP.at<double>(0, 2) = -((2.0 * cx / (width - 1)) - 1.0);
  mP.at<double>(0, 3) = 0.0;

  mP.at<double>(1, 0) = 0.0;
  mP.at<double>(1, 1) = -(2.0 * fy / (height - 1));
  mP.at<double>(1, 2) = -((2.0 * cy / (height - 1)) - 1.0);
  mP.at<double>(1, 3) = 0.0;

  mP.at<double>(2, 0) = 0.0;
  mP.at<double>(2, 1) = 0.0;
  mP.at<double>(2, 2) = (focalmax + focalmin) / (focalmin - focalmax);
  mP.at<double>(2, 3) = 2.0 * focalmax * focalmin / (focalmin - focalmax);

  mP.at<double>(3, 0) = 0.0;
  mP.at<double>(3, 1) = 0.0;
  mP.at<double>(3, 2) = -1.0;
  mP.at<double>(3, 3) = 0.0;

  cv::Mat tntin = cv::Mat::eye(4, 4, CV_64FC1);
  //tntin.at<double>(0, 0) = -1.0;
  tntin.at<double>(2, 2) = -1.0;
  //tntin.at<double>(1, 1) = -1.0;
  mP = mP * tntin;

  for (int ii = 0; ii < 4; ii++)
  for (int jj = 0; jj < 4; jj++){
    _m[ii * 4 + jj] = mP.at<double>(jj, ii);
  }
}

tntin付近のコメントアウトについては、CGが写らないときにいじるといいかもしれない。

作った投影行列は

glUniformMatrix4fv(glGetUniformLocation(glProgram, "matPC"), 1, GL_FALSE, GLfloat[16]);

こんな感じで事前にバーテックスシェーダに送ってあげてください。

シェーダ使わないなら、描画するところで

glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glMultMatrixf(GLfloat[16]);

テクスチャに撮影画像を貼り付けて、そのテクスチャをレンダリングする。
これに関してはここを見れば全部わかる。
http://marina.sys.wakayama-u.ac.jp/~tokoi/?date=20...

(オフライン)事前にテクスチャを準備しておいて

cv::Mat timg(window_height, window_width, CV_8UC3);
glActiveTexture(GL_TEXTURE2);
glGenTextures(1, &imgTexId);
glBindTexture(GL_TEXTURE_RECTANGLE, imgTexId);
glTexImage2D(GL_TEXTURE_RECTANGLE, 0, GL_RGB, window_width, window_height,
  0, GL_BGR, GL_UNSIGNED_BYTE, timg.data);
glTexParameteri(GL_TEXTURE_RECTANGLE, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_RECTANGLE, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_RECTANGLE, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);
glTexParameteri(GL_TEXTURE_RECTANGLE, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER);
glBindTexture(GL_TEXTURE_RECTANGLE, 0);

(オンライン)撮影した画像(Mat)をテクスチャに送って

glTexImage2D(GL_TEXTURE_RECTANGLE, 0, GL_RGB, window_width, window_height,
  0, GL_BGR, GL_UNSIGNED_BYTE, captureImage.data);

(オンライン)そのテクスチャを描画

glNormal3d(0.0, 0.0, 1.0);
glBegin(GL_QUADS);
glTexCoord2d(0.0, 1.0); glVertex2d(-1.0, -1.0);
glTexCoord2d(1.0, 1.0); glVertex2d(1.0, -1.0);
glTexCoord2d(1.0, 0.0); glVertex2d(1.0, 1.0);
glTexCoord2d(0.0, 0.0); glVertex2d(-1.0, 1.0);
glEnd();

シェーダーの中身については参考サイトを見てください。

Arucoマーカ認識結果がそのまま変換行列になってるので、GLfloat[16]に突っ込むだけ。

MyAruco.cppの中身 (float *_mはGLfloat[16]です)。

// 推定結果を返す
void TMyAruco::GetMatrix(float *_m, int _id){
  // single
  if (markerType == ARUCO_SingleMarker){
    if (!detectFlag || rvecsSingle.size() <= _id){
      for (int ii = 0; ii < 16; ii++) _m[ii] = 0;
      return;
    }

    cv::Mat tR(3, 3, CV_64F);
    cv::Rodrigues(rvecsSingle[_id], tR);
		
    for (int yy = 0; yy < 3;yy++)
    for (int xx = 0; xx < 3; xx++){
      _m[xx * 4 + yy] = tR.at<double>(yy, xx);
    }
    _m[3] = 0; _m[7] = 0; _m[11] = 0;
    for (int ii = 0; ii < 3; ii++){
      _m[12 + ii] = tvecsSingle[_id][ii] * 1000.0;
    }
    _m[15] = 1.0;
  }

  // board
  else if (markerType == ARUCO_Board){
    if (!detectFlag){
    for (int ii = 0; ii < 16; ii++) _m[ii] = 0;
      return;
    }
    cv::Mat tR(3, 3, CV_64F);
    cv::Rodrigues(rvecBoard, tR);

    for (int yy = 0; yy < 3; yy++)
    for (int xx = 0; xx < 3; xx++){
      _m[xx * 4 + yy] = tR.at<double>(yy, xx);
    }
    _m[3] = 0; _m[7] = 0; _m[11] = 0;
    for (int ii = 0; ii < 3; ii++){
      _m[12 + ii] = tvecBoard[ii] * 1000.0;
    }
    _m[15] = 1.0;
  }

  // chess
  else if (markerType == ARUCO_ChessBoard){
    for (int ii = 0; ii < 16; ii++) _m[ii] = 0;
    return;
  }

  // diamond
  else if (markerType == ARUCO_Diamond){
    if (!detectFlag || rvecsDiamond.size() <= _id){
      for (int ii = 0; ii < 16; ii++) _m[ii] = 0;
      return;
    }

    cv::Mat tR(3, 3, CV_64F);
    cv::Rodrigues(rvecsDiamond[_id], tR);

    for (int yy = 0; yy < 3; yy++)
    for (int xx = 0; xx < 3; xx++){
      _m[xx * 4 + yy] = tR.at<double>(yy, xx);
    }
    _m[3] = 0; _m[7] = 0; _m[11] = 0;
    for (int ii = 0; ii < 3; ii++){
      _m[12 + ii] = tvecsDiamond[_id][ii] * 1000.0;
    }
    _m[15] = 1.0;
  }
}

一応、マーカを見つけられたかどうかのフラグがあります(bool detectFlag)
chessボードの関しては交点を求めるために使うので、とくに何も返さない。

で、これもバーテックスバッファに送って描画します

// 変換行列をGPUに送って
glUniformMatrix4fv(glGetUniformLocation(glProgram, "matM"), 1, GL_FALSE, GLfloat[16]);
// CGをレンダリング
Draw関数;

シェーダー使ってないならこんな感じ(多分)

glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glMultMatrixf(GLfloat[16]);
Draw関数;

一番上の画像のように紙の上に乗せたような感じにしたい場合は、
取得した行列に右から並進移動の変換行列をかけるといったような工夫が必要です。