Resumen de calibración de sensores SLAM: Cámara/IMU/LiDAR

---

La calibración de referencia interna de la cámara y la IMU, el método de calibración conjunta entre la cámara, la IMU y LiDAR, el entorno de instalación del kit de herramientas está en el entorno Ubuntu20.04 y las herramientas utilizadas incluyen: caja de herramientas MATLAB, kit de herramientas de calibración ROS, kalibr , OpenCV , imu_utils, calibración_cámara_lidar, lidar_imu_calib

1. Calibración de los parámetros internos de la cámara (monocular)

1.1 Solución 1: Caja de herramientas de MATLAB

Este es el más conveniente, consulte ( artículo ), abra matlab, busque la APLICACIÓN, despliegue el triángulo para ver más opciones y busque Camera Calibrator (también proporciona un programa para la configuración de doble objetivo, aquí hay una demostración de monocular):

Agregue la foto de la placa de calibración que se ha tomado (puede ser la secuencia de imágenes guardada en el plano 2).Al disparar la placa de calibración, lo mejor es ocupar más de la mitad de la pantalla, con traducción, frente, atrás, izquierda , y derecha, y rotación.

Introduzca la distancia real entre los tableros de ajedrez adyacentes en el tablero de calibración. La precisión de la calibración depende en gran medida de la precisión del tablero de calibración. Aquí es de 35 mm. Después de hacer clic en Aceptar, el programa extrae automáticamente los puntos de las esquinas y los empareja. Los resultados son los siguientes :

Seleccione el tipo de parámetro de distorsión calibrado, tanto la distorsión radial como la distorsión tangencial, y luego haga clic en calibrar para calcular.

A la derecha, se muestra el error de reproyección promedio por cuadro y el error de reproyección promedio en todos los cuadros, junto con una visualización de la pose para cada cuadro de imagen. Puede ver que el error de reproyección de dos cuadros es obviamente demasiado grande. Haga clic para ver la imagen con desenfoque de movimiento. Puede hacer clic derecho en la imagen del cuadro correspondiente a la izquierda para eliminarlo y luego volver a calcular. La reproyección promedio El error del resultado final es de 0,16 píxeles, y la precisión ya es muy alta.

Haga clic en Parámetros de calibración de la cámara de salida para ver las variables de los parámetros de calibración:

1.2 Solución 2: utilice el kit de herramientas de calibración de ROS

Consulte la sección 7.1 de mi artículo ( enlace ).

1.3 Solución 3: utilice la herramienta de calibración kalibr

Kalibr proporciona las funciones de calibración de múltiples IMU, calibración de IMU de cámara y calibración de múltiples cámaras Aquí, el comando de calibración de múltiples cámaras se usa para calibrar una sola cámara.

1.3.1 instalar kalibr

# for Ubuntu18.04
# 安装依赖
sudo apt update
sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev ros-melodic-vision-opencv ros-melodic-image-transport-plugins ros-melodic-cmake-modules python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev
sudo pip install python-igraph --upgrade
# 创建工作空间并下载源码
mkdir -p kalibr_ws/src
cd kalibr_ws/src
git clone https://github.com/ethz-asl/kalibr.git
# 编译
cd .. 
source /opt/ros/melodic/setup.bash
catkin init
catkin config --extend /opt/ros/melodic
catkin config --merge-devel # Necessary for catkin_tools >= 0.4.
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin_make -DCMAKE_BUILD_TYPE=Release -j4

# for Ubuntu20.04
# 安装依赖
sudo apt update
sudo apt-get install python3-setuptools python3-rosinstall ipython3 libeigen3-dev libboost-all-dev doxygen libopencv-dev ros-noetic-vision-opencv ros-noetic-image-transport-plugins ros-noetic-cmake-modules python3-software-properties software-properties-common libpoco-dev python3-matplotlib python3-scipy python3-git python3-pip libtbb-dev libblas-dev liblapack-dev libv4l-dev python3-catkin-tools python3-igraph libsuitesparse-dev libgtk-3-dev
# 下面两个可能没反映或者错误，请等待和多尝试几次
pip3 install attrdict
pip3 install wxPython
# 创建工作空间并下载源码
mkdir -p kalibr_ws/src
cd kalibr_ws/src
git clone https://github.com/ethz-asl/kalibr.git
# 编译
cd .. 
source /opt/ros/noetic/setup.bash
catkin init
catkin config --extend /opt/ros/noetic
catkin config --merge-devel # Necessary for catkin_tools >= 0.4.
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin_make -DCMAKE_BUILD_TYPE=Release -j8

1.3.2 Preparar la placa de calibración

Prepare la placa de calibración Aprilgrid, que es la placa de calibración recomendada por kalibr, como se muestra en la figura a continuación. Puede ir al sitio web oficial para descargar la placa de calibración proporcionada por el autor, pero necesita acceder a Internet científicamente, por lo que puede usar el paquete de funciones del autor para generar:

