TopView Transformation (鳥瞰図変換) 3 ~ 画素間補間

　前回のエントリで鳥瞰図変換を実現しましたが，作成した鳥瞰図には色情報を保持していない画素が存在しました．（黒のライン）これは，画素変換をする下記の変換式によってうまくマッピングが取れていないことが原因です．

　上記数式では，元画像座標 を変換後座標 に変換する関数 でした．ここで， に関してループを回して計算していたので，変換後の画像で埋まらない座標がでてきてしまいました．

　今日は，上記の関数 の逆関数 を求めることで，この問題を解決します．具体的には，変換後の画像に関してループを回し，元画像の色情報を該当ピクセルにコピーします．で，逆関数を求めるために上記の数式を変換後座標 に関してではなく，元画像座標 に関して解きます．

　解いた結果は下記数式です．

変換後写真

　前回のエントリで黒ピクセルだった箇所に色が付いていることがわかります．ただ，補間を全くしてないので，画像が荒くなってしまっています．

双一次補間法

　上記の変換写真は，最近傍法（もっとも近いピクセルの画素値を使う）を用いて計算しました．結果，カメラから離れた場所の画像が割と荒くなってしまいました．次に，双一次補間法という方法を用いて補間したいと思います．補間のイメージは下記のとおりです．

　ここでは，変換後の位置座標に近い 4 近傍の画素値を用いて按分します．使用する計算式は下記のとおりです．

変換後の結果が以下です．画素のギザギザが少しなめらかになってます．

　下記，コード片です．ベタ書きしてしまったのでとっても汚いですがご容赦を！

/*
 * undistort.cpp
 *
 *  Created on: May 29, 2016
 *    Author: koichi
 */
#include <iostream>
#include <math.h>
#include "opencv2/opencv.hpp"
//#include "opencv2/calib3d.hpp"

//#define INTERPOLATED
#define BILINEAR

using namespace std;
using namespace cv;

