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------------------------

%Programm:

% 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;