rosrun kalibr kalibr_create_target_pdf --type apriltag --nx [NUM_COLS] --ny [NUM_ROWS] --tsize [TAG_WIDTH_M] --tspace [TAG_SPACING_PERCENT]
eg: rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 8 --ny 8 --tsize 0.1 --tspace 0.3
# 作者给的：
eg: rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.088 --tspace 0.3

Si se informa un error: ningún módulo llamado 'pyx', consulte los problemas de github para resolver:

sudo apt install python-pyx #18.04
sudo apt install python3-pyx #20.04

Se generará un archivo pdf en el directorio actual:

1.3.3 Método de calibración

(1) Primero registre datos, conduzca su propia cámara, traduzca hacia arriba, abajo, izquierda, derecha, adelante y atrás y gire tres veces cada uno cuando registre datos, y finalmente hay un movimiento aleatorio para mejorar la robustez.

rosbag record /image_raw -O images.bag

La velocidad de fotogramas recomendada por kalibr es de 4 Hz. Aquí hay un comando de caída de fotogramas, que puede soltar el tema_antiguo en el rosbag original a un tema_nuevo de 4 Hz, y luego grabar el tema_nuevo mientras se reproduce el rosbag:

rosrun topic_tools throttle messages old_topic 4.0 new_topic

(2) Entonces puedes calibrar

source devel/setup.bash
rosrun kalibr kalibr_calibrate_cameras --target april_6x6_24x24mm.yaml --bag images.bag --bag-from-to 5 20 --models pinhole-radtan --topics /image_raw --show-extraction

Entre ellos, varios archivos de parámetros deben prestar atención a:

• --bag images.bag: Ruta del paquete registrado;
• --bag-from-to 5 20Se refiere a la lectura de los 5 a los 20 de la bolsa, que es cortar una parte del principio y el final, porque puede haber algunos temblores violentos al principio y al final, lo que causará errores. todo el proceso es suave, no puedes cortarlo;
• --target april_6x6_24x24mm.yaml: archivo de parámetros de la placa de calibración, en el que el autor de los dos parámetros tagSize y tagSpacing explicó específicamente el significado, modifique de acuerdo con la placa de calibración impresa por usted mismo

target_type: 'aprilgrid'   #gridtype
tagCols: 6              #number of apriltags
tagRows: 6             #number of apriltags
tagSize: 0.088          #size of apriltag, edge to edge [m]
tagSpacing: 0.3         #ratio of space between tags to tagSize
codeOffset: 0           #code offset for the first tag in the aprilboard

• --models pinhole-radtan: Modelo de cámara, aquí está el modelo de cámara estenopeica, pinhole-radtan es el modelo de cámara más utilizado, incluida la distorsión radial y la distorsión tangencial, para obtener más detalles, consulte la descripción del modelo de cámara proporcionada por el autor ;
• --topics /image_raw: tema de la imagen;
• --show-extraction: Puede mostrar la imagen y ver los puntos característicos extraídos.

Una vez completada la calibración, el resultado del dibujo aparecerá automáticamente (los datos que utilicé no son correctos, el error de reproyección es un poco grande, por lo que necesito registrar algunos datos más):

Genere simultáneamente los siguientes archivos de resultados:

• Parámetros internos de la cámara:

cam0:
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [-0.294719325148729, 0.08601391290708914, -0.0003238494917967103, -0.00041999959826033857]
  distortion_model: radtan
  intrinsics: [461.14152593310064, 459.7917242849438, 363.2138823275693, 235.61324563304655]
  resolution: [752, 480]
  rostopic: /cam0/image_raw 

• archivo txt de resultados de calibración;
• Un pdf de la gráfica de los resultados de la calibración.

1.4 Solución 4: escriba un programa para llamar a OpenCV para la calibración

Consulte el libro "Inicio rápido de OpenCV4"

#include <opencv2/opencv.hpp>
#include <fstream>
#include <iostream>
#include <vector>
#include <cstring>
#include <sstream>
#include <cstdlib>
using namespace std;
using namespace cv;

void initUndistAndRemap(vector<Mat> imgs, Mat cameraMatrix, Mat distCoeffs, Size imageSize, vector<Mat> &undistImgs)
{
    
    
    // 计算映射坐标矩阵
    Mat R = Mat::eye(3, 3, CV_32F);
    Mat mapx = Mat(imageSize, CV_32FC1);
    Mat mapy = Mat(imageSize, CV_32FC1);
    // 内参矩阵/畸变系数/...
    initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, imageSize, CV_32FC1, mapx, mapy);
    for (int i = 0; i < imgs.size(); i++)
    {
    
    
        Mat undistImg;
        remap(imgs[i], undistImg, mapx, mapy, INTER_LINEAR);
        undistImgs.push_back(undistImg);
    }
}

void undist(vector<Mat> imgs, Mat cameraMatrix, Mat distCoeffs, vector<Mat> &undistImgs)
{
    
    
    for (int i = 0; i < imgs.size(); i++)
    {
    
    
        Mat undistImg;
        // 单幅图像去畸变:畸变图像/去畸变后的图像/内参矩阵/畸变系数
        undistort(imgs[i], undistImg, cameraMatrix, distCoeffs);
        undistImgs.push_back(undistImg);
    }
}

