Software des NXT 3D Laser Scanners

Aus HSHL Mechatronik
Zur Navigation springen Zur Suche springen

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;

Abgerufen von „https://wiki.hshl.de/wiki/index.php?title=Software_des_NXT_3D_Laser_Scanners&oldid=1358“