/**
* 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 "PangolinDSOViewer.h"
#include "KeyFrameDisplay.h"

#include "util/settings.h"
#include "util/globalCalib.h"
#include "FullSystem/HessianBlocks.h"
#include "FullSystem/FullSystem.h"
#include "FullSystem/ImmaturePoint.h"

namespace dso
{
namespace IOWrap
{
    bool isRotationMatrix(const cv::Mat &R)
    {
        cv::Mat Rt;
        cv::transpose(R, Rt);
        cv::Mat shouldBeIdentity = Rt * R;
        cv::Mat I = cv::Mat::eye(3,3, shouldBeIdentity.type());

        return  cv::norm(I, shouldBeIdentity) < 1e-6;

    }

    Eigen::Vector3d rot2rvec(Eigen::Matrix<double, 3, 3>& Mat, bool changeAngleSign){
        // Continue the convertion here.
        // Convert from matrix to axis-angle representation.
        double angle = acos( (Mat.trace() - 1) / 2 );
        Eigen::Vector3d vec = {Mat(2, 1) - Mat(1, 2), Mat(0, 2) - Mat(2, 0), Mat(1, 0) - Mat(0, 1)};
        // The unit vector
        Eigen::Vector3d axis = {( 1.0 / 2.0 * sin(angle) ) * vec(0), ( 1.0 / 2.0 * sin(angle) ) * vec(1), ( 1.0 / 2.0 * sin(angle) ) * vec(2)};
        Eigen::Vector3d rvec;
        if (changeAngleSign){
           rvec  = -angle * axis;
        }
        else{
            rvec = angle * axis;
        }
        return rvec;
    };

void drawLine(int x, int y, int z){
    glLineWidth(10);
    glBegin(GL_LINES);
    glVertex3f((float)x,(float)y,(float)z);
    glVertex3f((float)x,(float)y,(float)z+1);
    glEnd();
}

void drawCube(float x, float y, float z){
    glColor3f(0, 1, 0);
    glLineWidth(5);
//    glColor3ui(133, 247, 208);
    glBegin(GL_LINES);

    // Bottom
//    glColor3ui(133, 247, 208);
    glVertex3f(x, y, z);
    glVertex3f(x, y+1, z);

//    glColor3ui(253, 59, 86);
    glVertex3f(x, y, z);
    glVertex3f(x+1, y, z);

//    glColor3ui(147, 175, 215);
    glVertex3f(x, y+1, z);
    glVertex3f(x+1, y+1, z);

//    glColor3ui(80, 209, 168);
    glVertex3f(x+1, y, z);
    glVertex3f(x+1, y+1, z);

    // Top
//    glColor3ui(154, 13, 88);
    glVertex3f(x, y, z+1);
    glVertex3f(x, y+1, z+1);

//    glColor3ui(253, 59, 86);
    glVertex3f(x, y, z+1);
    glVertex3f(x+1, y, z+1);

//    glColor3ui(5, 26, 72);
    glVertex3f(x, y+1, z+1);
    glVertex3f(x+1, y+1, z+1);

//    glColor3ui(72, 182, 8);
    glVertex3f(x+1, y, z+1);
    glVertex3f(x+1, y+1, z+1);

    // Sides
//    glColor3ui(28, 122, 71);
    glVertex3f(x, y, z);
    glVertex3f(x, y, z+1);

//    glColor3ui(244, 207, 185);
    glVertex3f(x, y+1, z);
    glVertex3f(x, y+1, z+1);

//    glColor3ui(88, 153, 225);
    glVertex3f(x+1, y, z);
    glVertex3f(x+1, y, z+1);

//    glColor3ui(184, 151, 253);
    glVertex3f(x+1, y+1, z);
    glVertex3f(x+1, y+1, z+1);

    glEnd();

}

PangolinDSOViewer::PangolinDSOViewer(int w, int h, bool startRunThread)
{
	this->w = w;
	this->h = h;
	running=true;


	{
		boost::unique_lock<boost::mutex> lk(openImagesMutex);
		internalVideoImg = new MinimalImageB3(w,h);
		internalKFImg = new MinimalImageB3(w,h);
		internalResImg = new MinimalImageB3(w,h);
		videoImgChanged=kfImgChanged=resImgChanged=true;

		internalVideoImg->setBlack();
		internalKFImg->setBlack();
		internalResImg->setBlack();
	}


	{
		currentCam = new KeyFrameDisplay();
	}

	needReset = false;


    if(startRunThread)
        runThread = boost::thread(&PangolinDSOViewer::run, this);

}


PangolinDSOViewer::~PangolinDSOViewer()
{
	close();
	runThread.join();
}


void PangolinDSOViewer::run()
{
	printf("START PANGOLIN!\n");

	pangolin::CreateWindowAndBind("Main",2*w,2*h);
	const int UI_WIDTH = 180;

	glEnable(GL_DEPTH_TEST);

	// 3D visualization
	pangolin::OpenGlRenderState Visualization3D_camera(
		pangolin::ProjectionMatrix(w,h,400,400,w/2,h/2,0.1,1000),
		pangolin::ModelViewLookAt(-0,-5,-10, 0,0,0, pangolin::AxisNegY)
		);

	pangolin::View& Visualization3D_display = pangolin::CreateDisplay()
		.SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -w/(float)h)
		.SetHandler(new pangolin::Handler3D(Visualization3D_camera));


	// 3 images
	pangolin::View& d_kfDepth = pangolin::Display("imgKFDepth")
	    .SetAspect(w/(float)h);

	pangolin::View& d_video = pangolin::Display("imgVideo")
	    .SetAspect(w/(float)h);

	pangolin::View& d_residual = pangolin::Display("imgResidual")
	    .SetAspect(w/(float)h);

	pangolin::GlTexture texKFDepth(w,h,GL_RGB,false,0,GL_RGB,GL_UNSIGNED_BYTE);
	pangolin::GlTexture texVideo(w,h,GL_RGB,false,0,GL_RGB,GL_UNSIGNED_BYTE);
	pangolin::GlTexture texResidual(w,h,GL_RGB,false,0,GL_RGB,GL_UNSIGNED_BYTE);


    pangolin::CreateDisplay()
		  .SetBounds(0.0, 0.3, pangolin::Attach::Pix(UI_WIDTH), 1.0)
		  .SetLayout(pangolin::LayoutEqual)
		  .AddDisplay(d_kfDepth)
		  .AddDisplay(d_video)
		  .AddDisplay(d_residual);

	// parameter reconfigure gui
	pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));

	pangolin::Var<int> settings_pointCloudMode("ui.PC_mode",1,1,4,false);

	pangolin::Var<bool> settings_showKFCameras("ui.KFCam",false,true);
	pangolin::Var<bool> settings_showCurrentCamera("ui.CurrCam",true,true);
	pangolin::Var<bool> settings_showTrajectory("ui.Trajectory",true,true);
	pangolin::Var<bool> settings_showFullTrajectory("ui.FullTrajectory",false,true);
	pangolin::Var<bool> settings_showActiveConstraints("ui.ActiveConst",true,true);
	pangolin::Var<bool> settings_showAllConstraints("ui.AllConst",false,true);

    // Added by Ivan Podmogilnyi 17.02.2022
    pangolin::Var<bool> ifDrawCube("ui.drawCube", false, true);
    pangolin::Var<bool> changeAngleSign("ui.ChangeAngleSign", false, true);
    pangolin::Var<double> xSkew("ui.x", 0, -100, 100);
    pangolin::Var<double> ySkew("ui.y", 0, -100, 100);
    pangolin::Var<double> zSkew("ui.z", 0, -100, 100);
    //End of append by Ivan

	pangolin::Var<bool> settings_show3D("ui.show3D",true,true);
	pangolin::Var<bool> settings_showLiveDepth("ui.showDepth",true,true);
	pangolin::Var<bool> settings_showLiveVideo("ui.showVideo",true,true);
    pangolin::Var<bool> settings_showLiveResidual("ui.showResidual",false,true);

	pangolin::Var<bool> settings_showFramesWindow("ui.showFramesWindow",false,true);
	pangolin::Var<bool> settings_showFullTracking("ui.showFullTracking",false,true);
	pangolin::Var<bool> settings_showCoarseTracking("ui.showCoarseTracking",false,true);


	pangolin::Var<int> settings_sparsity("ui.sparsity",1,1,20,false);
	pangolin::Var<double> settings_scaledVarTH("ui.relVarTH",0.001,1e-10,1e10, true);
	pangolin::Var<double> settings_absVarTH("ui.absVarTH",0.001,1e-10,1e10, true);
	pangolin::Var<double> settings_minRelBS("ui.minRelativeBS",0.1,0,1, false);


	pangolin::Var<bool> settings_resetButton("ui.Reset",false,false);


	pangolin::Var<int> settings_nPts("ui.activePoints",setting_desiredPointDensity, 50,5000, false);
	pangolin::Var<int> settings_nCandidates("ui.pointCandidates",setting_desiredImmatureDensity, 50,5000, false);
	pangolin::Var<int> settings_nMaxFrames("ui.maxFrames",setting_maxFrames, 4,10, false);
	pangolin::Var<double> settings_kfFrequency("ui.kfFrequency",setting_kfGlobalWeight,0.1,3, false);
	pangolin::Var<double> settings_gradHistAdd("ui.minGradAdd",setting_minGradHistAdd,0,15, false);

	pangolin::Var<double> settings_trackFps("ui.Track fps",0,0,0,false);
	pangolin::Var<double> settings_mapFps("ui.KF fps",0,0,0,false);

    // Obtaining the calibration matrix
    cv::Mat K = cv::Mat::eye(3, 3, cv::DataType<double>::type);
    CalibHessian hcalib = *Hcalib;
    std::cout << "Hessian calib" << std::endl;
    K.at<double>(0,0) = 190.978477;
    std::cout << "fx: " << K.at<double>(0, 0);
    K.at<double>(1,1) = 190.973307;
    std::cout << "fy: " << K.at<double>(1, 1);
    K.at<double>(0,2) = 254.931706;
    std::cout << "cx: " << K.at<double>(0, 2);
    K.at<double>(1,2) = 256.897442;
    std::cout << "cy: " << K.at<double> (1, 2);

    // Obtaining the distortion coefficients
    std::vector<double> distCoeffs(5);
    distCoeffs[0] = 0.003482389402;
    distCoeffs[1] = 0.000715034845;
    distCoeffs[2] = -0.002053236141;
    distCoeffs[3] = 0.000202936736;
    distCoeffs[4] = 0;

	// Default hooks for exiting (Esc) and fullscreen (tab).
	while( !pangolin::ShouldQuit() && running )
	{
		// Clear entire screen
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

		if(setting_render_display3D)
		{
			// Activate efficiently by object
			Visualization3D_display.Activate(Visualization3D_camera);
			boost::unique_lock<boost::mutex> lk3d(model3DMutex);
			//pangolin::glDrawColouredCube();
                        // 17.02.2022 Added by Ivan Podmogilnyi.
                        //drawLine(xSkew, ySkew, zSkew);
                        if (ifDrawCube){
                            drawCube(xSkew, ySkew, zSkew);

                            std::cout << "draw cube in pangolin success";

                            cv::Point3d point1 = {xSkew, ySkew, zSkew};
                            cv::Point3d point2 = {xSkew, ySkew+1, zSkew};
                            cv::Point3d point3 = {xSkew+1, ySkew, zSkew};
                            cv::Point3d point4 = {xSkew+1, ySkew+1, zSkew};
                            cv::Point3d point5 = {xSkew, ySkew, zSkew+1};
                            cv::Point3d point6 = {xSkew, ySkew+1, zSkew+1};
                            cv::Point3d point7 = {xSkew+1, ySkew, zSkew+1};
                            cv::Point3d point8 = {xSkew+1, ySkew+1, zSkew+1};

                            cv::Vec3b color1 = {133, 247, 208};
                            cv::Vec3b color2 = {253, 59, 86};
                            cv::Vec3b color3 = {147, 175, 215};
                            cv::Vec3b color4 = {80, 209, 168};
                            cv::Vec3b color5 = {154, 13, 88};
                            cv::Vec3b color6 = {253, 59, 86};
                            cv::Vec3b color7 = {5, 26, 72};
                            cv::Vec3b color8 = {72, 182, 8};
                            cv::Vec3b color9 = {28, 122, 71};
                            cv::Vec3b color10 = {88, 153, 225};
                            cv::Vec3b color11 = {184, 151, 253};
                            cv::Vec3b color12 = {244, 207, 185};

                            std::vector<cv::Point3d> objectPoints = {point1, point2, point3, point4, point5, point6, point7, point8};
                            std::vector<cv::Point2d> imagePoints;

                            auto s = lastPose;
                            std::cout << "getting last pose success" << std::endl;
                            if (s != nullptr){
                                bool write = true;
                                if(!s->poseValid) {
                                    write=false;
                                }
                                std::cout << "is pose valid? " << write << std::endl;
                                // Obtaining the current camera pose
                                Eigen::Matrix<double, 3, 3> rotMat = s->camToWorld.so3().matrix();
                                Eigen::Vector3d rvec = rot2rvec(rotMat, changeAngleSign);
                                std::vector<double> rVec(rvec.data(), rvec.data() + rvec.rows() * rvec.cols());
                                Eigen::Vector3d tvec = s->camToWorld.translation();
                                std::vector<double> tVec(tvec.data(), tvec.data() + tvec.rows() * tvec.cols());

                                // Projecting points
                                cv::projectPoints(objectPoints, rVec, tVec, K, distCoeffs, imagePoints);
                                cv::Mat image(h, w, CV_16UC3);
            //                    cv::cvtColor(image, image, cv::COLOR_BGR2RGB);

                                // Getting the current image.
                                openImagesMutex.lock();
                                for(int i=0;i<h;i++){
                                    for (int j=0;j<w;j++){
                                        image.at<cv::Vec3b>(i, j)[0] = internalVideoImg->data[i*w + j][0];
                                        image.at<cv::Vec3b>(i, j)[1] = internalVideoImg->data[i*w + j][1];
                                        image.at<cv::Vec3b>(i, j)[2] = internalVideoImg->data[i*w + j][2];
                                    }
                                }
                                openImagesMutex.unlock();

                                // Drawing the line on the image
            //                    cv::line(image, imagePoints[0], imagePoints[1], color1, 5);
            //                    cv::line(image, imagePoints[0], imagePoints[2], color2, 5);
            //                    cv::line(image, imagePoints[1], imagePoints[3], color3, 5);
            //                    cv::line(image, imagePoints[2], imagePoints[3], color4, 5);
            //                    cv::line(image, imagePoints[4], imagePoints[5], color5, 5);
            //                    cv::line(image, imagePoints[4], imagePoints[6], color6, 5);
            //                    cv::line(image, imagePoints[5], imagePoints[7], color7, 5);
            //                    cv::line(image, imagePoints[6], imagePoints[7], color8, 5);
            //                    cv::line(image, imagePoints[0], imagePoints[4], color9, 5);
            //                    cv::line(image, imagePoints[1], imagePoints[5], color10, 5);
            //                    cv::line(image, imagePoints[2], imagePoints[6], color11, 5);
            //                    cv::line(image, imagePoints[3], imagePoints[7], color12, 5);

                                cv::line(image, imagePoints[0], imagePoints[1], color1, 2);
                                cv::line(image, imagePoints[0], imagePoints[2], color1, 2);
                                cv::line(image, imagePoints[1], imagePoints[3], color1, 2);
                                cv::line(image, imagePoints[2], imagePoints[3], color1, 2);
                                cv::line(image, imagePoints[4], imagePoints[5], color1, 2);
                                cv::line(image, imagePoints[4], imagePoints[6], color1, 2);
                                cv::line(image, imagePoints[5], imagePoints[7], color1, 2);
                                cv::line(image, imagePoints[6], imagePoints[7], color1, 2);
                                cv::line(image, imagePoints[0], imagePoints[4], color1, 2);
                                cv::line(image, imagePoints[1], imagePoints[5], color1, 2);
                                cv::line(image, imagePoints[2], imagePoints[6], color1, 2);
                                cv::line(image, imagePoints[3], imagePoints[7], color1, 2);

                                // Updating the current image and updating the inner image.
                                openImagesMutex.lock();
                                for(int i=0;i<h;i++){
                                    for(int j=0;j<w;j++){
                                        internalVideoImg->data[i*w + j][0] = image.at<Vec3b>(i, j)[0];
                                        internalVideoImg->data[i*w + j][1] = image.at<Vec3b>(i, j)[1];
                                        internalVideoImg->data[i*w + j][2] = image.at<Vec3b>(i, j)[2];
                                    }
                                }
            //                    texVideo.Upload(internalVideoImg->data,GL_BGR,GL_UNSIGNED_BYTE);
                                openImagesMutex.unlock();
                            }
                        }


			int refreshed=0;
			for(KeyFrameDisplay* fh : keyframes)
			{

				float blue[3] = {0,0,1};
				if(this->settings_showKFCameras) fh->drawCam(1,blue,0.1);


				refreshed += (int)(fh->refreshPC(refreshed < 10, this->settings_scaledVarTH, this->settings_absVarTH,
						this->settings_pointCloudMode, this->settings_minRelBS, this->settings_sparsity));
				fh->drawPC(1);
			}
			if(this->settings_showCurrentCamera) currentCam->drawCam(2,0,0.2);
			drawConstraints();
			lk3d.unlock();
		}



		openImagesMutex.lock();
		if(videoImgChanged) 	        texVideo.Upload(internalVideoImg->data,GL_BGR,GL_UNSIGNED_BYTE);
		if(kfImgChanged) 		texKFDepth.Upload(internalKFImg->data,GL_BGR,GL_UNSIGNED_BYTE);
		if(resImgChanged) 		texResidual.Upload(internalResImg->data,GL_BGR,GL_UNSIGNED_BYTE);
		videoImgChanged=kfImgChanged=resImgChanged=false;
		openImagesMutex.unlock();




		// update fps counters
		{
			openImagesMutex.lock();
			float sd=0;
			for(float d : lastNMappingMs) sd+=d;
			settings_mapFps=lastNMappingMs.size()*1000.0f / sd;
			openImagesMutex.unlock();
		}
		{
			model3DMutex.lock();
			float sd=0;
			for(float d : lastNTrackingMs) sd+=d;
			settings_trackFps = lastNTrackingMs.size()*1000.0f / sd;
			model3DMutex.unlock();
		}


		if(setting_render_displayVideo)
		{
			d_video.Activate();
			glColor4f(1.0f,1.0f,1.0f,1.0f);
			texVideo.RenderToViewportFlipY();
		}

		if(setting_render_displayDepth)
		{
			d_kfDepth.Activate();
			glColor4f(1.0f,1.0f,1.0f,1.0f);
			texKFDepth.RenderToViewportFlipY();
		}

		if(setting_render_displayResidual)
		{
			d_residual.Activate();
			glColor4f(1.0f,1.0f,1.0f,1.0f);
			texResidual.RenderToViewportFlipY();
		}


	    // update parameters
	    this->settings_pointCloudMode = settings_pointCloudMode.Get();

	    this->settings_showActiveConstraints = settings_showActiveConstraints.Get();
	    this->settings_showAllConstraints = settings_showAllConstraints.Get();
	    this->settings_showCurrentCamera = settings_showCurrentCamera.Get();
	    this->settings_showKFCameras = settings_showKFCameras.Get();
	    this->settings_showTrajectory = settings_showTrajectory.Get();
	    this->settings_showFullTrajectory = settings_showFullTrajectory.Get();

		setting_render_display3D = settings_show3D.Get();
		setting_render_displayDepth = settings_showLiveDepth.Get();
		setting_render_displayVideo =  settings_showLiveVideo.Get();
		setting_render_displayResidual = settings_showLiveResidual.Get();

		setting_render_renderWindowFrames = settings_showFramesWindow.Get();
		setting_render_plotTrackingFull = settings_showFullTracking.Get();
		setting_render_displayCoarseTrackingFull = settings_showCoarseTracking.Get();


	    this->settings_absVarTH = settings_absVarTH.Get();
	    this->settings_scaledVarTH = settings_scaledVarTH.Get();
	    this->settings_minRelBS = settings_minRelBS.Get();
	    this->settings_sparsity = settings_sparsity.Get();

	    setting_desiredPointDensity = settings_nPts.Get();
	    setting_desiredImmatureDensity = settings_nCandidates.Get();
	    setting_maxFrames = settings_nMaxFrames.Get();
	    setting_kfGlobalWeight = settings_kfFrequency.Get();
	    setting_minGradHistAdd = settings_gradHistAdd.Get();


	    if(settings_resetButton.Get())
	    {
	    	printf("RESET!\n");
	    	settings_resetButton.Reset();
	    	setting_fullResetRequested = true;
	    }

		// Swap frames and Process Events
		pangolin::FinishFrame();


	    if(needReset) reset_internal();
	}


	printf("QUIT Pangolin thread!\n");
	printf("I'll just kill the whole process.\nSo Long, and Thanks for All the Fish!\n");

	exit(1);
}


void PangolinDSOViewer::close()
{
	running = false;
}

void PangolinDSOViewer::join()
{
	runThread.join();
	printf("JOINED Pangolin thread!\n");
}

void PangolinDSOViewer::reset()
{
	needReset = true;
}

void PangolinDSOViewer::reset_internal()
{
	model3DMutex.lock();
	for(size_t i=0; i<keyframes.size();i++) delete keyframes[i];
	keyframes.clear();
	allFramePoses.clear();
	keyframesByKFID.clear();
	connections.clear();
	model3DMutex.unlock();


	openImagesMutex.lock();
	internalVideoImg->setBlack();
	internalKFImg->setBlack();
	internalResImg->setBlack();
	videoImgChanged= kfImgChanged= resImgChanged=true;
	openImagesMutex.unlock();

	needReset = false;
}


void PangolinDSOViewer::drawConstraints()
{
	if(settings_showAllConstraints)
	{
		// draw constraints
		glLineWidth(1);
		glBegin(GL_LINES);

		glColor3f(0,1,0);
		glBegin(GL_LINES);
		for(unsigned int i=0;i<connections.size();i++)
		{
			if(connections[i].to == 0 || connections[i].from==0) continue;
			int nAct = connections[i].bwdAct + connections[i].fwdAct;
			int nMarg = connections[i].bwdMarg + connections[i].fwdMarg;
			if(nAct==0 && nMarg>0  )
			{
				Sophus::Vector3f t = connections[i].from->camToWorld.translation().cast<float>();
				glVertex3f((GLfloat) t[0],(GLfloat) t[1], (GLfloat) t[2]);
				t = connections[i].to->camToWorld.translation().cast<float>();
				glVertex3f((GLfloat) t[0],(GLfloat) t[1], (GLfloat) t[2]);
			}
		}
		glEnd();
	}

	if(settings_showActiveConstraints)
	{
		glLineWidth(3);
		glColor3f(0,0,1);
		glBegin(GL_LINES);
		for(unsigned int i=0;i<connections.size();i++)
		{
			if(connections[i].to == 0 || connections[i].from==0) continue;
			int nAct = connections[i].bwdAct + connections[i].fwdAct;

			if(nAct>0)
			{
				Sophus::Vector3f t = connections[i].from->camToWorld.translation().cast<float>();
				glVertex3f((GLfloat) t[0],(GLfloat) t[1], (GLfloat) t[2]);
				t = connections[i].to->camToWorld.translation().cast<float>();
				glVertex3f((GLfloat) t[0],(GLfloat) t[1], (GLfloat) t[2]);
			}
		}
		glEnd();
	}

	if(settings_showTrajectory)
	{
		float colorRed[3] = {1,0,0};
		glColor3f(colorRed[0],colorRed[1],colorRed[2]);
		glLineWidth(3);

		glBegin(GL_LINE_STRIP);
		for(unsigned int i=0;i<keyframes.size();i++)
		{
			glVertex3f((float)keyframes[i]->camToWorld.translation()[0],
					(float)keyframes[i]->camToWorld.translation()[1],
					(float)keyframes[i]->camToWorld.translation()[2]);
		}
		glEnd();
	}

	if(settings_showFullTrajectory)
	{
		float colorGreen[3] = {0,1,0};
		glColor3f(colorGreen[0],colorGreen[1],colorGreen[2]);
		glLineWidth(3);

		glBegin(GL_LINE_STRIP);
		for(unsigned int i=0;i<allFramePoses.size();i++)
		{
			glVertex3f((float)allFramePoses[i][0],
					(float)allFramePoses[i][1],
					(float)allFramePoses[i][2]);
		}
		glEnd();
	}
}

void PangolinDSOViewer::publishGraph(const std::map<uint64_t, Eigen::Vector2i, std::less<uint64_t>, Eigen::aligned_allocator<std::pair<const uint64_t, Eigen::Vector2i>>> &connectivity)
{
    if(!setting_render_display3D) return;
    if(disableAllDisplay) return;

	model3DMutex.lock();
    connections.resize(connectivity.size());
	int runningID=0;
	int totalActFwd=0, totalActBwd=0, totalMargFwd=0, totalMargBwd=0;
    for(std::pair<uint64_t,Eigen::Vector2i> p : connectivity)
	{
		int host = (int)(p.first >> 32);
        int target = (int)(p.first & (uint64_t)0xFFFFFFFF);

		assert(host >= 0 && target >= 0);
		if(host == target)
		{
			assert(p.second[0] == 0 && p.second[1] == 0);
			continue;
		}

		if(host > target) continue;

		connections[runningID].from = keyframesByKFID.count(host) == 0 ? 0 : keyframesByKFID[host];
		connections[runningID].to = keyframesByKFID.count(target) == 0 ? 0 : keyframesByKFID[target];
		connections[runningID].fwdAct = p.second[0];
		connections[runningID].fwdMarg = p.second[1];
		totalActFwd += p.second[0];
		totalMargFwd += p.second[1];

        uint64_t inverseKey = (((uint64_t)target) << 32) + ((uint64_t)host);
		Eigen::Vector2i st = connectivity.at(inverseKey);
		connections[runningID].bwdAct = st[0];
		connections[runningID].bwdMarg = st[1];

		totalActBwd += st[0];
		totalMargBwd += st[1];

		runningID++;
	}


	model3DMutex.unlock();
}
void PangolinDSOViewer::publishKeyframes(
		std::vector<FrameHessian*> &frames,
		bool final,
		CalibHessian* HCalib)
{
	if(!setting_render_display3D) return;
    if(disableAllDisplay) return;

	boost::unique_lock<boost::mutex> lk(model3DMutex);
	for(FrameHessian* fh : frames)
	{
		if(keyframesByKFID.find(fh->frameID) == keyframesByKFID.end())
		{
			KeyFrameDisplay* kfd = new KeyFrameDisplay();
			keyframesByKFID[fh->frameID] = kfd;
			keyframes.push_back(kfd);
		}
		keyframesByKFID[fh->frameID]->setFromKF(fh, HCalib);
	}
}
void PangolinDSOViewer::publishCamPose(FrameShell* frame,
		CalibHessian* HCalib)
{
    if(!setting_render_display3D) return;
    if(disableAllDisplay) return;

	boost::unique_lock<boost::mutex> lk(model3DMutex);
	struct timeval time_now;
	gettimeofday(&time_now, NULL);
	lastNTrackingMs.push_back(((time_now.tv_sec-last_track.tv_sec)*1000.0f + (time_now.tv_usec-last_track.tv_usec)/1000.0f));
	if(lastNTrackingMs.size() > 10) lastNTrackingMs.pop_front();
	last_track = time_now;

	if(!setting_render_display3D) return;

	currentCam->setFromF(frame, HCalib);
	allFramePoses.push_back(frame->camToWorld.translation().cast<float>());
}


void PangolinDSOViewer::pushLiveFrame(FrameHessian* image)
{
	if(!setting_render_displayVideo) return;
    if(disableAllDisplay) return;

	boost::unique_lock<boost::mutex> lk(openImagesMutex);

	for(int i=0;i<w*h;i++)
		internalVideoImg->data[i][0] =
		internalVideoImg->data[i][1] =
		internalVideoImg->data[i][2] =
			image->dI[i][0]*0.8 > 255.0f ? 255.0 : image->dI[i][0]*0.8;

	videoImgChanged=true;
}

bool PangolinDSOViewer::needPushDepthImage()
{
    return setting_render_displayDepth;
}
void PangolinDSOViewer::pushDepthImage(MinimalImageB3* image)
{

    if(!setting_render_displayDepth) return;
    if(disableAllDisplay) return;

	boost::unique_lock<boost::mutex> lk(openImagesMutex);

	struct timeval time_now;
	gettimeofday(&time_now, NULL);
	lastNMappingMs.push_back(((time_now.tv_sec-last_map.tv_sec)*1000.0f + (time_now.tv_usec-last_map.tv_usec)/1000.0f));
	if(lastNMappingMs.size() > 10) lastNMappingMs.pop_front();
	last_map = time_now;

	memcpy(internalKFImg->data, image->data, w*h*3);
	kfImgChanged=true;
}

}
}
