Contenido principal

Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

Construya un mapa a partir de datos Lidar usando SLAM

Desde R2024a

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

Figure Point Cloud Player contains an axes object. The axes object with title Lidar Sensor Data, xlabel X (m), ylabel Y (m) contains an object of type scatter.

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

Figure contains an axes object. The axes object with title View Set Display, xlabel X, ylabel Y contains an object of type graphplot.

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

Figure contains an axes object. The axes object with title Point Cloud Map, xlabel X, ylabel Y contains 2 objects of type scatter, graphplot.

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

Figure contains an axes object. The axes object with title Loop Closure Connections, xlabel X, ylabel Y contains an object of type graphplot.

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

Figure contains an axes object. The axes object with title View Set Display (after optimization), xlabel X, ylabel Y contains an object of type graphplot.

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

Figure contains an axes object. The axes object with title Point Cloud Map (after optimization), xlabel X, ylabel Y contains 2 objects of type scatter, graphplot.

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:

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:

Referencias

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

Consulte también

Temas