bool LoadData(const string &imagePath, const string &imageFilename, vector<Mat> &imgs)
{
    
    
    ifstream Left;
    Left.open(imageFilename.c_str());
    while (!Left.eof())
    {
    
    
        string s;
        getline(Left, s);
        if (!s.empty())
        {
    
    
            stringstream ss;
            ss << s;
            double t;
            string imageName;
            ss >> t;
            ss >> imageName;
            Mat img = imread(imagePath + "/" + imageName);
            imgs.push_back(img);
            if (!img.data)
            {
    
    
                cout << "请输入正确的图像文件" << endl;
                return 0;
            }
        }
    }

    return 1;
}

int main(int argc, char **argv)
{
    
    
    if (argc != 6)
    {
    
    
        cerr << endl
             << "Usage: ./CameraCalibration path_to_CalibrateImage path_to_calibdata.txt board_size_cols board_size_rows corners_of_checkerboard(mm)" << endl
             << "eg: ./CameraCalibration ../CalibrateData ../CalibrateData/calibdata.txt 9 6 10" << endl;

        return 1;
    }

    // -1 读取数据
    vector<Mat> imgs;
    LoadData(argv[1], argv[2], imgs);
    // 棋盘格内角点行列数
    int bcols, brows;
    // 棋盘格每个方格的真实尺寸
    double side_length;
    stringstream ss;
    ss << argv[3];
    ss >> bcols;
    ss.clear();
    ss << argv[4];
    ss >> brows;
    ss.clear();
    ss << argv[5];
    ss >> side_length;
    Size board_size = Size(bcols, brows);

    // -2 提取并计算标定板角点像素坐标
    // 多副图像分别放入vector<vector<Point2f>>
    vector<vector<Point2f>> imgsPoints;
    for (int i = 0; i < imgs.size(); i++)
    {
    
    
        Mat img1 = imgs[i], gray1;
        cvtColor(img1, gray1, COLOR_BGR2GRAY);
        vector<Point2f> img1_points;
        // 第一个参数是输入的棋盘格图像
        // 第二个参数是棋盘格内部的角点的行列数（注意：不是棋盘格的行列数，而内部角点-不包括边缘-的行列数）
        // 第三个参数是检测到的棋盘格角点，类型为std::vector<cv::Point2f>
        bool ret = findChessboardCorners(gray1, board_size, img1_points);

        // 细化标定板角点坐标（亚像素）,Size(5, 5)为细化方格坐标领域范围
        find4QuadCornerSubpix(gray1, img1_points, Size(5, 5));

        // 第一个参数是棋盘格图像（8UC3）既是输入也是输出
        // 第二个参数是棋盘格内部角点的行、列
        // 第三个参数是检测到的棋盘格角点
        // 第四个参数是cv::findChessboardCorners()的返回值。
        drawChessboardCorners(img1, board_size, img1_points, ret);
        // string windowNumber = to_string(i);
        // imshow("chessboard corners"+windowNumber, img1);

        imgsPoints.push_back(img1_points);
    }

    // -3 使用棋盘格每个内角点的世界坐标
    vector<vector<Point3f>> objectPoints; // 空间三维坐标（位于一个平面内，以此为xy坐标平面）
    for (int i = 0; i < imgsPoints.size(); i++)
    {
    
    
        vector<Point3f> tempPointSet;
        for (int j = 0; j < board_size.height; j++)
        {
    
    
            for (int k = 0; k < board_size.width; k++)
            {
    
    
                // 假设标定板为世界坐标系的z平面，即z=0
                Point3f realPoint;
                realPoint.x = j * side_length;
                realPoint.y = k * side_length;
                realPoint.z = 0;
                tempPointSet.push_back(realPoint);
            }
        }
        objectPoints.push_back(tempPointSet);
    }

    // -4 内参及畸变标定
    Size imageSize; // 图像尺寸
    imageSize.width = imgs[0].cols;
    imageSize.height = imgs[0].rows;
    // 定义内外参
    Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); // 相机内参数矩阵
    Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));   // 相机的5个畸变系数
    vector<Mat> rvecs, tvecs;                               // 每幅图像的旋转向量/平移向量
    // 调用OpenCV标定函数
    calibrateCamera(objectPoints, imgsPoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, 0);
    cout << "相机的内参矩阵K=" << endl
         << cameraMatrix << endl;
    cout << "相机畸变系数:" << endl
         << distCoeffs << endl;

    // -5 保存数据
    ofstream calibrateFile;
    // 写入数据:
    calibrateFile.open("../calibrateCamera.txt"); // 没有自动创建
    if (!calibrateFile.is_open())                 // 文件是否打开
    {
    
    
        exit(EXIT_FAILURE); // 终止程序
    }
    calibrateFile << fixed;     // 开始写入,与cout相同
    calibrateFile.precision(5); // 写入小数精度
    calibrateFile << "cameraMatrix K=" << endl
                  << cameraMatrix; // 写入数据（覆盖
    calibrateFile << endl << "distCoeffs=" << endl
                  << distCoeffs;
    calibrateFile.close();

    // -6 图像去畸变
    vector<Mat> undistImgs;
    // 使用initUndistortRectifyMap()函数和remap()函数校正图像
    initUndistAndRemap(imgs, cameraMatrix, distCoeffs, imageSize, undistImgs); // 畸变图像/前文计算得到的内参矩阵/畸变系数/图像尺寸/去畸变后的图像,自定义函数是为了处理多副图像
    // undist(imgs, cameraMatrix, distCoeffs, undistImgs);//用undistort()函数直接计算校正图像,自定义函数是为了处理多副图像
    // 显示校正前后的图像(一张示例)
    for (int i = 0; i < 1; i++)
    {
    
    
        string windowNumber = to_string(i);
        imshow("chessboard corners without undistort -- image" + windowNumber, imgs[i]);
        imshow("chessboard corners with undistort -- image" + windowNumber, undistImgs[i]);
        imwrite("../chessboard corners without undistort.png", imgs[i]);
        imwrite("../chessboard corners with undistort.png", undistImgs[i]);
    }
    waitKey(0);

    // -7 单目投影(重投影)：根据成像模型及空间点三位坐标计算图像二维坐标
    vector<vector<Point2f>> imagePoints; // 存放二维坐标
    // 根据三维坐标和相机与世界坐标系时间的关系估计内角点像素坐标
    for (int i = 0; i < imgs.size(); i++)
    {
    
    
        Mat rvec = rvecs[i], tvec = tvecs[i];
        vector<Point3f> PointSets = objectPoints[i];
        vector<Point2f> imagePoint;                                                 // 存放二维坐标
        projectPoints(PointSets, rvec, tvec, cameraMatrix, distCoeffs, imagePoint); // 输入三维点/旋转及平移向量/前文计算得到的内参矩阵和畸变矩阵/输出二维点
        imagePoints.push_back(imagePoint);
    }

    // -8 计算重投影误差
    vector<vector<double>> ReProjectionError;
    vector<vector<double>> ReProjectionErrorX;
    vector<vector<double>> ReProjectionErrorY;
    double e = 0.0;
    for (int i = 0; i < imgs.size(); i++)
    {
    
    
        vector<Point2f> imagePoint = imagePoints[i]; // 存放二维坐标
        vector<double> er;
        vector<double> erX;
        vector<double> erY;
        for (int j = 0; j < imagePoint.size(); j++)
        {
    
    
            double eX = imagePoint[j].x - imgsPoints[i][j].x;
            double eY = imagePoint[j].y - imgsPoints[i][j].y;
            double error = sqrt(pow(eX, 2) + pow(eY, 2));
            erX.push_back(eX);
            erY.push_back(eY);
            er.push_back(error);
            e += error;
        }
        ReProjectionError.push_back(er);
        ReProjectionErrorX.push_back(erX);
        ReProjectionErrorY.push_back(erY);
    }
    // 计算估计值和图像中计算的真实时之间的平均误差
    cout << "平均重投影误差:" << e / (imagePoints[0].size() * imgs.size()) << endl;

    // -9 保存重投影误差数据
    ofstream ReProjectionErrorFile;
    ReProjectionErrorFile.open("../ReProjectionError.txt");
    ofstream ReProjectionErrorFileX;
    ReProjectionErrorFileX.open("../ReProjectionErrorX.txt");
    ofstream ReProjectionErrorFileY;
    ReProjectionErrorFileY.open("../ReProjectionErrorY.txt");
    if (!ReProjectionErrorFile.is_open() || !ReProjectionErrorFileX.is_open() || !ReProjectionErrorFileY.is_open())
    {
    
    
        exit(EXIT_FAILURE);
    }
    ReProjectionErrorFile << fixed;
    ReProjectionErrorFile.precision(5);
    ReProjectionErrorFileX << fixed;
    ReProjectionErrorFileX.precision(5);
    ReProjectionErrorFileY << fixed;
    ReProjectionErrorFileY.precision(5);
    for (int i = 0; i < imgs.size(); i++)
    {
    
    
        for (int j = 0; j < imagePoints[i].size(); j++)
        {
    
    
            ReProjectionErrorFile << ReProjectionError[i][j] << " ";
            ReProjectionErrorFileX << ReProjectionErrorX[i][j] << " ";
            ReProjectionErrorFileY << ReProjectionErrorY[i][j] << " ";
        }
        ReProjectionErrorFile << endl;
        ReProjectionErrorFileX << endl;
        ReProjectionErrorFileY << endl;
    }
    ReProjectionErrorFile.close();
    ReProjectionErrorFileX.close();
    ReProjectionErrorFileY.close();

    /*
    // 圆形标定板角点提取
    Mat img2 = imread("../CalibrateData/circle.png");
    Mat gray2;
    cvtColor(img2, gray2, COLOR_BGR2GRAY);
    Size board_size2 = Size(7, 7);          //圆形标定板圆心数目（行，列）
    vector<Point2f> img2_points;//检测角点，单副图像放入vector<Point2f>
    findCirclesGrid(gray2, board_size2, img2_points);           //计算圆形标定板检点
    find4QuadCornerSubpix(gray2, img2_points, Size(5, 5));      //细化圆形标定板角点坐标
    drawChessboardCorners(img2, board_size2, img2_points, true);
    imshow("圆形标定板角点检测结果", img2);
    waitKey();
    */

    return 0;
}

