Descargo de responsabilidad: este artículo es un buen artículo que el autor encontró mientras aprendía SLAM y lo compartiré con usted. Reimpreso
de: https://www.cnblogs.com/wellp/p/8877990.html
Guía
Lo siguiente es mi comprensión del documento LOAM y la lectura del código fuente de A-LOAM (la versión comentada en chino de A-LOAM se envió a github, consulte A-LOAM-NOTED), y finalmente impulsaré el Gauss- El método de Newton en el código fuente LOAM (El documento habla sobre el método LM) para resolver el problema de los mínimos cuadrados no lineales del registro ICP. Esta parte se simplifica con la biblioteca de funciones de optimización de Ceres en el código A-LOAM. CSDN también tiene un buen blog publicación, y Zhihu también tiene una muy buena. Dale me gusta al artículo, si tienes alguna pregunta, bienvenido a discutir en el área de comentarios.
El dibujo de palabras clave no es fácil, indique la fuente para la reimpresión: https://www.cnblogs.com/wellp/p/8877990.html
1. Lectura en papel
-
Resumen
LOAM es un método de cálculo de kilometraje láser sin detección de circuito cerrado, por lo que no agrega un marco de optimización de gráficos. Este algoritmo divide el problema SLAM en dos algoritmos para ejecutar en paralelo: un algoritmo de odometría, 10 Hz; el otro algoritmo de mapeo, 1 Hz. , y finalmente los dos La salida de la pose por parte del operador está integrada para lograr una salida en tiempo real de la pose de 10Hz. Ambos algoritmos usan la nube de puntos para extraer puntos de bordes afilados y puntos de superficies planas como puntos característicos, y luego realizan coincidencias de puntos característicos para estimar la posición de LIDAR y ajustar la posición. Al hacer coincidir puntos característicos, no se trata simplemente de punto a punto, sino de punto de borde a línea de borde (punto a línea) y de punto plano a parche plano (punto a superficie). La distancia residual establecida en la estimación de movimiento posterior es la distancia punto a línea y la distancia punto a superficie, por lo que siento que el proceso de coincidencia LOAM es como un algoritmo ICP que combina punto a línea y punto a superficie; cabe señalar que la línea de borde y el parche plano de los algoritmos de odometría y mapeo (es decir, al hacer coincidir la determinación del objetivo) es ligeramente diferente, consulte la introducción del tercer resumen para obtener más detalles. -
Algoritmo de odometría
1) La entrada ha eliminado la distorsión (la llamada eliminación de la distorsión también se interpreta como compensación de movimiento, que consiste en mapear todos los puntos en la nube de puntos Pk al mismo tiempo tk+1, eliminando la causada por el movimiento del vehículo y rotación lidar, distorsión de la nube de puntos), la nube de puntos del último escaneo, la nube de puntos observada gradualmente Pk+1 durante este escaneo, y el incremento entre poses en el tiempo [tk+1, t] (también interpretado como transformación de pose entre) ;
2) Si es un nuevo escaneo, que sea igual a 0;
3) Luego extraiga los puntos del borde y los puntos del plano de la nube de puntos Pk+1 como puntos característicos para formar un conjunto de puntos característicos del borde y un conjunto de puntos característicos de la superficie, respectivamente;
4) Para cada punto del borde y punto del plano en el conjunto, encontramos el borde correspondiente y el plano correspondiente, y luego construimos las ecuaciones residuales de distancia de punto a línea y punto a superficie de acuerdo con la fórmula (11), y luego estimamos el movimiento de Lidar Entre ellos, la variable de optimización es que el objetivo de optimización es la distancia residual, es decir, encontrar una óptima, de modo que la distancia del punto a la línea y del punto a la superficie sea la más pequeña;
5) A cada ecuación residual se le asigna un peso. Cuanto mayor sea la distancia desde el punto característico hasta el borde correspondiente (o la superficie correspondiente), menor será el peso asignado. Si la distancia es mayor que un cierto umbral, el peso se establece en 0;
6) Usar el método LM (el método de Gauss-Newton se usa en el código fuente) para resolver este problema de mínimos cuadrados no lineales y obtenerlo para que pueda actualizarse;
7) Cuando la optimización no lineal converge o el número de iteraciones alcanza el valor preestablecido, romper;
8) Si el algoritmo de odometría ha llegado al final de un escaneo, asigne todos los puntos de Pk+1 a tk+2 para obtener la compensación de movimiento de la nube de puntos Pk+1; de lo contrario, solo emita el actualizado, como Prepare for the próximo algoritmo de odometría.
El pseudocódigo en el documento es el siguiente:
- Algoritmo de mapeo
1) El algoritmo de mapeo consiste en convertir primero la nube de puntos que se ha distorsionado en el sistema de coordenadas global y luego combinarlo con el mapa local (mapa local o submapa, el código fuente utiliza la gestión del mapa local de los tres cubo de cuadrícula dimensional) Haga coincidir características y agréguele la nube de puntos registrada. Una vez que exceda el rango preestablecido, elimine la nube de puntos fuera del rango. El rango del submapa en el código fuente es 250m * 250m * 150m. Debería tenga en cuenta que el código fuente es para La gestión del submapa no se basa en el método de ventana deslizante de tiempo, sino en el método de división de rango espacial, por lo que el submapa correspondiente al último cuadro contendrá los primeros cuadros:
Este método de submapa contendrá más información de marco histórico que el método de ventana deslizante basado en el tiempo, y es más amigable para el registro de nubes de puntos Por supuesto, esto también conduce a un aumento en el uso de la memoria;
2) En el algoritmo de mapeo, el método para extraer puntos característicos es el mismo que antes, es decir, juzgar si un punto es un punto de borde o un punto de superficie según el valor de curvatura, pero el número de puntos característicos es 10 veces mayor. del algoritmo de odometría, específicamente ajustando la comparación de la implementación del Umbral del valor de curvatura, el código fuente en realidad no es 10 veces, de todos modos, extraerá más puntos característicos que el algoritmo de odometría;
3) Luego está el emparejamiento, es decir, encontrar la correspondencia de los puntos característicos en Qk. En el algoritmo de odometría, la determinación de la correspondencia es para la velocidad de cálculo más rápida. Utiliza la idea del vecino más cercano para encontrar la línea correspondiente y la superficie correspondiente. Específicamente,: Los dos vecinos más cercanos forman la línea de borde correspondiente, y los tres vecinos más cercanos forman el parche plano correspondiente; y el algoritmo de mapeo usa los grupos de nubes de puntos alrededor de los puntos característicos (el código fuente usa cinco vecinos más cercanos apuntan como un grupo de nubes de puntos) para realizar un análisis de componentes principales PCA (encontrar los valores propios y los vectores propios de la matriz de covarianza del grupo de nubes de puntos) para determinar el vector de dirección de la línea de borde correspondiente y el vector normal de la correspondiente parche plano. Una vez que se determina la correspondencia, el proceso de solución de optimización es el mismo que antes, y se realiza el registro en el mapa local, para lograr el propósito de ajustar la pose calculada por el algoritmo de odometría.
- proceso general del sistema
Aquí hablamos principalmente sobre el proceso de integración de la salida de pose de 1 Hz por mapeo y la salida de pose de 10 Hz por odometría: cuando el mapeo calcula un Pmap de pose, calcula el incremento △P = Pmap con una pose de odometría Podom sincronizada con su tiempo * Podom- 1, luego use △P para corregir la pose de odometría de alta frecuencia subsiguiente, la pose de alta frecuencia de 10 Hz corregida es la pose en tiempo real finalmente emitida por LOAM, y luego repita el proceso anterior hasta que el algoritmo de mapeo calcule una pose.
2. Lectura del código fuente de A-LOAM
-
Diagrama de nodos
Primero coloque un diagrama de nodos ros cuando ejecute A-LOAM, la estructura general es tan clara como el código. -
Descripción general del código
El código de A-LOAM es realmente claro y conciso, principalmente porque se utiliza la biblioteca de funciones de Ceres en lugar de la parte de la solución de optimización de ICP impulsada por Zhang Ji (se utiliza la derivación automática de Ceres en lugar de la función de optimización manual). Derivación analítica, la eficiencia será menor). El directorio de código completo es el siguiente:El directorio docker proporciona un entorno docker, que es conveniente para que los desarrolladores construyan un entorno.
El directorio include contiene un archivo de encabezado general simple y un TicToc similar a un temporizador. La unidad de tiempo es el
archivo de inicio del directorio ms launch ros
, y el directorio de salida compilado reservado para el directorio de imágenes., la imagen en README.md
directorio rviz_cfg archivo de configuración rviz
código principal del directorio src: 4 cpps corresponden a 4 nodos en 1. gráfico de nodo respectivamente, 1 hpp es cada funtor utilizado al construir el residuo función basada en Ceres Class (¿Se llama Functor, más intuitivo?)
3.kittiHelper.cpp
1) Nodo correspondiente: /kittiHelper
2) Función: lea los datos del conjunto de datos de odometría kitti, incluida la nube de puntos, las imágenes de las cámaras izquierda y derecha y la verdad básica de la pose (solo disponible en el conjunto de entrenamiento), y luego divídalo en 5 temas y publíquelo a una frecuencia de 10 Hz (modificable).Entre ellos, el tema que es realmente útil para el algoritmo es solo nube de puntos/velodyne_points, y los otros cuatro temas se visualizan en rviz.
3) Análisis de código: esta parte del código es principalmente para la liberación de mensajes ros. Cabe señalar que el sistema de coordenadas de la pose de valor real del conjunto de entrenamiento kitti es diferente del sistema de coordenadas de la nube de puntos. Como se muestra en la figura siguiente, coordenadas A-LOAM Ambos sistemas utilizan el sistema de coordenadas de la nube de puntos, por lo que es necesario transformar las coordenadas de la verdadera pose y transferirlas al sistema de coordenadas de la nube de puntos; el sistema de coordenadas de la verdadera pose es en realidad el sistema de coordenadas de la cámara izquierda.
A-LOAM también tiene un error de transformación de coordenadas aquí, como se muestra a continuación. Por supuesto, este error solo afecta la postura al visualizar la trayectoria real y no afecta el funcionamiento del algoritmo posterior. De hecho, debería ser más preciso para use la izquierda Los parámetros de calibración de la cámara y Lidar se transforman;
Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>());
// Eigen::Quaterniond q = q_transform * q_w_i;//* q_transform.inverse() debe agregarse aquí, como se muestra a continuación, que en realidad es más preciso Los parámetros de calibración de la cámara izquierda a Lidar proporcionados por KITTI deben usarse para transformar
Eigen::Quaterniond q = q_transform * q_w_i *q_transform.inverse();
q.normalize();
Eigen::Vector3d t = q_transform * gt_pose .topRightCorner<3 , 1>();
4.scanRegistration.cpp
1) Nodo correspondiente: /ascanRegistration
2) Función: filtre la nube de puntos de entrada y extraiga 4 características, a saber, características de puntos de borde nítidas, menos nítidas, características de superficie planas, menos planas
3) Análisis de código:
La función principal es principalmente suscribirse al tema de la nube de puntos publicado por el nodo anterior. Una vez que se recibe un marco de nube de puntos, la función de devolución de llamada laserCloudHandler se ejecutará una vez, así que concéntrese en esta función de devolución de llamada.
// Primero filtre la nube de puntos, elimine los puntos no válidos con valores NaN y los puntos dentro de la distancia MINIMUM_RANGE desde el origen del sistema de coordenadas Lidar pcl::removeNaNFromPointCloud(
laserCloudIn, laserCloudIn, indices);
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
(Suponga que 64 La resolución angular horizontal de la línea Lidar es de 0,2 grados, por lo que teóricamente cada exploración tiene 360/0,2 = 1800 puntos. Para facilitar la descripción, llamamos a la ID de cada punto de exploración fireID, es decir, fireID [0~ 1799] y el scanID correspondiente [0~63])
A continuación, calcule el scanID y fireID de la nube de puntos a través del ángulo de elevación y el ángulo horizontal del punto en el sistema de coordenadas Lidar. Cabe señalar que el método de cálculo aquí no es aplicable a la nube de puntos del conjunto de datos kitti, porque el la nube de puntos de kitti ha sido compensada por movimiento Aprobado.
point.intensity = scanID + scanPeriod * relTime;// La parte entera del campo de intensidad almacena el scanID, y la parte decimal almacena el fireID normalizado
laserCloudScans[scanID].push_back(point); // Coloca el punto en el hijo correspondiente de acuerdo con el scanID En la nube de puntos laserCloudScans,
la nube de subpuntos laserCloudScans luego se fusionan en una nube de puntos de marco laserCloud Tenga en cuenta que la nube de puntos de un solo marco laserCloud aquí puede considerarse como una nube de puntos ordenada y almacenarse en el orden de scanID y fireID
Copie el código
para (int i = 0; i < N_SCANS; i++)
{ scanStartInd[i] = laserCloud->size() + 5;// Registre el índice de inicio de cada escaneo, ignorando los primeros 5 puntos *laserCloud += laserCloudScans [i]; scanEndInd[i] = laserCloud->size() - 6;// Registra el índice final de cada escaneo, ignorando los últimos 5 puntos, la nube de puntos al principio y al final tiende a producir "costuras" no cerradas, lo cual no es bueno para extraer características de borde } Copie el código A continuación, calcule la curvatura del punto
复制代码
for (int i = 5; i < cloudSize - 5; i++)
{ float diffX = laserCloud->puntos[i - 5].x + laserCloud->puntos[i - 4].x + laserCloud->puntos[ i - 3].x + laserCloud->puntos[i - 2].x + laserCloud->puntos[i - 1].x - 10 * laserCloud->puntos[i].x + laserCloud->puntos[i + 1].x + laserCloud->puntos[i + 2].x + laserCloud->puntos[i + 3].x + laserCloud->puntos[i + 4].x + laserCloud->puntos[i + 5] .X; float diffY = laserCloud->puntos[i - 5].y + laserCloud->puntos[i - 4].y + laserCloud->puntos[i - 3].y + laserCloud->puntos[i - 2].y + laserCloud->puntos[i - 1].y - 10 * laserCloud->puntos[i].y + laserCloud->puntos[i + 1].y + laserCloud->puntos[i + 2].y + laserCloud ->puntos[i + 3].y + laserCloud->puntos[i + 4].y + laserCloud->puntos[i + 5].y;
float diffZ = laserCloud->puntos[i - 5].z + laserCloud->puntos[i - 4].z + laserCloud->puntos[i - 3].z + laserCloud->puntos[i - 2].z + laserCloud->puntos[i - 1].z - 10 * laserCloud->puntos[i].z + laserCloud->puntos[i + 1].z + laserCloud->puntos[i + 2].z + laserCloud ->puntos[i + 3].z + laserCloud->puntos[i + 4].z + laserCloud->puntos[i + 5].z;
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudSortInd[i] = i;
cloudNeighborPicked[i] = 0;// 点有没有被选选择为feature点
cloudLabel[i] = 0;// Label 2: corner_sharp
// Label 1: corner_less_sharp, 包含Label 2
// Label -1: surf_flat
// Label 0: surf_less_flat， 包含Label -1，因为点太多，最后会降采样
}
Copie el código
El siguiente bucle for grande es para calcular 4 tipos de puntos característicos de acuerdo con la curvatura
Copie el código
para (int i = 0; i < N_SCANS; i++)// Extraiga 4 tipos de puntos característicos según el orden de escaneo {
if (scanEndInd[i] - scanStartInd[i] < 6)// Si el número de los puntos de escaneo son menos de 7 puntos, solo omita continuar; pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud); for (int j = 0; j < 6; j++)// Divide el escaneo en 6 segmentos para realizar la detección de características { int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;// índice de inicio del subescaneado int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[ i]) * (j + 1) / 6 - 1;// finaliza el índice del subescaneado
TicToc t_tmp;
std::sort(cloudSortInd + sp, cloudSortInd + ep + 1, comp);// 根据曲率有小到大对subscan的点进行sort
t_q_sort += t_tmp.toc();
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--)// 从后往前，即从曲率大的点开始提取corner feature
{
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1)// 如果该点没有被选择过，并且曲率大于0.1
{
largestPickedNum++;
if (largestPickedNum <= 2)// 该subscan中曲率最大的前2个点认为是corner_sharp特征点
{
cloudLabel[ind] = 2;
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else if (largestPickedNum <= 20)// 该subscan中曲率最大的前20个点认为是corner_less_sharp特征点
{
cloudLabel[ind] = 1;
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else
{
break;
}
cloudNeighborPicked[ind] = 1;// 标记该点被选择过了
// 与当前点距离的平方 <= 0.05的点标记为选择过，避免特征点密集分布
for (int l = 1; l <= 5; l++)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
// 提取surf平面feature，与上述类似，选取该subscan曲率最小的前4个点为surf_flat
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++)
{
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < 0.1)
{
cloudLabel[ind] = -1;
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
if (smallestPickedNum >= 4)
{
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
// 其他的非corner特征点与surf_flat特征点一起组成surf_less_flat特征点
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0)
{
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
}
// 最后对该scan点云中提取的所有surf_less_flat特征点进行降采样，因为点太多了
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
Copie el código.
Finalmente, se publican los cuatro puntos característicos extraídos de la nube de puntos del marco actual y la nube de puntos del marco actual filtrada.
5.laserOdometry.cpp
1) Nodo correspondiente: /alaserOdometry
2) Función: en base a las cuatro características mencionadas anteriormente, se realiza el registro de características de nube de puntos entre fotogramas, es decir, Odometría Lidar simple
3) Análisis de código:
a. Primero aclarar el significado de varias variables clave
Copiar código
// La pose P del cuadro estimado por el hilo Lidar Odometry en el sistema de coordenadas mundiales, Transformación del cuadro actual al cuadro mundial
Eigen::Quaterniond q_w_curr(1, 0, 0, 0);
Eigen::Vector3d t_w_curr( 0, 0, 0);
// Variables de optimización para coincidencia de características de nube de puntos
double para_q[4] = {0, 0, 0, 1};
double para_t[3] = {0, 0, 0};
// Los dos siguientes son los mapeos de las variables de optimización para_q y para_t: representan el incremento entre la pose P en los dos sistemas de coordenadas mundiales, por ejemplo, △P = P0.inverse() * P1 Eigen:: MapEigen
:: Quaterniond q_last_curr(para_q);
Eigen::MapEigen::Vector3d t_last_curr(para_t);
Copie el código
b. Permítame presentar algunas funciones antes de la función principal
TransformToStart: transforma la nube de puntos en el sistema de coordenadas Lidar del cuadro actual al sistema de coordenadas Lidar del cuadro anterior (es decir, la pose inicial del cuadro actual, la pose inicial, por lo que el nombre de la función es TransformToStart), porque el La nube de puntos de kitti se eliminó Distorsión, por lo que ya no se considera la compensación de movimiento. (Si la nube de puntos no elimina la distorsión, use la diferencia de slerp para calcular la pose de cada punto en el momento del disparo y luego realice la transformación de coordenadas de TransformToStart. Por un lado, realiza la transformación al sistema de coordenadas Lidar del cuadro anterior; por otro lado, también se puede entender que todos los puntos unifican el momento de fuego al momento de inicio, es decir, se elimina la distorsión y se completa la compensación de movimiento)
Las siguientes cinco funciones de controlador son funciones de devolución de llamada que aceptan los cinco temas ascendentes y su función es almacenar en caché el mensaje en la cola correspondiente para su procesamiento posterior.
C. Luego está la función principal, que es la parte central del código del subproceso Odometry:
Copie el código
si (!systemInited)// El primer marco no coincide, simplemente guarde cornerPointsLessSharp en laserCloudCornerLast
// Guarde surfPointsLessFlat en laserCloudSurfLast y actualice el kdtree correspondiente
// Proporcione el objetivo para la siguiente coincidencia
{ systemInited = true; std: :cout < < "Inicialización finalizada \n"; } Una clave grande para bucle después de copiar el código :
for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)// ICP punto a línea y punto a superficie, iterar 2 veces { ... } Luego introduzca el contenido
de este ciclo for, y no introduzca el uso de Ceres Ahora, puede consultar la documentación de la biblioteca de funciones de Ceres, o el ejemplo de Ceres en "Catorce conferencias sobre SLAM visual" de Gao Xiang. Tenga en cuenta que la variable de optimización es el incremento para_q y para_t de la pose P en los dos sistemas de coordenadas mundiales
Copie el código
para (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)// ICP punto a línea y punto a plano, iterar 2 veces {
corner_correspondence = 0; plane_correspondence = 0;
//ceres::LossFunction *loss_function = NULL;
ceres::LossFunction* loss_function = new ceres::HuberLoss(0.1);
ceres::LocalParameterization* q_parameterization =
new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
problem.AddParameterBlock(para_q, 4, q_parameterization);
problem.AddParameterBlock(para_t, 3);
pcl::PointXYZI pointSel;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
TicToc t_data;
// 基于最近邻（只找2个最近邻点）原理建立corner特征点之间关联，find correspondence for corner features
for (int i = 0; i < cornerPointsSharpNum; ++i)
{
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);// 将当前帧的corner_sharp特征点O_cur，从当前帧的Lidar坐标系下变换到上一帧的Lidar坐标系下（记为点O，注意与前面的点O_cur不同），以利于寻找corner特征点的correspondence
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);// kdtree中的点云是上一帧的corner_less_sharp，所以这是在上一帧
// 的corner_less_sharp中寻找当前帧corner_sharp特征点O的最近邻点（记为A）
int closestPointInd = -1, minPointInd2 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)// 如果最近邻的corner特征点之间距离平方小于阈值，则最近邻点A有效
{
closestPointInd = pointSearchInd[0];
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
// 寻找点O的另外一个最近邻的点（记为点B） in the direction of increasing scan line
for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)// laserCloudCornerLast 来自上一帧的corner_less_sharp特征点,由于提取特征时是
{ // 按照scan的顺序提取的，所以laserCloudCornerLast中的点也是按照scanID递增的顺序存放的
// if in the same scan line, continue
if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)// intensity整数部分存放的是scanID
continue;
// if not in nearby scans, end the loop
if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
break;
double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
if (pointSqDis < minPointSqDis2)// 第二个最近邻点有效，更新点B
{
// find nearer point
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
// 寻找点O的另外一个最近邻的点B in the direction of decreasing scan line
for (int j = closestPointInd - 1; j >= 0; --j)
{
// if in the same scan line, continue
if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
continue;
// if not in nearby scans, end the loop
if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
break;
double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
if (pointSqDis < minPointSqDis2)// 第二个最近邻点有效，更新点B
{
// find nearer point
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
{ // 即特征点O的两个最近邻点A和B都有效
Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,
cornerPointsSharp->points[i].y,
cornerPointsSharp->points[i].z);
Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,
laserCloudCornerLast->points[closestPointInd].y,
laserCloudCornerLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,
laserCloudCornerLast->points[minPointInd2].y,
laserCloudCornerLast->points[minPointInd2].z);
double s;// 运动补偿系数，kitti数据集的点云已经被补偿过，所以s = 1.0
if (DISTORTION)
s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;
else
s = 1.0;
// 用点O，A，B构造点到线的距离的残差项，注意这三个点都是在上一帧的Lidar坐标系下，即，残差 = 点O到直线AB的距离
// 具体到介绍lidarFactor.cpp时再说明该残差的具体计算方法
ceres::CostFunction* cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
corner_correspondence++;
}
}
// 下面说的点符号与上述相同
// 与上面的建立corner特征点之间的关联类似，寻找平面特征点O的最近邻点ABC（只找3个最近邻点），即基于最近邻原理建立surf特征点之间的关联，find correspondence for plane features
for (int i = 0; i < surfPointsFlatNum; ++i)
{
TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)// 找到的最近邻点A有效
{
closestPointInd = pointSearchInd[0];
// get closest point's scan ID
int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;
// search in the direction of increasing scan line
for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
{
// if not in nearby scans, end the loop
if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
break;
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
// if in the same or lower scan line
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;// 找到的第2个最近邻点有效，更新点B，注意如果scanID准确的话，一般点A和点B的scanID相同
minPointInd2 = j;
}
// if in the higher scan line
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;// 找到的第3个最近邻点有效，更新点C，注意如果scanID准确的话，一般点A和点B的scanID相同,且与点C的scanID不同，与LOAM的paper叙述一致
minPointInd3 = j;
}
}
// search in the direction of decreasing scan line
for (int j = closestPointInd - 1; j >= 0; --j)
{
// if not in nearby scans, end the loop
if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
break;
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
// if in the same or higher scan line
if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)
{
// find nearer point
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
if (minPointInd2 >= 0 && minPointInd3 >= 0)// 如果三个最近邻点都有效
{
Eigen::Vector3d curr_point(surfPointsFlat->points[i].x,
surfPointsFlat->points[i].y,
surfPointsFlat->points[i].z);
Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,
laserCloudSurfLast->points[closestPointInd].y,
laserCloudSurfLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,
laserCloudSurfLast->points[minPointInd2].y,
laserCloudSurfLast->points[minPointInd2].z);
Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,
laserCloudSurfLast->points[minPointInd3].y,
laserCloudSurfLast->points[minPointInd3].z);
double s;
if (DISTORTION)
s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;
else
s = 1.0;
// 用点O，A，B，C构造点到面的距离的残差项，注意这三个点都是在上一帧的Lidar坐标系下，即，残差 = 点O到平面ABC的距离
// 同样的，具体到介绍lidarFactor.cpp时再说明该残差的具体计算方法
ceres::CostFunction* cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
plane_correspondence++;
}
}
}
printf("data association time %f ms \n", t_data.toc());
if ((corner_correspondence + plane_correspondence) < 10)
{
printf("less correspondence! *************************************************\n");
}
TicToc t_solver;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
// 基于构建的所有残差项，求解最优的当前帧位姿与上一帧位姿的位姿增量：para_q和para_t
ceres::Solve(options, &problem, &summary);
printf("solver time %f ms \n", t_solver.toc());
}
printf("optimización dos veces %f \n", t_opt.toc()); Después
de copiar el código
y pasar por dos registros de nube de puntos ICP punto a línea y punto a plano, el incremento de pose óptimo para_q y para_t, es decir, q_last_curr y t_last_curr
// Use el último incremento de pose calculado para actualizar la pose del cuadro anterior para obtener la pose del cuadro actual. Tenga en cuenta que la pose mencionada aquí se refiere a la pose en el sistema de coordenadas mundial t_w_curr = t_w_curr + q_w_curr * t_last_curr;
q_w_curr
= q_w_curr * q_last_curr
y finalmente:
(1) publicar la pose del marco actual en el sistema de coordenadas mundiales calculado por el subproceso Odometry actual, los puntos característicos corner_less_sharp, los puntos característicos surf_less_flat y la nube de puntos filtrada (reenviar intacto al nodo anterior para su procesamiento (filtrado simple) nube de puntos del marco actual de
(2) Actualice laserCloudCornerLast y laserCloudSurfLast y el kdtree correspondiente con cornerPointsLessSharp y surfPointsLessFlat para proporcionar el objetivo para la siguiente combinación de características de nube de puntos
Finalmente, permítanme explicar la frecuencia de cálculo de cada nodo:
(1) La frecuencia del tema de publicación del primer nodo/kittiHelper se establece en 10 Hz (por supuesto, se puede modificar),
(2) El nodo descendente es el segundo nodo/ascanRegistration. No se establece ninguna frecuencia. Después de recibir el mensaje ascendente, se realiza la extracción de características y el tema relevante se publica inmediatamente después de completar la extracción. Debido a que el tiempo de extracción de características es generalmente menor de 100ms, se puede considerar que el nodo publica La frecuencia también es de 10Hz
(3) El nodo aguas abajo es el tercer nodo/alaserOdometry, el ros::Rate rate(100) establecido por el ciclo grande; es decir, la odometría se ejecuta una vez a 100 Hz (se puede entender como una actualización de la odometría una vez a 100 Hz, lo que puede no necesariamente ejecutarse, dependiendo de lo que haya en el búfer. Si no hay datos, Odometry se ejecutará una vez si hay datos), y se establece una cola como contenedor de caché para cada tema recibido, con el objetivo de evitar el tiempo de ejecución. de una odometría exceda los 100 ms, por lo que la frecuencia de publicación de este nodo está permitida <= 10 Hz; Cabe señalar que hay una configuración de frecuencia de publicación al publicar puntos de características y nubes de puntos, que es para controlar la frecuencia de ejecución del último nodo.
if (frameCount % skipFrameNum == 0)
{ ... } (4) El último nodo es el 4º nodo/alaserMapping, no se establece ninguna frecuencia, directamente ros::spin(); es decir, un tema publicado por un upstream se recibe el nodo Simplemente ejecute la función de devolución de llamada una vez. Cabe señalar que es probable que el tiempo de procesamiento de este nodo supere los 100 ms. Para obtener el rendimiento general en tiempo real del algoritmo LOAM, aunque el nodo también configura un contenedor de caché que acepta temas, borrará el historial cachea los mensajes que no han sido procesados a tiempo Detalles Ver la introducción en la siguiente sección.
6.laserMapping.cpp
1) Nodo correspondiente: /alaserMapping
2) Función: en función de los puntos característicos corner_less_sharp y los puntos característicos surf_less_flat antes mencionados, realizar el registro de características de nube de puntos de marco y submapa, y ajustar la pose calculada por el subproceso Odometry
3) Análisis de código:
a. Del mismo modo, primero aclare el significado de varias variables clave
Copiar código
//Variables de optimización para función de nube de puntos que coinciden con
parámetros dobles[7] = {0, 0, 0, 1, 0, 0, 0};
// La pose P del cuadro estimada por el subproceso Mapping en el sistema de coordenadas mundial, porque el algoritmo de Mapping puede tardar más de 100 ms, por lo que //
esta pose P no es en tiempo real, y la pose en tiempo real P_realtime finalmente sale por LOAM necesita La pose de frecuencia relativamente baja calculada por el subproceso Mapping y
la pose de frecuencia relativamente alta calculada por el subproceso Odometry están integradas.Para obtener detalles, consulte el análisis de la función laserOdometryHandler más adelante. Además, debe tenerse en cuenta
que // a diferencia del subproceso Odometry, la pose P aquí, a saber, q_w_curr y t_w_curr, es en sí misma una variable de optimización durante la coincidencia.
Eigen::MapEigen::Quaterniond q_w_curr(parámetros);
Eigen::MapEigen::Vector3d t_w_curr(parámetros + 4);
// Las dos variables siguientes son el incremento entre la pose calculada por Odometry en el sistema de coordenadas mundial y la pose calculada por Mapping (es decir, transformación, transformación) // wmap_odom * wodom_curr = wmap_curr (es decir, el q/t_w_curr
anterior )
// transformación entre el mundo de odom y el marco del mundo de map
Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0);
Eigen::Vector3d t_wmap_wodom(0, 0, 0);
// La pose del cuadro calculada por el hilo Odometry en el sistema de coordenadas mundo
Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0);
Eigen::Vector3d t_wodom_curr(0, 0, 0);
Copiar código
b. introducir main de nuevo varias funciones antes de la función
Copie el código
// establezca la suposición inicial, incremente wmap_wodom del cuadro anterior * Odometry pose wodom_curr de este cuadro, con el objetivo de establecer un valor inicial para Mapping pose w_curr de este cuadro void transformAssociateToMap() { q_w_curr = q_wmap_wodom * q_wodom_curr;
t_w_curr
= q_wmap_wodom * t_wodom_curr + t_wmap_wodom; }
// Usado al final, cuando se calcula la pose de mapeo w_curr, actualiza el wmap_wodom incremental para preparar la próxima ejecución de la función transformAssociateToMap void
transformUpdate()
{ q_wmap_wodom = q_w_curr * q_wodom_curr.inverse(); t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr; }
// Usa la pose de mapeo w_curr para transformar el punto en el sistema de coordenadas Lidar al sistema de coordenadas mundial
void pointAssociateToMap(PointType const *const pi, PointType *const po)
{ Eigen::Vector3d point_curr(pi->x, pi- > y, pi->z); Eigen::Vector3d punto_w = q_w_curr * punto_curr + t_w_curr; po->x = punto_w.x(); po->y = punto_w.y(); po->z = punto_w. z (); po->intensidad = pi->intensidad; //po->intensidad = 1.0; }
// Esto no se usa, es la transformación inversa de pointAssociateToMap anterior, es decir, usar la pose w_curr de Mapping para transformar el punto en el sistema de coordenadas mundo al sistema de coordenadas Lidar void pointAssociateTobeMapped(PointType const *const pi, PointType
* const po)
{ Eigen::Vector3d point_w(pi->x, pi->y, pi->z); Eigen::Vector3d point_curr = q_w_curr.inverse() * (point_w - t_w_curr); po->x = point_curr .x() ; po->y = point_curr.y(); po->z = point_curr.z(); po->intensity = pi->intensity; } Varias funciones del controlador después de copiar el código son después de recibir el upstream publicar tema La función de devolución de llamada también almacenará en caché el mensaje en la cola correspondiente para su procesamiento posterior.
复制代码
// recibir odomtry
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
{ mBuf.lock(); odometryBuf.push(laserOdometry); mBuf.desbloquear();
// high frequence publish
Eigen::Quaterniond q_wodom_curr;
Eigen::Vector3d t_wodom_curr;
q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x;
...
t_wodom_curr.z() = laserOdometry->pose.pose.position.z;
// 为了保证LOAM整体的实时性，防止Mapping线程耗时>100ms导致丢帧，用上一次的增量wmap_wodom来更新
// Odometry的位姿，旨在用Mapping位姿的初始值（也可以理解为预测值）来实时输出，进而实现LOAM整体的实时性
Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr;
Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
...
pubOdomAftMappedHighFrec.publish(odomAftMapped);
}
Copie el código
C. Luego, el proceso de la función principal y la función principal:
La función principal es relativamente simple. Recibe el tema del nodo ascendente, define la función de devolución de llamada del mensaje correspondiente y luego ingresa a la función principal process () para realizar el mapeo, es decir, la coincidencia del marco y el submapa, y el ajuste fino. la pose calculada por Odometría.
En la función de proceso, personalmente siento que lo más inconveniente de entender es la definición y el mantenimiento del submapa, por lo que antes del análisis del código, primero introduzca el submapa en LOAM, directamente encima de la imagen.
El marco en el subproceso de mapeo en LOAM coincide con las características del submapa. El submapa utilizado es el área amarilla en la figura anterior. La característica de esquina y la característica de surf en el submapa se usan como el objetivo en la coincidencia; mientras que los dos puntos en la nube de puntos de un solo cuadro del cuadro actual Esta característica se utiliza como fuente en la coincidencia.
A continuación se explica el significado de varias variables globales relacionadas con el mantenimiento del submapa. Cabe señalar que LOAM administra cubos en el submapa en forma de arreglos unidimensionales.
Copiar código
// El número total de cubos, es decir, el número total de todas las cuadrículas pequeñas en la imagen de arriba 21 * 21 * 11 = 4851
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;
// Las siguientes dos variables son exactamente iguales, un poco redundantes, registre el índice del cubo válido en el submapa, tenga en cuenta que el número máximo de cubos en el submapa es 5 * 5 * 5 = 125 int laserCloudValidInd[125]; int laserCloudSurroundInd[125
]
;
// Almacene la matriz de características de la nube de puntos del cubo, el tamaño de la matriz es 4851, puntos en cada cubo
pcl::PointCloud::Ptr laserCloudCornerArray[laserCloudNum];
pcl::PointCloud::Ptr laserCloudSurfArray[laserCloudNum];
copie el código
Para garantizar la integridad general del algoritmo LOAM en tiempo real, el subproceso de asignación solo procesa cornerLastBuf.front() y otros mensajes sincronizados en el tiempo cada vez, y el caché histórico se borrará porque el subproceso de asignación tarda más de 100 ms. operaciones son las siguientes:
Copie el código
while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
!fullResBuf.empty() && !odometryBuf.empty())
{ mBuf.lock(); // odometryBuf solo conserva uno y cornerLastBuf.front( ) sincronización temporal del último mensaje while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec()) odometryBuf.pop() ; if (odometryBuf.empty()) { mBuf.unlock(); break; }
// surfLastBuf也如此
while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
surfLastBuf.pop();
if (surfLastBuf.empty())
{
mBuf.unlock();
break;
}
// fullResBuf也如此
while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
fullResBuf.pop();
if (fullResBuf.empty())
{
mBuf.unlock();
break;
}
...
// 清空cornerLastBuf的历史缓存，为了LOAM的整体实时性
while (!cornerLastBuf.empty())
{
cornerLastBuf.pop();
printf("drop lidar frame in mapping for real time performance \n");
}
mBuf.unlock();
}
Copie el código
Luego, sobre el mantenimiento de la ubicación del submapa, solo lea los comentarios a continuación:
Copiar código
// Incrementar wmap_wodom del cuadro anterior * Odometry pose wodom_curr de este cuadro, con el objetivo de establecer un valor inicial para Mapping pose w_curr de este cuadro
transformAssociateToMap();
TicToc t_shift;
// El siguiente es el cálculo de la posición del cuadro actual t_w_curr (la posición indicada por la estrella roja de cinco puntas en la figura anterior) Coordenadas IJK (consulte el eje de coordenadas en la figura anterior), // Consulte el nota de
LOAM_NOTED, lo siguiente es aproximadamente 25, la operación de 50 es equivalente a hacer zoom en unidades de 50m, porque LOAM usa una matriz unidimensional
// para administrar el cubo, y el índice de la matriz es solo un número positivo, por lo tanto, es necesario asegurarse de que las coordenadas IJK sean todas números positivos, por lo que se agrega laserCloudCenWidth /Heigh/Depth
// offset para hacer que la posición actual esté lo más cerca posible del centro del submapa, es decir, para hacer que la posición de la estrella de cinco puntas en la imagen de arriba cerca del centro de todas las cuadrículas tanto como sea posible, //
compensar laserCloudCenWidth/Heigh/Depth será dinámico Ajuste para garantizar que la posición actual esté ubicada en el centro del submapa tanto como sea posible .
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
int centerCubeK = int((t_w_curr.z( ) + 25.0) / 50.0) + laserCloudCenDepth;
//Dado que el resto de la computadora se redondea a cero, para no hacer que (-50.0, 50.0) se desplace a cero después del resto, cuando el resto es negativo, el resultado del resto se desplaza uniformemente hacia la izquierda en una unidad, eso es menos uno
si (t_w_curr.x() + 25.0 < 0)
centerCubeI–,
if (t_w_curr.y() + 25.0 < 0)
centerCubeJ–,
if (t_w_curr.z() + 25.0 < 0)
centerCubeK–;
// Los siguientes comentarios se refieren a LOAM_NOTED, combinados con el diagrama esquemático del submapa que dibujé para ilustrar las funciones de los siguientes 6 bucles while: // Tenga en cuenta que el
mapa de nube de puntos en el sistema de coordenadas mundial es fijo, pero podemos moverlo el sistema de coordenadas IJK, por lo que la función de estos 6 bucles while
es ajustar el sistema de coordenadas IJK (es decir, ajustar todas las posiciones del cubo), de modo que el rango de coordenadas de la estrella de cinco puntas en el sistema de coordenadas IJK es
// 3 < centerCubeI < 18, 3 < centerCubeJ < 8, 3 < centerCubeK < 18, el propósito es evitar
que el índice (es decir, las coordenadas IJK) se vuelva negativo cuando el cubo se expande la figura es el cubo expandido).
while (centerCubeI < 3)
{ for (int j = 0; j < laserCloudHeight; j++) { for (int k = 0; k < laserCloudDepth; k++) { int i = laserCloudWidth - 1; pcl::PointCloud::Ptr laserCloudCubeCornerPointer = laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; i >= 1; i–)// Cube[I]=cube[I-1] en la dirección I, borre el último cubo vacío, para lograr el efecto de mover un cubo en la dirección negativa del eje I en el sistema de coordenadas IJK
Desde la perspectiva del movimiento relativo, la estrella de cinco puntas en la figura mueve un cubo en la dirección positiva del eje I en el sistema de coordenadas IJK, como se muestra en la siguiente animación Shown, entonces
// usa centerCubeI al final ++, laserCloudCenWidth también ++, para preparar el cálculo de las coordenadas IJK del pentagrama en el siguiente cuadro de Mapping
{ laserCloudCornerArray [i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudSurfArray[i - 1 + ancho de nube láser * j + ancho de nube láser * altura de nube láser * k]; }
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeI++;
laserCloudCenWidth++;
}
while (centerCubeI >= laserCloudWidth - 3)
{ … }
while (centerCubeJ < 3)
{ … }
while (centerCubeJ >= laserCloudHeight - 3)
{ … }
while (centerCubeK < 3)
{ … }
while (centerCubeK >= laserCloudDepth - 3)
{ ... } Copie el código y ajuste la dirección del eje I una vez
La nube de puntos de características del submapa generado es la siguiente. Cabe señalar que el submapa generado por LOAM no se basa en el método de ventana deslizante de tiempo, sino en el método de división de rango espacial:
Copiar código
//Las dos definiciones siguientes pueden considerarse como la limpieza del submapa
int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;
// Expanda 2 cubos en las direcciones positiva y negativa del eje de coordenadas IJ, y expanda 1 cubo en las direcciones positiva y negativa del eje de coordenadas K. El cubo azul donde se encuentra la estrella de cinco puntas en la imagen de arriba es el posición actual // del cubo,
expandir El cubo es el cubo amarillo, y estos cubos son el rango del submapa
for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
{ for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) { for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++) { if (i >= 0 && i < laserCloudWidth && j >= 0 && j < laserCloudHeight && k >= 0 && k < laserCloudDepth)/ / Si las coordenadas son legales { // Registrar el índice de todos los cubos en el submapa y registrarlo como un índice válido laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k; laserCloudValidNum++;
laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudSurroundNum++;
}
} }
}
laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
for (int i = 0; i < laserCloudValidNum; i++)
{ // Superponga las nubes de puntos en el cubo con un índice válido para formar la nube de puntos característica del submapa *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]]; *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]]; } Copie el código . frame downsampled feature point El proceso ICP de origen en la nube también se divide en coincidencia de punto a línea y de punto a superficie. El proceso básico es el mismo que el subproceso de Odometría. La diferencia es que la forma de establecer la correspondencia (asociación) es diferente. Consulte la introducción a continuación para obtener más detalles. Primero, dirija el proceso ICP a la línea:
Copie el código
para (int i = 0; i < laserCloudCornerStackNum; i++)
{ pointOri = laserCloudCornerStack->points[i]; // Debe tenerse en cuenta que las nubes de puntos en el submapa están todas en el sistema de coordenadas mundial, y el las nubes de puntos en el cuadro actual son todas Es el sistema de coordenadas Lidar, por lo que // Al buscar el punto vecino más cercano, primero use la posición de mapeo predicha w_curr para transformar los puntos característicos en el sistema de coordenadas Lidar al sistema de coordenadas mundial pointAssociateToMap ( &pointOri, &pointSel); // En el submapa En el punto característico de la esquina (objetivo), busque los 5 puntos más cercanos al punto característico de la esquina del cuadro actual (origen) kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
if (pointSearchSqDis[4] < 1.0)
{
std::vector<Eigen::Vector3d> nearCorners;
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
laserCloudCornerFromMap->points[pointSearchInd[j]].y,
laserCloudCornerFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
nearCorners.push_back(tmp);
}
// 计算这个5个最近邻点的中心
center = center / 5.0;
// 协方差矩阵
Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
for (int j = 0; j < 5; j++)
{
Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
}
// 计算协方差矩阵的特征值和特征向量，用于判断这5个点是不是呈线状分布，此为PCA的原理
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
// if is indeed line feature
// note Eigen library sort eigenvalues in increasing order
Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);// 如果5个点呈线状分布，最大的特征值对应的特征向量就是该线的方向向量
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])// 如果最大的特征值 >> 其他特征值，则5个点确实呈线状分布，否则认为直线“不够直”
{
Eigen::Vector3d point_on_line = center;
Eigen::Vector3d point_a, point_b;
// 从中心点沿着方向向量向两端移动0.1m，构造线上的两个点
point_a = 0.1 * unit_direction + point_on_line;
point_b = -0.1 * unit_direction + point_on_line;
// 然后残差函数的形式就跟Odometry一样了，残差距离即点到线的距离，到介绍lidarFactor.cpp时再说明具体计算方法
ceres::CostFunction* cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
corner_num++;
}
}
} El proceso ICP de punto a línea sobre
el código de copia
es más robusto y preciso que el de Odometría (por supuesto, requiere más tiempo), porque no es simplemente buscar los 2 puntos de esquina más cercanos como la línea de destino, En su lugar, PCA calcula la dirección principal de los cinco vecinos más cercanos como la dirección de la línea y también comprueba si la línea es "suficientemente recta".
Luego mire el proceso ICP de punto a superficie:
Copie el código
para (int i = 0; i < laserCloudSurfStackNum; i++)
{ pointOri = laserCloudSurfStack->points[i];
pointAssociateToMap(&pointOri, &pointSel);
//也找5个最近邻点
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
// 求面的法向量就不是用的PCA了（虽然论文中说还是PCA），使用的是最小二乘拟合，是为了提效？不确定
// 假设平面不通过原点，则平面的一般方程为Ax + By + Cz + 1 = 0，用这个假设可以少算一个参数，提效。
Eigen::Matrix<double, 5, 3> matA0;
Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
// 用上面的2个矩阵表示平面方程就是 matA0 * norm（A, B, C） = matB0，这是个超定方程组，因为数据个数超过未知数的个数
if (pointSearchSqDis[4] < 1.0)
{
for (int j = 0; j < 5; j++)
{
matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
}
// 求解这个最小二乘问题，可得平面的法向量，find the norm of plane
Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
// Ax + By + Cz + 1 = 0，全部除以法向量的模长，方程依旧成立，而且使得法向量归一化了
double negative_OA_dot_norm = 1 / norm.norm();
norm.normalize();
// Here n(pa, pb, pc) is unit norm of plane
bool planeValid = true;
for (int j = 0; j < 5; j++)
{
// 点(x0, y0, z0)到平面Ax + By + Cz + D = 0 的距离公式 = fabs(Ax0 + By0 + Cz0 + D) / sqrt(A^2 + B^2 + C^2)
if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
{
planeValid = false;// 平面没有拟合好，平面“不够平”
break;
}
}
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
if (planeValid)
{
// 构造点到面的距离残差项，同样的，具体到介绍lidarFactor.cpp时再说明该残差的具体计算方法
ceres::CostFunction* cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
surf_num++;
}
}
} El proceso ICP de punto a superficie sobre
el código de copia
también es más sólido y preciso que el de Odometría (por supuesto, requiere más tiempo), porque no es simplemente buscar los 3 puntos de surf más cercanos como el plano objetivo. point , pero realiza un ajuste de plano basado en mínimos cuadrados en los cinco puntos vecinos más cercanos, y también verifica si el plano ajustado es "lo suficientemente plano", y luego algunas operaciones después de la coincidencia, principalmente incluida la actualización de la pose Incremento y actualización del submapa:
Copiar código
// Después de completar la coincidencia de características de ICP (iteración 2 veces), use la variable optimizada w_curr calculada por la coincidencia final para actualizar el wmap_wodom incremental para prepararse para la próxima actualización de mapeo transformUpdate ()
;
TicToc t_add;
// La función de los siguientes dos bucles for es operar punto por punto la nube de puntos característica del marco actual: convertir al sistema de coordenadas mundial y agregarlo al cubo en la posición correspondiente
para (int i = 0 ; i < laserCloudCornerStackNum; i++)
{ // Sistema de coordenadas Lidar al sistema de coordenadas mundial pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);
// 计算本次的特征点的IJK坐标，进而确定添加到哪个cube中
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudCornerArray[cubeInd]->push_back(pointSel);
}
}
for (int i = 0; i < laserCloudSurfStackNum; i++)
{ … } printf(“añadir puntos de tiempo %f ms\n”, t_add.toc());
TicToc t_filter;
//Debido a la nube de puntos recién agregada, vuelva a reducir la muestra de todos los cubos que ya tenían nubes de puntos antes
//Este lugar se puede optimizar simplemente: si el cubo anterior no tiene puntos nuevos, no hay necesidad de reducir la muestra
para (int i = 0; i < laserCloudValidNum; i++)
{ int ind = laserCloudValidInd[i];
pcl::PointCloud<PointType>::Ptr tmpCorner(new pcl::PointCloud<PointType>());
downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
downSizeFilterCorner.filter(*tmpCorner);
laserCloudCornerArray[ind] = tmpCorner;
pcl::PointCloud<PointType>::Ptr tmpSurf(new pcl::PointCloud<PointType>());
downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
downSizeFilterSurf.filter(*tmpSurf);
laserCloudSurfArray[ind] = tmpSurf;
}
printf("filter time %f ms \n", t_filter.toc());
Copie el código
Finalmente, publique algunos temas y podrá aceptar los resultados de la nube de puntos visuales y la trayectoria en rviz. Cabe señalar que la pose final calculada no es el sistema de coordenadas de la pose de valor real de KITTI. Si necesita usar la pose de valor real para la evaluación, debe realizar una transformación de coordenadas.
7.lidarFactor.hpp
1) Nodo correspondiente: Ninguno
2) Función: Calcular la distancia del elemento residual de punto a línea y de punto a superficie
3) Análisis de código:
Copiar código
// Cálculo de la distancia residual del punto a la línea
struct LidarEdgeFactor
{ LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_, double s_) : curr_point(curr_point_), last_point_a(last_point_point_a_), last_ (último_punto_b_), s(s_) {}
template <typename T>
bool operator()(const T *q, const T *t, T *residual) const
{
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};
//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
// 考虑运动补偿，ktti点云已经补偿过所以可以忽略下面的对四元数slerp插值以及对平移的线性插值
q_last_curr = q_identity.slerp(T(s), q_last_curr);
Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
Eigen::Matrix<T, 3, 1> lp;
// Odometry线程时，下面是将当前帧Lidar坐标系下的cp点变换到上一帧的Lidar坐标系下，然后在上一帧的Lidar坐标系计算点到线的残差距离
// Mapping线程时，下面是将当前帧Lidar坐标系下的cp点变换到world坐标系下，然后在world坐标系下计算点到线的残差距离
lp = q_last_curr * cp + t_last_curr;
// 点到线的计算如下图所示
Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
Eigen::Matrix<T, 3, 1> de = lpa - lpb;
// 最终的残差本来应该是residual[0] = nu.norm() / de.norm(); 为啥也分成3个，我也不知
// 道，从我试验的效果来看，确实是下面的残差函数形式，最后输出的pose精度会好一点点，这里需要
// 注意的是，所有的residual都不用加fabs，因为Ceres内部会对其求 平方 作为最终的残差项
residual[0] = nu.x() / de.norm();
residual[1] = nu.y() / de.norm();
residual[2] = nu.z() / de.norm();
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
const Eigen::Vector3d last_point_b_, const double s_)
{
return (new ceres::AutoDiffCostFunction<
LidarEdgeFactor, 3, 4, 3>(
// ^ ^ ^
// | | |
// Dimensión del residuo ____| | |
// Dimensión de la variable de optimización q_______| |
// Dimensión de la variable de optimización t______ |
new LidarEdgeFactor(curr_point , last_point_a , last_point_b , s )));
}
Eigen::Vector3d curr_point, last_point_a, last_point_b;
double s;
};
Copiar código
Diagrama de cálculo de distancia de punto a línea:
Copiar código // Calcular la distancia residual del punto a Odometryhilo
la superficie en el last_point_m(last_point_m_), s(s_) { // Puntos l, j y m son los tres puntos vecinos más cercanos encontrados mediante la búsqueda, y lo siguiente es calcular estos tres puntos El vector normal del plano formado ljlm ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m); // vector normal normalizado ljm_norm.normalize (); }
template <typename T>
bool operator()(const T *q, const T *t, T *residual) const
{
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};
Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};
Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
q_last_curr = q_identity.slerp(T(s), q_last_curr);
Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
Eigen::Matrix<T, 3, 1> lp;
lp = q_last_curr * cp + t_last_curr;
// 计算点到平面的残差距离，如下图所示
residual[0] = (lp - lpj).dot(ljm);
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
const double s_)
{
return (new ceres::AutoDiffCostFunction<
LidarPlaneFactor, 1, 4, 3>(
// ^ ^ ^
// | | |
// Dimensión del residuo ____| | |
// Dimensión de la variable de optimización q_____| |
// Dimensión de la variable de optimización t_____ |
new LidarPlaneFactor(curr_point , last_point_j , last_point_l , last_point_m , s )) );
}
Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
Eigen::Vector3d ljm_norm;
double s;
};
Copiar código
Diagrama de cálculo de distancia de punto a superficie:
La última estructura LidarPlaneNormFactor es para calcular la distancia residual desde el punto hasta el plano en el subproceso Mapeo. Debido a que se ingresa el parámetro () de la ecuación del plano, la fórmula de distancia desde el punto hasta el plano se usa directamente para el cálculo:
Distancia del punto (x0, y0, z0) al plano Ax + By + Cz + D = 0 = fabs(A x0 + B y0 + C z0 + D) / sqrt(A^2 + B^2 + C^2) , porque el vector normal (A, B, C) se ha normalizado, por lo que la fórmula de la distancia se puede abreviar como: distancia = fabs(A x0 + B y0 + C z0 + D), a saber:
residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm)
3. Iteración de Gauss-Newton en el código fuente LOAM manual
1. Transformación de coordenadas en el conjunto de datos kitti
Como se mencionó anteriormente, la pose de valor real de la odometría kitti es la transformación del sistema de coordenadas de la cámara izquierda en relación con su primer cuadro; la pose calculada directamente por A-LOAM es la transformación del sistema de coordenadas LIDAR en relación con su primer cuadro; debería también se debe tener en cuenta que el sistema de coordenadas del mundo del sensor es esencialmente el sistema de coordenadas del sensor del primer cuadro.
recordar:
La matriz de parámetros externos de LIDAR a la cámara izquierda es Tr, es decir, Tr transforma un punto de las coordenadas Velodyne al sistema de coordenadas de la cámara izquierda;
La pose (es decir, la transformación) del sistema de coordenadas LIDAR en el sistema de coordenadas universales del cuadro i-ésimo es Tilidar_w;
La pose del sistema de coordenadas de la cámara del marco i-ésimo en su sistema de coordenadas mundial es Ticamera_w;
La nube de puntos en el sistema de coordenadas LIDAR del marco i-ésimo es Pilidar_l;
La nube de puntos en el i-ésimo sistema de coordenadas de la cámara del fotograma es Picamera_l;
La nube de puntos en el sistema de coordenadas mundo LIDAR del marco i-ésimo es Pilidar_w;
La nube de puntos en el i-ésimo sistema de coordenadas del mundo de la cámara del cuadro es Picamera_w;
Luego están:
Ticamera_w * Tr * Pilidar_l = Tr * Tilidar_w * Pilidar_l (1)
| |_____| | |______|
| | | |
| Picamera_l | Pilidar_w
|________| |_________|
| |
Picamera_w == Picamera_w
De (1) la fórmula puede obtener:
Ticamera_w * Tr = Tr * Tilidar_w （2）
De (2) la fórmula puede obtener:
Ticamera_w = Tr * Tilidar_w * Tr-1（3）
Entonces, la fórmula (3) es la fórmula para convertir la pose calculada por A-LOAM al sistema de coordenadas de pose de valor verdadero kitti
PD:
Aquí hay un método rápido de estimación de Tr (no relacionado con este artículo): Si no se considera la traslación de xyz, solo está disponible la rotación, como se muestra en la figura anterior:
xcámara = -ylidar
ycamera = -zlidar
zcamera = xlidar
(xcámara，ycámara，zcacámara)T = （0 -1 0
0 0 -1
1 0 0）* (xlidar，ylidar，zlidar)T
es decir, Tr = (0 -1 0
0 0 -1
1 0 0）
2. La publicación de matriz jacobiana manual
está vinculada, =_=, https://wykxwyc.github.io/2019/08/01/The-Math-Formula-in-LeGO-LOAM/