3D Program

How it’s done: Stereo 3D Cloud Display without Post-Filter.

0

はじめに

本稿では、Visual C++にて作成したプログラム「Stereo 3D Cloud Display without Post Filter」 を以下の手順で解説します。

  • プログラムの流れ
  • Stereo 3D Cloud Display without Post-Filter Flow Chart
  • List of Flow Processes

また、 Stereo 3D Cloud Display with Post-Filter に関するページも参照してください。それでは、参りましょう。

プログラムの流れ

このプログラムはトップダウンの単純なプログラムとなっています。

先ず、プログラムに必要な関数を定義して、左右カメラから画像を取り入れます。その後、先に Stereo Calibration Program で得た Camera Intrinsic Data と Camera Extrinsic Data を Output File より読込みます。試しに左右カメラ画像の歪を修正して表示します。

Disparity Mapping の準備のため、画像を Greyscale に変えて少しボカします。Mapping には、StereoBM と StereoSGBM の二通りの方法を使いました。

この時点で、Camera Parameter を OpenCV Mat format (uchar) から、Visual C++ の float format に変換します。必要な Parameter は左右カメラの焦点距離と中心点。何故だか、3D画像の表示に float format が必要です。

Stereo 3D Cloud Display without Post-Filter Flow Chart

3D画像の表示は、引用したプログラムをそのまま使いました。フロー図に引用先を貼ってありますので、どうぞ。

Stereo 3D Cloud Display without Post-Filter Flow Chart

List of Flow Processes

以下が Stereo 3D Cloud Display without Post-Filter の Flow Processとなります。

Declaration of Parameters and Input Images:

このパートは関数の設定と左右カメラからの画像の取込みを行います。”//” でコメントアウトしている部分はヘッダーファイルに移動済です。

/////////////////////////////////////////////
//
// Declaration of Parameters
//

/* Declare image frames */
//Mat_<float> frame0;
//Mat_<float> frame1;
//Mat_<float> left = frame0;
//Mat_<float> right = frame1;
/* Release image buffers */
if (cap0.isOpened()) cap0.release();
if (cap1.isOpened()) cap1.release();
/* Declare image buffers */
VideoCapture cap0(0);
cap0 >> frame0;
VideoCapture cap1(1);
cap1 >> frame1;
 
/* Declare image windows */
//namedWindow("frame0", 1);
//namedWindow("frame1", 1);
 
cv::viz::Viz3d myWindow("Point Cloud");
 
int height = 480, width = 640;
cv::Mat_<cv::Vec3f> pCloud(height, width);
 
Mat cameraMatrix = Mat_<double>(3, 3, CV_32F);

Read Camera Parameters from Calibration Data File:

このパートでは、左右カメラの校正結果をデータファイルから読込みます。ステレオカメラの “Intrinsic” と “Extrinsic” データに関しては、OpenCV Documentationを参照してください。

////////////////////////////////////////////////////////////
//
// Read Camera parameters from Calibration Data File
//
//float fx = 300.5f, fy = 300.5f, cx = 319.5f, cy = 239.5f;
float fx = 525.0f, fy = 525.0f, cx = 319.5f, cy = 239.5f; // default
 
// Read parameters from camera intrinsic data file
Mat cameraMatrix0 = Mat_<float>(3, 3, /*CV_16SC2*/ CV_32FC1);
Mat distortCoeff1 = Mat_<float>(1, 14, /*CV_16SC2*/CV_32FC1);
Mat distortCoeff2 = Mat_<float>(1, 14, /*CV_16SC2*/CV_32FC1);
const string inputSettingsFile1 = /*argc > 1 ? argv[1] :*/ "intrinsics.yml";
FileStorage fs1(inputSettingsFile1, FileStorage::READ); // Read the settings
fs1["M1"] >> cameraMatrix0;
fs1["D1"] >> distortCoeff1;
fs1["D2"] >> distortCoeff2;
fs1.release();
Mat M1(cameraMatrix0.rows, cameraMatrix0.cols, CV_32FC1);
Mat D1(distortCoeff1.rows, distortCoeff1.cols, CV_32FC1);
Mat D2(distortCoeff2.rows, distortCoeff2.cols, CV_32FC1);
cameraMatrix0.convertTo(M1, CV_32FC1);
distortCoeff1.convertTo(D1, CV_32FC1);
distortCoeff2.convertTo(D2, CV_32FC1);
 
