<commit_before>/*
 * Copyright (c) 2012. The Regents of the University of California. All rights reserved.
 * Licensed pursuant to the terms and conditions available for viewing at:
 * http://opensource.org/licenses/BSD-3-Clause
 *
 * File: tracker.cpp
 * Author: <NAME>
 * Last Modified: 12.2.2012
 */

#include <PatchTracker/tracker.h>
#include <PatchTracker/patch.h>
#include <PatchTracker/nccsearch.h>

#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>

#include <Eigen/Eigen>

#include <iostream>

#ifdef USE_ACCELERATE
#include <Accelerate/Accelerate.h>
#endif

#ifdef USE_DISPATCH
#include <dispatch/dispatch.h>
#endif

#ifdef __APPLE__
#include "TargetConditionals.h"
#endif

namespace vrlt
{
    static inline bool in_image( const cv::Size &size, const cv::Point2i &loc )
    {
        return ( loc.x >= 0 && loc.x < size.width && loc.y >= 0 && loc.y < size.height );
    }

    struct SortPatches
    {
        bool operator()( const Patch *a, const Patch *b ) { return ( a->shouldTrack > b->shouldTrack ); }
    };
    
    void Tracker::precomputeGlobalPoses( Node *node )
    {
        if ( node->camera != NULL ) cameras.push_back( node->camera );
        node->precomputedGlobalPose = node->globalPose();
        if ( node->camera != NULL ) node->camera->calibration->makeK();
        ElementList::iterator it;
        for ( it = node->children.begin(); it != node->children.end(); it++ )
        {
            Node *child = (Node*)it->second;
            precomputeGlobalPoses( child );
        }   
    }
    
    Tracker::Tracker( Node *_root, int _maxnumpoints, std::string method, double threshold )
    : ntracked( 0 ), root( _root ), nattempted( 0 ), verbose( false ),
    maxnumpoints( _maxnumpoints ), firstlevel( 3 ), lastlevel( 1 ), niter( 10 ),
    recompute_sigmasq(true)
    {
        do_pose_update = true;
        
        precomputeGlobalPoses( root );
        
        cameras[0]->calibration->makeK();
        
        patches.resize( root->points.size() );
        visiblePatches.resize( root->points.size() );
        searchPatches.resize( root->points.size() );
        
        int i = 0;
        ElementList::iterator it;
        for ( it = root->points.begin(); it != root->points.end(); it++,i++ )
        {
            Point *point = (Point *)it->second;
            patches[i] = new Patch( point );
        }
        random_shuffle( patches.begin(), patches.end() );
        patchSearcher = NULL;
		patchSearcher = new PatchSearchNCC( maxnumpoints );
		patchSearcher->lowThreshold = threshold;
		patchSearcher->highThreshold = threshold;
        
        mynode = new Node;
        mycamera = new Camera;
        mycalibration = new Calibration;
        
        mycamera->name = "camera";
        mycamera->calibration = mycalibration;
        mycamera->node = mynode;
        
        mynode->name = "node";
        mynode->camera = mycamera;
    }
    
    Tracker::~Tracker()
    {
        ElementList::iterator it;
        int i = 0;
        for ( it = root->points.begin(); it != root->points.end(); it++,i++ )
        {
            delete patches[i];
        }
        delete patchSearcher;
        
        delete mynode;
        delete mycamera;
        delete mycalibration;
    }
    
    void Tracker::updateMatrices( Camera *source, Camera *target )
    {
        Sophus::SE3d poseinv = mynode->pose.inverse();
        Sophus::SE3d rel_pose = source->node->precomputedGlobalPose * poseinv;
        source->KAKinv = source->calibration->K * rel_pose.so3().matrix().cast<float>() * target->calibration->Kinv;
        source->Ka.col(0) = source->calibration->K * rel_pose.translation().cast<float>();
    }
    
