Skip to content

tutorial_motion_estimator

Jump to bottom
Edluel edited this page May 27, 2021 · 2 revisions

Tutorial - MotionEstimator

O MotionEstimator é o objeto responsável por calcular o movimento 3D (mudança de pose, dada por uma rotação e translação) entre duas imagens RGB-D. Para isto, o objeto utiliza duas nuvens de pontos 3D, obtidas a partir das imagens RGB-D, e calcula a transformação que alinha uma nuvem de pontos com a outra. Esta transformação corresponde ao inverso do movimento realizado pela câmera entre os instantes de tempo relacionados às duas nuvens de pontos utilizadas como entrada.

Este guia comenta por partes a aplicação motion_estimator_test, que apresenta como o MotionEstimatorRANSAC da biblioteca RGB-D RTK deve ser utilizado.

Especificamente, este documento detalha:

  1. Pré-requisitos
  2. Os arquivos de cabeçalho necessários (arquivos .h)
  3. O funcionamento do programa
  4. Como configurar e executar o programa

1. Pré-requisitos

Este guia assume que tutoriais relacionados a outras funcionalidades da biblioteca RGB-D RTK foram lidos previamente.

A tabela abaixo resume os principais objetos envolvidos e qual a sua funcionalidade dentro do programa.

Classe Objeto Funcionalidade
RGBDLoader loader Carregar e fornecer sequência de imagens RGB-D
ReconstructionsVisualizer visualizer Exibir visualização 3D da aplicação
EventLogger logger Imprimir/gravar em arquivo mensagens do programa
ConfigLoader param_loader Carregar e fornecer parâmetros do programa
KLTTracker tracker Rastrear pontos 2D de imagem para imagem

2. Arquivos de cabeçalho relacionados

Além dos arquivo de cabeçalho (arquivos .h) necessários para instanciar os objetos listados acima, também são necessários os seguintes includes:

#include <Eigen/Geometry>
#include <geometry.h>
#include <common_types.h>
#include <motion_estimator_ransac.h>

  • Eigen/Geometry: contém classes para representar transformações 3D (rotações/translações), como a classe Affine3f, utilizada pelo programa

  • common_types.h: contém o tipo estruturado Intrinsics, que armazena os parâmetros intrínsecos de calibração (p. ex. distância focal) da câmera RGB-D

  • geometry.h: contém a função para obter uma nuvem de pontos 3D a partir de dados RGB e profundidade

  • motion_estimator_ransac.h: contém a classe MotionEstimatorRANSAC, utilizada para calcular a transformação que alinha duas nuvens de pontos

3. Funcionamento do programa motion_estimator_test

Código do programa