Tome los datos en el libro como un ejemplo:

Puede ver la diferencia antes y después de la corrección de la distorsión, y las líneas rectas de la imagen ya no se deforman.

Trace el error de reproyección usando Python:

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 编     写：ZARD帧心
import numpy as np
import matplotlib.pyplot as plt

errorX = np.genfromtxt('ReProjectionErrorX.txt')  # 将文件中数据加载到数组里
errorY = np.genfromtxt('ReProjectionErrorY.txt')
(m, n) = np.shape(errorX)
error = (errorX**2 + errorY**2)**0.5

# 画每帧图像的重投影误差均值
error_image =  np.average(error, axis=1)  # 按行求均值
image = range(m)
plt.bar(image, error_image)
plt.xlabel('Image number',{
    
    'Size':30})
plt.ylabel('ReProjection Error (pixel)',{
    
    'Size':30})
plt.xticks(fontsize=30) #x轴刻度字体大小
plt.yticks(fontsize=30) #y轴刻度字体大小
plt.show()

# 画所有角点的重投影误差
x = errorX.reshape(m*n,1)
y = errorY.reshape(m*n,1)
r = error.reshape(m*n,1) # 用于映射颜色的数值
# np.set_printoptions(threshold=np.inf)
# print(r)
# print(max(r))
h = plt.scatter(x, y, c=r, marker='*', alpha=0.8, cmap='viridis')
plt.xlabel('X error (pixel)',{
    
    'Size':30})
