自分のカメラデバイスでのSLAMについて

私は以下のサイト

https://jp.mathworks.com/help/vision/ug/stereo-visual-simultaneous-localization-mapping.html

参考にしてファイル名「stereoslam.m」として自分の所持しているステレオカメラでSLAMを実行しようと考えています。

「Map Initialization」の項目まではエラーが生じませんでしたが「Data Management and Visualization」の項目で以下のエラーが生じます。

エラー: worldpointset
featureIndicesは要素数が 2 である配列にする必要があります。

エラー: worldpointset/checkFeatureIndices (行 707)
            validateattributes(featureIndices, {'numeric'}, ...

エラー: worldpointset/addCorrespondences (行 298)
            checkFeatureIndices(obj, featureIndices, numel(pointIndices));

エラー: stereoslam (行 74)
mapPointSet = addCorrespondences(mapPointSet, currKeyFrameId,
stereoMapPointsIdx, matchedPairs(:, 1));

上記の「worldpointset.m」関数のコード内の「featureIndices」の要素数を変更することはできるのですか?

自分で「worldpointset.m」に書き込もうとすると「worldpointset.m」の書き込みエラーが表示され、worldpointset.m内にコードを変更することができないです。

このエラーの解決方法をどなたか教えていただけませんか。

[Atsushi Ueno]

% featureIndicesは要素数が 2 である配列にする必要があります。

上記を機械的に追っていくと、

[xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeft, currIRight, ...
    currFeaturesLeft, currFeaturesRight, currPointsLeft, currPointsRight, stereoParams, initialPose, maxDisparity)

helperReconstructFromStereo関数(視差マップによるステレオ画像からのシーン再構成)で得られる

• matchedPairs(ステレオ画像間で合致した特徴点)の数が2個
• xyzPoints(上記特徴点に対応する3次元空間の座標)の数が?個

で、この数が一致する必要があります。このエラーが出たという事は、下記いずれかの原因が考えられます。

• 上記のxyzPoints, matchedPairsの行数が一致していない
• 下記(機械的に追ったコードの抜粋)の関数を通る間に何らかの理由で数が一致しなくなった

[xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeft, currIRight, ...
    currFeaturesLeft, currFeaturesRight, currPointsLeft, currPointsRight, stereoParams, initialPose, maxDisparity);
[mapPointSet, stereoMapPointsIdx] = addWorldPoints(mapPointSet, xyzPoints);
mapPointSet = addCorrespondences(mapPointSet, currKeyFrameId, stereoMapPointsIdx, matchedPairs(:, 1));
function obj = addCorrespondences(obj, viewId, pointIndeces, featureIndices)
    checkFeatureIndices(obj, featureIndices, numel(pointIndices));
end
function checkFeatureIndices(obj, featureIndices, numFeatures)
	validateattributes(featureIndices, {'numeric'}, ...
	{'vector', 'nonsparse', 'positive', integer', ...
	'numel', numFeatures}, obj,ClassName, 'featureIndices');
end

[圭介 川邉]

私は以前、このエラーが生じる前に以下のエラーが生じていたことから

helperReconstructFromStereo関数内のxyzPointsの値を変えています。

エラー:  * 
行列乗算に対する次元が正しくありません。最初の行列の列数が 2 番目の行列の行数と一致することを
確認してください。要素単位の乗算を実行するには '.*' を使用します。

エラー: helperReconstructFromStereo (行 33)
xyzPoints  = xyzPoints * currPose.Rotation + currPose.Translation

エラー: stereoslam (行 47)
[xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeft,
currIRight, ...

このエラーを解決するためhelperReconstructFromStereo関数内の

xyzPoints = xyzPoints * currPose.Rotation + currPose.Translation

を調べたところ、以下の値が得られました。

xyzPoints  =
　　　[]
   
currPose.Rotation =
    0.0008   -1.0000    0.0000
   -0.3305   -0.0003   -0.9438
    0.9438    0.0008   -0.3305
    
currPose.Translation =
    0.1590    0.1200    1.5280

xyzPointsが空行列であったため、値が0であることと上式から

xyzPointsの次元を合わせるためhelperReconstructFromStereo関数内を以下のように変えました。

xyzPoints=[]
↓
xyzPoints=zeros(1,3)

すると上記のエラーを解消することはできたのですが、本件のエラーのような

以下のエラーが生じました。

エラー: worldpointset
featureIndicesは要素数が 1 である配列にする必要があります。

エラー: worldpointset/checkFeatureIndices (行 707)
            validateattributes(featureIndices, {'numeric'}, ...

エラー: worldpointset/addCorrespondences (行 298)
            checkFeatureIndices(obj, featureIndices, numel(pointIndices));

エラー: stereoslam (行 72)
mapPointSet = addCorrespondences(mapPointSet, currKeyFrameId,
stereoMapPointsIdx, matchedPairs(:, 1));

このとき自分でエラーの原因を考えており、更にxyzPointsの値を以下の変えたところ

「featureIndicesは要素数が 2 である配列にする必要があります。」とエラーが生じました。

xyzPoints=zeros(1,3)
↓
xyzPoints=ones(1,3)

本来であれば、xyzPoints=zeros(1,3)でコードを使用していたため、

本件の本質は「featureIndicesは要素数が 1 である配列にする必要があります。」のエラーを解決する方法を探しています。

またmatchedPairsの値も調べたところ、以下の値が得られました。

matchedPairs =

  0×2 の空の uint32 行列

これらのことから今回のエラーは返答の上記の上項目の

「上記のxyzPoints, matchedPairsの行数が一致していない」

ということになるのでしょうか?

[Atsushi Ueno]

🤔

[圭介 川邉]

本件の質問について返信をしていただきありがとうございました。

helperFindValidFeaturePairs関数を実行した後に

helperReconstructFromStereo関数を実行したところ、indexPairs内の値が空間数にならず、2×2 の uint32 行列が得られ、

xyzPoints=[]の状態に戻しても以下のエラーを解消することができました。

エラー: stereoslam (行 74)
mapPointSet = addCorrespondences(mapPointSet, currKeyFrameId,
stereoMapPointsIdx, matchedPairs(:, 1));

本件のエラーは自己解決することはできたのですが、新たに別件でエラーが生じたため別で質問してみます。

エラーの原因について回答していただき、ありがとうございました。