int main(int argc, char** argv) {

  // 1. Load Raw Iamge
  string imagepath = "/home/koichi/my_photo-1.jpg";
  Mat rawImage = imread(imagepath, CV_LOAD_IMAGE_COLOR);
  namedWindow("Raw Image", WINDOW_AUTOSIZE);
  imshow("Raw Image", rawImage);

  // 2. Undistort Image Based on Calibration Matrix.
  Mat undistortImage(rawImage.rows, rawImage.cols, CV_8UC3);
  Matx33d K(  627.235434, 0,    654.957574,
    0,    630.482585,  494.346943,
    0,    0,    1    );
  cv::Vec4d D(-0.210146, 0.030563, 0.001172, -0.001306);
  cv::undistort(rawImage, undistortImage, K, D);
  namedWindow("Undistorted Image", WINDOW_AUTOSIZE);
  //line(undistortImage, Point(640, 0), Point(640, 960), Scalar(255, 255, 255), 10);
  //line(undistortImage, Point(0, 460), Point(1280, 460), Scalar(255, 255, 255), 10);
  imshow("Undistorted Image", undistortImage);

  // 3. Convert Image to Gray
  Mat grayImage(rawImage.rows, rawImage.cols, CV_8UC1);
  cvtColor(undistortImage, grayImage, COLOR_RGB2GRAY);
  namedWindow("Gray Image", WINDOW_AUTOSIZE);
  imshow("Gray Image", grayImage);

  // 4. Top View Conversion
  Mat topImage(rawImage.rows, rawImage.cols, CV_8UC3);
  topImage.setTo(Scalar(0));

  Mat topImageGray(rawImage.rows, rawImage.cols, CV_8UC1);
  topImageGray.setTo(Scalar(0));
  {
  double Hvc = 2;
  double Hc = 0.7;
  double Dvc = 1.7;
  double f = 630;
  double fp = f;
  //double theta = 15 / 180.0 * M_PI;
  double theta = 27 / 180.0 * M_PI;
  double s = sin(theta);
  double c = cos(theta);
  int cx = 640;
  int cy = 480;
  int cxp = 640;
  int cyp = 480;

  for (int y = 0; y < topImage.rows; y++) {
    for (int x = 0; x < topImage.cols; x++) {

#ifdef INTERPOLATED


    int xOrg = x - cx;
    int yOrg = - y + cy;

    int oldX = 0.5 + (Hvc / Hc) * (f / fp) * c * ( s/c - (yOrg*Hvc*s - fp*Hc*c + fp*Dvc*s) / (fp*Hc*s + Hvc*yOrg*c + fp*Dvc*c) ) * xOrg;
    int oldY = 0.5 + f * ((yOrg*Hvc*s - fp*Hc*c + fp*Dvc*s)/(fp*Hc*s + Hvc*yOrg*c + fp*Dvc*c));

    oldX = oldX + cxp;
    oldY = -oldY + cyp;


    if (oldX < 0 || topImage.cols - 1 < oldX || oldY < 0 || topImage.rows - 1 < oldY ) {
      continue;
    }

    topImageGray.data[y * topImageGray.cols + x] = grayImage.data[oldY * grayImage.cols + oldX];

    topImage.data[(y * topImage.cols + x) * topImage.channels()] = undistortImage.data[(oldY * topImage.cols + oldX) * topImage.channels()];
    topImage.data[(y * topImage.cols + x) * topImage.channels() + 1] = undistortImage.data[(oldY * topImage.cols + oldX) * topImage.channels() + 1];
    topImage.data[(y * topImage.cols + x) * topImage.channels() + 2] = undistortImage.data[(oldY * topImage.cols + oldX) * topImage.channels() + 2];

#else
#ifdef BILINEAR

    int xOrg = x - cx;
    int yOrg = - y + cy;

    double oldX = 0.5 + (Hvc / Hc) * (f / fp) * c * ( s/c - (yOrg*Hvc*s - fp*Hc*c + fp*Dvc*s) / (fp*Hc*s + Hvc*yOrg*c + fp*Dvc*c) ) * xOrg;
    double oldY = 0.5 + f * ((yOrg*Hvc*s - fp*Hc*c + fp*Dvc*s)/(fp*Hc*s + Hvc*yOrg*c + fp*Dvc*c));

    oldX = oldX + cxp;
    oldY = -oldY + cyp;

    if (oldX < 0 || topImage.cols - 1 < oldX || oldY < 0 || topImage.rows - 1 < oldY ) {
      continue;
    }

    if((int)oldX + 1 >= topImage.cols || (int)oldY + 1 >= topImage.rows) {
      topImage.data[(y * topImage.cols + x) * topImage.channels()] = undistortImage.data[((int)oldY * topImage.cols + (int)oldX) * topImage.channels()];
      topImage.data[(y * topImage.cols + x) * topImage.channels() + 1] = undistortImage.data[((int)oldY * topImage.cols + (int)oldX) * topImage.channels() + 1];
      topImage.data[(y * topImage.cols + x) * topImage.channels() + 2] = undistortImage.data[((int)oldY * topImage.cols + (int)oldX) * topImage.channels() + 2];
      continue;
    }


    for (int i = 0; i < topImage.channels(); i++) {

      uchar f11 = undistortImage.data[((int)oldY * topImage.cols + (int)oldX) * topImage.channels() + i];
      uchar f12 = undistortImage.data[(((int)oldY + 1) * topImage.cols + (int)oldX) * topImage.channels() + i];
      uchar f21 = undistortImage.data[((int)oldY * topImage.cols + (int)oldX + 1) * topImage.channels() + i];
      uchar f22 = undistortImage.data[(((int)oldY + 1) * topImage.cols + (int)oldX + 1) * topImage.channels() + i];

      double dx2 = (int)oldX + 1 - oldX;
      double dx1 = oldX - (int)oldX;

      double dy2 = (int)oldY + 1 - oldY;
      double dy1 = oldY - (int)oldY;

      topImage.data[(y * topImage.cols + x) * topImage.channels() + i] = dy2 * (f11 * dx2 + f21 * dx1) + dy1 * (f12 * dx2 + f22 * dx1);
    }
#else

    int xOrg = x - cx;
    int yOrg = - y + cy;

    int newX = fp / Hvc * Hc * xOrg / (f * s - yOrg * c);
    int newY = fp / Hvc * (Hc * (f * c + yOrg * s) / (f * s - yOrg * c) - Dvc);

    newX = newX + cxp;
    newY = -newY + cyp;

    if (newX < 0 || topImage.cols - 1 < newX || newY < 0 || topImage.rows - 1 < newY ) {
      continue;
    }

    topImageGray.data[newY * topImageGray.cols + newX] = grayImage.data[y * grayImage.cols + x];

    topImage.data[(newY * topImage.cols + newX) * topImage.channels()] = undistortImage.data[(y * topImage.cols + x) * topImage.channels()];
    topImage.data[(newY * topImage.cols + newX) * topImage.channels() + 1] = undistortImage.data[(y * topImage.cols + x) * topImage.channels() + 1];
    topImage.data[(newY * topImage.cols + newX) * topImage.channels() + 2] = undistortImage.data[(y * topImage.cols + x) * topImage.channels() + 2];


#endif
#endif


    }
  }
  }

  namedWindow("Top Image", WINDOW_AUTOSIZE);
  imshow("Top Image", topImage);
  //namedWindow("Top Image Gray", WINDOW_AUTOSIZE);
  //imshow("Top Image Gray", topImageGray);

  while(true) {
  waitKey(0);
  }
  return 0;
}