plt.ylabel('Y error (pixel)',{
    
    'Size':30})
plt.xticks(fontsize=30) #x轴刻度字体大小
plt.yticks(fontsize=30) #y轴刻度字体大小
cb=plt.colorbar(h) # 显示颜色条
font = {
    
    'weight' : 'normal',
        'size'   : 30, }
cb.ax.tick_params(labelsize=30)  #设置色标刻度字体大小。
cb.set_label('ReProjection Error',fontdict=font) #设置colorbar的标签字体及其大小
plt.show()

El siguiente es el resultado de dibujar los mismos datos que el Esquema 1 MATLAB, que está lejos de MATLAB. Puede ser que haya un problema con mi escritura de código. Si alguien ve dónde debe modificarse, hágamelo saber, gracias tú

Más tarde, utilicé un método simple para eliminar errores graves y obtuve los siguientes resultados, que son mejores pero aún están lejos de MATLAB

2. Calibración de los parámetros internos de la IMU

Use imu_utils para calibrar la IMU, consulte: Artículo
(1) Dependencias de instalación

sudo apt-get install libdw-dev

(2) Descargue imu_utils y code_utils , pero imu_utils de gaowenliang tiene problemas con la unidad, use la versión mintar de imu_utils
(3) Preste atención a algunos problemas antes de compilar:

• code_utils depende de eigen, OpenCV3 , ceres , estos son enlaces a mis artículos anteriores, y están sus tutoriales de instalación;
• No compile imu_utils y code_utils al mismo tiempo, imu_utils depende de code_utils, así que compile code_utils primero y luego compile imu_utils.

(4) Ponlo en tu espacio de trabajo y compila:

catkin_make

• Error de compilación: code_utils-master/src/sumpixel_test.cpp:2:24: fatal error: backward.hpp:No such file or directoryencontrado en code_utils sumpixel_test.cpp, modificar

// #include "backward.hpp"
// 为 
#include “code_utils/backward.hpp”

• Si encuentra muchos errores en la biblioteca estándar de C++, primero piense en intentar modificar el estándar de C++:

# set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS "-std=c++14")

(4) Registre imu.bag y deje que la IMU permanezca inmóvil durante 30-120 minutos, cuanto más tiempo mejor, registre la bolsa de la IMU

rosbag record /imu -O imu.bag

(5) Calibre la IMU, aquí están los datos de muestra de imu_utils como ejemplo:

rosbag play -r 200 imu_utils/imu.bag  #(这里要写你录制的包的路径，200倍速播放)
source ./devel/setup.bash
roslaunch imu_utils A3.launch #示例数据

Tenga en cuenta que el archivo aquí A3.launchcontiene algunos parámetros, como el tema y el nombre de la IMU, y la duración de la calibración, modifíquelos según sea necesario:

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/djiros/imu"/>
        <param name="imu_name" type="string" value= "A3"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "150"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

Una vez completada la calibración, se generarán mynteye_imu_param.yaml y varios archivos txt en $(find imu_utils)/data/:

