Método do Fluxo Óptico Lucas-Kanade

Verificação dos Parâmetros de Entrada

O primeiro passo na implementação é validar os parâmetros passados para o programa. Neste caso, esperamos receber apenas o caminho para o diretório contendo o arquivo de associação "associate.txt", o que resulta na verificação de argc igual a 2.

string caminho_dataset = argv[1];
string arquivo_associado = caminho_dataset + "/associate.txt";

Aqui definimos duas variáveis do tipo string para armaeznar, respectivamente, o caminho absoluto do diretório e o caminho completo do arquivo de associação.

Leitura do Arquivo de Associação

Criamos um fluxo de entrada de arquivo para processar o arquivo de associação:

ifstream fin(arquivo_associado);
if (!fin) 
{
    cerr<<"Não foi possível encontrar o arquivo associate.txt!"<<endl;
    return 1;
}

É importante notar que a verificação !fin não testa se o ponteiro é nulo, mas sim utiliza o operador de sobrecarga definido na classe ifstream, que retorna true se a abertura do arquivo falhar.

Estrutura de Dados para Pontos-Chave

Para facilitar a manipulação dos pontos-chave durante o rastreamento pelo método LK, utilizamos uma lista encadeada:

list<cv::Point2f> pontos_chave;

A escolha por uma lista se deve à sua eficiência em operações de inserção e remoção, necesssárias quando pontos são perdidos durante o rastreamento.

Processamento das Imagens

Implementamos um loop para processar as sequências de imagens:

for (int indice = 0; indice < 100; indice++)
{
    // Leitura dos dados do arquivo de associação
    fin>>tempo_rgb>>arquivo_rgb>>tempo_depth>>arquivo_depth;
    
    // Carregamento das imagens
    cor = cv::imread(caminho_dataset + "/" + arquivo_rgb);
    profundidade = cv::imread(caminho_dataset + "/" + arquivo_depth, -1);
    
    // Verificação de carregamento correto
    if (cor.data == nullptr || profundidade.data == nullptr)
        continue;
}

No primeiro frame, detectamos pontos de interesse usando o método FAST:

if (indice == 0)
{
    vector<cv::KeyPoint> kps;
    cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
    detector->detect(cor, kps);
    for (auto kp : kps)
        pontos_chave.push_back(kp.pt);
    ultima_cor = cor;
    continue;
}

Rastreamento pelo Método LK

Para os frames subsequentes, aplicamos o método Lucas-Kenade para rastrear os pontos:

vector<cv::Point2f> proximos_pontos;
vector<cv::Point2f> pontos_anteriores;
for (auto kp : pontos_chave)
    pontos_anteriores.push_back(kp);

vector<unsigned char> status;
vector<float> erro;

// Cálculo do fluxo óptico
cv::calcOpticalFlowPyrLK(ultima_cor, cor, pontos_anteriores, proximos_pontos, status, erro);

Após o rastreamento, removemos os pontos que não foram bem rastreados:

int i = 0;
for (auto iter = pontos_chave.begin(); iter != pontos_chave.end(); i++)
{
    if (status[i] == 0)
    {
        iter = pontos_chave.erase(iter);
        continue;
    }
    *iter = proximos_pontos[i];
    iter++;
}

Visualização dos Resultados

Finalmente, visualizamos os pontos-chave na imagem atual:

cv::Mat img_mostra = cor.clone();
for (auto kp : pontos_chave)
    cv::circle(img_mostra, kp, 5, cv::Scalar(0, 250, 0), 1);
cv::imshow("pontos_chave", img_mostra);
cv::waitKey(0);

Estimação de Pose 2D-2D

Conversão de Coordenadas Pixel para Câmera

Primeiro, implementamos uma função para converter coordenadas de pixel para coordenadas normalizadas da câmera:

Point2d pixel2camera(const Point2d& p, const Mat& K) {
    return Point2d(
        (p.x - K.at<double>(0,2))/K.at<double>(0,0),
        (p.y - K.at<double>(1,2))/K.at<double>(1,1)
    );
}

Estimação de Pose

A função principal para estimação de pose recebe pontos de interesse correspondentes e calcula a matriz de rotação e vetor de translação:

