main_dso_pangolin.cpp

/**
* This file is part of DSO.
* 
* Copyright 2016 Technical University of Munich and Intel.
* Developed by Jakob Engel <engelj at in dot tum dot de>,
* for more information see <http://vision.in.tum.de/dso>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* DSO is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* DSO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with DSO. If not, see <http://www.gnu.org/licenses/>.
*/



#include <thread>
#include <locale.h>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>

#include "IOWrapper/Output3DWrapper.h"
#include "IOWrapper/ImageDisplay.h"


#include <boost/thread.hpp>
#include "util/settings.h"
#include "util/globalFuncs.h"
#include "util/DatasetReader.h"
#include "util/globalCalib.h"

#include "util/NumType.h"
#include "FullSystem/FullSystem.h"
#include "OptimizationBackend/MatrixAccumulators.h"
#include "FullSystem/PixelSelector2.h"



#include "IOWrapper/Pangolin/PangolinDSOViewer.h"
#include "IOWrapper/OutputWrapper/SampleOutputWrapper.h"

#include "util/UDPServer.h"


std::string vignette = "";
std::string gammaCalib = "";
std::string source = "";
std::string calib = "";
std::string poses = ""; // Added for reading in available camera poses
double rescale = 1;
bool reverse = false;
bool disableROS = false;
int start=0;
int end=100000;
bool prefetch = false;
float playbackSpeed=0;	// 0 for linearize (play as fast as possible, while sequentializing tracking & mapping). otherwise, factor on timestamps.
bool preload=false;
bool useSampleOutput=false;


int mode=0;

bool firstRosSpin=false;

using namespace dso;


void my_exit_handler(int s)
{
	printf("Caught signal %d\n",s);
	exit(1);
}

void exitThread()
{
	struct sigaction sigIntHandler;
	sigIntHandler.sa_handler = my_exit_handler;
	sigemptyset(&sigIntHandler.sa_mask);
	sigIntHandler.sa_flags = 0;
	sigaction(SIGINT, &sigIntHandler, NULL);

	firstRosSpin=true;
	while(true) pause();
}


/*
 Depending on the "preset" argument passed to the main function
 (=> real-time or no real-time?)
 We set the appropriate variables in the util/settings.h file
 */
void settingsDefault(int preset)
{
	printf("\n=============== PRESET Settings: ===============\n");
	if(preset == 0 || preset == 1)
	{
		printf("DEFAULT settings:\n"
				"- %s real-time enforcing\n"
				"- 2000 active points\n"
				"- 5-7 active frames\n"
				"- 1-6 LM iteration each KF\n"
				"- original image resolution\n", preset==0 ? "no " : "1x");

		playbackSpeed = (preset==0 ? 0 : 1);
		preload = preset==1;
		setting_desiredImmatureDensity = 1500;
		setting_desiredPointDensity = 2000;
		setting_minFrames = 5;
		setting_maxFrames = 7;
		setting_maxOptIterations=6;
		setting_minOptIterations=1;

		setting_logStuff = false;
	}

	if(preset == 2 || preset == 3)
	{
		printf("FAST settings:\n"
				"- %s real-time enforcing\n"
				"- 800 active points\n"
				"- 4-6 active frames\n"
				"- 1-4 LM iteration each KF\n"
				"- 424 x 320 image resolution\n", preset==0 ? "no " : "5x");

		playbackSpeed = (preset==2 ? 0 : 5);
		preload = preset==3;
		setting_desiredImmatureDensity = 600;
		setting_desiredPointDensity = 800;
		setting_minFrames = 4;
		setting_maxFrames = 6;
		setting_maxOptIterations=4;
		setting_minOptIterations=1;

		benchmarkSetting_width = 424;
		benchmarkSetting_height = 320;

		setting_logStuff = false;
		//setting_logStuff = true;
	}

	printf("==============================================\n");
}




/*
 Parse the whole argument string for any relevant arguments:
 */