3. Calibración conjunta de cámara e IMU

Utilice la herramienta de calibración kalibr, que se ha instalado anteriormente.La frecuencia recomendada dada por otros tutoriales es de 20 Hz para la cámara y 200 Hz para la IMU. Como no puedo navegar por Internet científicamente, no puedo descargar los datos de muestra proporcionados por el autor. Los datos aquí se obtienen de este blog: Instalación y compilación de Ubuntu18.04+kalibr toolbox de calibración
(1) Grabación
Suscribirse a dos noticias , imágenes y temas imu:

rosbag record /imu_raw /image_raw

Al grabar, intente moverse lo más suavemente posible. Si la velocidad es demasiado rápida, la calidad de imagen obtenida por la cámara es demasiado pobre y no debe ser demasiado lenta. Para estimular cada eje de la imu, el autor proporciona una demostración video para referencia de recopilación de datos y rotación tres veces cada uno, y un período de movimiento irregular (tenga cuidado de no hacer ejercicio demasiado vigoroso). La placa de calibración está completamente impresa y se recomienda que la distancia de la cámara sea de 0,8 m al grabar paquetes de datos

(2) Calibración

source devel/setup.bash
rosrun kalibr kalibr_calibrate_imu_camera --target dynamic/april_6x6.yaml --bag dynamic/dynamic.bag --cam dynamic/camchain_mono.yaml --imu dynamic/imu_adis16448.yaml --bag-from-to 5 45 --imu-models scale-misalignment --timeoffset-padding 0.1 --show-extraction

Similar a 1.3, parámetros:

• --dynamic/dynamic.bag: Ruta del paquete registrado;
• --bag-from-to 5 45Leer de bolsa 5s a 45s;
• --target april_6x6.yaml: archivo de parámetros de la placa de calibración;

target_type: 'aprilgrid'   #gridtype
tagCols: 6              #number of apriltags
tagRows: 6             #number of apriltags
tagSize: 0.088          #size of apriltag, edge to edge [m]
tagSpacing: 0.3         #ratio of space between tags to tagSize
codeOffset: 0           #code offset for the first tag in the aprilboard

• --cam dynamic/camchain.yaml: La referencia interna de la cámara calibrada antes (usar directamente estos datos no es correcto), si no está calibrada con kalibr, es posible que deba modificar un archivo de acuerdo con el siguiente formato:

cam0:
  cam_overlaps: [1, 3]
  camera_model: pinhole
  distortion_coeffs: [-0.0016509958435871643, 0.02437222940989351, -0.03582816956989852,
    0.019860839087717054]
  distortion_model: equidistant
  intrinsics: [461.487246372674, 460.1113992557959, 356.39105303227853, 231.15719697054647]
  resolution: [752, 480]
  rostopic: /cam0/image_raw

• --imu dynamic/imu_adis16448.yaml: Utilice la referencia interna de la IMU calibrada por el método anterior, complete el siguiente formato

rostopic: /imu0
update_rate: 200.0 #Hz

accelerometer_noise_density: 0.01 #continous
accelerometer_random_walk: 0.0002

gyroscope_noise_density: 0.005 #continous
gyroscope_random_walk: 4.0e-06

• --imu-models scale-misalignment: Modelo paramétrico de la IMU
• --timeoffset-padding 0.1: Cuanto mayor sea el valor, mayor será el tiempo de ejecución de la calibración

Una vez completada la calibración, el dibujo de resultados aparecerá automáticamente:

al mismo tiempo, se generan varios archivos de resultados en la carpeta de datos, que incluyen:

• La diferencia de sincronización de tiempo entre los parámetros internos y externos de la cámara y la imu (el retraso de la IMU con respecto a la cámara):

cam0:
  T_cam_imu:
  - [0.01676354570202121, 0.9998590327410452, -0.000947724452812317, 0.06843047675330072]
  - [-0.9998594579273542, 0.016763745309816058, 0.0002030674908090371, -0.014939612219976138]
  - [0.00021892627629230532, 0.0009441871264906492, 0.999999530290868, -0.0038646995667698156]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [1, 3]
  camera_model: pinhole
  distortion_coeffs: [-0.0016509958435871643, 0.02437222940989351, -0.03582816956989852, 0.019860839087717054]
  distortion_model: equidistant
  intrinsics: [461.487246372674, 460.1113992557959, 356.39105303227853, 231.15719697054647]
  resolution: [752, 480]
  rostopic: /cam0/image_raw
  timeshift_cam_imu: 5.9040012306710654e-05

• Parámetros internos y externos de la IMU:

imu0:
  T_i_b:
  - [1.0, 0.0, 0.0, 0.0]
  - [0.0, 1.0, 0.0, 0.0]
  - [0.0, 0.0, 1.0, 0.0]
  - [0.0, 0.0, 0.0, 1.0]
  accelerometer_noise_density: 0.01
  accelerometer_random_walk: 0.0002
  gyroscope_noise_density: 0.005
  gyroscope_random_walk: 4.0e-06
  model: calibrated
  rostopic: /imu0
  time_offset: 0.0
  update_rate: 200.0

