Posicionamento de fusão multissensor (construção de mapa de nuvem de 3 pontos e posicionamento baseado em mapa) Detecção de loop ScanContext de 3 pontos

Posicionamento de fusão multissensor (construção de mapa de nuvem de 3 pontos e posicionamento baseado em mapa) Detecção de loop ScanContext de 3 pontos

Consulte o blog
para fazer o posicionamento de direção automática a partir do zero (11):
Avanço do sistema de direção automática de correção de loop fechado e combate real do projeto (4) Reconhecimento de cena 3D e detecção de loop fechado na construção de mapas de alta precisão de direção automática .
5) Use SC-LEGO-LOAM para construção de mapa de nuvem de pontos em grande escala e otimização de loop fechado

Código-fonte:
parte do código-fonte usado aqui é do github sc-lego-loam. A
estrutura geral do código é do código de correção de loop fechado do github tag11 de Rengan

ps: Porque sou um pouco branco, aqui é só expressar minhas próprias opiniões e aprender ideias, espero me corrigir

Idéia de código

Quadro geral

Vamos dar uma olhada na estrutura geral do
blog de referência a partir do zero para fazer o posicionador automático da unidade (IX): construir uma otimização

da configuração do sistema para completar o módulo de detecção de loop para módulo

Entre eles, o módulo de detecção de loop fechado contém duas partes: detecção de quadro de loop fechado e correspondência de nuvem de pontos
. Os blocos de código correspondentes são os seguintes:

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

Ao chamar ScanContext, a detecção de loop da nuvem de ponto de quadro atual e da nuvem de ponto de quadro histórico é realizada, e key_frame_index (quadro de loop detectado) e yaw diff (ângulo de desvio de pose) são obtidos; então, o quadro de loop detectado é enviado para nuvem de ponto de CloudRegistration Módulo de registro

Escrita de código

Transplantado sc-lego-loam de código-fonte relevante e arquivos de biblioteca do ScanContext

Adicionar arquivo de origem relacionado scan_context / Scancontext.cpp


adicionar arquivo de cabeçalho

Observe que CMakeLists.txt adiciona

Adição de caminho nativo OpenCV

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

Chame principalmente duas funções em SC

scManager.detectLoopClosureID (); função de detecção de quadro de loopback, retorna -1 se não detectado, retorna o índice do quadro correspondente quando o quadro de loopback é detectado

scManager.makeAndSaveScancontextAndKeys (* raw_cloud_ptr); Salve a nuvem de pontos do quadro atual em tempo real para correspondência de loop

Função de escrita

SaveCurrentFrame (int pcd_idx) é
usado para salvar a nuvem de pontos do quadro atual, imitando o método de registro da nuvem de pontos, o pcd da nuvem de pontos do quadro atual é obtido do disco em tempo real e armazenado no ponteiro raw_cloud_ptr, e scManager.makeAndSaveScancontextAndKeys é chamado para salvar o quadro atual

DetectScanContext (int & key_frame_index, int pcd_idx)
detecta o quadro de loop, se verdadeiro, exibe o índice e a posição de desvio do quadro de loop; caso contrário, flase retorna -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 ，检测到回环帧返回   对应帧 的索引 
}

Funções de chamada

Substitua a parte de detecção de loop de DetectNearestKeyFrame pelo Sr. Ren Qian, por exemplo: ScanContext faz um posicionamento aproximado. Depois de detectar o quadro de loop, envie o quadro de loop para o próximo módulo de registro de nuvem de ponto CloudRegistration (posicionamento preciso) para completar o posicionamento de loop inteiro. Consertar! ! !

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;
}

Depuração de erros

Durante o processo de depuração, ocorre o motivo da leitura fora dos limites. O motivo é que, no módulo de correspondência de nuvem de pontos, a estratégia de registro adotada é o método Scan2Map. A construção do mapa exige a recuperação da fusão de quadros perto do quadro de anel. Se o índice for 0, o mapa é construído Quando o quadro próximo não existe, isso levará ao erro de apontar para fora dos limites.

Parte modificada: adicionar JointMap

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

efeito final

Use a detecção de loop DetectNearestKeyFrame de Rengan

rosservice call /optimize_map

Gerar optimized.txt

para avaliação evo

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

Detecção de loop usando DetectScanContext

Através da comparação de dados, pode-se ver que há pouca diferença entre o módulo de detecção de loop do Sr. Ren Qian