2 つのビューからの structure from motion

ライブスクリプトを開く

structure from motion (SfM) は、一連の 2 次元イメージから 3 次元シーンの構造を推定するプロセスです。この例では、キャリブレートされたカメラの姿勢を 2 つのイメージから推定し、未知の倍率によるシーンの 3 次元構造を再構成した後、既知のサイズのオブジェクトを検出することで実際の倍率を復元する方法を説明します。

概要

この例では、カメラキャリブレーターアプリを使ってキャリブレートされたカメラで撮影した 2 次元イメージのペアから、3 次元シーンを再構成する方法を説明します。アルゴリズムは以下のステップから構成されます。

2 つのイメージ間で点の疎集合をマッチングする。2 つのイメージ間で点の対応関係を見つけるには複数の方法があります。この例では、関数 detectMinEigenFeatures を使用して最初のイメージのコーナーを検出し、vision.PointTracker を使ってこれらを 2 番目のイメージ内で追跡します。あるいは、extractFeatures を使用した後で matchFeatures を使用する方法もあります。
estimateEssentialMatrix を使用して基礎行列を推定する。
関数 estrelpose を使用してカメラの動きを計算する。
2 つのイメージ間で点の稠密集合をマッチングする。detectMinEigenFeatures を使用して点を再検出します。'MinQuality' の設定を小さくしてより多くの点を取得します。その後、vision.PointTracker を使って稠密な点を 2 番目のイメージ内で追跡します。
triangulate を使用して、マッチする点の 3 次元での位置を決定する。
既知のサイズのオブジェクトを検出する。このシーンには半径が 10 cm であるとわかっている地球儀があります。pcfitsphere を使用して、点群内で地球儀を見つけます。
実際のスケールを復元し、メートル法で再構成する。

イメージのペアの読み取り

ワークスペースにイメージのペアを読み込みます。

imageDir = fullfile(toolboxdir("vision"),"visiondata","upToScaleReconstructionImages");
images = imageDatastore(imageDir);
I1 = readimage(images, 1);
I2 = readimage(images, 2);
figure
imshowpair(I1, I2, 'montage'); 
title('Original Images');

カメラパラメーターの読み込み

この例ではカメラキャリブレーターアプリで計算されたカメラパラメーターを使用します。パラメーターは cameraIntrinsics オブジェクトに保存されており、カメラの内部パラメーターとレンズ歪み係数が含まれています。

% Load precomputed camera intrinsics
data = load("sfmCameraIntrinsics.mat");
intrinsics = data.intrinsics;

レンズ歪みの除去

レンズ歪みは最終的な再構成の精度に影響することがあります。歪みは、関数 undistortImage を使用して各イメージから除去できます。この処理は、レンズの半径方向歪みによって曲がった線をまっすぐにします。

I1 = undistortImage(I1, intrinsics);
I2 = undistortImage(I2, intrinsics);
figure 
imshowpair(I1, I2, "montage");
title("Undistorted Images");

イメージ間の点の対応関係の検出

追跡に適した特徴を検出します。'MinQuality' の値を増やして、検出される点を減らし、検出点がイメージ全体により均一に分布するようにします。カメラの動きがあまり大きくない場合、点の対応関係を確立するには KLT アルゴリズムによる追跡が適しています。

% Detect feature points
imagePoints1 = detectMinEigenFeatures(im2gray(I1), MinQuality=0.1);

% Visualize detected points
figure
imshow(I1, InitialMagnification = 50);
title("150 Strongest Corners from the First Image");
hold on
plot(selectStrongest(imagePoints1, 150));

% Create the point tracker
tracker = vision.PointTracker(MaxBidirectionalError=1, NumPyramidLevels=5);

% Initialize the point tracker
imagePoints1 = imagePoints1.Location;
initialize(tracker, imagePoints1, I1);

% Track the points
[imagePoints2, validIdx] = step(tracker, I2);
matchedPoints1 = imagePoints1(validIdx, :);
matchedPoints2 = imagePoints2(validIdx, :);

% Visualize correspondences
figure
showMatchedFeatures(I1, I2, matchedPoints1, matchedPoints2);
title("Tracked Features");

基本行列の推定

関数 estimateEssentialMatrix を使用して基本行列を計算し、エピポーラ制約を満たすインライア点を求めます。

% Estimate the fundamental matrix
[E, epipolarInliers] = estimateEssentialMatrix(...
    matchedPoints1, matchedPoints2, intrinsics, Confidence = 99.99);

% Find epipolar inliers
inlierPoints1 = matchedPoints1(epipolarInliers, :);
inlierPoints2 = matchedPoints2(epipolarInliers, :);