void parseArgument(char* arg)
{
	int option;
	float foption;
	char buf[1000];


    if(1==sscanf(arg,"sampleoutput=%d",&option))
    {
        if(option==1)
        {
            useSampleOutput = true;
            printf("USING SAMPLE OUTPUT WRAPPER!\n");
        }
        return;
    }

    if(1==sscanf(arg,"quiet=%d",&option))
    {
        if(option==1)
        {
            setting_debugout_runquiet = true;
            printf("QUIET MODE, I'll shut up!\n");
        }
        return;
    }

	if(1==sscanf(arg,"preset=%d",&option))
	{
		settingsDefault(option);
		return;
	}


	if(1==sscanf(arg,"rec=%d",&option))
	{
		if(option==0)
		{
			disableReconfigure = true;
			printf("DISABLE RECONFIGURE!\n");
		}
		return;
	}



	if(1==sscanf(arg,"noros=%d",&option))
	{
		if(option==1)
		{
			disableROS = true;
			disableReconfigure = true;
			printf("DISABLE ROS (AND RECONFIGURE)!\n");
		}
		return;
	}

	if(1==sscanf(arg,"nolog=%d",&option))
	{
		if(option==1)
		{
			setting_logStuff = false;
			printf("DISABLE LOGGING!\n");
		}
		return;
	}
	if(1==sscanf(arg,"reverse=%d",&option))
	{
		if(option==1)
		{
			reverse = true;
			printf("REVERSE!\n");
		}
		return;
	}
	if(1==sscanf(arg,"nogui=%d",&option))
	{
		if(option==1)
		{
			disableAllDisplay = true;
			printf("NO GUI!\n");
		}
		return;
	}
	if(1==sscanf(arg,"nomt=%d",&option))
	{
		if(option==1)
		{
			multiThreading = false;
			printf("NO MultiThreading!\n");
		}
		return;
	}
	if(1==sscanf(arg,"prefetch=%d",&option))
	{
		if(option==1)
		{
			prefetch = true;
			printf("PREFETCH!\n");
		}
		return;
	}
	if(1==sscanf(arg,"start=%d",&option))
	{
		start = option;
		printf("START AT %d!\n",start);
		return;
	}
	if(1==sscanf(arg,"end=%d",&option))
	{
		end = option;
		printf("END AT %d!\n",start);
		return;
	}

	if(1==sscanf(arg,"files=%s",buf))
	{
		source = buf;
		printf("loading data from %s!\n", source.c_str());
		return;
	}

	if(1==sscanf(arg,"calib=%s",buf))
	{
		calib = buf;
		printf("loading calibration from %s!\n", calib.c_str());
		return;
	}

	if(1==sscanf(arg,"vignette=%s",buf))
	{
		vignette = buf;
		printf("loading vignette from %s!\n", vignette.c_str());
		return;
	}

	if(1==sscanf(arg,"gamma=%s",buf))
	{
		gammaCalib = buf;
		printf("loading gammaCalib from %s!\n", gammaCalib.c_str());
		return;
	}

	if(1==sscanf(arg,"rescale=%f",&foption))
	{
		rescale = foption;
		printf("RESCALE %f!\n", rescale);
		return;
	}

	if(1==sscanf(arg,"speed=%f",&foption))
	{
		playbackSpeed = foption;
		printf("PLAYBACK SPEED %f!\n", playbackSpeed);
		return;
	}

	if(1==sscanf(arg,"save=%d",&option))
	{
		if(option==1)
		{
			debugSaveImages = true;
			if(42==system("rm -rf images_out")) printf("system call returned 42 - what are the odds?. This is only here to shut up the compiler.\n");
			if(42==system("mkdir images_out")) printf("system call returned 42 - what are the odds?. This is only here to shut up the compiler.\n");
			if(42==system("rm -rf images_out")) printf("system call returned 42 - what are the odds?. This is only here to shut up the compiler.\n");
			if(42==system("mkdir images_out")) printf("system call returned 42 - what are the odds?. This is only here to shut up the compiler.\n");
			printf("SAVE IMAGES!\n");
		}
		return;
	}

	if(1==sscanf(arg,"mode=%d",&option))
	{

		mode = option;
		if(option==0)
		{
			printf("PHOTOMETRIC MODE WITH CALIBRATION!\n");
		}
		if(option==1)
		{
			printf("PHOTOMETRIC MODE WITHOUT CALIBRATION!\n");
			setting_photometricCalibration = 0;
			setting_affineOptModeA = 0; //-1: fix. >=0: optimize (with prior, if > 0).
			setting_affineOptModeB = 0; //-1: fix. >=0: optimize (with prior, if > 0).
		}
		if(option==2)
		{
			printf("PHOTOMETRIC MODE WITH PERFECT IMAGES!\n");
			setting_photometricCalibration = 0;
			setting_affineOptModeA = -1; //-1: fix. >=0: optimize (with prior, if > 0).
			setting_affineOptModeB = -1; //-1: fix. >=0: optimize (with prior, if > 0).
                        setting_minGradHistAdd=3;
		}
		return;
	}
        /*
         Add feature to read in custom data 
         */
	if(1==sscanf(arg,"poses=%s",buf))
	{
            poses = buf;
            printf("loading additional camera poses from %s!\n", poses.c_str());
            setting_useCameraPoses = true;
            return;
	}
	/*
         Add feature to init kalmanFilter with GPS or Pose
         */
	if(1==sscanf(arg, "initKalmanFilter=%d", &option))
        {
            if(option==1)
            {
                setting_initKalmanFilter = true;
		printf("Will initialize KalmanFilter from poses or gps!\n");
            }
            return;
        }
        
        /*
         Add feature to read in GPS measurements via UDP
         */
        if(1==sscanf(arg, "udp=%d", &option))
        {
            if(option==1)
            {
                setting_useUDP = true;
            }
            return;
        }
        
        /*
        Add feature to read in GPS measurements via UDP
        */
        if(1==sscanf(arg, "http=%d", &option))
        {
            if(option==1)
            {
                setting_useHTTP = true;
                
            }
            return;
        }
        
       
	printf("could not parse argument \"%s\"!!!!\n", arg);
}



