# fpt\_trcks/app.cpp

/*******************************************************************************
    Copyright (c) 2021-2022 Qualcomm Technologies, Inc.
    All rights reserved.
    
    Redistribution and use in source and binary forms, with or without
    modification, are permitted (subject to the limitations in the disclaimer
    below) provided that the following conditions are met:
    
    * Redistributions of source code must retain the above copyright notice, this
        list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice,
        this list of conditions and the following disclaimer in the documentation
        and/or other materials provided with the distribution.
    * Neither the name of Qualcomm Technologies, Inc. nor the names of its
        contributors may be used to endorse or promote products derived from this
        software without specific prior written permission.
    
    NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY
    THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
    CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT
    NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
    PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
    OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
    WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
    OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
    ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
    
    @brief
    Program to run the FastADAS Feature Point Tracking pipeline(s).
    *******************************************************************************/
    
    #include <stdio.h>
    #include <stdlib.h>
    #include <string.h>
    #include <vector>
    #include <iostream>
    #include <fstream>
    
    #ifdef USE_OPENCV
    #include <opencv2/highgui/highgui.hpp>
    #include <opencv2/calib3d.hpp>
    #include <opencv2/opencv.hpp>
    #include <opencv2/core/mat.hpp>
    #endif
    
    #include <fadas.h>
    #include "../util/util.h"
    
    using namespace std;

    int main( int argc, char** argv )
    {
        char str[256];
        const char* full_path = str;
        const size_t balign = 128;
        if( FADAS_ERROR_NONE != FadasInit( nullptr ) )  // initialize all FADAS features
        {
            UTIL_ERROR( "FadasInit failed\n" );
            return -1;
        }
    
        const char* path0 = "fpt_0000000.pgm";
        const char* seq_str = "fpt_%07u.pgm";
        uint32_t seq_first = 0;
        uint32_t seq_last = 2;
        const int32_t srch_win = 31;
        const int32_t nms_win = srch_win;
        const int32_t fpe_qual = 30;
        const int32_t n_lvls = 3;
    
        FadasImage_t srcImg0 = { 0 };
        if( UTIL_ReadPGM( path0, srcImg0, true, FADAS_BUF_TYPE_IN ) == false )
        {
            UTIL_ERROR( "reading file" );
            FadasDeInit();
            return -1;
        }
    
        FadasImage_t srcImg1 = { 0 };
        if( FadasCreateImage( FADAS_BUF_TYPE_IN, srcImg0.props.width, srcImg0.props.height, FADAS_IMAGE_FORMAT_Y, &srcImg1, balign ) != FADAS_ERROR_NONE )
        {
            UTIL_ERROR( "FadasCreateImage failed" );
            FadasDestroyImage( &srcImg0 );
            FadasDeInit();
            return -1;
        }
    
        int32_t max_iter = 30;
        float32_t eps = 0.01f;
    
        uint32_t nCorners = 0;
        const uint32_t nCornersMax = 1024;
        float32_t FADAS_ALIGN128( ptX[nCornersMax] );
        float32_t FADAS_ALIGN128( ptY[nCornersMax] );
        int32_t   FADAS_ALIGN128( ptStatus[nCornersMax] );
        uint32_t  FADAS_ALIGN128( ptScore[nCornersMax] );
        float32_t FADAS_ALIGN128( predX[nCornersMax] );
        float32_t FADAS_ALIGN128( predY[nCornersMax] );
        float32_t FADAS_ALIGN128( ptXpred[nCornersMax] );
        float32_t FADAS_ALIGN128( ptYpred[nCornersMax] );
    
        // Let feature set ROI to entire image unless we only want to focus in on one ROI (e.g., bounding box)
        FadasROI_t roi = { 0 };
    
        std::vector<cv::Point2f> p0, p1;
    
    #if defined(USE_OPENCV) && 0  // 1 = use OCV GFFT instead of FPE
        cv::Mat ocvimg0( srcImg0.props.height, srcImg0.props.width, CV_8UC1, srcImg0.plane[0], srcImg0.props.stride[0] );
        cv::TermCriteria termcrit0( cv::TermCriteria::COUNT | cv::TermCriteria::EPS, max_iter, max_eps );
        cv::goodFeaturesToTrack( ocvimg0, p0, nCornersMax, 0.01, nms_win, cv::Mat(), 3, false, 0.04 );
        nCorners = p0.size();
        for( uint32_t i = 0; i < nCorners; ++i )
        {
            ptX[i] = p0[i].x;
            ptY[i] = p0[i].y;
        }
    #else
        FadasFPE* fpe = FadasFPE_Create( srcImg0.props );
        if( fpe == nullptr )
        {
            UTIL_ERROR( "Could not create detector" );
            FadasDestroyImage( &srcImg0 );
            FadasDestroyImage( &srcImg1 );
            FadasDeInit();
            return -1;
        }
    
        if( FadasFPE_SetNmsNxM( fpe, nms_win, nms_win ) != FADAS_ERROR_NONE )
        {
            UTIL_ERROR( "FadasFPE_SetNmsNxM failed\n" );
        }
    
        if( FadasFPE_Run( FADAS_FPE_PIPELINE_FAST10ScoresNmsNxM, fpe, &srcImg0, &roi, fpe_qual,
                          nCornersMax, &nCorners, ptX, ptY, ptScore ) != FADAS_ERROR_NONE )
        {
            UTIL_ERROR( "FadasFPE_Run failed\n" );
        }
    
        if( FADAS_ERROR_NONE != FadasFPE_Destroy(fpe) )
        {
            UTIL_ERROR( "FadasFPE_Destroy failed\n" );
        }
    #endif
    
    #if defined(USE_OPENCV)
        for( uint32_t k = 0; k < nCorners; ++k )
        {
            p0.push_back( cv::Point2f( ptX[k], ptY[k] ) );
        }
    
        cv::Mat ocvimg0_dsply( ocvimg0.size(), CV_8UC3 );
        cv::cvtColor( ocvimg0, ocvimg0_dsply, cv::COLOR_GRAY2RGB );
        for( uint32_t k = 0; k < nCorners; ++k )
        {
            cv::circle( ocvimg0_dsply, cv::Point( ptX[k], ptY[k] ), 1, cv::Scalar( 0, 0, 255 ), 2 );
        }
    
        cv::namedWindow( "Detection", cv::WINDOW_AUTOSIZE );
        cv::imshow( "Detection", ocvimg0_dsply );
        cv::waitKey( 1000 );
    #endif
    
    #ifdef MOTION_MODEL_PREDICTIONS
        FadasFPT* fpt = FadasFPT_Create( FADAS_FPT_PIPELINE_LK_PRED, srcImg0.props, srch_win, n_lvls );
    #else
        FadasFPT* fpt = FadasFPT_Create( FADAS_FPT_PIPELINE_LK, srcImg0.props, srch_win, n_lvls );
    #endif
        if( fpt == nullptr )
        {
            UTIL_ERROR( "Could not create tracker" );
            FadasDestroyImage( &srcImg0 );
            FadasDestroyImage( &srcImg1 );
            FadasDeInit();
            return -1;
        }
    
    #ifdef USE_OPENCV
        cv::Mat ocvimg_trcks = cv::Mat::zeros( srcImg0.props.height, srcImg0.props.width, CV_8UC3 );
    #endif

        // START sequence loop
        for( uint32_t i = seq_first; i <= seq_last; i++ )
        {
            snprintf( str, sizeof(str), seq_str, i );
    
            if( UTIL_ReadPGM( full_path, srcImg1 ) == false )
            {
                UTIL_ERROR( "Cannot read %s", full_path );
                break;
            }
    
            for( uint32_t i = 0; i < nCornersMax; ++i )
            {
                predX[i] = 0;
                predY[i] = 0;
            }
    
    #if defined(USE_OPENCV) && 0  // 0 = FPT, 1 = OCV LKOF
            cv::Mat ocv0( srcImg0.props.height, srcImg0.props.width, CV_8UC1, srcImg0.plane[0], srcImg0.props.stride[0] );
            cv::Mat ocv1( srcImg1.props.height, srcImg1.props.width, CV_8UC1, srcImg1.plane[0], srcImg1.props.stride[0] );
    
            vector<cv::Mat> pyr0, pyr1;
            cv::buildOpticalFlowPyramid( ocv0, pyr0, cv::Size( srch_win, srch_win ), n_lvls, true );
            cv::buildOpticalFlowPyramid( ocv1, pyr1, cv::Size( srch_win, srch_win ), n_lvls, true );
    
            vector<uchar> status;
            vector<float> errs;
            cv::TermCriteria termcrit( cv::TermCriteria::COUNT | cv::TermCriteria::EPS, max_iter, max_eps );
    
            p1.clear();
            if( p0.size() > 0 )
            {
    #if 0
                cv::calcOpticalFlowPyrLK( ocv0, ocv1, p0, p1, status, errs, cv::Size( srch_win, srch_win ), 3, termcrit, 0, 0.0001 );
    #elif 1
                p1 = p0;
                for( int j = 0; j < p1.size(); ++j )
                {
                    p1[j].x += predX[j];
                    p1[j].y += predY[j];
                }
                cv::calcOpticalFlowPyrLK( ocv0, ocv1, p0, p1, status, errs, cv::Size( srch_win, srch_win ), 3, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW );
    #elif 1
                p1 = p0;
                for( int j = 0; j < p1.size(); ++j )
                {
                    p1[j].x += predX[j];
                    p1[j].y += predY[j];
                }
                cv::calcOpticalFlowPyrLK( pyr0, pyr1, p0, p1, status, errs, cv::Size( srch_win, srch_win ), 3, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW );
    #else
                cv::calcOpticalFlowPyrLK( pyr0, pyr1, p0, p1, status, errs, cv::Size( srch_win, srch_win ), 3, termcrit, 0, 0.0001 );
    #endif
            }
    
            nCorners = p1.size();
            for( int i = 0; i < nCorners; ++i )
            {
                ptX[i] = p1[i].x;
                ptY[i] = p1[i].y;
                ptStatus[i] = status[i];
            }
    #else
            for( uint32_t j = 0; j < nCorners; ++j )
            {
                ptXpred[j] = ptX[j] + predX[j];
                ptYpred[j] = ptY[j] + predY[j];
            }
    
            if( FADAS_ERROR_NONE != FadasFPT_Run( fpt, &srcImg0, &srcImg1, &roi, nCorners, ptX, ptY, ptStatus, max_iter, eps, ptXpred, ptYpred ) )
            {
                UTIL_ERROR( "FadasFPT_Run failed\n" );
                break;
            }
    #endif
    
    #ifdef USE_OPENCV
            const char* seq_str_out = "fpt_%07u.jpg";
            cv::Mat ocvimg1( srcImg1.props.height, srcImg1.props.width, CV_8UC1, srcImg1.plane[0], srcImg1.props.stride[0] );
            cv::Mat ocvimg1_dsply( ocvimg1.size(), CV_8UC3 );
            cv::cvtColor( ocvimg1, ocvimg1_dsply, cv::COLOR_GRAY2RGB );
            for( int k = 0; k < nCorners; ++k )
            {
                if( ptStatus[k] == 1 )
                {
                    cv::Point pt( ptX[k], ptY[k] );
                    cv::line( ocvimg_trcks, pt, p0[k], cv::Scalar( 128, 128, 128 ), 2 );
                    cv::circle( ocvimg1_dsply, pt, 1, cv::Scalar( 0, 255, 0 ), 2 );
                }
                else
                    cv::circle( ocvimg1_dsply, cv::Point( ptX[k], ptY[k] ), 1, cv::Scalar( 0, 0, 255 ), 2 );
            }
            cv::Mat img_ocv1_trcks;
            cv::add( ocvimg1_dsply, ocvimg_trcks, img_ocv1_trcks );
            snprintf( str, sizeof(str), seq_str_out, i );
            cv::imwrite( full_path, img_ocv1_trcks );
            cv::namedWindow( "Tracked", cv::WINDOW_AUTOSIZE );
            cv::imshow( "Tracked", img_ocv1_trcks );
            cv::waitKey( 0 );

            // reduce set to only tracked features
            p1.clear();
            for( uint32_t k = 0; k < nCorners; ++k )
            {
                if( ptStatus[k] == 1 )
                {
                    p1.push_back( cv::Point2f( ptX[k], ptY[k] ) );
                }
            }
    
            nCorners = p1.size();
            for( uint32_t i = 0; i < nCorners; ++i )
            {
                ptX[i] = p1[i].x;
                ptY[i] = p1[i].y;
            }
            printf( "%d features tracked\n", nCorners );
            p0 = p1;
    
            uint32_t feat_trckd = nCorners;
    #else
            uint32_t feat_trckd = 0;
            for( uint32_t k = 0; k < nCorners; ++k )
            {
                if( ptStatus[k] == 1 )
                {
                    ++feat_trckd;
                }
            }
    #endif
            printf( "%d features tracked\n", feat_trckd );
    
            memcpy( srcImg0.plane[0], srcImg1.plane[0], srcImg1.props.stride[0] * srcImg1.props.height );
        }
    
        if( nullptr != fpt )
            if( FADAS_ERROR_NONE != FadasFPT_Destroy( fpt ) )
                UTIL_ERROR( "FadasFPT_Destroy failed\n" );

        FadasDestroyImage( &srcImg0 );
        FadasDestroyImage( &srcImg1 );
        FadasDeInit();
    
        return 0;
    }
    Copy to clipboard

Last Published: Sep 30, 2024

[Previous Topic
fpt/app.cpp](https://docs.qualcomm.com/bundle/publicresource/80-63309-1/topics/fpt.md) [Next Topic
uyvy\_remap/app.cpp](https://docs.qualcomm.com/bundle/publicresource/80-63309-1/topics/uyvy-remap.md)