// Read parameters from camera extrinsic data file
Mat R01 = Mat_<float>(3, 3, /*CV_16SC2*/CV_32FC1);
Mat R02 = Mat_<float>(3, 3, /*CV_16SC2*/CV_32FC1);
Mat P01 = Mat_<float>(3, 4, /*CV_16SC2*/CV_32FC1);
Mat P02 = Mat_<float>(3, 4, /*CV_16SC2*/CV_32FC1);
const string inputSettingsFile2 = /*argc > 1 ? argv[1] :*/ "extrinsics.yml";
FileStorage fs2(inputSettingsFile2, FileStorage::READ); // Read the settings
fs2["R1"] >> R01;
fs2["R2"] >> R02;
fs2["P1"] >> P01;
fs2["P2"] >> P02;
fs2.release();
Mat R1, R2, P1, P2;
R01.convertTo(R1, CV_32FC1);
R02.convertTo(R2, CV_32FC1);
P01.convertTo(P1, CV_32FC1);
P02.convertTo(P2, CV_32FC1);
//
/////////////////////////////////////////////////////////////////////

Transformation and Rectification of Image Map:

このパートでは、先ほどのパートで読込んだカメラの校正値を使って、左右カメラから取込んだ画像の歪を修正して表示します。

////////////////////////////////////////////////////
//
// Create transformation and rectification maps
//
Mat cam1map1, cam1map2;
Mat cam2map1, cam2map2;
 
cv::initUndistortRectifyMap(M1, D1, R1, P1, Size(640, 480), CV_32FC1/*CV_16SC2*/, cam1map1, cam1map2);
cv::initUndistortRectifyMap(M1, D2, R2, P2, Size(640, 480), CV_32FC1/*CV_16SC2*/, cam2map1, cam2map2);
 
/////////////////////////////
/*Rectification of images*/
 
Mat leftStereoUndistorted, rightStereoUndistorted; //Create matrices for storing rectified images
cap0 >> frame0;
cap1 >> frame1;
 
//Rectify and undistort images
cv::remap(frame0, leftStereoUndistorted, cam1map1, cam1map2, INTER_LINEAR);
cv::remap(frame1, rightStereoUndistorted, cam2map1, cam2map2, INTER_LINEAR);
 
frame0 = leftStereoUndistorted;
frame1 = rightStereoUndistorted;
 
//Show rectified and undistorted images
cv::imshow("LeftUndistorted", frame0); imshow("RightUndistored", frame1);
 
MessageBoxW(0, _T("Camera distortioncorrected."), _T("Working?"), MB_OK);

Prepare Camera Images to Process Disparity Mapping:

このパートでは、Disparity Mapping の準備のため、画像を Greyscale に変えて少しボカします。

//
// Declare disparity map
//
Mat disparity_data;
Mat disparity_map;
double min = 0, max = 255;
 
cv::Mat colorImage = cv::imread("rgb.png");
 
//
// Read input images from left and right cameras
//
/* Declare image buffers */
Mat left = frame0;
Mat right = frame1;
//VideoCapture cap0(0);
//VideoCapture cap1(1);
//cap0.isOpened();
/*if (cap0.isOpened())*/ //cap0 >> left;
cv::imshow("Left Image", left);
//cap1.isOpened();
/*if (cap1.isOpened())*/ //cap1 >> right;
cv::imshow("Right Image", right);
if (cv::waitKey(30) >= 0) return;
//colorImage = frame0;
//cap0 >> colorImage;
/* Release image buffers */
//cap0.release();
//cap1.release();
 
MessageBoxW(0, _T("Press OK to continue."), _T("Working"), MB_OK);
cv::destroyAllWindows();
 
/* Declare image windows */
cv::cvtColor(left, left, /*COLOR_BGR2Luv*/ COLOR_BGR2GRAY);   // This line converts the color image "frame" to the grey scale image "left."
cv::GaussianBlur(left, left, Size(7, 7), 1.5, 1.5);
cv::cvtColor(right, right, /*COLOR_BGR2Luv*/ COLOR_BGR2GRAY);   // This line converts the color image "frame" to the grey scale image "right."
cv::GaussianBlur(right, right, Size(7, 7), 1.5, 1.5);

Disparity Mapping:

Disparity Mapping では、StereoBM と StereoSGBM の2つの Function を試してみました。ただ、ここで 3D画像の表示に使っているのは StereoSGBM です。StereoBM を試みたい場合は、Parameter を書き換えるだけで対応出来ます。OpenCV では普通に Disparity Mapping を使っていますが、Noise Sensitive の為に Post-Filter が必要と感じています。この件については Stereo 3D Cloud Display with Post-Filter に関するページを参照してください。

//
// StereoBM disparity
//
//
// Declare StereoBM default setting
//
Ptr<StereoBM> sbm = StereoBM::create();
sbm->compute(left, right, disparity_data);
cv::minMaxLoc(disparity_data, &min, &max);
cv::imshow("disparity_data", disparity_data);
MessageBoxW(0, _T("disparity_data"), _T("Working"), MB_OK);
disparity_data.convertTo(disparity_map, /*CV_32FC4*/ CV_8UC1, 255 / (max - min), -255 * min / (max - min));
cv::imshow("disparity_map", disparity_map);
MessageBoxW(0, _T("disparity_map"), _T("Working"), MB_OK);
//
// Declare StereoSGBM default setting
//
Ptr<StereoSGBM> sgbm = StereoSGBM::create(CV_STEREO_BM_BASIC, 48, 1, 1, 2, 0, 0, 0, false, 0, StereoSGBM::MODE_HH);
sgbm->compute(left, right, disparity_data);
cv::minMaxLoc(disparity_data, &min, &max);
cv::imshow("disparity_data", disparity_data);
cv::waitKey(30);
disparity_data.convertTo(disparity_map, CV_32FC4 /* CV_8UC1*/, 255 / (max - min), -255 * min / (max - min));
//        normalize(disparity_data, disparity_map, 0, 255, CV_MINMAX, CV_8U);
cv::imshow("disparity_map", disparity_map);
cv::waitKey(30);

Converting Camera Parameters to “float” format:

ここで、Camera Parameter を OpenCV Mat format (uchar) から、Visual C++ の float format に変換します。必要な Parameter は左右カメラの焦点距離と中心点。これらを float format に変換しなければ、3D画像の表示は機能しません。VC++の MessageBox() 使う時と同様ですね。

//////////////////////////////////////////////////////////////////////
//
// Conversion from YML file format (string) to usable float numbers
cv::Mat c1c = cameraMatrix0;   // Reads the image as a single-channel gray image
cv::Mat f1c(c1c.rows, c1c.cols, CV_32F);    // note the last argument
//
// Convert cameraMatrix data from "uchar" to "float".
c1c.convertTo(f1c, CV_32FC1);
//////////////////////////////////////////////////////////////////
 
//////////////////////////////////////////////////////////////////
// Assign cameraMatrix(Mat float format) to **buffer(float pointer)
//  Read camera parameters from cameraMatrix() & assign focal length (fx, fy) and a center point (cx,cy).
//
float **buffer = new float*[f1c.rows];
for (int r = 0; r < f1c.rows; r++)
{
buffer[r] = new float[f1c.cols];
for (int c = 0; c < f1c.cols; c++)
{
buffer[r][c] = f1c.at<float/*int*/>(r, c);
}
}
//
// Assign cameraMatrix data to fx, fy, cx, cy
//
fx = buffer[0][0]; cx = buffer[0][2];
fy = buffer[1][1]; cy = buffer[1][2];
 
//
// delete **buffer(float pointer)
//
for (int r = 0; r < f1c.rows; r++) {
delete[] buffer[r];
}
delete[] buffer;
//////////////////////////////////////////////////////////////////
colorImage = frame0;   // おまけ

Display 3D Point Cloud using OpenCV 3.0 Viz function:

この機能は単に “OpenCV3.0 Vizを用いた3次元点群の表示” の引用です。下記アドレスのページを参照してください。

“OpenCV3.0 Vizを用いた3次元点群の表示”  https://qiita.com/SatoshiGachiFujimoto/items/eb3891116d4f49cd342d.

まとめ

如何でしたか。本稿ではVisual C++にて作成したプログラム「Stereo 3D Cloud Display without Post Filter」 を解説しました。最後までお読みいただき、ありがとうございました。

返信を残す

メールアドレスが公開されることはありません。 ※ が付いている欄は必須項目です