SLAM de punto de referencia usando marcadores AprilTag
Este ejemplo muestra cómo combinar datos de odometría del robot y marcadores fiduciales observados llamados AprilTags para estimar mejor la trayectoria del robot y las posiciones de los puntos de referencia en el entorno. El ejemplo utiliza un enfoque de gráfico de pose y un enfoque de gráfico de factores, y compara los dos gráficos.
Descargar conjunto de datos y cargar datos del sensor
Descargue un archivo .zip que contiene datos sin procesar registrados desde un rosbag en un ClearPath Robotics™ Jackal™. Los datos sin procesar incluyen:
Imágenes tomadas con una cámara de red Axis™ M1013 con una resolución de 640 x 480.
Datos de odometría preprocesados y sincronizados con los datos de la imagen.
Descomprima el archivo, que contiene el conjunto de datos como un archivo rosbag y como un archivo .mat . Puede utilizar la función auxiliar exampleHelperExtractDataFromRosbag , que se proporciona al final de este ejemplo, para ver cómo se extrajeron los datos del rosbag y se preprocesaron. Para utilizar la función auxiliar, debe tener una licencia de ROS Toolbox .
filename = matlab.internal.examples.downloadSupportFile("spc/robotics","apriltag_odometry_slam_dataset.zip"); unzip(filename)
En este ejemplo, se imprimió un conjunto de marcadores AprilTag y se colocó aleatoriamente en el entorno de prueba. El gráfico de pose y los gráficos de factor tratan las etiquetas como puntos de referencia, que son características distinguibles del entorno que le permiten localizar con mayor precisión. Al utilizar los elementos intrínsecos de la cámara y el tamaño de cada marcador, puede convertir las imágenes con observaciones de AprilTag en mediciones de puntos de referencia utilizando la función readAprilTag (Computer Vision Toolbox) . Estas medidas de referencia son relativas a la estructura del robot actual. Este ejemplo incluye los parámetros intrínsecos de la cámara en el archivo cameraParams.mat , pero para determinar los parámetros intrínsecos de su propia cámara, siga los pasos del ejemplo Camera Calibration Using AprilTag Markers (Computer Vision Toolbox) , o use un patrón de tablero de ajedrez con la app Camera Calibrator (Computer Vision Toolbox) . Los marcadores AprilTag utilizados en este ejemplo son de la familia "tag46h11", sin ID duplicados. El tamaño de la etiqueta impresa es de 136,65 mm.
tagFamily = "tag36h11"; tagSize = 136.65; % mm
Cargue los datos del sensor y los parámetros de la cámara en el espacio de trabajo de MATLAB® .
load apriltag_odometry_slam_dataset/apriltag_odometry_slam_dataset.mat load cameraParams.mat
Cree una estructura de datos dictionary vacía para mantener la asignación entre los ID de etiqueta y sus ID de nodo.
tagToNodeIDDic = dictionary([],[]);
Los sensores registraron los datos de odometría en el archivo .mat desde el marco de odometría hasta el marco láser que se mueve con el robot. Aplique una transformación fija entre el marco del láser y el marco de la cámara.
R1 = [0 0 1; -1 0 0; 0 -1 0]; Ta = blkdiag(R1,1); % The camera frame z-axis points forward and y-axis points down Tb = eye(4); T2(1,3) = -0.11; T(3,3) = 0.146; % Fixed translation of camera frame origin to 'laser' frame
Construya un gráfico de pose a partir de datos del sensor
Después de importar los datos del sensor sincronizado y los parámetros de la cámara, cree un gráfico de pose tridimensional de estimaciones de nodos a partir de las mediciones en los datos del sensor. El gráfico de pose contiene estimaciones de nodos, restricciones de bordes y cierres de bucles que estiman las poses del robot.
Primero crea el gráfico de pose.
pg = poseGraph3D;
Al utilizar marcadores fiduciales como AprilTags, el patrón de bloques en la imagen proporciona identificaciones únicas para cada observación de punto de referencia. Esta información de identificación reduce la necesidad de algoritmos de asociación de datos difíciles al realizar localización y mapeo simultáneos (SLAM).
Cree una variable para almacenar la pose anterior y el ID del nodo.
lastTform = []; lastPoseNodeId = 1;
Cree variables para almacenar todos los ID de los nodos de punto de referencia.
lmkIDs = [];
Este ejemplo estima la trayectoria del robot basándose en las mediciones de pose de los datos del odómetro y las mediciones de puntos de referencia de las observaciones de AprilTag. Repita los datos del sensor y siga estos pasos:
Agregue datos de odometría al gráfico de pose usando la función
addRelativePose. Calcule las poses relativas entre cada odometría antes de agregarlas al gráfico de pose.Agregue mediciones de puntos de referencia de las observaciones de AprilTag en las imágenes usando la función
readAprilTag. Debido a que una imagen puede contener varias etiquetas, repita todos los ID devueltos. Agregue puntos de referencia relativos al nodo de pose actual utilizando la funciónaddPointLandmark.Muestre la imagen con marcadores alrededor de las observaciones de AprilTag. La imagen se actualiza a medida que recorre los datos.
figure(Visible="on") for i = 1:numel(imageData) % Add odometry data to pose graph T = odomData{i}; if isempty(lastTform) nodePair = addRelativePose(pg,[0 0 0 1 0 0 0],[],lastPoseNodeId); else relPose = exampleHelperComputeRelativePose(lastTform,T); nodePair = addRelativePose(pg,relPose,[],lastPoseNodeId); end currPoseNodeId = nodePair(2); % Add landmark measurement based on AprilTag observation in the image. I = imageData{i}; [id,loc,poseRigid3d,detectedFamily] = readAprilTag(I,tagFamily,cameraParams.Intrinsics,tagSize); for j = 1:numel(id) % Insert observation markers to image. markerRadius = 6; numCorners = size(loc,1); markerPosition = [loc(:,:,j) repmat(markerRadius,numCorners,1)]; I = insertShape(I,FilledCircle=markerPosition,Color="red",Opacity=1); t = poseRigid3d(j).Translation/1000; % change from mm to meters po = [t(:); 1]; p = Tb*Ta*po; if isKey(tagToNodeIDDic,id(j)) lmkNodeId = tagToNodeIDDic(id(j)); if ~ismember(lmkNodeId,lmkIDs) lmkIDs = [lmkIDs lmkNodeId]; % store unique landmark IDs end addPointLandmark(pg,p(1:3)',[],currPoseNodeId,lmkNodeId); else nodePair = addPointLandmark(pg,p(1:3)',[],currPoseNodeId); tagToNodeIDDic(id(j)) = nodePair(2); end end % Show the image with AprilTag observation markers. imshow(I) drawnow lastTform = T; lastPoseNodeId = currPoseNodeId; end
Optimice el gráfico de pose e inspeccione los resultados
Después de construir el gráfico de pose, optimícelo usando la función optimizePoseGraph .
pgUpd = optimizePoseGraph(pg);
Visualice el gráfico de pose inicial y optimizado. El gráfico de pose optimizado muestra estas mejoras:
Se ha corregido la posición inicial del robot con respecto a los puntos de referencia.
Los puntos de referencia en cada pared están mejor alineados.
Se ha corregido la desviación vertical en la trayectoria del robot a lo largo de la dirección z.
figure(Visible="on") show(pg); axis equal xlim([-10.0 5.0]) ylim([-2.0 12.0]) title("Pose Graph Result Before Optimization XY View")
figure(Visible="on") show(pgUpd); axis equal xlim([-10.0 5.0]) ylim([-2.0 12.0]) title("Pose Graph Result After Optimization XY View")
% Vertical drift. figure(Visible="on") show(pg); axis square xlim([-10.0 5.0]) zlim([-2.0 2.0]) view(0,0) title("Pose Graph Result Before Optimization XZ View")
show(pgUpd); axis square xlim([-10.0 5.0]) zlim([-2.0 2.0]) view(0,0) title("Pose Graph Result After Optimization XZ View")
Crear gráfico de factores a partir de datos de sensores
Alternativamente, puede crear un gráfico de factores basado en las mediciones de los datos del sensor. El uso de un gráfico de factores hace que sea más fácil incluir sensores adicionales en la optimización y también optimiza más rápido que el gráfico de pose, aunque puede llevar más tiempo configurarlo y construirlo.
Cree el gráfico de factores como un objeto factorGraph .
G = factorGraph;
Cree variables para almacenar la pose anterior y la ID del nodo.
lastTform = []; lastPoseNodeId = 1; tagToNodeIDDic = dictionary([],[]);
Repita los datos del sensor y siga estos pasos para agregar factores al gráfico de factores:
Agregue datos de odometría al gráfico de factores como un factor relacionando poses usando el objeto
factorTwoPoseSE3. Calcule las poses relativas entre cada odometría antes de agregarlas al gráfico de factores.Agregue mediciones de puntos de referencia de las observaciones de AprilTag en las imágenes usando la función
readAprilTag. Debido a que la imagen puede contener varias etiquetas, repita todos los ID devueltos. Agregue puntos de referencia relativos al nodo de pose actual utilizando el objetofactorPoseSE3AndPointXYZ.Muestre la imagen con marcadores alrededor de las observaciones de AprilTag. La imagen se actualiza a medida que recorre los datos.
for i = 1:numel(imageData) % Add odometry data to factor graph T = odomData{i}; if isempty(lastTform) % Add a prior factor to loosely fix the initial node at the origin fPrior = factorPoseSE3Prior(1); addFactor(G,fPrior); newPoseNodeId = lastPoseNodeId + 1; % The measurement represents a relative pose in SE(3) between lastPoseNode and newPoseNode. fctr = factorTwoPoseSE3([lastPoseNodeId newPoseNodeId],Measurement=[0 0 0 1 0 0 0]); addFactor(G,fctr); else % Calculate relative pose between nodes of lastPoseNodeId and newPoseNodeId relPose = exampleHelperComputeRelativePose(lastTform,T); newPoseNodeId = G.NumNodes + 1; fctr = factorTwoPoseSE3([lastPoseNodeId newPoseNodeId],Measurement=relPose); addFactor(G,fctr); end currPoseNodeId = newPoseNodeId; % Add landmark measurement based on AprilTag observation in the image. I = imageData{i}; [id,loc,poseRigid3d,detectedFamily] = readAprilTag(I,tagFamily,cameraParams.Intrinsics,tagSize); for j = 1:numel(id) % Insert observation markers to image. markerRadius = 6; numCorners = size(loc,1); markerPosition = [loc(:,:,j),repmat(markerRadius,numCorners,1)]; I = insertShape(I,FilledCircle=markerPosition,Color="red",Opacity=1); t = poseRigid3d(j).Translation/1000; % change from mm to meter po = [t(:); 1]; p = Tb*Ta*po; if isKey(tagToNodeIDDic,id(j)) lmkNodeId = tagToNodeIDDic(id(j)); % The measurement represents a relative position [dx,dy,dz] between landmark point and current pose node. fctr = factorPoseSE3AndPointXYZ([currPoseNodeId lmkNodeId],Measurement=p(1:3)'); addFactor(G,fctr); else newPointNodeId = G.NumNodes + 1; fctr = factorPoseSE3AndPointXYZ([currPoseNodeId newPointNodeId],Measurement=p(1:3)'); addFactor(G,fctr); tagToNodeIDDic(id(j)) = newPointNodeId; end end % Show the image with AprilTag observation markers. imshow(I) drawnow limitrate lastTform = T; lastPoseNodeId = currPoseNodeId; end
Optimice el gráfico de factores e inspeccione los resultados
Después de construir el gráfico de factores, optimícelo usando la función de objeto optimize con opciones de solver definidas por un objeto factorGraphSolverOptions . Establezca la propiedad VerbosityLevel en 2 para mostrar más detalles del proceso de optimización.
opt = factorGraphSolverOptions(VerbosityLevel=2); soln = optimize(G,opt)
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 4.984516e+02 0.00e+00 5.76e+01 0.00e+00 0.00e+00 1.00e+04 0 2.56e-03 6.04e-03
1 8.515843e+02 -3.53e+02 5.76e+01 7.53e+01 -7.13e-01 5.00e+03 1 1.94e-02 2.70e-02
2 8.515843e+02 -3.53e+02 5.76e+01 7.53e+01 -7.13e-01 2.50e+03 0 1.06e-03 3.04e-02
3 8.515843e+02 -3.53e+02 5.76e+01 7.53e+01 -7.13e-01 1.25e+03 0 6.45e-04 3.26e-02
4 8.515843e+02 -3.53e+02 5.76e+01 7.53e+01 -7.13e-01 6.25e+02 0 7.57e-04 3.54e-02
5 8.515843e+02 -3.53e+02 5.76e+01 7.53e+01 -7.13e-01 3.12e+02 0 7.01e-04 3.78e-02
6 8.515843e+02 -3.53e+02 5.76e+01 7.53e+01 -7.13e-01 1.56e+02 0 6.11e-04 3.96e-02
7 8.515843e+02 -3.53e+02 5.76e+01 7.53e+01 -7.13e-01 7.81e+01 0 6.51e-04 4.14e-02
8 5.087687e+02 -1.03e+01 5.76e+01 4.26e+01 -2.13e-02 3.91e+01 0 7.57e-04 4.35e-02
9 1.365586e+02 3.62e+02 3.27e+01 1.99e+01 7.77e-01 1.17e+02 0 3.05e-03 4.68e-02
10 4.583660e+02 -3.22e+02 3.27e+01 6.72e+01 -2.40e+00 5.86e+01 1 9.06e-03 5.62e-02
11 5.329830e+01 8.33e+01 1.94e+01 3.35e+01 6.64e-01 5.86e+01 0 4.06e-03 6.06e-02
12 5.142921e+01 1.87e+00 1.91e+01 3.05e+01 3.72e-02 2.93e+01 1 1.44e-02 7.54e-02
13 8.811991e+00 4.26e+01 4.56e+00 1.74e+01 9.59e-01 8.79e+01 1 1.11e-02 8.68e-02
14 1.032690e+02 -9.45e+01 4.56e+00 5.11e+01 -1.23e+01 4.39e+01 1 1.75e-02 1.05e-01
15 9.869378e+00 -1.06e+00 4.56e+00 2.55e+01 -1.63e-01 2.20e+01 0 9.05e-04 1.06e-01
16 3.688774e+00 5.12e+00 2.19e+00 1.28e+01 9.18e-01 6.59e+01 0 2.74e-03 1.09e-01
17 3.409290e+01 -3.04e+01 2.19e+00 3.84e+01 -1.16e+01 3.30e+01 1 8.06e-03 1.17e-01
18 4.290375e+00 -6.02e-01 2.19e+00 1.92e+01 -3.01e-01 1.65e+01 0 6.80e-04 1.18e-01
19 2.344156e+00 1.34e+00 1.79e+00 9.61e+00 8.38e-01 4.94e+01 0 2.68e-03 1.21e-01
20 1.153071e+01 -9.19e+00 1.79e+00 2.88e+01 -6.83e+00 2.47e+01 1 8.27e-03 1.30e-01
21 2.301715e+00 4.24e-02 3.62e+00 1.44e+01 4.43e-02 1.24e+01 0 2.67e-03 1.33e-01
22 1.395292e+00 9.06e-01 1.65e+00 7.21e+00 8.84e-01 3.71e+01 1 1.27e-02 1.46e-01
23 2.835470e+00 -1.44e+00 1.65e+00 2.17e+01 -1.95e+00 1.85e+01 1 1.05e-02 1.59e-01
24 1.118728e+00 2.77e-01 1.76e+00 1.08e+01 5.51e-01 1.85e+01 0 3.10e-03 1.64e-01
25 7.683906e-01 3.50e-01 1.50e+00 1.09e+01 7.36e-01 1.85e+01 1 1.11e-02 1.77e-01
26 5.116551e-01 2.57e-01 9.30e-01 1.10e+01 7.67e-01 5.56e+01 1 1.31e-02 1.92e-01
27 3.957137e+00 -3.45e+00 9.30e-01 3.31e+01 -8.79e+00 2.78e+01 1 1.04e-02 2.04e-01
28 4.955209e-01 1.61e-02 1.77e+00 1.65e+01 5.60e-02 1.39e+01 0 2.89e-03 2.08e-01
29 2.135810e-01 2.82e-01 7.81e-01 8.27e+00 8.94e-01 4.17e+01 1 1.15e-02 2.21e-01
30 1.599750e+00 -1.39e+00 7.81e-01 2.17e+01 -9.94e+00 2.09e+01 1 1.26e-02 2.35e-01
31 2.592216e-01 -4.56e-02 7.81e-01 1.23e+01 -3.75e-01 1.04e+01 0 1.46e-03 2.39e-01
32 1.358879e-01 7.77e-02 2.36e-01 6.16e+00 8.56e-01 3.13e+01 0 4.65e-03 2.44e-01
33 8.608814e-01 -7.25e-01 2.36e-01 1.57e+01 -1.11e+01 1.56e+01 1 8.79e-03 2.53e-01
34 1.788477e-01 -4.30e-02 2.36e-01 9.13e+00 -7.68e-01 7.82e+00 0 5.21e-04 2.54e-01
35 1.064787e-01 2.94e-02 3.08e-01 4.56e+00 7.69e-01 2.35e+01 0 2.57e-03 2.56e-01
36 4.251862e-01 -3.19e-01 3.08e-01 1.12e+01 -8.44e+00 1.17e+01 1 1.49e-02 2.71e-01
37 1.229167e-01 -1.64e-02 3.08e-01 6.74e+00 -4.99e-01 5.87e+00 0 5.38e-04 2.72e-01
38 8.755732e-02 1.89e-02 1.78e-01 3.37e+00 8.22e-01 1.76e+01 0 4.46e-03 2.77e-01
39 2.110173e-01 -1.23e-01 1.78e-01 8.08e+00 -6.32e+00 8.80e+00 1 1.31e-02 2.91e-01
40 9.263937e-02 -5.08e-03 1.78e-01 4.98e+00 -2.96e-01 4.40e+00 0 1.05e-03 2.92e-01
41 7.771281e-02 9.84e-03 1.22e-01 2.49e+00 8.35e-01 1.32e+01 0 4.50e-03 2.97e-01
42 1.160680e-01 -3.84e-02 1.22e-01 5.86e+00 -3.85e+00 6.60e+00 1 1.26e-02 3.10e-01
43 7.727460e-02 4.38e-04 3.67e-01 3.70e+00 4.96e-02 3.30e+00 0 4.57e-03 3.15e-01
44 6.863371e-02 8.64e-03 1.28e-01 1.90e+00 9.32e-01 9.90e+00 1 1.67e-02 3.32e-01
45 6.766036e-02 9.73e-04 2.30e-02 1.25e+00 9.83e-01 9.90e+00 1 1.34e-02 3.46e-01
46 6.764183e-02 1.85e-05 8.05e-05 9.00e-02 1.00e+00 9.90e+00 1 1.26e-02 3.62e-01
Terminating: Function tolerance reached. |cost_change|/cost: 1.078674e-07 <= 1.000000e-06
Solver Summary (v 2.0.0-eigen-(3.3.4)-no_lapack-eigensparse-no_openmp-no_custom_blas)
Original Reduced
Parameter blocks 585 585
Parameters 4039 4039
Effective parameters 3468 3468
Residual blocks 804 804
Residuals 4125 4125
Minimizer TRUST_REGION
Sparse linear algebra library EIGEN_SPARSE
Trust region strategy DOGLEG (TRADITIONAL)
Given Used
Linear solver SPARSE_NORMAL_CHOLESKY SPARSE_NORMAL_CHOLESKY
Threads 1 1
Linear solver ordering AUTOMATIC 585
Cost:
Initial 4.984516e+02
Final 6.764183e-02
Change 4.983839e+02
Minimizer iterations 47
Successful steps 22
Unsuccessful steps 25
Time (in seconds):
Preprocessor 0.003480
Residual only evaluation 0.021286 (47)
Jacobian & residual evaluation 0.054238 (22)
Linear solver 0.219521 (22)
Minimizer 0.371506
Postprocessor 0.000229
Total 0.375216
Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 1.078674e-07 <= 1.000000e-06)
soln = struct with fields:
InitialCost: 498.4516
FinalCost: 0.0676
NumSuccessfulSteps: 22
NumUnsuccessfulSteps: 25
TotalTime: 0.3752
TerminationType: 0
IsSolutionUsable: 1
OptimizedNodeIDs: [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 … ]
FixedNodeIDs: [1×0 double]
La información de la solución devuelta indica que la función de objeto optimize ha reducido en gran medida el coste de la ruta, lo que indica que el proceso de optimización ha mejorado la ruta. El valor IsSolutionUsable de 1 también indica que la solución es viable. Si optimize no devuelve resultados convergentes, considere ajustar las opciones del solver , como el número de iteraciones máximas, dependiendo de los cost_change y gradient. Valorespara cada paso.
Recupere los resultados optimizados y visualice la trayectoria del robot en azul y los puntos de referencia en verde.
fgNodeStates = exampleHelperShowFactorGraphResult(G);
Conclusión
Estas imágenes muestran las trayectorias finales del robot, generadas por el gráfico de pose y el gráfico de factores, superpuestas en el plano del área de la oficina mediante una transformación para mostrar la precisión de la trayectoria estimada y las ubicaciones de los puntos de referencia estimados. Si reproduce las imágenes nuevamente, podrá ver una fuerte correlación entre el gráfico de pose y el gráfico de factor. Todos los puntos de referencia verdes están en las paredes o cerca de ellas, y las dos líneas ajustadas basadas en los puntos de referencia detectados se superponen con las paredes del pasillo. Sin embargo, aunque estas dos soluciones son visualmente similares, podemos compararlas numéricamente.
Estas imágenes muestran las posiciones de AprilTags para su referencia. Tenga en cuenta que hay un AprilTag que no es captado por la cámara, marcado con un círculo rojo.
Para comparar numéricamente la diferencia entre los resultados del gráfico de pose y el gráfico de factor, recupere los estados de los nodos del gráfico de pose actualizado y calcule la diferencia absoluta promedio para las posiciones del robot, los ángulos de rotación y las posiciones de los puntos de referencia entre este y el gráfico de factor.
pgUpdNodeStates = nodeEstimates(pgUpd); [robotPosDiff,robotRotDiff,lmkDiff] = exampleHelperComputeDifference(pgUpdNodeStates,fgNodeStates,lmkIDs)
robotPosDiff = 1×3
10-3 ×
0.0524 0.0924 0.9312
robotRotDiff = 1×3
10-3 ×
0.0198 0.1625 0.1355
lmkDiff = 1×3
10-3 ×
0.0562 0.0865 0.8646
Tenga en cuenta que, para comparar mejor con el resultado del gráfico de pose, especificó una estimación inicial para la pose del nodo inicial del gráfico de factores en el origen utilizando un objeto factorPoseSE3Prior . Sin el factor anterior, el proceso de optimización del gráfico de factores da como resultado un gráfico que tiene poses y posiciones relativas óptimas, pero las poses y posiciones absolutas no son necesariamente correctas. La diferencia absoluta promedio es bastante pequeña, lo que significa que ambos métodos brindan resultados optimizados similares. En comparación con el gráfico de pose, el tiempo de optimización del gráfico de factores es mucho más corto, pero lleva más tiempo configurar y construir el gráfico de factores.
You can also select a web site from the following list:
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)