Program Listing for File Relocalizer.cpp

↰ Return to documentation for file (lib/Tracking/Relocalizer.cpp)

#include "Relocalizer.h"
#include "DataStructures/Frame.h"
#include "DataStructures/KeyFrame.h"
#include "Tracking/SE3Tracker.h"
#include "IOWrapper/ImageDisplay.h"

namespace lsd_slam
{


Relocalizer::Relocalizer()
    : isRunning( false ),
      KFForReloc(),
      nextRelocIDX( 0 ),
      maxRelocIDX( 0 ),
      continueRunning( false ),
        hasResult( false ),
        resultKF( nullptr ),
        resultFrameID( 0 ),
        resultFrameToKeyframe( SE3() ),
        _printRelocalizationInfo( false )
{
    for(int i=0;i<RELOCALIZE_THREADS;i++)
        running[i] = false;

}


Relocalizer::~Relocalizer()
{
    stop();
}


void Relocalizer::stop()
{
    continueRunning = false;
    exMutex.lock();
    newCurrentFrameSignal.notify_all();
    exMutex.unlock();
    for(int i=0;i<RELOCALIZE_THREADS;i++)
    {
        relocThreads[i].join();
        running[i] = false;
    }
    isRunning = false;


    KFForReloc.clear();
    CurrentRelocFrame.reset();
}


void Relocalizer::updateCurrentFrame(std::shared_ptr<Frame> currentFrame)
{
    std::lock_guard<std::mutex> lock(exMutex);

    if(hasResult) return;

    this->CurrentRelocFrame = currentFrame;
//  int doneLast = KFForReloc.size() - (maxRelocIDX-nextRelocIDX);
    maxRelocIDX = nextRelocIDX + KFForReloc.size();
    newCurrentFrameSignal.notify_all();

//  f("tried last on %d. set new current frame %d. trying %d to %d!\n",
//          doneLast,
//          currentFrame->id(), nextRelocIDX, maxRelocIDX);

    // Opening the window from deep within the stack was causing issues on OSX.
    // It was a good diagnostic but probably not the best UI choice anyway...
    // if (displayDepthMap)
    //  Util::displayImage( "DebugWindow DEPTH", cv::Mat(currentFrame->height(), currentFrame->width(), CV_32F, currentFrame->image())*(1/255.0f), false );
    //
    // int pressedKey = Util::waitKey(1);
    // handleKey(pressedKey);
}

void Relocalizer::start(std::vector<KeyFrame::SharedPtr> &allKeyframesList)
{
    // make KFForReloc List
    KFForReloc.clear();
    for(unsigned int k=0;k < allKeyframesList.size(); k++)
    {
        // insert
        const int ridx = rand()%(KFForReloc.size());
        auto itr = KFForReloc.begin();
        for( int i = 0; i < ridx; ++i ) { ++itr; }
        KFForReloc.insert(itr, allKeyframesList[k]);
    }
    nextRelocIDX=0;
    maxRelocIDX=KFForReloc.size();

    hasResult = false;
    continueRunning = true;
    isRunning = true;

    // start threads
    for(int i=0;i<RELOCALIZE_THREADS;i++)
    {
        relocThreads[i] = boost::thread(&Relocalizer::threadLoop, this, i);
        running[i] = true;
    }
}

bool Relocalizer::waitResult(int milliseconds)
{
    std::lock_guard<std::mutex> lock(exMutex);
    if(hasResult) return true;

    // !!TODO.  Change to C++11 condition variable
    //resultReadySignal.timed_wait(lock, boost::posix_time::milliseconds(milliseconds));
    return hasResult;
}

RelocalizerResult Relocalizer::getResult( void ) //Frame* &out_keyframe, std::shared_ptr<Frame> &frame, int &out_successfulFrameID, SE3 &out_frameToKeyframe)
{
    std::lock_guard<std::mutex> lock(exMutex);
    if(hasResult)
    {
        return RelocalizerResult( resultKF, resultRelocFrame, resultFrameID, resultFrameToKeyframe );
    }
    else
    {
        return RelocalizerResult();
    }
}


void Relocalizer::threadLoop(int idx)
{
    if(!multiThreading && idx != 0) return;

    SE3Tracker* tracker = new SE3Tracker(Conf().slamImageSize );

//  std::lock_guard<std::mutex> lock(exMutex);
    exMutex.lock();
    while(continueRunning)
    {
        // if got something: do it (unlock in the meantime)
        if(nextRelocIDX < maxRelocIDX && CurrentRelocFrame)
        {
            KeyFrame::SharedPtr todo( KFForReloc[nextRelocIDX%KFForReloc.size()] );
            nextRelocIDX++;
            if(todo->neighbors.size() <= 2) continue;

            std::shared_ptr<Frame> myRelocFrame = CurrentRelocFrame;

            exMutex.unlock();

            // initial Alignment
            SE3 todoToFrame = tracker->trackFrameOnPermaref(todo, myRelocFrame, SE3());

            // try neighbours
            float todoGoodVal = tracker->pointUsage * tracker->lastGoodCount() / (tracker->lastGoodCount()+tracker->lastBadCount());
            if(todoGoodVal > relocalizationTH)
            {
                int numGoodNeighbours = 0;
                int numBadNeighbours = 0;

                float bestNeightbourGoodVal = todoGoodVal;
                float bestNeighbourUsage = tracker->pointUsage;
                KeyFrame::SharedPtr bestKF(todo);
                SE3 bestKFToFrame = todoToFrame;
                for(auto nkf : todo->neighbors)
                {
                    SE3 nkfToFrame_init = se3FromSim3((nkf->frame()->getCamToWorld().inverse() * todo->frame()->getCamToWorld() * sim3FromSE3(todoToFrame.inverse(), 1))).inverse();
                    SE3 nkfToFrame = tracker->trackFrameOnPermaref(nkf, myRelocFrame, nkfToFrame_init);

                    float goodVal = tracker->pointUsage * tracker->lastGoodCount() / (tracker->lastGoodCount()+tracker->lastBadCount());
                    if(goodVal > relocalizationTH*0.8 && (nkfToFrame * nkfToFrame_init.inverse()).log().norm() < 0.1)
                        numGoodNeighbours++;
                    else
                        numBadNeighbours++;

                    if(goodVal > bestNeightbourGoodVal)
                    {
                        bestNeightbourGoodVal = goodVal;
                        bestKF = nkf;
                        bestKFToFrame = nkfToFrame;
                        bestNeighbourUsage = tracker->pointUsage;
                    }
                }

                if(numGoodNeighbours > numBadNeighbours || numGoodNeighbours >= 5)
                {
                    if(_printRelocalizationInfo)
                        printf("RELOCALIZED! frame %d on %d (bestNeighbour %d): good %2.1f%%, usage %2.1f%%, GoodNeighbours %d / %d\n",
                                myRelocFrame->id(), todo->id(), bestKF->id(),
                                100*bestNeightbourGoodVal, 100*bestNeighbourUsage,
                                numGoodNeighbours, numGoodNeighbours+numBadNeighbours);

                    // set everything to stop!
                    continueRunning = false;
                    exMutex.lock();
                    resultRelocFrame = myRelocFrame;
                    resultFrameID = myRelocFrame->id();
                    resultKF = bestKF;
                    resultFrameToKeyframe = bestKFToFrame.inverse();
                    resultReadySignal.notify_all();
                    hasResult = true;
                    exMutex.unlock();
                }
                else
                {
                    if(_printRelocalizationInfo)
                        printf("FAILED RELOCALIZE! frame %d on %d (bestNeighbour %d): good %2.1f%%, usage %2.1f%%, GoodNeighbours %d / %d\n",
                                myRelocFrame->id(), todo->id(), bestKF->id(),
                                100*bestNeightbourGoodVal, 100*bestNeighbourUsage,
                                numGoodNeighbours, numGoodNeighbours+numBadNeighbours);
                }
            }

            exMutex.lock();
        }
        else
        {
            // !!TODO.  Make this wok in C++11
            //newCurrentFrameSignal.wait(lock);
        }
    }
    exMutex.unlock();

    delete tracker;
}

}