• archivo txt de resultados de calibración
• El documento pdf del dibujo del resultado de la calibración.

4. Calibración conjunta de cámara y LiDAR

El método de calibración es el paquete de calibracióncalibration_camera_lidar separado de Autoware
(1) cmake instala nlopt normalmente

git clone https://github.com/stevengj/nlopt.git
cd nlopt
mkdir build && cd build
cmake ..
make
sudo make install

Tenga cuidado de no usar sudo apt-get install para instalar, catkin_make encontrará problemas más adelante
(2) Compilecalibration_camera_lidar:

catkin_make

• El paquete anterior necesita agregar compatibilidad con ROS de versión melódica o noética, modifique la línea CMakelists.txt 72/94/114:

# if ("{ROS_VERSION}" MATCHES "(indigo|jade|kinetic)")
if ("{ROS_VERSION}" MATCHES "(indigo|jade|kinetic|melodic|noetic)")

• Si se informa un error

jsk_recognition_msgsConfig.cmake
jsk_recognition_msgs-config.cmake

Explique que falta el paquete jsk-recognition-msgs para la compilación, instale:

sudo apt-get install ros-<ROS VERSION NAME>-jsk-recognition-msgs

• Error: error: ‘CV_RGB’ was not declared in this scope; did you mean ‘CV_YUV2RGB’?de acuerdo con el indicador de posición, cambie el CV_RGB de la posición de error a cvScalar
• Error: e rror: no match for ‘operator=’ (operand types are ‘CvMat’ and ‘cv::Mat’)
  Este es el problema de la conversión mutua entre CvMat y cv::Mat. Cambie las dos oraciones incorrectas a:

*m_intrinsic =  cvMat(m_intrinsic_opencv2to1);
*m_dist = cvMat(m_dist_opencv2to1);

• Error: cambie cv_image->imageel error a Tenga en cuenta que en diferentes versiones, cvIplimage puede escribirse como Iplimage y cvMat puede escribirse como CvMat.cv_image->imagecvIplImage(cv_image->image)
  

Todo lo anterior son problemas con la versión de OpenCV.El código está escrito en base a opencv2 y no es compatible con opencv3/4.

• CMakeFiles/calibration_publisher.dir/nodes/calibration_publisher/calibration_publisher.cpp.o: En la función void cv::operator>><cv::Mat>(cv::FileNode const&, cv::Mat&)': /usr/local/include/opencv2/core/persistence.hpp:1261: undefined reference to cv::Mat::Mat()'
  Enlace opencv para el nodo de calibración_publisher en CMakeLists.txt

target_link_libraries(calibration_publisher
        ${catkin_LIBRARIES}
        ${OpenCV_LIBS})

(3) Paquete de grabación: elija arreglar el automóvil, mueva la placa de calibración, el movimiento de la placa de calibración es similar a la calibración de la cámara, en relación con la parte delantera del automóvil, lejos y cerca, izquierda y derecha, y varias posturas de inclinación (puede optar por utilizar Autoware o grabar); (4) rosbasg record TOPICPaquete de funciones de ejecución
:

source devel/setup.bash
# 新开一个终端执行roscore
rosrun calibration_camera_lidar calibration_toolkit

(5) Método de calibración
1) Use Autoware para reproducir y pausar el conjunto de datos;
2) Seleccione el tema de la imagen después del inicio (elija según el tema que publicó);
3) Seleccione el tipo de calibración: cámara a Velodyne;
porque la herramienta es basado en autoware, así que aquí El tema de la nube de puntos debe convertirse a /points_raw.Si el tema de la nube de puntos es incorrecto, se puede modificar de las siguientes dos maneras:

• Cambie directamente el ID de fotograma a: velodyne en la configuración de parámetros del controlador de radar y cambie el tema a: /points_raw
• Cuando juegues a través de rosbag, cambia el tema
  . Por ejemplo:

rosbag play autoware-20221006.bag /rslidar_points:=/points_raw

4) Haga clic en Aceptar para abrir la interfaz de calibración. El tamaño del patrón que se completará en el cuadro de texto sobre la interfaz es la longitud lateral del tablero de ajedrez en el tablero de calibración (m), y el número del patrón es el tamaño de la esquina (tenga en cuenta que es el número de puntos de las esquinas internas, es decir, la longitud y el ancho de la cuadrícula de tablero de ajedrez.-1)

5) Antes de la calibración, importe el archivo yml calibrado por los parámetros internos de la cámara (debe ser el resultado de la calibración usando este paquete de funciones, y este paso no es necesario, porque los siguientes pasos calibrarán los parámetros internos de la cámara al mismo tiempo) , haga clic en cargar para importar el archivo; una vez completada la importación, cameramat /distcoeff /imagesize mostrará el valor del parámetro interno correspondiente;