    void Tracker::updateMatrices( Camera *target )
    {
        Sophus::SE3d poseinv = mynode->pose.inverse();
        for ( int i = 0; i < cameras.size(); i++ )
        {
            Sophus::SE3d rel_pose = cameras[i]->node->precomputedGlobalPose * poseinv;
            cameras[i]->KAKinv = cameras[i]->calibration->K * rel_pose.so3().matrix().cast<float>() * target->calibration->Kinv;
            cameras[i]->Ka.col(0) = cameras[i]->calibration->K * rel_pose.translation().cast<float>();
        }
    }
    
    void checkPoint( void *context, size_t i )
    {
        Tracker *tracker = (Tracker *)context;
        
        Point *point = tracker->patches[i]->point;
        point->tracked = false;

        tracker->searchPatches[i] = NULL;

        Eigen::Vector4f pointData;
        Eigen::Vector3f PX;
        Eigen::Vector2f proj;
        Eigen::Vector3f normalData;
        Eigen::Vector3f PN;

        // NB: Eigen is column major
        // check in front of camera
        pointData = point->position.cast<float>();
#ifdef USE_ACCELERATE
        vDSP_dotpr( tracker->poseMatrix.data() + 3, 4, pointData.data(), 1, PX.data()+2, 4 );
#else
        PX[2] = tracker->poseMatrix.row(2).dot( pointData );
#endif
        if ( PX[2] < 0 ) {
            return;
        }
        
        // check in image
#ifdef USE_ACCELERATE
        vDSP_dotpr( tracker->poseMatrix.data()    , 4, pointData.data(), 1, PX.data()  , 4 );
        vDSP_dotpr( tracker->poseMatrix.data() + 1, 4, pointData.data(), 1, PX.data()+1, 4 );
#else
        PX[0] = tracker->poseMatrix.row(0).dot( pointData );
        PX[1] = tracker->poseMatrix.row(1).dot( pointData );
#endif
        proj[0] = tracker->f * (PX[0]/PX[2]) + tracker->u;
        proj[1] = tracker->f * (PX[1]/PX[2]) + tracker->v;
        cv::Point2i pos( (int)proj[0], (int)proj[1] );
        if ( !in_image( tracker->current_camera->image.size(), pos ) )
        {
            return;
        }
                
        normalData = point->normal.cast<float>();
#ifdef USE_ACCELERATE
        vDSP_dotpr( tracker->poseMatrix.data()    , 4, normalData.data(), 1, PN.data()  , 3 );
        vDSP_dotpr( tracker->poseMatrix.data() + 1, 4, normalData.data(), 1, PN.data()+1, 3 );
        vDSP_dotpr( tracker->poseMatrix.data() + 2, 4, normalData.data(), 1, PN.data()+2, 3 );
#else
        PN = tracker->poseMatrix.block<3,3>(0,0) * normalData;
#endif
      
        float D;
#ifdef USE_ACCELERATE
        vDSP_dotpr( PN.data(), 1, PX.data(), 1, &D, 3 );
        D = -D;
#else
        D = -(PN * PX);
#endif
        
        // choose source image
        Patch *sourcepatch = tracker->patches[i];
        sourcepatch->shouldTrack = true;
        sourcepatch->bestLevel = 4;
        sourcepatch->PX = PX;
        sourcepatch->PN = PN;
        sourcepatch->D = D;
        sourcepatch->center = proj;
        sourcepatch->warpcenter = sourcepatch->center;
        sourcepatch->source = NULL;
        sourcepatch->updateTarget( tracker->current_camera );
        sourcepatch->chooseSource( tracker->use_new_cameras );
        if ( sourcepatch->source == NULL ) {
            return;
        }
        
        tracker->visiblePatches[i] = sourcepatch;
    }
    
    void preparePatch( void *context, size_t i )
    {
        Tracker *tracker = (Tracker*)context;
        
        Patch *sourcepatch = tracker->searchPatches[i];
        <commit_msg>bug fixes in tracker<commit_after>/*
 * Copyright (c) 2012. The Regents of the University of California. All rights reserved.
 * Licensed pursuant to the terms and conditions available for viewing at:
 * http://opensource.org/licenses/BSD-3-Clause
 *
 * File: tracker.cpp
 * Author: <NAME>
 * Last Modified: 12.2.2012
 */

#include <PatchTracker/tracker.h>
#include <PatchTracker/patch.h>
#include <PatchTracker/nccsearch.h>

#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>

#include <Eigen/Eigen>

#include <iostream>

#ifdef USE_ACCELERATE
#include <Accelerate/Accelerate.h>
#endif

#ifdef USE_DISPATCH
#include <dispatch/dispatch.h>
#endif

#ifdef __APPLE__
#include "TargetConditionals.h"
#endif

namespace vrlt
{
    static inline bool in_image( const cv::Size &size, const cv::Point2i &loc )
    {
        return ( loc.x >= 0 && loc.x < size.width && loc.y >= 0 && loc.y < size.height );
    }

    struct SortPatches
    {
        bool operator()( const Patch *a, const Patch *b ) { return ( a->shouldTrack > b->shouldTrack ); }
    };
    
    void Tracker::precomputeGlobalPoses( Node *node )
    {
        if ( node->camera != NULL ) cameras.push_back( node->camera );
        node->precomputedGlobalPose = node->globalPose();
        if ( node->camera != NULL ) node->camera->calibration->makeK();
        ElementList::iterator it;
        for ( it = node->children.begin(); it != node->children.end(); it++ )
        {
            Node *child = (Node*)it->second;
            precomputeGlobalPoses( child );
        }   
    }
    
    Tracker::Tracker( Node *_root, int _maxnumpoints, std::string method, double threshold )
    : ntracked( 0 ), root( _root ), nattempted( 0 ), verbose( false ),
    maxnumpoints( _maxnumpoints ), firstlevel( 3 ), lastlevel( 1 ), niter( 10 ),
    recompute_sigmasq(true)
    {
        do_pose_update = true;
        
        precomputeGlobalPoses( root );
        
        cameras[0]->calibration->makeK();
        
        patches.resize( root->points.size() );
        visiblePatches.resize( root->points.size() );
        searchPatches.resize( root->points.size() );
        
        int i = 0;
        ElementList::iterator it;
        for ( it = root->points.begin(); it != root->points.end(); it++,i++ )
        {
            Point *point = (Point *)it->second;
            patches[i] = new Patch( point );
        }
        random_shuffle( patches.begin(), patches.end() );
        patchSearcher = NULL;
		patchSearcher = new PatchSearchNCC( maxnumpoints );
		patchSearcher->lowThreshold = threshold;
		patchSearcher->highThreshold = threshold;
        
        mynode = new Node;
        mycamera = new Camera;
        mycalibration = new Calibration;
        
        mycamera->name = "camera";
        mycamera->calibration = mycalibration;
        mycamera->node = mynode;
        
        mynode->name = "node";
        mynode->camera = mycamera;
    }
    
    Tracker::~Tracker()
    {
        ElementList::iterator it;
        int i = 0;
        for ( it = root->points.begin(); it != root->points.end(); it++,i++ )
        {
            delete patches[i];
        }
        delete patchSearcher;
        
        delete mynode;
        delete mycamera;
        delete mycalibration;
    }
    
    void Tracker::updateMatrices( Camera *source, Camera *target )
    {
        Sophus::SE3d poseinv = mynode->pose.inverse();
        Sophus::SE3d rel_pose = source->node->precomputedGlobalPose * poseinv;
        source->KAKinv = source->calibration->K * rel_pose.so3().matrix().cast<float>() * target->calibration->Kinv;
        source->Ka = source->calibration->K * rel_pose.translation().cast<float>();
    }
    
    void Tracker::updateMatrices( Camera *target )
    {
        Sophus::SE3d poseinv = mynode->pose.inverse();
        for ( int i = 0; i < cameras.size(); i++ )
        {
            Sophus::SE3d rel_pose = cameras[i]->node->precomputedGlobalPose * poseinv;
            cameras[i]->KAKinv = cameras[i]->calibration->K * rel_pose.so3().matrix().cast<float>() * target->calibration->Kinv;
            cameras[i]->Ka = cameras[i]->calibration->K * rel_pose.translation().cast<float>();
        }
    }
    
    void checkPoint( void *context, size_t i )
    {
        Tracker *tracker = (Tracker *)context;
        
        Point *point = tracker->patches[i]->point;
        point->tracked = false;

        tracker->searchPatches[i] = NULL;

        Eigen::Vector4f pointData;
        Eigen::Vector3f PX;
        Eigen::Vector2f proj;
        Eigen::Vector3f normalData;
        Eigen::Vector3f PN;

        // NB: Eigen is column major
        // check in front of camera
        pointData = point->position.cast<float>();
#ifdef USE_ACCELERATE
        vDSP_dotpr( tracker->poseMatrix.data() + 3, 4, pointData.data(), 1, PX.data()+2, 4 );
#else
        PX[2] = tracker->poseMatrix.row(2).dot( pointData );
#endif
        if ( PX[2] < 0 ) {
            return;
        }
        
        // check in image
#ifdef USE_ACCELERATE
        vDSP_dotpr( tracker->poseMatrix.data()    , 4, pointData.data(), 1, PX.data()  , 4 );
        vDSP_dotpr( tracker->poseMatrix.data() + 1, 4, pointData.data(), 1, PX.data()+1, 4 );
#else
        PX[0] = tracker->poseMatrix.row(0).dot( pointData );
        PX[1] = tracker->poseMatrix.row(1).dot( pointData );
#endif
        proj[0] = tracker->f * (PX[0]/PX[2]) + tracker->u;
        proj[1] = tracker->f * (PX[1]/PX[2]) + tracker->v;
        cv::Point2i pos( (int)proj[0], (int)proj[1] );
        if ( !in_image( tracker->current_camera->image.size(), pos ) )
        {
            return;
        }
                
        normalData = point->normal.cast<float>();
#ifdef USE_ACCELERATE
        vDSP_dotpr( tracker->poseMatrix.data()    , 4, normalData.data(), 1, PN.data()  , 3 );
        vDSP_dotpr( tracker->poseMatrix.data() + 1, 4, normalData.data(), 1, PN.data()+1, 3 );
        vDSP_dotpr( tracker->poseMatrix.data() + 2, 4, normalData.data(), 1, PN.data()+2, 3 );
#else
        PN = tracker->poseMatrix.block<3,3>(0,0) * normalData;
#endif
      
        float D;
#ifdef USE_ACCELERATE
        vDSP_dotpr( PN.data(), 1, PX.data(), 1, &D, 3 );
        D = -D;
#else
        D = -(PN.dot(PX));
#endif
        
        // choose source image
        Patch *sourcepatch = tracker->patches[i];
        sourcepatch->shouldTrack = true;
        sourcepatch->bestLevel = 4;
        sourcepatch->PX = PX;
        sourcepatch->PN = PN;
        sourcepatch->D = D;
        sourcepatch->center = proj;
        sourcepatch->warpcenter = sourcepatch->center;
        sourcepatch->source = NULL;
        sourcepatch->updateTarget( tracker->current_camera );
        sourcepatch->chooseSource( tracker->use_new_cameras );
        if ( sourcepatch->source == NULL ) {
            return;
        }
        
        tracker->visiblePatches[i] = sourcepatch;
    }
    
    void preparePatch( void *context, size_t i )
    {
        Tracker *tracker = (Tracker*)context;
        
        Patch *sourcepatch = tracker->searchPatches[i];
        
{'fraction_non_alphanumeric': 0.14742954255771137, 'fraction_numerical': 0.01274606996176179, 'mean_word_length': 2.286711659297184, 'pattern_counts': {'":': 0, '<': 45, '<?xml version=': 0, '>': 225, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5909743', 'n_tokens_mistral': 4732, 'n_tokens_neox': 4531, 'n_words': 954}