Software des NXT 3D Laser Scanners
Matlab code
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % Lego NXT Laser3DScanner % In Dieser Programmierung soll ein NXT angesteuert werden der % ein Objekt dreht & eine Webcam, die das Objektaufnimmt um es % am Computer als 3D Figur anzeigt. % % * Authoren: Christoph Dörner & Michael Deitel % * Date: 01/12/2013 - 12/01/2014 % * Hochschule Hamm-Lippstadt, www.hshl.de % * basierend auf RWTH - Mindstorms NXT Toolbox: % * http://www.mindstorms.rwth-aachen.de % * MATLAB Image Acquisition Toolbox: % * www.mathworks.com/products/imaq/? % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%--------------------------Beginn Programm------------------------
%Hauptprogramm:
% 1. Sicherstellung der Installation der RWTH-Mindstorm NXT Toolbox
% 2. Schließen aller bestehender Handle (falls diese existieren)
% 3. NXT Verbindung herstellen
% 4. Globale Bekanntmachung des NXT Handle
% 5. Körper Rotieren lassen, Kamera aktivieren und Film aufnehmen
% 6. Kamerawinkel wird gesetzt
% 7. Farbe des Lasers wird gesetzt
% 8. Kalibrirungsbild wird zum ausgleichen der Kameraposition genommen,
% falls diese schief zum Objekt steht
% 9. Den Motormittelpunkt auswählen
%10. Den Bildbereich des Objektes auswählen
%11. Bilder werden nach und nach in einer for-Schleife verarbeitet
%12. Dazu wird jedes in ein Binärbild von der Laserfarbe konvertiert
% und die Position der Laserlinie analysiert
%13. Nach der Schleife werden die Werte verechnet und in der
% 3D-Grafik angezeigt
%----------------Anfang des Scanvorgangs---------------------------
% Sicherstellung, dass die RWTH - Mindstorms NXT Toolbox installiert ist
if verLessThan('RWTHMindstormsNXT', '2.00');
error(strcat('This program requires the RWTH - Mindstorms NXT Toolbox ' ...
,'version 2.00 or greater. Go to http://www.mindstorms.rwth-aachen.de' ...
,' and follow the installation instructions!'));
end
% Schliessen alle bestehender Handle, falls diese existieren COM_CloseNXT all
% Vorbereitung des Arbeitsplatzes indem sicherheitshalber alle % Variablen gelöscht und alle Fenster geschlossen werden close all clear all
% Öffnen der NXT Bluetooth Verbindung anhand der Initialisierungsdatei
'bluetooth.ini'
handle = COM_OpenNXT('bluetooth.ini');
% Globale bekanntmachung des NXT Handle, damit alle nachfolgenden % Funktionen auf dieses Handle zugreifen können. COM_SetDefaultNXT(handle);
% Motoreinstellung (Grad der Umdrehung & Geschwindigkeit) Fahrgeschwindigkeit = -10; % Prozent UmdrehungenInDeg = 360; % Grad
Objekt = NXTMotor(MOTOR_A); % Motor A wird angesteuert Objekt.SpeedRegulation = false; % Kein Sync Mode Objekt.Power = Fahrgeschwindigkeit; % Fahrgeschwindigkeit in Prozent Objekt.TachoLimit = UmdrehungenInDeg; % Umdrehungen in Deg Objekt.ActionAtTachoLimit = 'Brake'; % Nicht auslaufen
% Initialisierung des Motors Objekt.Stop('off');
% Öffnen der WebCam
vid = videoinput('winvideo',1,'YUY2_640x480');
% Bildrate festlegen framerate = 30;
% Aufnahmezeit und Anzahl der Bilder festlegen
set(vid,'FrameGrabInterval',1);
capturetime = 5.5;
interval = get(vid,'FrameGrabInterval');
numframes = floor(capturetime * framerate / interval)
set(vid,'FramesPerTrigger',numframes);
set(vid,'LoggingMode','disk');
avi = avifile('Objekt','fps',framerate);
set(vid,'DiskLogger',avi);
start(vid);
pause(1.2);
% Hier wird die Fahrt gestartet Objekt.SendToNXT();
% Warte bis die Aufnahme beendet ist und dann fortfahren wait(vid,Inf); avi = get(vid,'DiskLogger'); avi = close(avi);
pause(5);
%------------------Ende des Scanvorgangs------------------------ %--------------Auswerten der .avi Videodatei in 3D---------------
% Parameter
% Videodatei die analysiert werden soll filename = 'Objekt.avi';
% Festlegung des Kamerawinkels (pi/4 (45°) wobei pi=180°) camAngle = pi/9;
% Festlegung der Laserfarbe zum Orientieren Rot=1 Gruen=2 Blau=3 laserInd = 1;
% Kalibrierungsframe calFrame = 1;
% Den Scanfilm aufrufen und den Laser herausfiltern
m = aviread(filename, calFrame);
calImg = m.cdata(:,:,laserInd);
%Kalibrierungsbild anzeigen figure(1); imshow(calImg); xlabel({'Calibrate rotation of image.','Draw line from top to bottom that should be verticle.', 'Left click to start. Right click to end.'}); [rotX, rotY] = getline(1); rotAngle = 180/pi*atan2(rotY(2)-rotY(1),rotX(2)-rotX(1))-90;
% Bild rotieren figure(1);clf; calImg = imrotate(calImg, rotAngle); imshow(calImg);
% Zentrierung bestimmen xlabel('Click mouse for center line'); set(gcf, 'Pointer', 'fullcrosshair'); [x,y]=ginput(1); line([x, x], [1,size(calImg,2)]);
% Zu interessierende Region lokalisieren xlabel('Draw rectangle over region of interest'); rect = getrect; if rect(1)>x
xoffset = rect(1)-x;
else
xoffset = (rect(1)+rect(3))-x;
end; rectangle('Position', rect, 'EdgeColor', [1,0,0]); set(gcf, 'Pointer', 'arrow');
% Unschärfefilter erstellen um das Bild zu glätten % bevor der Laser gefunden wird h = ones(5,1)/5;
% Schwelle einstellen zum finden der Laserlinie threshold = 0.1;
% Laserlinie finden nfo = aviinfo(filename); Nframes = 1; frameTab = 1:Nframes:nfo.NumFrames; for k=1:length(frameTab)
disp(sprintf('Analyzing frame #%d', frameTab(k)));
% Form vom Scanfilm erkennen
m = aviread(filename,frameTab(k));
% Alles rausfiltern außer die Laserlinie & rotieren des Bildes img = imrotate(m.cdata(:,:,laserInd), rotAngle); % Bild auf die uns interessierende Region schneiden imgCrop = imcrop(img, rect); % Bild filtern & von uint8 nach double convertieren, da wir % eine große zahl an Bildern haben und die Funktionen mit double % arbeiten imgFilt = filter2(h,im2double(imgCrop)); % für jede Zeile maximale Bildintensität finden & dessen x-Position [mx(k,:), rtmp(k,:)] = max(imgFilt');
% Sollten in der Laserlinie Löcher sein werden diese ausgefüllt
if (length(find(mx(k,:)>threshold))>=2)
r(k,:)=interp1(find(mx(k,:)>threshold),...
rtmp(k,find(mx(k,:)>threshold)),1:size(rtmp,2));
else
r(k,:)=zeros(1,size(rtmp,2));
end;
% Bild Zeichnen und Laserlinie berechnen
figure(2);clf;
imshow(imgFilt);
hold on;
plot(r(k,:),1:size(r,2),'r');
end;
% X-Position der Laserlinie und des Bildes zum realen Radius % konvertieren if xoffset>0
R = (r+xoffset)./sin(camAngle);
else
R = (rect(3)-r-xoffset)./sin(camAngle);
end; theta = linspace(0,2*pi,length(frameTab))';
% Konvertieren zu den Oberflächenkoordinaten X = R.*repmat(cos(theta),1,size(imgFilt,1)); Y = R.*repmat(sin(theta),1,size(imgFilt,1)); Z = -repmat(1:size(imgFilt,1),length(theta),1);
% Resultat (Scan) anzeigen figure(22); S = surf(X,Y,Z,R); set(S, 'LineStyle', 'none'); axis square;