void estimar_pose_2d2d(const vector<KeyPoint>& pontos1, 
                      const vector<KeyPoint>& pontos2, 
                      const vector<DMatch>& correspondencias, 
                      Mat& R, Mat& t)
{
    // Extração das coordenadas dos pontos correspondentes
    vector<Point2f> pts1, pts2;
    for (int i = 0; i < correspondencias.size(); i++)
    {
        pts1.push_back(pontos1[correspondencias[i].queryIdx].pt);
        pts2.push_back(pontos2[correspondencias[i].trainIdx].pt);
    }

    // Cálculo da matriz fundamental
    Mat matriz_fundamental = findFundamentalMat(pts1, pts2, CV_FM_8POINT);
    
    // Cálculo da matriz essencial
    Point2d ponto_principal(325.1, 249.7);
    double distancia_focal = 521;
    Mat matriz_essencial = findEssentialMat(pts1, pts2, distancia_focal, ponto_principal);
    
    // Recuperação da rotação e translação
    recoverPose(matriz_essencial, pts1, pts2, R, t, distancia_focal, ponto_principal);
}

Verificação da Restrição Epipolar

Para validar os resultados, implementamos uma função que verifica a restrição epipolar para cada par de pontos correspondentes:

void verificar_restricao_epipolar(const vector<KeyPoint>& pontos1, 
                                 const vector<KeyPoint>& pontos2, 
                                 const Mat& R, 
                                 const Mat& t, 
                                 const vector<DMatch>& correspondencias)
{
    // Construção da matriz t^R
    Mat t_x = (Mat_<double>(3,3) <<
               0, -t.at<double>(2,0), t.at<double>(1,0),
               t.at<double>(2,0), 0, -t.at<double>(0,0),
               -t.at<double>(1,0), t.at<double>(0,0), 0);
    
    // Parâmetros intrísecos da câmera
    Mat K = (Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    
    // Verificação para cada par de pontos
    for (DMatch m : correspondencias)
    {
        Point2d pt1 = pixel2camera(pontos1[m.queryIdx].pt, K);
        Mat y1 = (Mat_<double>(3,1) << pt1.x, pt1.y, 1);
        Point2d pt2 = pixel2camera(pontos2[m.trainIdx].pt, K);
        Mat y2 = (Mat_<double>(3,1) << pt2.x, pt2.y, 1);
        Mat d = y2.t() * t_x * R * y1;
        cout << "restrição epipolar = " << d << endl;
    }
}

Código Completo de Implementação

Configuração do CMake

project(implementacaoLK)

set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++11 -O3")

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(implementacaoLK arquivo_principal.cpp)
target_link_libraries(implementacaoLK ${OpenCV_LIBS})

Código Principal

#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/calib3d/calib3d.hpp>

using namespace std;
using namespace cv;

// Função de conversão de coordenadas
Point2d pixel2camera(const Point2d& p, const Mat& K);

// Detecção de características correspondentes
void encontrar_correspondencias(const Mat& img1, const Mat& img2,
                              vector<KeyPoint>& kp1, vector<KeyPoint>& kp2,
                              vector<DMatch>& matches);

// Estimação de pose 2D-2D
void estimar_pose_2d2d(const vector<KeyPoint>& kp1, 
                     const vector<KeyPoint>& kp2, 
                     const vector<DMatch>& matches, 
                     Mat& R, Mat& t);

int main(int argc, char** argv)
{
    if (argc != 2)
    {
        cout << "Uso: implementacaoLK caminho_para_dataset" << endl;
        return 1;
    }
    
    string caminho_dataset = argv[1];
    string arquivo_associado = caminho_dataset + "/associate.txt";
    
    ifstream fin(arquivo_associado);
    if (!fin) 
    {
        cerr << "Não foi possível encontrar o arquivo associate.txt!" << endl;
        return 1;
    }
    
    string arquivo_rgb, arquivo_depth, tempo_rgb, tempo_depth;
    list<cv::Point2f> pontos_chave;
    cv::Mat cor, profundidade, ultima_cor;
    
    for (int indice = 0; indice < 100; indice++)
    {
        fin >> tempo_rgb >> arquivo_rgb >> tempo_depth >> arquivo_depth;
        
        cor = cv::imread(caminho_dataset + "/" + arquivo_rgb);
        profundidade = cv::imread(caminho_dataset + "/" + arquivo_depth, -1);
        
        if (indice == 0)
        {
            // Detecção de características no primeiro frame
            vector<cv::KeyPoint> kps;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect(cor, kps);
            for (auto kp : kps)
                pontos_chave.push_back(kp.pt);
            ultima_cor = cor;
            continue;
        }
        
        if (cor.data == nullptr || profundidade.data == nullptr)
            continue;
            
        // Rastreamento LK para frames subsequentes
        vector<cv::Point2f> proximos_pontos;
        vector<cv::Point2f> pontos_anteriores;
        for (auto kp : pontos_chave)
            pontos_anteriores.push_back(kp);
            
        vector<unsigned char> status;
        vector<float> erro;
        
        auto inicio = chrono::steady_clock::now();
        cv::calcOpticalFlowPyrLK(ultima_cor, cor, pontos_anteriores, proximos_pontos, status, erro);
        auto fim = chrono::steady_clock::now();
        chrono::duration<double> tempo_utilizado = chrono::duration_cast<chrono::duration<double>>(fim - inicio);
        
        cout << "Tempo utilizado pelo LK Flow: " << tempo_utilizado.count() << " segundos." << endl;
        
        // Remoção de pontos perdidos
        int i = 0;
        for (auto iter = pontos_chave.begin(); iter != pontos_chave.end(); i++)
        {
            if (status[i] == 0)
            {
                iter = pontos_chave.erase(iter);
                continue;
            }
            *iter = proximos_pontos[i];
            iter++;
        }
        
        cout << "Pontos rastreados: " << pontos_chave.size() << endl;
        
        if (pontos_chave.size() == 0)
        {
            cout << "Todos os pontos foram perdidos." << endl;
            break; 
        }
        
        // Visualização
        cv::MostrarImagem img_mostra = cor.clone();
        for (auto kp : pontos_chave)
            cv::circle(img_mostra, kp, 5, cv::Scalar(0, 250, 0), 1);
        cv::imshow("pontos_chave", img_mostra);
        cv::waitKey(0);
        
        ultima_cor = cor;
    }
    
    return 0;
}

Point2d pixel2camera(const Point2d& p, const Mat& K) {
    return Point2d(
        (p.x - K.at<double>(0,2))/K.at<double>(0,0),
        (p.y - K.at<double>(1,2))/K.at<double>(1,1)
    );
}

void encontrar_correspondencias(const Mat& img1, const Mat& img2,
                              vector<KeyPoint>& kp1, vector<KeyPoint>& kp2,
                              vector<DMatch>& matches) {
    // Inicialização
    Mat descritores1, descritores2;
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> extractor = ORB::create();
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    
    // Detecção de pontos de interesse
    detector->detect(img1, kp1);
    detector->detect(img2, kp2);
    
    // Extração de descritores
    extractor->compute(img1, kp1, descritores1);
    extractor->compute(img2, kp2, descritores2);
    
    // Correspondência de descritores
    vector<DMatch> correspondencias_brutas;
    matcher->match(descritores1, descritores2, correspondencias_brutas);
    
    // Filtragem das correspondências
    double min_dist = 10000, max_dist = 0;
    for (int i = 0; i < descritores1.rows; i++) {
        double dist = correspondencias_brutas[i].distance;
        min_dist = min(min_dist, dist);
        max_dist = max(max_dist, dist);
    }
    
    for (int i = 0; i < descritores1.rows; i++) {
        if (correspondencias_brutas[i].distance <= max(2*min_dist, 30.0)) {
            matches.push_back(correspondencias_brutas[i]);
        }
    }
}

void estimar_pose_2d2d(const vector<KeyPoint>& kp1, 
                     const vector<KeyPoint>& kp2, 
                     const vector<DMatch>& matches, 
                     Mat& R, Mat& t) {
    // Parâmetros intrísecos da câmera
    Mat K = (Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    
    // Extração das coordenadas dos pontos correspondentes
    vector<Point2f> pts1, pts2;
    for (int i = 0; i < matches.size(); i++) {
        pts1.push_back(kp1[matches[i].queryIdx].pt);
        pts2.push_back(kp2[matches[i].trainIdx].pt);
    }
    
    // Cálculo da matriz fundamental
    Mat matriz_fundamental = findFundamentalMat(pts1, pts2, CV_FM_8POINT);
    cout << "matriz_fundamental é:" << endl << matriz_fundamental << endl;
    
    // Cálculo da matriz essencial
    Point2d ponto_principal(325.1, 249.7);
    double distancia_focal = 521;
    Mat matriz_essencial = findEssentialMat(pts1, pts2, distancia_focal, ponto_principal);
    cout << "matriz_essencial é:" << endl << matriz_essencial << endl;
    
    // Cálculo da matriz de homografia
    Mat matriz_homografia = findHomography(pts1, pts2, RANSAC, 3);
    cout << "matriz_homografia é:" << endl << matriz_homografia << endl;
    
    // Recuperação da rotação e translação
    recoverPose(matriz_essencial, pts1, pts2, R, t, distancia_focal, ponto_principal);
    cout << "R é:" << endl << R << endl;
    cout << "t é:" << endl << t << endl;
}