int main(int argc, char **argv)
{
    EventLogger& logger = EventLogger::getInstance();
    logger.setVerbosityLevel(EventLogger::L_DEBUG);

    RGBDLoader loader;
    KLTTracker tracker;
    Intrinsics intr(0);
    ReconstructionVisualizer visualizer;

    Mat frame, depth;
    string index_file;
    float ransac_distance_threshold, ransac_inliers_ratio;
    Eigen::Affine3f pose = Eigen::Affine3f::Identity();
    Eigen::Affine3f trans = Eigen::Affine3f::Identity();
    pcl::PointCloud<PointT>::Ptr prev_cloud(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr curr_cloud(new pcl::PointCloud<PointT>);

    if(argc != 2)
    {
        logger.print(EventLogger::L_INFO, "[motion_estimation_test.cpp] Usage: %s <path/to/config_file.yaml>\n", argv[0]);
        exit(0);
    }
    ConfigLoader param_loader(argv[1]);
    param_loader.checkAndGetString("index_file", index_file);
    param_loader.checkAndGetFloat("ransac_distance_threshold", ransac_distance_threshold);
    param_loader.checkAndGetFloat("ransac_inliers_ratio", ransac_inliers_ratio);
    loader.processFile(index_file);

    MotionEstimatorRANSAC motion_estimator(intr, ransac_distance_threshold,
                                            ransac_inliers_ratio);

    //Track points on each image
    for(int i = 0; i < loader.num_images_; i++)
    {
        //Load RGB-D image and point cloud 
        loader.getNextImage(frame, depth);
        *curr_cloud = getPointCloud(frame, depth, intr);

        //Track feature points in the current frame
        tracker.track(frame);

        //Estimate motion between the current and the previous frame/point clouds
        if(i > 0)
        {
            trans = motion_estimator.estimate(tracker.prev_pts_, prev_cloud,
                                              tracker.curr_pts_, curr_cloud);
            pose = pose*trans;
        }

        //View tracked points
        for(size_t k = 0; k < tracker.curr_pts_.size(); k++)
        {
            Point2i pt1 = tracker.prev_pts_[k];
            Point2i pt2 = tracker.curr_pts_[k];
            circle(frame, pt1, 1, CV_RGB(255,0,0), -1);
            circle(frame, pt2, 3, CV_RGB(0,0,255), -1);
            line(frame, pt1, pt2, CV_RGB(0,0,255));
        }

        if(i == 0) visualizer.addReferenceFrame(pose, "origin");
        //visualizer.addQuantizedPointCloud(curr_cloud, 0.3, pose);
        visualizer.viewReferenceFrame(pose);
        //visualizer.viewPointCloud(curr_cloud, pose);
        //visualizer.viewQuantizedPointCloud(curr_cloud, 0.02, pose);

        visualizer.spinOnce();

        //Show RGB-D image
        imshow("Image view", frame);
        imshow("Depth view", depth);
        char key = waitKey(1);
        if(key == 27 || key == 'q' || key == 'Q')
        {
            logger.print(EventLogger::L_INFO, "[motion_estimator_test.cpp] Exiting\n", argv[0]);
            break;
        }

        //Let the prev. cloud in the next frame be the current cloud
        *prev_cloud = *curr_cloud;
    }

    visualizer.close();

    return 0;
}

O programa motion_estimator_test recebe um parâmetro de linha de comando, sendo ele uma string contendo o caminho completo para um arquivo de configuração. Caso esta string não seja informada, o programa se encerra com uma mensagem informando como executá-lo corretamente.

float ransac_distance_threshold, ransac_inliers_ratio;
Eigen::Affine3f pose = Eigen::Affine3f::Identity();
Eigen::Affine3f trans = Eigen::Affine3f::Identity();
pcl::PointCloud<PointT>::Ptr prev_cloud(new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr curr_cloud(new pcl::PointCloud<PointT>);
Intrinsics intr(0);

O método Identity() da classe Affine3f atribuído as variáveis pose e trans retorna uma matriz identidade. Já as variáveis prev_cloud e curr_cloud são nuvens de e PointT na classe PointCloud significa que cada ponto terá dados de cor RGB e cordenadas tridimensionais. E ao final do bloco é chamada a função especial Ìntrisics que inicializa o objeto intr que contem os parâmetros de calibração da câmera.

No bloco acima ransac_distance_threshold é o limite utilizado pelo algorítimo RANSAC e ransac_inliers_ratio são os pontos aceitos de acordo com o limite de erro dado. RANSAC é abreviação de "RANdom SAmple Consensus" e significa que ele pega esses pontos randomicamente e só valida se o erro entre eles for aceitável.

O MotionEstimatorRANSAC na linha abaixo é baseado no RANSAC.

MotionEstimatorRANSAC motion_estimator(intr, ransac_distance_threshold,
                                       ransac_inliers_ratio);

A linha acima instancia um objeto da classe MotionEstimatorRANSAC. Este objeto é o responsável por calcular o movimento de câmera com base nos dados de nuvens de pontos 3D.

Em seguida, para cada imagem RGB-D do dataset, o programa procede como descrito a seguir.

loader.getNextImage(frame, depth); //carregar imagem
*curr_cloud = getPointCloud(frame, depth, intr);//salva os pontos de quadros anteriores
tracker.track(frame);[//](//rastreia)rastreia a movimentação na núvem de pontos

Após obter a imagem RGB-D atual da sequência e a nuvem de pontos correspondente, é realizado um rastreamento de pontos 2D (features) na imagem atual pelo tracker (rastreador de pontos).

if(i > 0)
{
    trans = motion_estimator.estimate(tracker.prev_pts_, prev_cloud,
                                      tracker.curr_pts_, curr_cloud);
    pose = pose*trans;
}

No bloco acima, caso mais de uma imagem tenha sido processada pelo tracker, o programa é capaz de calcular o movimento que a câmera realizou entre duas imagens RGB-D. Especificamente, as linhas acima fornecem ao MotionEstimator os pontos 2D e 3D da imagem anterior e também da imagem atual. O MotionEstimator calcula a transformação 3D que a câmera realizou e retorna o resultado na variável trans.

Como trans contém apenas o movimento de câmera realizado entre a imagem anterior a atual, o resultado é multiplicado com a transformação anterior, armazenado na variável pose. Desta forma, pose irá conter qual a posição/orientação da câmera da imagem atual em relação à origem do sistema de coordenadas (que é o sistema de coordenadas centrado na primeira câmera da sequência de imagens).

As próximas linhas do programa são responsáveis por mostrar em uma imagem os pontos que estão sendo rastreados e também a visualização 3D da nuvem de pontos e movimento da câmera.

*prev_cloud = *curr_cloud;

Por fim, a linha acima atribui a nuvem de pontos atual ao objeto que armazena a nuvem de pontos anterior. Desta forma, na próxima iteração do laço, o movimento será calculado sempre em relação à nuvem de pontos anterior à nuvem de pontos atual.

4. Configuração e execução do programa

  1. Dentro da pasta da biblioteca RGB-D RTK, abra o arquivo config_files/tum_odometry.yaml (caso deseje utilizar um dataset TUM RGB-D) ou config_files/marker_detection.yaml (caso deseje utilizar um dataset Natalnet RGB-D).

  2. Altere o parâmetro index_file do arquivo para que ele contenha entre as aspas o caminho do dataset a ser utilizado na execução do programa. A linha deve ficar como mostrada a seguir:

index_file: "PATH/ar_sm_coffeemaker_only/index", onde PATH deve ser algo como "/home/usuario/datasets/"

  1. Após salvar e fechar o arquivo, execute o comando no terminal para executar o programa passando o caminho completo para o arquivo de configuração desejado:

motion_estimator_test /home/usuario/rgbd_rtk/config_files/marker_detection.yaml

Installing prerequisites

Installing RGB-D RTK

Tutoriais em Português

Getting to know RGB-D RTK

  • Soon
Clone this wiki locally