6) Ajuste el tamaño de la ventana, haga clic en la parte negra con el mouse, presione b para ajustar el color, presione 2 para cambiar de modo, ajuste el ángulo de visión y muestre los datos de la nube de puntos.Método de ajuste de visualización LiDAR: mover la nube de puntos : rotar nube de puntos: cambiar de modo: zoom de ángulo
de visión : tamaño de nube de puntos: cambiar el color de fondo de la ventana de la nube de puntos:上下左右方向键、PgUp、PgDn
a、d、w、s、q、e
数字1和数字2
减号缩小、加号放大
o键缩小点云、p放大点云（建议放大，较为清晰）
b（建议背景全部调整成白色，清晰）

Detenga el conjunto de datos después de reproducir un cuadro adecuado (el tablero de calibración está completo y claro en la imagen y la nube de puntos) y haga clic en Capturar para agregar un cuadro de datos a la secuencia de calibración alternativa. Luego, ajuste el ángulo de los datos del láser en la esquina inferior derecha para que el eje de coordenadas golpee el centro de la placa de calibración, haga clic en el botón izquierdo para marcar el centro rojo y agréguelo al cuadro de calibración. Cuando la marca roja es incorrecta, puede hacer clic derecho para cancelar la marca roja. Si no está satisfecho con este marco alternativo, puede hacer clic en Eliminar en la esquina superior derecha. Como se muestra abajo:

Marque alrededor de 30 cuadros, luego haga clic en calibrar para comenzar a calcular los resultados de la calibración, y CameraExtrinsicMat /cameramat /distcoeff /imagesize mostrará los valores de los parámetros correspondientes. Después de calcular el resultado, puede hacer clic en proyectar para proyectar el punto láser en la placa de calibración en la imagen para verificar aproximadamente si la calibración es correcta o no, el punto rojo en la imagen y finalmente hacer clic en Guardar en la esquina superior izquierda para guarde el resultado de la calibración (solo guarde el resultado de la calibración conjunta, incluido el interior y el exterior de la cámara Referencia: sistema de coordenadas de la cámara al sistema de coordenadas del radar; guárdelo como un archivo .yml, no guarde los datos utilizados para la calibración, porque los datos es demasiado grande, puede guardarlo si es necesario)

(7) Después de guardar el resultado, puede visualizar el resultado de la fusión de la cámara y la calibración de LiDAR a través de Autoware, reproducir los datos en Simulación, hacer clic en Calibration Publisher en la interfaz de detección para cargar el archivo de resultados de calibración, la ID del marco LIDAR en el datos de calibración de target_frame; el marco de la cámara visual en los datos de calibración de camera_frame ID; en la columna de selección de referencia, cargue el archivo yml

Haga clic en la imagen de puntos nuevamente y luego abra rviz. Abra la barra de menú Paneles-Agregar nuevo panel, seleccione el complemento ImageViewPlugin, seleccione la imagen y el tema de la nube de puntos, y luego continúe reproduciendo el conjunto de datos, puede ver la fusión de la imagen y la nube de puntos

5. Calibración conjunta de LiDAR e IMU

Utilice el paquete de calibración lidar_imu_calib. Después de la calibración en los dos capítulos anteriores, puede calcular directamente los parámetros externos de LiDAR e IMU, pero también puede confirmar si la calibración anterior es correcta o si el dispositivo no tiene un sensor visual.

5.1 Solución 1: código abierto de la Universidad de Zhejiang lidar_IMU_calib

5.2 Solución 2: alineación de lidar

5.3 Solución 3: lidar_imu_calib

Este paquete de funciones solo calibra el componente de actitud en la conversión entre lidar e imu
(1) Descargue el código fuente de github y compílelo en su propio espacio de trabajo:

catkin_make

Si encuentra una gran cantidad de errores de PCL, es porque la versión ROS de ubuntu 20.04 es noética y su propio pcl es pcl-1.10. Esta versión debe compilarse con c ++ 14 o superior y modificarse en CMakeLists. txt (2
) Paquete de grabación: El entorno de grabación debe estar en lugares con muchas características planas y lineales, como edificios con estructuras ricas, no en entornos degradados (una gran cantidad de árboles, arbustos o túneles), y el automóvil se mueve lentamente ( 3)
Calibración

• Modifique la IMU, el nombre del tema del mensaje LiDAR y la ruta del paquete en el archivo de inicio:

<launch>
    <node pkg="lidar_imu_calib" type="calib_exR_lidar2imu_node" name="calib_exR_lidar2imu_node" output="screen">
        <param name="/lidar_topic" value="/velodyne_points"/>
        <param name="/imu_topic" value="/imu/data"/>
        <param name="/bag_file" value="/media/zard/SLAMData/2017-06-08-15-52-45_3.bag"/>
    </node>
</launch>

• Paquete de función de ejecución:

roslaunch lidar_imu_calib calib_exR_lidar2imu.launch

El paquete de funciones mostrará mensajes lidar agregados gradualmente y, finalmente, mostrará la cantidad de mensajes lidar y la cantidad de imu, formando la cantidad de restricciones, y el resultado del cálculo aparecerá después de un tiempo: