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

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

Accepted Answer

Tohru Kikawada
Tohru Kikawada on 16 Dec 2018
残念ながら現時点で画像と点群を直接レジストレーションするような機能は提供されていません。
実現する手段として、画像と点群の対応点を手動で指定し、変位量マップを生成する方法が挙げられます。
下記にサンプルプログラムを挙げます。
ご参考になれば幸いです。
画像と点群の位置関係の可視化
点群に画像をマッピング
サンプルコード
%% 点群と画像の位置合わせ
%% 初期化
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');
  1 Comment
Kenta
Kenta on 17 Dec 2018
ご回答いただき誠にありがとうございます。
非常によい結果を得ることができました。提示いただいたコードをもとに色々勉強させていただきます。非常に丁寧に対応いただき、誠にありがとうございました。

Sign in to comment.

More Answers (1)

Tohru Kikawada
Tohru Kikawada on 15 Dec 2021
Lidarとカメラのキャリブレーションについては R2020b で新しくリリースされた Lidar Toolbox で機能が提供されています。
詳細は下記をご覧ください。
  1 Comment
Kenta
Kenta on 15 Dec 2021
@Tohru Kikawada こちらの機能については、2020b以降で対応されているとのこと、教えていただき、ありがとうございます。ぜひそちらにも取り組んでみようと思います。

Sign in to comment.

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!