int main( int argc, char** argv )
{
    //setlocale(LC_ALL, "");
    // This will go through every passed argument
    // e.g. quiet=True, preset=2 ( no real-time (800 pts etc.) ), mode=1 ( NO photometric calibration )
    // and parses it to set their respective global variables
    // compare here: https://github.com/JakobEngel/dso
    for(int i=1; i<argc;i++)
            parseArgument(argv[i]);

    // hook the "exitThread()" function at the beginning of this file to crtl+C.
    boost::thread exThread = boost::thread(exitThread);

    // Create a reader object for an image folder based on the parsed arguments
    // That is: Pass the geometric and photometric calibration file
    // as well as the vignette image
    ImageFolderReader* reader = new ImageFolderReader(source,calib, gammaCalib, vignette, poses);
    reader->setGlobalCalibration();


    // If we are using a mode that needs a photometric calibration
    // (mode=0) then the default setting from util/settings.cpp is used
    // (setting_photometricCalibration = 2), therefore the reader should have
    // gotten proper information. If this is not the case, then exit the program
    if(setting_photometricCalibration > 0 && reader->getPhotometricGamma() == 0)
    {
            printf("ERROR: dont't have photometric calibation. Need to use commandline options mode=1 or mode=2 ");
            exit(1);
    }



    // Prepare variables for storing start and end information
    int lstart=start;
    int lend = end;
    int linc = 1;
    // If the image sequence should get reversed, swap start and end
    if(reverse)
    {
            printf("REVERSE!!!!");
            lstart=end-1;
            if(lstart >= reader->getNumImages())
                    lstart = reader->getNumImages()-1;
            lend = start;
            linc = -1;
    }

    // This is the heart of the algorithm. Almost everything is contained in
    // the "FullSystem". Create it:
    FullSystem* fullSystem = new FullSystem();
    // The gamma function is set per complete image sequence set.
    // The photometric gamma is determined based on the file "pcalib.txt",
    // which contains 256 values ranging from 0.0 to 255.0.
    // If a camera does an automatic exposure, pixels will have different
    // brightness values. I believe, that the difference in brightness between
    // two exposure times is what is stored in the text file. By inverting the 
    // irradiance (component "B" in the function) we can determine the camera
    // response function "G", if we know the exposure times "t" (those are 
    // contained in the file "times.txt": Frame Number, Timestamp, Milliseconds)
    // 
    // 
    // More details are given in the paper (page 3, eq. 4):
    // https://vision.in.tum.de/_media/spezial/bib/engel2016monodataset.pdf

    fullSystem->setGammaFunction(reader->getPhotometricGamma());
    // if playback speed is the default value, set linearizeOperation to true
    fullSystem->linearizeOperation = (playbackSpeed==0);

    // Pass on available cameraPoses to FullSystem, if applicable:
    fullSystem->setCameraPoses(reader->getCameraPoses());


    // ********
    // UDP Server for GPS measurements
    // ********
    //if(setting_useUDP)
    //{
        // Create a server object to accept incoming client requests (i.e. the smartphone)
        boost::asio::io_service io_service;
        UDPServer udpServer(io_service);
        fullSystem->setUDPServer(udpServer);

        std::thread udpThread([&io_service]() { io_service.run(); });
    //}
        
    

    IOWrap::PangolinDSOViewer* viewer = 0;
	if(!disableAllDisplay)
    {
        viewer = new IOWrap::PangolinDSOViewer(wG[0],hG[0], false);
        fullSystem->outputWrapper.push_back(viewer);
    }

    IOWrap::SampleOutputWrapper* sampleOutput = 0;

    if(useSampleOutput)
    {
        sampleOutput = new IOWrap::SampleOutputWrapper();
        if(setting_useHTTP)
        {
            sampleOutput->httpPOSTRequest.setActive();
            sampleOutput->httpPOSTRequest.addProject();
        }
        else
        {
            sampleOutput->httpPOSTRequest.setInactive();
        }
        fullSystem->setHTTPPOSTRequest(sampleOutput->httpPOSTRequest);
        
        fullSystem->outputWrapper.push_back(sampleOutput);
    }
        

    

    // START of main loop (runs on worker thread)
    // to make MacOS happy: run this in dedicated thread -- and use this one to run the GUI.
    std::thread runthread([&]() {
        
        std::vector<int> idsToPlay;
        std::vector<double> timesToPlayAt;
        // Go over all available image files
        for(int i=lstart;i>= 0 && i< reader->getNumImages() && linc*i < linc*lend;i+=linc)
        {
            // store what IDs we want to use (it is possible to skip frames, so IDs will be different)
            idsToPlay.push_back(i);
            if(timesToPlayAt.size() == 0)
            {
                timesToPlayAt.push_back((double)0);
            }
            else
            {
                double tsThis = reader->getTimestamp(idsToPlay[idsToPlay.size()-1]);
                double tsPrev = reader->getTimestamp(idsToPlay[idsToPlay.size()-2]);
                timesToPlayAt.push_back(timesToPlayAt.back() +  fabs(tsThis-tsPrev)/playbackSpeed);
            }
        }

        
        std::vector<ImageAndExposure*> preloadedImages;
        if(preload)
        {
            printf("LOADING ALL IMAGES!\n");
            // Get which IDs we want to use and push them back in the image list
            for(int ii=0;ii<(int)idsToPlay.size(); ii++)
            {
                int i = idsToPlay[ii];
                preloadedImages.push_back(reader->getImage(i));
            }
        }

        struct timeval tv_start;
        gettimeofday(&tv_start, NULL);
        clock_t started = clock();
        double sInitializerOffset=0;

        //*****************************************************
        //
        // Here is the main procedure for tracking the images!
        //
        //*****************************************************
        for(int ii=0;ii<(int)idsToPlay.size(); ii++)
        {
            //*****************************************************
            //
            // STEP1: Store time when the reconstruction was started
            //
            //*****************************************************
            if(!fullSystem->initialized)	// if not initialized: reset start time.
            {
                gettimeofday(&tv_start, NULL);
                started = clock();
                sInitializerOffset = timesToPlayAt[ii];
            }
            
            int i = idsToPlay[ii];

            //*****************************************************
            //
            // STEP2: Load image on-the-fly or use preloaded images
            //
            //*****************************************************
            ImageAndExposure* img;
            if(preload)
                img = preloadedImages[ii];
            else
                img = reader->getImage(i);


            //*****************************************************
            //
            // STEP3: Determine if the current frame needs to be skipped
            //
            //*****************************************************
            bool skipFrame=false;
            if(playbackSpeed!=0)
            {
                struct timeval tv_now; gettimeofday(&tv_now, NULL);
                double sSinceStart = sInitializerOffset + ((tv_now.tv_sec-tv_start.tv_sec) + (tv_now.tv_usec-tv_start.tv_usec)/(1000.0f*1000.0f));

                if(sSinceStart < timesToPlayAt[ii])
                    usleep((int)((timesToPlayAt[ii]-sSinceStart)*1000*1000));
                else if(sSinceStart > timesToPlayAt[ii]+0.5+0.1*(ii%2))
                {
                    printf("SKIPFRAME %d (play at %f, now it is %f)!\n", ii, timesToPlayAt[ii], sSinceStart);
                    skipFrame=true;
                }
            }


            //*****************************************************
            //
            // STEP4: Add the loaded image as an active frame into the queue
            //
            //*****************************************************
            
            // Poll for new GPS poses:
            //io_service.poll();
        	
            if(!skipFrame) fullSystem->addActiveFrame(img, i);

            // Free memory reserved for a pointer to the image object
            delete img;

            //*****************************************************
            //
            // STEP5: Check if Init failed or "Reset" was pressed
            //
            //*****************************************************
            if(fullSystem->initFailed || setting_fullResetRequested)
            {
                if(ii < 250 || setting_fullResetRequested)
                {
                    printf("RESETTING!\n");

                    std::vector<IOWrap::Output3DWrapper*> wraps = fullSystem->outputWrapper;
                    delete fullSystem;

                    for(IOWrap::Output3DWrapper* ow : wraps) ow->reset();

                    fullSystem = new FullSystem();
                    fullSystem->setGammaFunction(reader->getPhotometricGamma());
                    fullSystem->linearizeOperation = (playbackSpeed==0);

                    fullSystem->outputWrapper = wraps;

                    if(setting_useUDP)
                    {
                        fullSystem->setUDPServer(udpServer);
                    }
                    if(setting_useHTTP)
                    {
                        fullSystem->setHTTPPOSTRequest(sampleOutput->httpPOSTRequest);
                    }

                    
                    setting_fullResetRequested=false;
                }
            }

            //*****************************************************
            //
            // STEP6: Check if Tracking was lost. if true, DSO will abort
            //
            //*****************************************************
            if(fullSystem->isLost)
            {
                    printf("LOST!!\n");
                    break;
            }

        }
        
        
        //*****************************************************
        //
        // FINISH - Mapping completed or aborted: Now print statistics
        //
        //*****************************************************
        fullSystem->blockUntilMappingIsFinished();
        clock_t ended = clock();
        struct timeval tv_end;
        gettimeofday(&tv_end, NULL);


        fullSystem->printResult("result.txt");


        int numFramesProcessed = abs(idsToPlay[0]-idsToPlay.back());
        double numSecondsProcessed = fabs(reader->getTimestamp(idsToPlay[0])-reader->getTimestamp(idsToPlay.back()));
        double MilliSecondsTakenSingle = 1000.0f*(ended-started)/(float)(CLOCKS_PER_SEC);
        double MilliSecondsTakenMT = sInitializerOffset + ((tv_end.tv_sec-tv_start.tv_sec)*1000.0f + (tv_end.tv_usec-tv_start.tv_usec)/1000.0f);
        printf("\n======================"
                "\n%d Frames (%.1f fps)"
                "\n%.2fms per frame (single core); "
                "\n%.2fms per frame (multi core); "
                "\n%.3fx (single core); "
                "\n%.3fx (multi core); "
                "\n======================\n\n",
                numFramesProcessed, numFramesProcessed/numSecondsProcessed,
                MilliSecondsTakenSingle/numFramesProcessed,
                MilliSecondsTakenMT / (float)numFramesProcessed,
                1000 / (MilliSecondsTakenSingle/numSecondsProcessed),
                1000 / (MilliSecondsTakenMT / numSecondsProcessed));
        
        //fullSystem->printFrameLifetimes();
        if(setting_logStuff)
        {
            std::ofstream tmlog;
            tmlog.open("logs/time.txt", std::ios::trunc | std::ios::out);
            tmlog << 1000.0f*(ended-started)/(float)(CLOCKS_PER_SEC*reader->getNumImages()) << " "
                  << ((tv_end.tv_sec-tv_start.tv_sec)*1000.0f + (tv_end.tv_usec-tv_start.tv_usec)/1000.0f) / (float)reader->getNumImages() << "\n";
            tmlog.flush();
            tmlog.close();
        }

    });
    // END of main loop (runs on worker thread)

    // Rendering loop (runs on main (GUI) thread)
    if(viewer != 0)
        viewer->run();

    runthread.join();
    udpThread.join();
    
    for(IOWrap::Output3DWrapper* ow : fullSystem->outputWrapper)
    {
            ow->join();
            delete ow;
    }


    printf("DELETE FULLSYSTEM!\n");
    delete fullSystem;

    printf("DELETE READER!\n");
    delete reader;

    printf("EXIT NOW!\n");
    return 0;
}