Lidarと360度​カメラのセンサーフュ​ージョンについて

Lidarと360度カメラのセンサーフュージョンについて質問があります。Lidarからのpcapファイルを読み込んだあとに、その3次元画像と360度カメラの画像を統合したいです。具体的には、3次元画像と360度カメラの画像を対応付けて、カメラ画像を点群に張り付けたいと考えています。そのための方法などを教えて頂けないでしょうか。よろしくお願いいたします。

Answer 1

[Tohru Kikawada]

残念ながら現時点で画像と点群を直接レジストレーションするような機能は提供されていません。

実現する手段として、画像と点群の対応点を手動で指定し、変位量マップを生成する方法が挙げられます。

下記にサンプルプログラムを挙げます。

ご参考になれば幸いです。

画像と点群の位置関係の可視化

点群に画像をマッピング

サンプルコード

%% 点群と画像の位置合わせ

%% 初期化
clear; close all; clc;

%% 画像を読み込み、可視化
I = imread('outside.jpg');
I = I * 4; % 明るさを補正
hFig1 = figure;
hAxesImg = imshow(I);
shg;

%% 点群読み込み、可視化
% 直接VelodyneのLiDARのPCAPファイルを読み込むことも可能
% fileName = "outside.pcap";
% deviceModel = "VLP16";
% veloReader = velodyneFileReader(fileName,deviceModel);
% pt = readFrame(veloReader);
d = load('pointCloud');
pt = d.pt;
hFig2 = figure, hAxesLidar = pcshow(pt,'MarkerSize',50);
shg;

%% 対応点を取得データカーソルモードをオン
% Shiftキーを押しながら対応点をクリックしていく
dcm1 = datacursormode(hFig1);
dcm1.Enable = "on";
dcm1.UpdateFcn = @myfunction;
dcm2 = datacursormode(hFig2);
dcm2.Enable = "on";
dcm2.UpdateFcn = @myfunction;
shg;
pause;

%% 画像、点群のそれぞれの行列インデックス取得
imgIndex = hAxesImg.UserData;
lidarIndex = hAxesLidar.Children(1).UserData;

%% 画像と点群の対応点を可視化

% 画像のRow, Columnのインデックス作成
[rowImage,colImage] = ind2sub([size(I,1) size(I,2)],imgIndex);
Iout = insertMarker(I,[colImage rowImage],'o','Size',80);

% 点群のRow, Columnのインデックス作成
locs = pt.Location;
Intensity = im2double(pt.Intensity);
[rowLidar,colLidar] = ind2sub([size(locs,1) size(locs,2)],lidarIndex);

% 可視化して確認
figure;
pcshow(locs);
hold on;
x = locs(:,:,1);
y = locs(:,:,2);
z = locs(:,:,3);
scatter3(x(lidarIndex),y(lidarIndex),z(lidarIndex),...
    20,'red','filled');
xImage = [-size(I,2)/2 size(I,2)/2;-size(I,2)/2 size(I,2)/2]/100;
yImage = [80 80; 80 80];
zImage = [size(I,1)/2 size(I,1)/2; -size(I,1)/2 -size(I,1)/2]/100;
xImage3D = (colImage-size(I,2)/2)/100;
zImage3D = -(rowImage-size(I,1)/2)/100;
yImage3D = 80*ones(size(xImage3D));
surf(xImage,yImage,zImage,...
     'CData',I,...
     'FaceColor','texturemap');
plot3([xImage3D x(lidarIndex)]',...
    [yImage3D y(lidarIndex)]',...
    [zImage3D z(lidarIndex)]',...
    'LineWidth',1,'Color','red');
 
% 反射強度に対して対応点を表示
figure;
showMatchedFeatures(I,Intensity,[colImage rowImage],[colLidar rowLidar],'montage');


%% 幾何学変換を使う場合
% 変換が非線形のためうまくいかない
tform = fitgeotrans([colImage rowImage],[colLidar rowLidar],'projective');
Jregistered = imwarp(I,tform,'OutputView',imref2d([size(locs,1) size(locs,2)]));
ptCol = pointCloud(locs,'Color',Jregistered);
figure, pcshow(ptCol,'MarkerSize',80);
hold on;
axis([-5 5 -5 5 -1 1]);

%% 変位マップを使う場合
% 非線形変換のため場所ごとに変位ベクトルを推定する
% 代表点を元に内挿、外挿を行い変位ベクトルを推定
% 推定された変位ベクトルを使って画像を点群の座標系の補正
[xq,yq] = meshgrid(1:size(locs,2),1:size(locs,1));
F1 = scatteredInterpolant(colLidar,rowLidar,colImage-colLidar,'natural','linear');
F2 = scatteredInterpolant(colLidar,rowLidar,rowImage-rowLidar,'natural','linear');
vqx = F1(xq,yq);
vqy = F2(xq,yq);
D = cat(3,vqx,vqy);
Jregistered2 = imwarp(I,D);
figure, image(Jregistered2);
ptCol = pointCloud(locs,'Color',Jregistered2);
figure, pcshow(ptCol,'MarkerSize',30);
hold on;

%% 点群を書き出し
pcwrite(ptCol,'outside_colorized.ply');

[Kenta]

ご回答いただき誠にありがとうございます。

非常によい結果を得ることができました。提示いただいたコードをもとに色々勉強させていただきます。非常に丁寧に対応いただき、誠にありがとうございました。

Answer 2

[Tohru Kikawada]

Lidarとカメラのキャリブレーションについては R2020b で新しくリリースされた Lidar Toolbox で機能が提供されています。

詳細は下記をご覧ください。

https://jp.mathworks.com/help/lidar/ug/lidar-and-camera-calibration.html

[Kenta]

@Tohru Kikawada こちらの機能については、2020b以降で対応されているとのこと、教えていただき、ありがとうございます。ぜひそちらにも取り組んでみようと思います。