Construya un mapa a partir de datos Lidar usando SLAM
Este ejemplo muestra cómo procesar datos LiDAR 3D de un sensor montado en un vehículo para construir progresivamente un mapa y estimar la trayectoria de un vehículo utilizando localización y mapeo simultáneos (SLAM). Además de los datos LiDAR 3-D, también se utiliza un sensor de navegación inercial (INS) para ayudar a construir el mapa. Los mapas creados de esta manera pueden facilitar la planificación de rutas para la navegación de vehículos o pueden usarse para la localización.
Visión general
El ejemplo Build a Map from Lidar Data (Computer Vision Toolbox) utiliza datos LiDAR 3-D y lecturas IMU para construir progresivamente un mapa del entorno atravesado por un vehículo. Si bien este enfoque permite crear un mapa consistente a nivel local, sólo es adecuado para mapear áreas pequeñas. En secuencias más largas, la deriva se acumula y genera un error significativo. Para superar esta limitación, este ejemplo reconoce lugares visitados previamente e intenta corregir la deriva acumulada utilizando el enfoque gráfico SLAM.
Cargar y explorar datos registrados
Los datos utilizados en este ejemplo son parte del conjunto de datos SLAM de Velodyne y representan cerca de 6 minutos de datos registrados. Descargue los datos a un directorio temporal.
Nota: Esta descarga puede tardar unos minutos.
baseDownloadURL = 'https://www.mrt.kit.edu/z/publ/download/velodyneslam/data/scenario1.zip'; dataFolder = fullfile(tempdir,'kit_velodyneslam_data_scenario1',filesep); options = weboptions('Timeout',Inf); zipFileName = dataFolder + "scenario1.zip"; % Get the full file path to the PNG files in the scenario1 folder pointCloudFilePattern = fullfile(dataFolder,'scenario1','scan*.png'); numExpectedFiles = 2513; folderExists = exist(dataFolder,'dir'); if ~folderExists % Create a folder in a temporary directory to save the downloaded zip % file mkdir(dataFolder); disp('Downloading scenario1.zip (153 MB) ...') websave(zipFileName,baseDownloadURL,options); % Unzip downloaded file unzip(zipFileName,dataFolder); elseif folderExists && numel(dir(pointCloudFilePattern)) < numExpectedFiles % Redownload the data if it got reduced in the temporary directory disp('Downloading scenario1.zip (153 MB) ...') websave(zipFileName,baseDownloadURL,options); % Unzip downloaded file unzip(zipFileName,dataFolder) end
Downloading scenario1.zip (153 MB) ...
Utilice la función helperReadDataset para leer datos de la carpeta creada en formato timetable. Las nubes de puntos capturadas por el LiDAR se almacenan en forma de archivos de imagen PNG. Extraiga la lista de nombres de archivos de nube de puntos en la variable pointCloudTable. Para leer los datos de la nube de puntos del archivo de imagen, utilice la función helperReadPointCloudFromFile. Esta función toma un nombre de archivo de imagen y devuelve un objeto pointCloud (Computer Vision Toolbox). Las lecturas del INS se leen directamente desde un archivo de configuración y se almacenan en la variable insDataTable.
datasetTable = helperReadDataset(dataFolder,pointCloudFilePattern); pointCloudTable = datasetTable(:,1); insDataTable = datasetTable(:,2:end);
Lea la primera nube de puntos y muéstrela en el línea de comandosMATLAB ® .
ptCloud = helperReadPointCloudFromFile(pointCloudTable.PointCloudFileName{1});
disp(ptCloud) pointCloud with properties:
Location: [64×870×3 single]
Count: 55680
XLimits: [-78.4980 77.7062]
YLimits: [-76.8795 74.7502]
ZLimits: [-2.4839 2.6836]
Color: []
Normal: []
Intensity: []
Muestra la primera lectura INS. timetable contiene información Heading, Pitch, Roll, X, Y y Z del INS.
disp(insDataTable(1,:))
Timestamps Heading Pitch Roll X Y Z
____________________ _______ ________ _________ _______ _______ ______
13-Jun-2010 06:27:31 1.9154 0.007438 -0.019888 -2889.1 -2183.9 116.47
Visualice las nubes de puntos utilizando pcplayer (Computer Vision Toolbox), una pantalla de nubes de puntos en tiempo real. El vehículo recorre un recorrido formado por dos bucles. En el primer bucle, el vehículo realiza una serie de giros y regresa al punto de partida. En el segundo bucle, el vehículo realiza una serie de giros a lo largo de otra ruta y regresa nuevamente al punto de partida.
% Specify limits of the player xlimits = [-45 45]; % meters ylimits = [-45 45]; zlimits = [-10 20]; % Create a streaming point cloud display object lidarPlayer = pcplayer(xlimits,ylimits,zlimits); % Customize player axes labels xlabel(lidarPlayer.Axes,'X (m)') ylabel(lidarPlayer.Axes,'Y (m)') zlabel(lidarPlayer.Axes,'Z (m)') title(lidarPlayer.Axes,'Lidar Sensor Data') % Skip evey other frame since this is a long sequence skipFrames = 2; numFrames = height(pointCloudTable); for n = 1:skipFrames:numFrames % Read a point cloud fileName = pointCloudTable.PointCloudFileName{n}; ptCloud = helperReadPointCloudFromFile(fileName); % Visualize point cloud view(lidarPlayer,ptCloud); pause(0.01) end
Construir un mapa usando odometría
Primero, utilice el enfoque explicado en el ejemplo Build a Map from Lidar Data (Computer Vision Toolbox) para construir un mapa. El enfoque consta de los siguientes pasos:
Alinear escaneos Lidar: Alinear escaneos LiDAR sucesivos utilizando una técnica de registro de nube de puntos. Este ejemplo utiliza
pcregisterndt(Computer Vision Toolbox) para registrar escaneos. Al componer sucesivamente estas transformaciones, cada nube de puntos se transforma nuevamente en el marco de referencia de la primera nube de puntos.Combinar escaneos alineados: Genere un mapa combinando todas las nubes de puntos transformadas.
Este enfoque de construir un mapa de forma incremental y estimar la trayectoria del vehículo se denomina odometría.
Utilice un objeto pcviewset (Computer Vision Toolbox) para almacenar y administrar datos en múltiples vistas. Un conjunto de vistas consta de un conjunto de vistas conectadas.
Cada vista almacena información asociada con una sola vista. Esta información incluye la posición absoluta de la vista, los datos del sensor de nube de puntos capturados en esa vista y un identificador único para la vista. Agregue vistas al conjunto de vistas usando
addView(Computer Vision Toolbox).Para establecer una conexión entre vistas utilice
addConnection(Computer Vision Toolbox). Una conexión almacena información como la transformación relativa entre las vistas conectadas, la incertidumbre involucrada en el cálculo de esta medición (representada como una matriz de información) y los identificadores de vista asociados.Utilice el método
plot(Computer Vision Toolbox) para visualizar las conexiones establecidas por el conjunto de vistas. Estas conexiones se pueden utilizar para visualizar la trayectoria recorrida por el vehículo.
hide(lidarPlayer) % Set random seed to ensure reproducibility rng(0); % Create an empty view set vSet = pcviewset; % Initialize point cloud processing parameters downsamplePercent = 0.1; regGridSize = 3; % Initialize transformations absTform = rigidtform3d; % Absolute transformation to reference frame relTform = rigidtform3d; % Relative transformation between successive scans viewId = 1; skipFrames = 5; numFrames = height(pointCloudTable); displayRate = 100; % Update display every 100 frames for n = 1:skipFrames:numFrames % Read point cloud fileName = pointCloudTable.PointCloudFileName{n}; ptCloudOrig = helperReadPointCloudFromFile(fileName); % Process point cloud % - Segment and remove ground plane % - Segment and remove ego vehicle ptCloud = helperProcessPointCloud(ptCloudOrig); % Downsample the processed point cloud ptCloud = pcdownsample(ptCloud,"random",downsamplePercent); firstFrame = (n==1); if firstFrame % Add first point cloud scan as a view to the view set vSet = addView(vSet,viewId,absTform,"PointCloud",ptCloudOrig); viewId = viewId + 1; ptCloudPrev = ptCloud; continue; end % Use INS to estimate an initial transformation for registration initTform = helperComputeInitialEstimateFromINS(relTform, ... insDataTable(n-skipFrames:n,:)); % Compute rigid transformation that registers current point cloud with % previous point cloud relTform = pcregisterndt(ptCloud,ptCloudPrev,regGridSize, ... "InitialTransform",initTform); % Update absolute transformation to reference frame (first point cloud) absTform = rigidtform3d(absTform.A * relTform.A); % Add current point cloud scan as a view to the view set vSet = addView(vSet,viewId,absTform,"PointCloud",ptCloudOrig); % Add a connection from the previous view to the current view, representing % the relative transformation between them vSet = addConnection(vSet,viewId-1,viewId,relTform); viewId = viewId + 1; if mod(viewId,displayRate) == 0 plot(vSet) drawnow end ptCloudPrev = ptCloud; initTform = relTform; end title('View Set Display')
El objeto de conjunto de vistas vSet ahora contiene vistas y conexiones. En la tabla Vistas de vSet, la variable AbsolutePose especifica la postura absoluta de cada vista con respecto a la primera vista. En la tabla Connections de vSet, la variable RelativePose especifica restricciones relativas entre las vistas conectadas, la variable InformationMatrix especifica, para cada borde, la incertidumbre asociada con una conexión.
% Display the first few views and connections
head(vSet.Views) ViewId AbsolutePose PointCloud
______ ________________ ______________
1 1×1 rigidtform3d 1×1 pointCloud
2 1×1 rigidtform3d 1×1 pointCloud
3 1×1 rigidtform3d 1×1 pointCloud
4 1×1 rigidtform3d 1×1 pointCloud
5 1×1 rigidtform3d 1×1 pointCloud
6 1×1 rigidtform3d 1×1 pointCloud
7 1×1 rigidtform3d 1×1 pointCloud
8 1×1 rigidtform3d 1×1 pointCloud
head(vSet.Connections)
ViewId1 ViewId2 RelativePose InformationMatrix
_______ _______ ________________ _________________
1 2 1×1 rigidtform3d {6×6 double}
2 3 1×1 rigidtform3d {6×6 double}
3 4 1×1 rigidtform3d {6×6 double}
4 5 1×1 rigidtform3d {6×6 double}
5 6 1×1 rigidtform3d {6×6 double}
6 7 1×1 rigidtform3d {6×6 double}
7 8 1×1 rigidtform3d {6×6 double}
8 9 1×1 rigidtform3d {6×6 double}
Ahora, construya un mapa de nube de puntos utilizando el conjunto de vistas creado. Alinee las poses absolutas de la vista con las nubes de puntos en el conjunto de vistas usando pcalign (Computer Vision Toolbox). Especifique un tamaño de cuadrícula para controlar la resolución del mapa. El mapa se devuelve como un objeto pointCloud.
ptClouds = vSet.Views.PointCloud; absPoses = vSet.Views.AbsolutePose; mapGridSize = 0.2; ptCloudMap = pcalign(ptClouds,absPoses,mapGridSize);
Tenga en cuenta que la ruta recorrida utilizando este enfoque se desvía con el tiempo. Si bien el camino a lo largo del primer circuito de regreso al punto de partida parece razonable, el segundo circuito se desvía significativamente del punto de partida. La deriva acumulada hace que el segundo bucle termine a varios metros del punto de partida.
Un mapa elaborado únicamente utilizando odometría es inexacto. Muestra el mapa de nube de puntos construido con la ruta recorrida. Tenga en cuenta que el mapa y la ruta recorrida para el segundo bucle no son consistentes con el primer bucle.
figure pcshow(ptCloudMap); hold on plot(vSet) title('Point Cloud Map','Color','w')
Corregir la deriva mediante la optimización del gráfico de poses
Graph SLAM es una técnica ampliamente utilizada para resolver la deriva en la odometría. El enfoque gráfico SLAM crea de forma incremental un gráfico, donde los nodos corresponden a las poses del vehículo y los bordes representan las mediciones del sensor que restringen las poses conectadas. Un gráfico de este tipo se llama gráfico de pose. El gráfico de pose contiene bordes que codifican información contradictoria, debido al ruido o imprecisiones en la medición. Luego, los nodos en el gráfico construido se optimizan para encontrar el conjunto de posiciones del vehículo que expliquen de manera óptima las mediciones. Esta técnica se llama optimización de gráficos de pose.
Para crear un gráfico de poses a partir de un conjunto de vistas, puede utilizar la función createPoseGraph (Computer Vision Toolbox). Esta función crea un nodo para cada vista y un borde para cada conexión en el conjunto de vistas. Para optimizar el gráfico de pose, puede utilizar la función optimizePoseGraph.
Un aspecto clave que contribuye a la eficacia del SLAM gráfico para corregir la deriva es la detección precisa de bucles, es decir, lugares que se han visitado previamente. Esto se llama detección de cierre de bucle o reconocimiento de lugar. Agregar bordes al gráfico de poses correspondientes a los cierres de bucle proporciona una medición contradictoria para las poses de los nodos conectados, que se puede resolver durante la optimización del gráfico de poses.
Los cierres de bucle se pueden detectar utilizando descriptores que caracterizan el entorno local visible para el sensor Lidar. El descriptor Contexto de escaneo [1] es uno de esos descriptores que se pueden calcular a partir de una nube de puntos utilizando la función scanContextDescriptor (Computer Vision Toolbox). Este ejemplo utiliza un objeto scanContextLoopDetector (Computer Vision Toolbox) para administrar los descriptores de contexto de escaneo que corresponden a cada vista. Utiliza la función de objeto detectLoop (Computer Vision Toolbox) para detectar cierres de bucle con un algoritmo de búsqueda de descriptor de dos fases. En la primera fase, calcula los subdescriptores de la clave del anillo para encontrar posibles candidatos para el bucle. En la segunda fase, clasifica las vistas como cierres de bucle al establecer un umbral de la distancia del contexto de escaneo.
% Set random seed to ensure reproducibility rng(0); % Create an empty view set vSet = pcviewset; % Create a loop closure detector loopDetector = scanContextLoopDetector; % Initialize transformations absTform = rigidtform3d; % Absolute transformation to reference frame relTform = rigidtform3d; % Relative transformation between successive scans maxTolerableRMSE = 3; % Maximum allowed RMSE for a loop closure candidate to be accepted viewId = 1; for n = 1:skipFrames:numFrames % Read point cloud fileName = pointCloudTable.PointCloudFileName{n}; ptCloudOrig = helperReadPointCloudFromFile(fileName); % Process point cloud % - Segment and remove ground plane % - Segment and remove ego vehicle ptCloud = helperProcessPointCloud(ptCloudOrig); % Downsample the processed point cloud ptCloud = pcdownsample(ptCloud,"random",downsamplePercent); firstFrame = (n==1); if firstFrame % Add first point cloud scan as a view to the view set vSet = addView(vSet,viewId,absTform,"PointCloud",ptCloudOrig); % Extract the scan context descriptor from the first point cloud descriptor = scanContextDescriptor(ptCloudOrig); % Add the first descriptor to the loop closure detector addDescriptor(loopDetector,viewId,descriptor) viewId = viewId + 1; ptCloudPrev = ptCloud; continue; end % Use INS to estimate an initial transformation for registration initTform = helperComputeInitialEstimateFromINS(relTform, ... insDataTable(n-skipFrames:n,:)); % Compute rigid transformation that registers current point cloud with % previous point cloud relTform = pcregisterndt(ptCloud,ptCloudPrev,regGridSize, ... "InitialTransform",initTform); % Update absolute transformation to reference frame (first point cloud) absTform = rigidtform3d(absTform.A * relTform.A); % Add current point cloud scan as a view to the view set vSet = addView(vSet,viewId,absTform,"PointCloud",ptCloudOrig); % Add a connection from the previous view to the current view representing % the relative transformation between them vSet = addConnection(vSet,viewId-1,viewId,relTform); % Extract the scan context descriptor from the point cloud descriptor = scanContextDescriptor(ptCloudOrig); % Add the descriptor to the loop closure detector addDescriptor(loopDetector,viewId,descriptor) % Detect loop closure candidates loopViewId = detectLoop(loopDetector); % A loop candidate was found if ~isempty(loopViewId) loopViewId = loopViewId(1); % Retrieve point cloud from view set loopView = findView(vSet,loopViewId); ptCloudOrig = loopView.PointCloud; % Process point cloud ptCloudOld = helperProcessPointCloud(ptCloudOrig); % Downsample point cloud ptCloudOld = pcdownsample(ptCloudOld,"random",downsamplePercent); % Use registration to estimate the relative pose [relTform,~,rmse] = pcregisterndt(ptCloud,ptCloudOld, ... regGridSize,"MaxIterations",50); acceptLoopClosure = rmse <= maxTolerableRMSE; if acceptLoopClosure % For simplicity, use a constant, small information matrix for % loop closure edges infoMat = 0.01 * eye(6); % Add a connection corresponding to a loop closure vSet = addConnection(vSet,loopViewId,viewId,relTform,infoMat); end end viewId = viewId + 1; ptCloudPrev = ptCloud; initTform = relTform; end
Cree un gráfico de pose a partir del conjunto de vistas utilizando el método createPoseGraph (Computer Vision Toolbox). El gráfico de pose es un objeto digraph con:
Nodos que contienen la pose absoluta de cada vista
Bordes que contienen las restricciones de pose relativas de cada conexión
G = createPoseGraph(vSet); disp(G)
digraph with properties:
Edges: [539×3 table]
Nodes: [503×2 table]
% Display view set to display loop closure detections figure hG = plot(vSet); % Update axes limits to focus on loop closure detections xlim([-50 50]); ylim([-100 50]); % Find and highlight loop closure connections loopEdgeIds = find(abs(diff(G.Edges.EndNodes,1,2)) > 1); highlight(hG,'Edges',loopEdgeIds,'EdgeColor','red','LineWidth',3) title('Loop Closure Connections')
Optimice el gráfico de pose usando optimizePoseGraph.
optimG = optimizePoseGraph(G,'g2o-levenberg-marquardt');
vSetOptim = updateView(vSet,optimG.Nodes);Muestra el conjunto de vistas con poses optimizadas. Observe que los bucles detectados ahora están fusionados, lo que da como resultado una trayectoria más precisa.
figure
plot(vSetOptim)
title('View Set Display (after optimization)')
Las poses absolutas en el conjunto de vistas optimizadas ahora se pueden usar para construir un mapa más preciso. Utilice la función pcalign (Computer Vision Toolbox) para alinear las nubes de puntos del conjunto de vista con las posiciones absolutas del conjunto de vista optimizado en un único mapa de nubes de puntos. Especifique un tamaño de cuadrícula para controlar la resolución del mapa de nube de puntos creado.
mapGridSize = 0.2; ptClouds = vSetOptim.Views.PointCloud; absPoses = vSetOptim.Views.AbsolutePose; ptCloudMap = pcalign(ptClouds,absPoses,mapGridSize); figure pcshow(ptCloudMap); % Overlay view set display hold on plot(vSetOptim); title('Point Cloud Map (after optimization)','Color','w')
Si bien la precisión aún se puede mejorar, este mapa de nube de puntos es significativamente más preciso.
Acelere el flujo de trabajo SLAM mediante la generación de código de GPU
La mayoría de las funciones de este ejemplo admiten la generación de código de GPU. Para ver un ejemplo que muestra cómo realizar SLAM Lidar 3D en una GPU NVIDIA®, consulte el siguiente ejemplo:
Build a Map from Lidar Data Using SLAM on GPU (Computer Vision Toolbox)
Lidar 3D SLAM utilizando otros algoritmos de registro
Este ejemplo utiliza pcregisterndt (Computer Vision Toolbox) para alinear nubes de puntos sucesivas. Puede consultar los siguientes ejemplos que proporcionan un enfoque alternativo para registrar nubes de puntos:
Build a Map with Lidar Odometry and Mapping (LOAM) Using Unreal Engine Simulation (Lidar Toolbox): utiliza
pcregisterloam(Lidar Toolbox) para registrar las nubes de puntos ypcmaploam(Lidar Toolbox) para refinar la alineación.Design Lidar SLAM Algorithm Using Unreal Engine Simulation Environment (Computer Vision Toolbox): utiliza
pcregistericp(Computer Vision Toolbox) para registrar las nubes de puntos yscanContextLoopDetector(Computer Vision Toolbox) para detectar cierres de bucle.Aerial Lidar SLAM Using FPFH Descriptors (Lidar Toolbox): utiliza un enfoque de detección y coincidencia de características para encontrar la posición relativa entre nubes de puntos y
pcregistericp(Computer Vision Toolbox) para refinar la alineación.
Referencias
G. Kim y A. Kim, "Escanear contexto: Descriptor espacial egocéntrico para reconocimiento de lugares dentro de un mapa de nube de puntos 3D", 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, 2018, págs. 4802-4809.
Funciones y clases de soporte
helperReadDataset lee datos de la carpeta especificada en un horario.
function datasetTable = helperReadDataset(dataFolder,pointCloudFilePattern) %helperReadDataset Read Velodyne SLAM Dataset data into a timetable % datasetTable = helperReadDataset(dataFolder) reads data from the % folder specified in dataFolder into a timetable. The function % expects data from the Velodyne SLAM Dataset. % % See also fileDatastore, helperReadINSConfigFile. % Create a file datastore to read in files in the right order fileDS = fileDatastore(pointCloudFilePattern,'ReadFcn', ... @helperReadPointCloudFromFile); % Extract the file list from the datastore pointCloudFiles = fileDS.Files; imuConfigFile = fullfile(dataFolder,'scenario1','imu.cfg'); insDataTable = helperReadINSConfigFile(imuConfigFile); % Delete the bad row from the INS config file insDataTable(1447,:) = []; % Remove columns that will not be used datasetTable = removevars(insDataTable, ... {'Num_Satellites','Latitude','Longitude','Altitude','Omega_Heading', ... 'Omega_Pitch','Omega_Roll','V_X','V_Y','V_ZDown'}); datasetTable = addvars(datasetTable,pointCloudFiles,'Before',1, ... 'NewVariableNames',"PointCloudFileName"); end
helperProcessPointCloud procesa una nube de puntos eliminando los puntos que pertenecen al plano de tierra y al vehículo ego.
function ptCloud = helperProcessPointCloud(ptCloudIn,method) %helperProcessPointCloud Process pointCloud to remove ground and ego vehicle % ptCloud = helperProcessPointCloud(ptCloudIn,method) processes % ptCloudIn by removing the ground plane and the ego vehicle. % method can be "planefit" or "rangefloodfill". % % See also pcfitplane, pointCloud/findPointsInCylinder. arguments ptCloudIn (1,1) pointCloud method string {mustBeMember(method,["planefit","rangefloodfill"])} = "rangefloodfill" end isOrganized = ~ismatrix(ptCloudIn.Location); if (method=="rangefloodfill" && isOrganized) % Segment ground using floodfill on range image groundFixedIdx = segmentGroundFromLidarData(ptCloudIn, ... "ElevationAngleDelta",11); else % Segment ground as the dominant plane with reference normal % vector pointing in positive z-direction maxDistance = 0.4; maxAngularDistance = 5; referenceVector= [0 0 1]; [~,groundFixedIdx] = pcfitplane(ptCloudIn,maxDistance, ... referenceVector,maxAngularDistance); end if isOrganized groundFixed = false(size(ptCloudIn.Location,1),size(ptCloudIn.Location,2)); else groundFixed = false(ptCloudIn.Count,1); end groundFixed(groundFixedIdx) = true; % Segment ego vehicle as points within a cylindrical region of the sensor sensorLocation = [0 0 0]; egoRadius = 3.5; egoFixed = findPointsInCylinder(ptCloudIn,egoRadius,"Center",sensorLocation); % Retain subset of point cloud without ground and ego vehicle if isOrganized indices = ~groundFixed & ~egoFixed; else indices = find(~groundFixed & ~egoFixed); end ptCloud = select(ptCloudIn,indices); end
helperComputeInitialEstimateFromINS estima una transformación inicial para el registro a partir de las lecturas del INS.
function initTform = helperComputeInitialEstimateFromINS(initTform,insData) % If no INS readings are available, return if isempty(insData) return; end % The INS readings are provided with X pointing to the front, Y to the left % and Z up. Translation below accounts for transformation into the lidar % frame. insToLidarOffset = [0 -0.79 -1.73]; % See DATAFORMAT.txt Tnow = [-insData.Y(end) insData.X(end) insData.Z(end)].' + insToLidarOffset'; Tbef = [-insData.Y(1) insData.X(1) insData.Z(1)].' + insToLidarOffset'; % Since the vehicle is expected to move along the ground, changes in roll % and pitch are minimal. Ignore changes in roll and pitch, use heading only. Rnow = rotmat(quaternion([insData.Heading(end) 0 0],'euler','ZYX','point'),'point'); Rbef = rotmat(quaternion([insData.Heading(1) 0 0],'euler','ZYX','point'),'point'); T = [Rbef Tbef; 0 0 0 1] \ [Rnow Tnow; 0 0 0 1]; initTform = rigidtform3d(T); end