% Display inlier matches
figure
showMatchedFeatures(I1, I2, inlierPoints1, inlierPoints2);
title("Epipolar Inliers");

カメラの姿勢の計算

最初のカメラに対する 2 番目のカメラの位置と向きを計算します。スケールに沿った並進のみが計算されるため、loc は並進単位ベクトルであることに注意してください。

relPose = estrelpose(E, intrinsics, inlierPoints1, inlierPoints2);

マッチする点の 3 次元位置の再構成

'MinQuality' の値をより低くして最初のイメージで点を再検出し、より多くの点を取得します。新しい点を 2 番目のイメージで追跡します。関数 triangulate を使用して、マッチする点に対応する 3 次元での位置を推定します。この関数は直接線形変換 (DLT) アルゴリズム [1] を実装します。最初のイメージに対応するカメラの光学的中心に原点を配置します。

% Detect dense feature points. Use an ROI to exclude points close to the
% image edges.
border = 30;
roi = [border, border, size(I1, 2)- 2*border, size(I1, 1)- 2*border];
imagePoints1 = detectMinEigenFeatures(im2gray(I1), ROI = roi, ...
    MinQuality = 0.001);

% Create the point tracker
tracker = vision.PointTracker(MaxBidirectionalError=1, NumPyramidLevels=5);

% Initialize the point tracker
imagePoints1 = imagePoints1.Location;
initialize(tracker, imagePoints1, I1);

% Track the points
[imagePoints2, validIdx] = step(tracker, I2);
matchedPoints1 = imagePoints1(validIdx, :);
matchedPoints2 = imagePoints2(validIdx, :);

% Compute the camera matrices for each position of the camera
% The first camera is at the origin looking along the Z-axis. Thus, its
% transformation is identity.
camMatrix1 = cameraProjection(intrinsics, rigidtform3d);
camMatrix2 = cameraProjection(intrinsics, pose2extr(relPose));

% Compute the 3-D points
points3D = triangulate(matchedPoints1, matchedPoints2, camMatrix1, camMatrix2);

% Get the color of each reconstructed point
numPixels = size(I1, 1) * size(I1, 2);
allColors = reshape(I1, [numPixels, 3]);
colorIdx = sub2ind([size(I1, 1), size(I1, 2)], round(matchedPoints1(:,2)), ...
    round(matchedPoints1(:, 1)));
color = allColors(colorIdx, :);

% Create the point cloud
ptCloud = pointCloud(points3D, Color=color);

3 次元点群の表示

関数 plotCamera を使用してカメラの位置と向きを可視化し、関数 pcshow を使用して点群を可視化します。

% Visualize the camera locations and orientations
cameraSize = 0.3;
figure
plotCamera(Size=cameraSize, Color="r", Label="1", Opacity=0);
hold on
grid on
plotCamera(AbsolutePose=relPose, Size=cameraSize, ...
    Color="b", Label="2", Opacity=0);

% Visualize the point cloud
pcshow(ptCloud, VerticalAxis="y", VerticalAxisDir="down", MarkerSize=45);

% Rotate and zoom the plot
camorbit(0, -30);
camzoom(1.5);

% Label the axes
xlabel("x-axis");
ylabel("y-axis");
zlabel("z-axis")

title("Up to Scale Reconstruction of the Scene");

球体の点群への近似による地球儀の検出

関数 pcfitsphere を使用して球体を 3 次元点に近似させることで、点群内で地球儀を見つけます。

% Detect the globe
globe = pcfitsphere(ptCloud, 0.1);

% Display the surface of the globe
plot(globe);
title("Estimated Location and Size of the Globe");
hold off

メートル法によるシーンの再構成

地球儀の実際の半径は 10 cm です。これにより、3 次元点の座標をセンチメートル単位で求めることができます。

% Determine the scale factor
scaleFactor = 10 / globe.Radius;

% Scale the point cloud
ptCloud = pointCloud(points3D * scaleFactor, Color=color);
relPose.Translation = relPose.Translation * scaleFactor;

% Visualize the point cloud in centimeters
cameraSize = 2; 
figure
plotCamera(Size=cameraSize, Color="r", Label="1", Opacity=0);
hold on
grid on
plotCamera(AbsolutePose=relPose, Size=cameraSize, ...
    Color="b", Label="2", Opacity=0);

% Visualize the point cloud
pcshow(ptCloud, VerticalAxis="y", VerticalAxisDir="down", MarkerSize=45);
camorbit(0, -30);
camzoom(1.5);

% Label the axes
xlabel("x-axis (cm)");
ylabel("y-axis (cm)");
zlabel("z-axis (cm)")
title("Metric Reconstruction of the Scene");