Posicionamiento de fusión multisensor (construcción de mapas de nubes de 3 puntos y posicionamiento basado en mapas) Detección de bucles ScanContext de 3 realizaciones

Posicionamiento de fusión multisensor (construcción de mapas de nubes de 3 puntos y posicionamiento basado en mapas) Detección de bucles ScanContext de 3 realizaciones

Blog de referencia
cero hecho posicionamiento automático de la unidad (XI):
sistema de piloto automático de corrección de circuito cerrado y elemento de combate avanzado (iv) un mapa de piloto automático de reconocimiento de escena tridimensional detectado con precisión sistemas de construcción y
piloto automático de combate y elemento ( 5) Utilice SC-LEGO-LOAM para la construcción de mapas de nubes de puntos a gran escala y la optimización de circuito cerrado

Código fuente:
parte del código fuente utilizado aquí es de github sc-lego-loam. El
marco de código general es de Rengan github tag11 código de corrección de ciclo cerrado

pd: porque soy un poco blanco, aquí es solo para expresar mis propias opiniones e ideas de aprendizaje, espero corregirme

Idea de código

Marco general

Echemos un vistazo al marco general del
blog de referencia desde cero para hacer el posicionador automático del variador (IX): construya una optimización

de la configuración del sistema de esto, para completar la detección de bucle módulo a módulo

Entre ellos, el módulo de detección de bucle cerrado contiene dos partes: la coincidencia de la detección de tramas de bucle cerrado y la nube de puntos.
Los bloques de código correspondientes son los siguientes:

    //闭环检测模块  （任乾）
    if (!DetectNearestKeyFrame(key_frame_index))
        return false;
        
	//点云配准 
    if (!CloudRegistration(key_frame_index))
        return false;

Al llamar a ScanContext, la detección de bucle se realiza en la nube de puntos del cuadro actual y la nube de puntos del cuadro histórico, y se obtienen key_frame_index (cuadro de bucle detectado) y yaw diff (ángulo de desviación de pose); luego, el cuadro de bucle detectado se envía a la nube de puntos de registro. Módulo de registro

Escritura de código

Trasplantado sc-lego-franco de ScanContext código fuente pertinente y archivos de bibliotecas

Agregar archivo de origen relacionado scan_context / Scancontext.cpp


agregar archivo de encabezado

Tenga en cuenta que CMakeLists.txt agrega

Adición de ruta nativa OpenCV

#本机OpenCV路径查看
locate   OpenCVConfig.cmake

Llame principalmente a dos funciones en SC

scManager.detectLoopClosureID (); Función de detección de tramas de bucle invertido, devuelve -1 si no se detecta y devuelve el índice de la trama correspondiente cuando se detecta una trama de bucle invertido

scManager.makeAndSaveScancontextAndKeys (* raw_cloud_ptr); Guarda la nube de puntos del marco actual en tiempo real para la coincidencia de bucles

Función de escritura

SaveCurrentFrame (int pcd_idx) se
utiliza para guardar la nube de puntos del marco actual, imitando el método de registro de la nube de puntos, obtener el pcd de la nube de puntos del marco actual del disco en tiempo real y almacenarlo en el puntero raw_cloud_ptr, y llamar a scManager.makeAndSaveScancontextAndKeys para guardar el marco actual

DetectScanContext (int & key_frame_index, int pcd_idx)
detecta el marco del bucle, si es verdadero, genera el índice y la pose de desviación del marco del bucle; de ​​lo contrario, flase devuelve -1

void LoopClosing::SaveCurrentFrame(int pcd_idx){
    
       
        // 读取并存储当前点云
    std::string file_path = key_frames_path_ + "/key_frame_" + std::to_string(pcd_idx) + ".pcd";
    CloudData::CLOUD_PTR raw_cloud_ptr(new CloudData::CLOUD());
     pcl::io::loadPCDFile(file_path, *raw_cloud_ptr);     //   raw_cloud_ptr  存储当前帧点云的point
    scManager.makeAndSaveScancontextAndKeys(*raw_cloud_ptr);     
}

// scan context  回环检测模块
int LoopClosing::DetectScanContext(int& key_frame_index,int pcd_idx) {
    
            
    SaveCurrentFrame(pcd_idx);

    auto detectResult = scManager.detectLoopClosureID();     //  进行回环帧的检测  first: nn index, second: yaw diff

    key_frame_index = detectResult.first;    // 获取回环帧的索引
    yawDiffRad = detectResult.second; // not use for v1 (because pcl icp withi initial somthing wrong...)
    
    return   key_frame_index;    //没有检测到返回 -1 ，检测到回环帧返回   对应帧 的索引 
}

Funciones de llamada

Reemplace la parte de detección de bucle de DetectNearestKeyFrame por el Sr.Ren Qian, por ejemplo: ScanContext hace un posicionamiento aproximado. Después de detectar el cuadro de bucle, envíe el cuadro de bucle al siguiente módulo de registro de nube de puntos CloudRegistration (posicionamiento fino) para completar todo el posicionamiento del bucle ¡Reparar! ! !

bool LoopClosing::Update(const KeyFrame key_frame, const KeyFrame key_gnss) {
    
    
    has_new_loop_pose_ = false;

    all_key_frames_.push_back(key_frame);
    all_key_gnss_.push_back(key_gnss);

    int key_frame_index = 0;

   //  scan  context   回环检测 
    if (DetectScanContext(key_frame_index,key_frame.index)  == -1)       //  key_frame_index  闭环检测帧 ；   key_frame.index  当前帧数
        return false;

    //任乾  闭环检测模块
    // if (!DetectNearestKeyFrame(key_frame_index))
    //     return false;
    
    if (!CloudRegistration(key_frame_index))
        return false;

    has_new_loop_pose_ = true;
        return true;
}

Error de depuración

Durante el proceso de depuración, se produjo el motivo de la lectura fuera de los límites. El motivo es que en el módulo de coincidencia de nubes de puntos, la estrategia de registro adoptada es el método Scan2Map. La construcción del mapa requiere la recuperación de la fusión del marco cerca del marco del anillo. Si el índice es 0, el mapa se construye Cuando el marco cercano no existe, dará lugar al error de señalar fuera de límites.

Parte modificada: agregar en JointMap

//越界保护
  if (i < 0 || i >= all_key_frames_.size( ) )    continue;

efecto final

Utilice la detección de bucle DetectNearestKeyFrame de Rengan

rosservice call /optimize_map

Genere optimizado.txt

para la evaluación de evo

evo_ape kitti ground_truth.txt optimized.txt -r full --plot --plot_mode xyz

Detección de bucle mediante DetectScanContext

A través de la comparación de datos, se puede ver que hay poca diferencia entre el módulo de detección de bucle del Sr.Ren Qian