v1

src/FullSystem/CoarseInitializer.h

@@ -0,0 +1,192 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+#include "util/NumType.h"
+#include "OptimizationBackend/MatrixAccumulators.h"
+#include "IOWrapper/Output3DWrapper.h"
+#include "util/settings.h"
+#include "vector"
+#include <math.h>
+
+
+
+
+namespace dso
+{
+struct CalibHessian;
+struct FrameHessian;
+
+
+struct Pnt
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	// index in jacobian. never changes (actually, there is no reason why).
+	float u,v;
+
+	// idepth / isgood / energy during optimization.
+	float idepth;
+	bool isGood;
+	Vec2f energy;		// (UenergyPhotometric, energyRegularizer)
+	bool isGood_new;
+	float idepth_new;
+	Vec2f energy_new;
+
+	float iR;
+	float iRSumNum;
+
+	float lastHessian;
+	float lastHessian_new;
+
+	// max stepsize for idepth (corresponding to max. movement in pixel-space).
+	float maxstep;
+
+	// idx (x+y*w) of closest point one pyramid level above.
+	int parent;
+	float parentDist;
+
+	// idx (x+y*w) of up to 10 nearest points in pixel space.
+	int neighbours[10];
+	float neighboursDist[10];
+
+	float my_type;
+	float outlierTH;
+};
+
+class CoarseInitializer {
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	CoarseInitializer(int w, int h);
+	~CoarseInitializer();
+
+
+	void setFirst(	CalibHessian* HCalib, FrameHessian* newFrameHessian);
+	bool trackFrame(FrameHessian* newFrameHessian, std::vector<IOWrap::Output3DWrapper*> &wraps);
+	void calcTGrads(FrameHessian* newFrameHessian);
+
+	int frameID;
+	bool fixAffine;
+	bool printDebug;
+
+	Pnt* points[PYR_LEVELS];
+	int numPoints[PYR_LEVELS];
+	AffLight thisToNext_aff;
+	SE3 thisToNext;
+
+
+	FrameHessian* firstFrame;
+	FrameHessian* newFrame;
+private:
+	Mat33 K[PYR_LEVELS];
+	Mat33 Ki[PYR_LEVELS];
+	double fx[PYR_LEVELS];
+	double fy[PYR_LEVELS];
+	double fxi[PYR_LEVELS];
+	double fyi[PYR_LEVELS];
+	double cx[PYR_LEVELS];
+	double cy[PYR_LEVELS];
+	double cxi[PYR_LEVELS];
+	double cyi[PYR_LEVELS];
+	int w[PYR_LEVELS];
+	int h[PYR_LEVELS];
+	void makeK(CalibHessian* HCalib);
+
+	bool snapped;
+	int snappedAt;
+
+	// pyramid images & levels on all levels
+	Eigen::Vector3f* dINew[PYR_LEVELS];
+	Eigen::Vector3f* dIFist[PYR_LEVELS];
+
+	Eigen::DiagonalMatrix<float, 8> wM;
+
+	// temporary buffers for H and b.
+	Vec10f* JbBuffer;			// 0-7: sum(dd * dp). 8: sum(res*dd). 9: 1/(1+sum(dd*dd))=inverse hessian entry.
+	Vec10f* JbBuffer_new;
+
+	Accumulator9 acc9;
+	Accumulator9 acc9SC;
+
+
+	Vec3f dGrads[PYR_LEVELS];
+
+	float alphaK;
+	float alphaW;
+	float regWeight;
+	float couplingWeight;
+
+	Vec3f calcResAndGS(
+			int lvl,
+			Mat88f &H_out, Vec8f &b_out,
+			Mat88f &H_out_sc, Vec8f &b_out_sc,
+			const SE3 &refToNew, AffLight refToNew_aff,
+			bool plot);
+	Vec3f calcEC(int lvl); // returns OLD NERGY, NEW ENERGY, NUM TERMS.
+	void optReg(int lvl);
+
+	void propagateUp(int srcLvl);
+	void propagateDown(int srcLvl);
+	float rescale();
+
+	void resetPoints(int lvl);
+	void doStep(int lvl, float lambda, Vec8f inc);
+	void applyStep(int lvl);
+
+	void makeGradients(Eigen::Vector3f** data);
+
+    void debugPlot(int lvl, std::vector<IOWrap::Output3DWrapper*> &wraps);
+	void makeNN();
+};
+
+
+
+
+struct FLANNPointcloud
+{
+    inline FLANNPointcloud() {num=0; points=0;}
+    inline FLANNPointcloud(int n, Pnt* p) :  num(n), points(p) {}
+	int num;
+	Pnt* points;
+	inline size_t kdtree_get_point_count() const { return num; }
+	inline float kdtree_distance(const float *p1, const size_t idx_p2,size_t /*size*/) const
+	{
+		const float d0=p1[0]-points[idx_p2].u;
+		const float d1=p1[1]-points[idx_p2].v;
+		return d0*d0+d1*d1;
+	}
+
+	inline float kdtree_get_pt(const size_t idx, int dim) const
+	{
+		if (dim==0) return points[idx].u;
+		else return points[idx].v;
+	}
+	template <class BBOX>
+		bool kdtree_get_bbox(BBOX& /* bb */) const { return false; }
+};
+
+}
+
+

src/FullSystem/CoarseTracker.h

@@ -0,0 +1,176 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+ 
+#include "util/NumType.h"
+#include "vector"
+#include <math.h>
+#include "util/settings.h"
+#include "OptimizationBackend/MatrixAccumulators.h"
+#include "IOWrapper/Output3DWrapper.h"
+
+
+
+
+namespace dso
+{
+struct CalibHessian;
+struct FrameHessian;
+struct PointFrameResidual;
+
+class CoarseTracker {
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+	CoarseTracker(int w, int h);
+	~CoarseTracker();
+
+	bool trackNewestCoarse(
+			FrameHessian* newFrameHessian,
+			SE3 &lastToNew_out, AffLight &aff_g2l_out,
+			int coarsestLvl, Vec5 minResForAbort,
+			IOWrap::Output3DWrapper* wrap=0);
+
+	void setCoarseTrackingRef(
+			std::vector<FrameHessian*> frameHessians);
+
+	void makeK(
+			CalibHessian* HCalib);
+
+	bool debugPrint, debugPlot;
+
+	Mat33f K[PYR_LEVELS];
+	Mat33f Ki[PYR_LEVELS];
+	float fx[PYR_LEVELS];
+	float fy[PYR_LEVELS];
+	float fxi[PYR_LEVELS];
+	float fyi[PYR_LEVELS];
+	float cx[PYR_LEVELS];
+	float cy[PYR_LEVELS];
+	float cxi[PYR_LEVELS];
+	float cyi[PYR_LEVELS];
+	int w[PYR_LEVELS];
+	int h[PYR_LEVELS];
+
+    void debugPlotIDepthMap(float* minID, float* maxID, std::vector<IOWrap::Output3DWrapper*> &wraps);
+    void debugPlotIDepthMapFloat(std::vector<IOWrap::Output3DWrapper*> &wraps);
+
+	FrameHessian* lastRef;
+	AffLight lastRef_aff_g2l;
+	FrameHessian* newFrame;
+	int refFrameID;
+
+	// act as pure ouptut
+	Vec5 lastResiduals;
+	Vec3 lastFlowIndicators;
+	double firstCoarseRMSE;
+private:
+
+
+	void makeCoarseDepthL0(std::vector<FrameHessian*> frameHessians);
+	float* idepth[PYR_LEVELS];
+	float* weightSums[PYR_LEVELS];
+	float* weightSums_bak[PYR_LEVELS];
+
+
+	Vec6 calcResAndGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH);
+	Vec6 calcRes(int lvl, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH);
+	void calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l);
+	void calcGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l);
+
+	// pc buffers
+	float* pc_u[PYR_LEVELS];
+	float* pc_v[PYR_LEVELS];
+	float* pc_idepth[PYR_LEVELS];
+	float* pc_color[PYR_LEVELS];
+	int pc_n[PYR_LEVELS];
+
+	// warped buffers
+	float* buf_warped_idepth;
+	float* buf_warped_u;
+	float* buf_warped_v;
+	float* buf_warped_dx;
+	float* buf_warped_dy;
+	float* buf_warped_residual;
+	float* buf_warped_weight;
+	float* buf_warped_refColor;
+	int buf_warped_n;
+
+
+    std::vector<float*> ptrToDelete;
+
+
+	Accumulator9 acc;
+};
+
+
+class CoarseDistanceMap {
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+	CoarseDistanceMap(int w, int h);
+	~CoarseDistanceMap();
+
+	void makeDistanceMap(
+			std::vector<FrameHessian*> frameHessians,
+			FrameHessian* frame);
+
+	void makeInlierVotes(
+			std::vector<FrameHessian*> frameHessians);
+
+	void makeK( CalibHessian* HCalib);
+
+
+	float* fwdWarpedIDDistFinal;
+
+	Mat33f K[PYR_LEVELS];
+	Mat33f Ki[PYR_LEVELS];
+	float fx[PYR_LEVELS];
+	float fy[PYR_LEVELS];
+	float fxi[PYR_LEVELS];
+	float fyi[PYR_LEVELS];
+	float cx[PYR_LEVELS];
+	float cy[PYR_LEVELS];
+	float cxi[PYR_LEVELS];
+	float cyi[PYR_LEVELS];
+	int w[PYR_LEVELS];
+	int h[PYR_LEVELS];
+
+	void addIntoDistFinal(int u, int v);
+
+
+private:
+
+	PointFrameResidual** coarseProjectionGrid;
+	int* coarseProjectionGridNum;
+	Eigen::Vector2i* bfsList1;
+	Eigen::Vector2i* bfsList2;
+
+	void growDistBFS(int bfsNum);
+};
+
+}
+

src/FullSystem/FullSystem.h

@@ -0,0 +1,324 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+#define MAX_ACTIVE_FRAMES 100
+
+#include <deque>
+#include "util/NumType.h"
+#include "util/globalCalib.h"
+#include "vector"
+ 
+#include <iostream>
+#include <fstream>
+#include "util/NumType.h"
+#include "FullSystem/Residuals.h"
+#include "FullSystem/HessianBlocks.h"
+#include "util/FrameShell.h"
+#include "util/IndexThreadReduce.h"
+#include "OptimizationBackend/EnergyFunctional.h"
+#include "FullSystem/PixelSelector2.h"
+
+#include <math.h>
+
+namespace dso
+{
+namespace IOWrap
+{
+class Output3DWrapper;
+}
+
+class PixelSelector;
+class PCSyntheticPoint;
+class CoarseTracker;
+struct FrameHessian;
+struct PointHessian;
+class CoarseInitializer;
+struct ImmaturePointTemporaryResidual;
+class ImageAndExposure;
+class CoarseDistanceMap;
+
+class EnergyFunctional;
+
+template<typename T> inline void deleteOut(std::vector<T*> &v, const int i)
+{
+	delete v[i];
+	v[i] = v.back();
+	v.pop_back();
+}
+template<typename T> inline void deleteOutPt(std::vector<T*> &v, const T* i)
+{
+	delete i;
+
+	for(unsigned int k=0;k<v.size();k++)
+		if(v[k] == i)
+		{
+			v[k] = v.back();
+			v.pop_back();
+		}
+}
+template<typename T> inline void deleteOutOrder(std::vector<T*> &v, const int i)
+{
+	delete v[i];
+	for(unsigned int k=i+1; k<v.size();k++)
+		v[k-1] = v[k];
+	v.pop_back();
+}
+template<typename T> inline void deleteOutOrder(std::vector<T*> &v, const T* element)
+{
+	int i=-1;
+	for(unsigned int k=0; k<v.size();k++)
+	{
+		if(v[k] == element)
+		{
+			i=k;
+			break;
+		}
+	}
+	assert(i!=-1);
+
+	for(unsigned int k=i+1; k<v.size();k++)
+		v[k-1] = v[k];
+	v.pop_back();
+
+	delete element;
+}
+
+
+inline bool eigenTestNan(const MatXX &m, std::string msg)
+{
+	bool foundNan = false;
+	for(int y=0;y<m.rows();y++)
+		for(int x=0;x<m.cols();x++)
+		{
+			if(!std::isfinite((double)m(y,x))) foundNan = true;
+		}
+
+	if(foundNan)
+	{
+		printf("NAN in %s:\n",msg.c_str());
+		std::cout << m << "\n\n";
+	}
+
+
+	return foundNan;
+}
+
+
+
+
+
+class FullSystem {
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	FullSystem();
+	virtual ~FullSystem();
+
+	// adds a new frame, and creates point & residual structs.
+	void addActiveFrame(ImageAndExposure* image, int id);
+
+	// marginalizes a frame. drops / marginalizes points & residuals.
+	void marginalizeFrame(FrameHessian* frame);
+	void blockUntilMappingIsFinished();
+
+	float optimize(int mnumOptIts);
+
+	void printResult(std::string file);
+
+	void debugPlot(std::string name);
+
+	void printFrameLifetimes();
+	// contains pointers to active frames
+
+    std::vector<IOWrap::Output3DWrapper*> outputWrapper;
+
+	bool isLost;
+	bool initFailed;
+	bool initialized;
+	bool linearizeOperation;
+
+
+	void setGammaFunction(float* BInv);
+	void setOriginalCalib(const VecXf &originalCalib, int originalW, int originalH);
+
+    //Addition
+    FrameShell* getLastPose();
+    CalibHessian* getHCalib();
+
+private:
+
+	CalibHessian Hcalib;
+
+
+
+
+	// opt single point
+	int optimizePoint(PointHessian* point, int minObs, bool flagOOB);
+	PointHessian* optimizeImmaturePoint(ImmaturePoint* point, int minObs, ImmaturePointTemporaryResidual* residuals);
+
+	double linAllPointSinle(PointHessian* point, float outlierTHSlack, bool plot);
+
+	// mainPipelineFunctions
+	Vec4 trackNewCoarse(FrameHessian* fh);
+	void traceNewCoarse(FrameHessian* fh);
+	void activatePoints();
+	void activatePointsMT();
+	void activatePointsOldFirst();
+	void flagPointsForRemoval();
+	void makeNewTraces(FrameHessian* newFrame, float* gtDepth);
+	void initializeFromInitializer(FrameHessian* newFrame);
+	void flagFramesForMarginalization(FrameHessian* newFH);
+
+
+	void removeOutliers();
+
+
+	// set precalc values.
+	void setPrecalcValues();
+
+
+	// solce. eventually migrate to ef.
+	void solveSystem(int iteration, double lambda);
+	Vec3 linearizeAll(bool fixLinearization);
+	bool doStepFromBackup(float stepfacC,float stepfacT,float stepfacR,float stepfacA,float stepfacD);
+	void backupState(bool backupLastStep);
+	void loadSateBackup();
+	double calcLEnergy();
+	double calcMEnergy();
+	void linearizeAll_Reductor(bool fixLinearization, std::vector<PointFrameResidual*>* toRemove, int min, int max, Vec10* stats, int tid);
+	void activatePointsMT_Reductor(std::vector<PointHessian*>* optimized,std::vector<ImmaturePoint*>* toOptimize,int min, int max, Vec10* stats, int tid);
+	void applyRes_Reductor(bool copyJacobians, int min, int max, Vec10* stats, int tid);
+
+	void printOptRes(const Vec3 &res, double resL, double resM, double resPrior, double LExact, float a, float b);
+
+	void debugPlotTracking();
+
+	std::vector<VecX> getNullspaces(
+			std::vector<VecX> &nullspaces_pose,
+			std::vector<VecX> &nullspaces_scale,
+			std::vector<VecX> &nullspaces_affA,
+			std::vector<VecX> &nullspaces_affB);
+
+	void setNewFrameEnergyTH();
+
+
+	void printLogLine();
+	void printEvalLine();
+	void printEigenValLine();
+	std::ofstream* calibLog;
+	std::ofstream* numsLog;
+	std::ofstream* errorsLog;
+	std::ofstream* eigenAllLog;
+	std::ofstream* eigenPLog;
+	std::ofstream* eigenALog;
+	std::ofstream* DiagonalLog;
+	std::ofstream* variancesLog;
+	std::ofstream* nullspacesLog;
+
+	std::ofstream* coarseTrackingLog;
+
+	// statistics
+	long int statistics_lastNumOptIts;
+	long int statistics_numDroppedPoints;
+	long int statistics_numActivatedPoints;
+	long int statistics_numCreatedPoints;
+	long int statistics_numForceDroppedResBwd;
+	long int statistics_numForceDroppedResFwd;
+	long int statistics_numMargResFwd;
+	long int statistics_numMargResBwd;
+	float statistics_lastFineTrackRMSE;
+
+
+
+
+
+
+
+	// =================== changed by tracker-thread. protected by trackMutex ============
+	boost::mutex trackMutex;
+	std::vector<FrameShell*> allFrameHistory;
+	CoarseInitializer* coarseInitializer;
+	Vec5 lastCoarseRMSE;
+
+
+	// ================== changed by mapper-thread. protected by mapMutex ===============
+	boost::mutex mapMutex;
+	std::vector<FrameShell*> allKeyFramesHistory;
+
+	EnergyFunctional* ef;
+	IndexThreadReduce<Vec10> treadReduce;
+
+	float* selectionMap;
+	PixelSelector* pixelSelector;
+	CoarseDistanceMap* coarseDistanceMap;
+
+	std::vector<FrameHessian*> frameHessians;	// ONLY changed in marginalizeFrame and addFrame.
+	std::vector<PointFrameResidual*> activeResiduals;
+	float currentMinActDist;
+
+
+	std::vector<float> allResVec;
+
+
+
+	// mutex etc. for tracker exchange.
+	boost::mutex coarseTrackerSwapMutex;			// if tracker sees that there is a new reference, tracker locks [coarseTrackerSwapMutex] and swaps the two.
+	CoarseTracker* coarseTracker_forNewKF;			// set as as reference. protected by [coarseTrackerSwapMutex].
+	CoarseTracker* coarseTracker;					// always used to track new frames. protected by [trackMutex].
+	float minIdJetVisTracker, maxIdJetVisTracker;
+	float minIdJetVisDebug, maxIdJetVisDebug;
+
+
+
+
+
+	// mutex for camToWorl's in shells (these are always in a good configuration).
+	boost::mutex shellPoseMutex;
+
+
+
+/*
+ * tracking always uses the newest KF as reference.
+ *
+ */
+
+	void makeKeyFrame( FrameHessian* fh);
+	void makeNonKeyFrame( FrameHessian* fh);
+	void deliverTrackedFrame(FrameHessian* fh, bool needKF);
+	void mappingLoop();
+
+	// tracking / mapping synchronization. All protected by [trackMapSyncMutex].
+	boost::mutex trackMapSyncMutex;
+	boost::condition_variable trackedFrameSignal;
+	boost::condition_variable mappedFrameSignal;
+	std::deque<FrameHessian*> unmappedTrackedFrames;
+	int needNewKFAfter;	// Otherwise, a new KF is *needed that has ID bigger than [needNewKFAfter]*.
+	boost::thread mappingThread;
+	bool runMapping;
+	bool needToKetchupMapping;
+
+	int lastRefStopID;
+};
+}
+

src/FullSystem/FullSystemDebugStuff.cpp

@@ -0,0 +1,365 @@
+/**
+* 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/>.
+*/
+
+
+/*
+ * KFBuffer.cpp
+ *
+ *  Created on: Jan 7, 2014
+ *      Author: engelj
+ */
+
+#include "FullSystem/FullSystem.h"
+ 
+#include "stdio.h"
+#include "util/globalFuncs.h"
+#include <Eigen/LU>
+#include <algorithm>
+#include "IOWrapper/ImageDisplay.h"
+#include "IOWrapper/ImageRW.h"
+#include "util/globalCalib.h"
+#include <Eigen/SVD>
+#include <Eigen/Eigenvalues>
+#include <algorithm>
+
+#include "FullSystem/ImmaturePoint.h"
+
+
+namespace dso
+{
+
+
+	void FullSystem::debugPlotTracking()
+	{
+		if(disableAllDisplay) return;
+		if(!setting_render_plotTrackingFull) return;
+		int wh = hG[0]*wG[0];
+
+		int idx=0;
+		for(FrameHessian* f : frameHessians)
+		{
+			std::vector<MinimalImageB3* > images;
+
+			// make images for all frames. will be deleted by the FrameHessian's destructor.
+			for(FrameHessian* f2 : frameHessians)
+				if(f2->debugImage == 0) f2->debugImage = new MinimalImageB3(wG[0], hG[0]);
+
+			for(FrameHessian* f2 : frameHessians)
+			{
+				MinimalImageB3* debugImage=f2->debugImage;
+				images.push_back(debugImage);
+
+				Eigen::Vector3f* fd = f2->dI;
+
+				Vec2 affL = AffLight::fromToVecExposure(f2->ab_exposure, f->ab_exposure, f2->aff_g2l(), f->aff_g2l());
+
+				for(int i=0;i<wh;i++)
+				{
+					// BRIGHTNESS TRANSFER
+					float colL = affL[0] * fd[i][0] + affL[1];
+					if(colL<0) colL=0; if(colL>255) colL =255;
+					debugImage->at(i) = Vec3b(colL, colL, colL);
+				}
+			}
+
+
+			for(PointHessian* ph : f->pointHessians)
+			{
+				assert(ph->status == PointHessian::ACTIVE);
+				if(ph->status == PointHessian::ACTIVE || ph->status == PointHessian::MARGINALIZED)
+				{
+					for(PointFrameResidual* r : ph->residuals)
+						r->debugPlot();
+					f->debugImage->setPixel9(ph->u+0.5, ph->v+0.5, makeRainbow3B(ph->idepth_scaled));
+				}
+			}
+
+
+			char buf[100];
+			snprintf(buf, 100, "IMG %d", idx);
+			IOWrap::displayImageStitch(buf, images);
+			idx++;
+		}
+
+		IOWrap::waitKey(0);
+
+	}
+
+
+	void FullSystem::debugPlot(std::string name)
+	{
+		if(disableAllDisplay) return;
+		if(!setting_render_renderWindowFrames) return;
+		std::vector<MinimalImageB3* > images;
+
+
+
+
+		float minID=0, maxID=0;
+		if((int)(freeDebugParam5+0.5f) == 7 || (debugSaveImages&&false))
+		{
+			std::vector<float> allID;
+			for(unsigned int f=0;f<frameHessians.size();f++)
+			{
+				for(PointHessian* ph : frameHessians[f]->pointHessians)
+					if(ph!=0) allID.push_back(ph->idepth_scaled);
+
+				for(PointHessian* ph : frameHessians[f]->pointHessiansMarginalized)
+					if(ph!=0) allID.push_back(ph->idepth_scaled);
+
+				for(PointHessian* ph : frameHessians[f]->pointHessiansOut)
+					if(ph!=0) allID.push_back(ph->idepth_scaled);
+			}
+			std::sort(allID.begin(), allID.end());
+			int n = allID.size()-1;
+			minID = allID[(int)(n*0.05)];
+			maxID = allID[(int)(n*0.95)];
+
+
+			// slowly adapt: change by maximum 10% of old span.
+			float maxChange = 0.1*(maxIdJetVisDebug - minIdJetVisDebug);
+			if(maxIdJetVisDebug < 0  || minIdJetVisDebug < 0 ) maxChange = 1e5;
+
+
+			if(minID < minIdJetVisDebug - maxChange)
+				minID = minIdJetVisDebug - maxChange;
+			if(minID > minIdJetVisDebug + maxChange)
+				minID = minIdJetVisDebug + maxChange;
+
+
+			if(maxID < maxIdJetVisDebug - maxChange)
+				maxID = maxIdJetVisDebug - maxChange;
+			if(maxID > maxIdJetVisDebug + maxChange)
+				maxID = maxIdJetVisDebug + maxChange;
+
+			maxIdJetVisDebug = maxID;
+			minIdJetVisDebug = minID;
+
+		}
+
+
+
+
+
+
+
+
+
+
+
+
+		int wh = hG[0]*wG[0];
+		for(unsigned int f=0;f<frameHessians.size();f++)
+		{
+			MinimalImageB3* img = new MinimalImageB3(wG[0],hG[0]);
+			images.push_back(img);
+			//float* fd = frameHessians[f]->I;
+			Eigen::Vector3f* fd = frameHessians[f]->dI;
+
+
+			for(int i=0;i<wh;i++)
+			{
+				int c = fd[i][0]*0.9f;
+				if(c>255) c=255;
+				img->at(i) = Vec3b(c,c,c);
+			}
+
+			if((int)(freeDebugParam5+0.5f) == 0)
+			{
+				for(PointHessian* ph : frameHessians[f]->pointHessians)
+				{
+					if(ph==0) continue;
+
+					img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, makeRainbow3B(ph->idepth_scaled));
+				}
+				for(PointHessian* ph : frameHessians[f]->pointHessiansMarginalized)
+				{
+					if(ph==0) continue;
+					img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, makeRainbow3B(ph->idepth_scaled));
+				}
+				for(PointHessian* ph : frameHessians[f]->pointHessiansOut)
+					img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,255,255));
+			}
+			else if((int)(freeDebugParam5+0.5f) == 1)
+			{
+				for(PointHessian* ph : frameHessians[f]->pointHessians)
+				{
+					if(ph==0) continue;
+					img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, makeRainbow3B(ph->idepth_scaled));
+				}
+
+				for(PointHessian* ph : frameHessians[f]->pointHessiansMarginalized)
+					img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,0));
+
+				for(PointHessian* ph : frameHessians[f]->pointHessiansOut)
+					img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,255,255));
+			}
+			else if((int)(freeDebugParam5+0.5f) == 2)
+			{
+
+			}
+			else if((int)(freeDebugParam5+0.5f) == 3)
+			{
+				for(ImmaturePoint* ph : frameHessians[f]->immaturePoints)
+				{
+					if(ph==0) continue;
+					if(ph->lastTraceStatus==ImmaturePointStatus::IPS_GOOD ||
+							ph->lastTraceStatus==ImmaturePointStatus::IPS_SKIPPED ||
+							ph->lastTraceStatus==ImmaturePointStatus::IPS_BADCONDITION)
+					{
+						if(!std::isfinite(ph->idepth_max))
+							img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,0));
+						else
+						{
+							img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, makeRainbow3B((ph->idepth_min + ph->idepth_max)*0.5f));
+						}
+					}
+				}
+			}
+			else if((int)(freeDebugParam5+0.5f) == 4)
+			{
+				for(ImmaturePoint* ph : frameHessians[f]->immaturePoints)
+				{
+					if(ph==0) continue;
+
+					if(ph->lastTraceStatus==ImmaturePointStatus::IPS_GOOD)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,255,0));
+					if(ph->lastTraceStatus==ImmaturePointStatus::IPS_OOB)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,0,0));
+					if(ph->lastTraceStatus==ImmaturePointStatus::IPS_OUTLIER)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,255));
+					if(ph->lastTraceStatus==ImmaturePointStatus::IPS_SKIPPED)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,255,0));
+					if(ph->lastTraceStatus==ImmaturePointStatus::IPS_BADCONDITION)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,255,255));
+					if(ph->lastTraceStatus==ImmaturePointStatus::IPS_UNINITIALIZED)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,0));
+				}
+			}
+			else if((int)(freeDebugParam5+0.5f) == 5)
+			{
+				for(ImmaturePoint* ph : frameHessians[f]->immaturePoints)
+				{
+					if(ph==0) continue;
+
+					if(ph->lastTraceStatus==ImmaturePointStatus::IPS_UNINITIALIZED) continue;
+					float d = freeDebugParam1 * (sqrtf(ph->quality)-1);
+					if(d<0) d=0;
+					if(d>1) d=1;
+					img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,d*255,(1-d)*255));
+				}
+
+			}
+			else if((int)(freeDebugParam5+0.5f) == 6)
+			{
+				for(PointHessian* ph : frameHessians[f]->pointHessians)
+				{
+					if(ph==0) continue;
+					if(ph->my_type==0)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,0,255));
+					if(ph->my_type==1)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,0,0));
+					if(ph->my_type==2)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,255));
+					if(ph->my_type==3)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,255,255));
+				}
+				for(PointHessian* ph : frameHessians[f]->pointHessiansMarginalized)
+				{
+					if(ph==0) continue;
+					if(ph->my_type==0)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,0,255));
+					if(ph->my_type==1)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(255,0,0));
+					if(ph->my_type==2)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,255));
+					if(ph->my_type==3)
+						img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,255,255));
+				}
+
+			}
+			if((int)(freeDebugParam5+0.5f) == 7)
+			{
+				for(PointHessian* ph : frameHessians[f]->pointHessians)
+				{
+					img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, makeJet3B((ph->idepth_scaled-minID) / ((maxID-minID))));
+				}
+				for(PointHessian* ph : frameHessians[f]->pointHessiansMarginalized)
+				{
+					if(ph==0) continue;
+					img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,0));
+				}
+			}
+		}
+		IOWrap::displayImageStitch(name.c_str(), images);
+		IOWrap::waitKey(5);
+
+		for(unsigned int i=0;i<images.size();i++)
+			delete images[i];
+
+
+
+		if((debugSaveImages&&false))
+		{
+			for(unsigned int f=0;f<frameHessians.size();f++)
+			{
+				MinimalImageB3* img = new MinimalImageB3(wG[0],hG[0]);
+				Eigen::Vector3f* fd = frameHessians[f]->dI;
+
+				for(int i=0;i<wh;i++)
+				{
+					int c = fd[i][0]*0.9f;
+					if(c>255) c=255;
+					img->at(i) = Vec3b(c,c,c);
+				}
+
+				for(PointHessian* ph : frameHessians[f]->pointHessians)
+				{
+					img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, makeJet3B((ph->idepth_scaled-minID) / ((maxID-minID))));
+				}
+				for(PointHessian* ph : frameHessians[f]->pointHessiansMarginalized)
+				{
+					if(ph==0) continue;
+					img->setPixelCirc(ph->u+0.5f, ph->v+0.5f, Vec3b(0,0,0));
+				}
+
+				char buf[1000];
+				snprintf(buf, 1000, "images_out/kf_%05d_%05d_%02d.png",
+						frameHessians.back()->shell->id,  frameHessians.back()->frameID, f);
+				IOWrap::writeImage(buf,img);
+
+				delete img;
+			}
+		}
+
+
+
+
+	}
+
+
+
+
+
+
+}

src/FullSystem/FullSystemMarginalize.cpp

@@ -0,0 +1,220 @@
+/**
+* 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/>.
+*/
+
+
+/*
+ * KFBuffer.cpp
+ *
+ *  Created on: Jan 7, 2014
+ *      Author: engelj
+ */
+
+#include "FullSystem/FullSystem.h"
+ 
+#include "stdio.h"
+#include "util/globalFuncs.h"
+#include <Eigen/LU>
+#include <algorithm>
+#include "IOWrapper/ImageDisplay.h"
+#include "util/globalCalib.h"
+
+#include <Eigen/SVD>
+#include <Eigen/Eigenvalues>
+#include "FullSystem/ResidualProjections.h"
+#include "FullSystem/ImmaturePoint.h"
+
+#include "OptimizationBackend/EnergyFunctional.h"
+#include "OptimizationBackend/EnergyFunctionalStructs.h"
+
+#include "IOWrapper/Output3DWrapper.h"
+
+#include "FullSystem/CoarseTracker.h"
+
+namespace dso
+{
+
+
+
+void FullSystem::flagFramesForMarginalization(FrameHessian* newFH)
+{
+	if(setting_minFrameAge > setting_maxFrames)
+	{
+		for(int i=setting_maxFrames;i<(int)frameHessians.size();i++)
+		{
+			FrameHessian* fh = frameHessians[i-setting_maxFrames];
+			fh->flaggedForMarginalization = true;
+		}
+		return;
+	}
+
+
+	int flagged = 0;
+	// marginalize all frames that have not enough points.
+	for(int i=0;i<(int)frameHessians.size();i++)
+	{
+		FrameHessian* fh = frameHessians[i];
+		int in = fh->pointHessians.size() + fh->immaturePoints.size();
+		int out = fh->pointHessiansMarginalized.size() + fh->pointHessiansOut.size();
+
+
+		Vec2 refToFh=AffLight::fromToVecExposure(frameHessians.back()->ab_exposure, fh->ab_exposure,
+				frameHessians.back()->aff_g2l(), fh->aff_g2l());
+
+
+		if( (in < setting_minPointsRemaining *(in+out) || fabs(logf((float)refToFh[0])) > setting_maxLogAffFacInWindow)
+				&& ((int)frameHessians.size())-flagged > setting_minFrames)
+		{
+//			printf("MARGINALIZE frame %d, as only %'d/%'d points remaining (%'d %'d %'d %'d). VisInLast %'d / %'d. traces %d, activated %d!\n",
+//					fh->frameID, in, in+out,
+//					(int)fh->pointHessians.size(), (int)fh->immaturePoints.size(),
+//					(int)fh->pointHessiansMarginalized.size(), (int)fh->pointHessiansOut.size(),
+//					visInLast, outInLast,
+//					fh->statistics_tracesCreatedForThisFrame, fh->statistics_pointsActivatedForThisFrame);
+			fh->flaggedForMarginalization = true;
+			flagged++;
+		}
+		else
+		{
+//			printf("May Keep frame %d, as %'d/%'d points remaining (%'d %'d %'d %'d). VisInLast %'d / %'d. traces %d, activated %d!\n",
+//					fh->frameID, in, in+out,
+//					(int)fh->pointHessians.size(), (int)fh->immaturePoints.size(),
+//					(int)fh->pointHessiansMarginalized.size(), (int)fh->pointHessiansOut.size(),
+//					visInLast, outInLast,
+//					fh->statistics_tracesCreatedForThisFrame, fh->statistics_pointsActivatedForThisFrame);
+		}
+	}
+
+	// marginalize one.
+	if((int)frameHessians.size()-flagged >= setting_maxFrames)
+	{
+		double smallestScore = 1;
+		FrameHessian* toMarginalize=0;
+		FrameHessian* latest = frameHessians.back();
+
+
+		for(FrameHessian* fh : frameHessians)
+		{
+			if(fh->frameID > latest->frameID-setting_minFrameAge || fh->frameID == 0) continue;
+			//if(fh==frameHessians.front() == 0) continue;
+
+			double distScore = 0;
+			for(FrameFramePrecalc &ffh : fh->targetPrecalc)
+			{
+				if(ffh.target->frameID > latest->frameID-setting_minFrameAge+1 || ffh.target == ffh.host) continue;
+				distScore += 1/(1e-5+ffh.distanceLL);
+
+			}
+			distScore *= -sqrtf(fh->targetPrecalc.back().distanceLL);
+
+
+			if(distScore < smallestScore)
+			{
+				smallestScore = distScore;
+				toMarginalize = fh;
+			}
+		}
+
+//		printf("MARGINALIZE frame %d, as it is the closest (score %.2f)!\n",
+//				toMarginalize->frameID, smallestScore);
+		toMarginalize->flaggedForMarginalization = true;
+		flagged++;
+	}
+
+//	printf("FRAMES LEFT: ");
+//	for(FrameHessian* fh : frameHessians)
+//		printf("%d ", fh->frameID);
+//	printf("\n");
+}
+
+
+
+
+void FullSystem::marginalizeFrame(FrameHessian* frame)
+{
+	// marginalize or remove all this frames points.
+
+	assert((int)frame->pointHessians.size()==0);
+
+
+	ef->marginalizeFrame(frame->efFrame);
+
+	// drop all observations of existing points in that frame.
+
+	for(FrameHessian* fh : frameHessians)
+	{
+		if(fh==frame) continue;
+
+		for(PointHessian* ph : fh->pointHessians)
+		{
+			for(unsigned int i=0;i<ph->residuals.size();i++)
+			{
+				PointFrameResidual* r = ph->residuals[i];
+				if(r->target == frame)
+				{
+					if(ph->lastResiduals[0].first == r)
+						ph->lastResiduals[0].first=0;
+					else if(ph->lastResiduals[1].first == r)
+						ph->lastResiduals[1].first=0;
+
+
+					if(r->host->frameID < r->target->frameID)
+						statistics_numForceDroppedResFwd++;
+					else
+						statistics_numForceDroppedResBwd++;
+
+					ef->dropResidual(r->efResidual);
+					deleteOut<PointFrameResidual>(ph->residuals,i);
+					break;
+				}
+			}
+		}
+	}
+
+
+
+    {
+        std::vector<FrameHessian*> v;
+        v.push_back(frame);
+        for(IOWrap::Output3DWrapper* ow : outputWrapper)
+            ow->publishKeyframes(v, true, &Hcalib);
+    }
+
+
+	frame->shell->marginalizedAt = frameHessians.back()->shell->id;
+	frame->shell->movedByOpt = frame->w2c_leftEps().norm();
+
+	deleteOutOrder<FrameHessian>(frameHessians, frame);
+	for(unsigned int i=0;i<frameHessians.size();i++)
+		frameHessians[i]->idx = i;
+
+
+
+
+	setPrecalcValues();
+	ef->setAdjointsF(&Hcalib);
+}
+
+
+
+
+}

src/FullSystem/FullSystemOptPoint.cpp

@@ -0,0 +1,209 @@
+/**
+* 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/>.
+*/
+
+
+/*
+ * KFBuffer.cpp
+ *
+ *  Created on: Jan 7, 2014
+ *      Author: engelj
+ */
+
+#include "FullSystem/FullSystem.h"
+ 
+#include "stdio.h"
+#include "util/globalFuncs.h"
+#include <Eigen/LU>
+#include <algorithm>
+#include "IOWrapper/ImageDisplay.h"
+#include "util/globalCalib.h"
+
+#include <Eigen/SVD>
+#include <Eigen/Eigenvalues>
+#include "FullSystem/ImmaturePoint.h"
+#include "math.h"
+
+namespace dso
+{
+
+
+
+PointHessian* FullSystem::optimizeImmaturePoint(
+		ImmaturePoint* point, int minObs,
+		ImmaturePointTemporaryResidual* residuals)
+{
+	int nres = 0;
+	for(FrameHessian* fh : frameHessians)
+	{
+		if(fh != point->host)
+		{
+			residuals[nres].state_NewEnergy = residuals[nres].state_energy = 0;
+			residuals[nres].state_NewState = ResState::OUTLIER;
+			residuals[nres].state_state = ResState::IN;
+			residuals[nres].target = fh;
+			nres++;
+		}
+	}
+	assert(nres == ((int)frameHessians.size())-1);
+
+	bool print = false;//rand()%50==0;
+
+	float lastEnergy = 0;
+	float lastHdd=0;
+	float lastbd=0;
+	float currentIdepth=(point->idepth_max+point->idepth_min)*0.5f;
+
+
+
+
+
+
+	for(int i=0;i<nres;i++)
+	{
+		lastEnergy += point->linearizeResidual(&Hcalib, 1000, residuals+i,lastHdd, lastbd, currentIdepth);
+		residuals[i].state_state = residuals[i].state_NewState;
+		residuals[i].state_energy = residuals[i].state_NewEnergy;
+	}
+
+	if(!std::isfinite(lastEnergy) || lastHdd < setting_minIdepthH_act)
+	{
+		if(print)
+			printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n",
+				nres, lastHdd, lastEnergy);
+		return 0;
+	}
+
+	if(print) printf("Activate point. %d residuals. H=%f. Initial Energy: %f. Initial Id=%f\n" ,
+			nres, lastHdd,lastEnergy,currentIdepth);
+
+	float lambda = 0.1;
+	for(int iteration=0;iteration<setting_GNItsOnPointActivation;iteration++)
+	{
+		float H = lastHdd;
+		H *= 1+lambda;
+		float step = (1.0/H) * lastbd;
+		float newIdepth = currentIdepth - step;
+
+		float newHdd=0; float newbd=0; float newEnergy=0;
+		for(int i=0;i<nres;i++)
+			newEnergy += point->linearizeResidual(&Hcalib, 1, residuals+i,newHdd, newbd, newIdepth);
+
+		if(!std::isfinite(lastEnergy) || newHdd < setting_minIdepthH_act)
+		{
+			if(print) printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n",
+					nres,
+					newHdd,
+					lastEnergy);
+			return 0;
+		}
+
+		if(print) printf("%s %d (L %.2f) %s: %f -> %f (idepth %f)!\n",
+				(true || newEnergy < lastEnergy) ? "ACCEPT" : "REJECT",
+				iteration,
+				log10(lambda),
+				"",
+				lastEnergy, newEnergy, newIdepth);
+
+		if(newEnergy < lastEnergy)
+		{
+			currentIdepth = newIdepth;
+			lastHdd = newHdd;
+			lastbd = newbd;
+			lastEnergy = newEnergy;
+			for(int i=0;i<nres;i++)
+			{
+				residuals[i].state_state = residuals[i].state_NewState;
+				residuals[i].state_energy = residuals[i].state_NewEnergy;
+			}
+
+			lambda *= 0.5;
+		}
+		else
+		{
+			lambda *= 5;
+		}
+
+		if(fabsf(step) < 0.0001*currentIdepth)
+			break;
+	}
+
+	if(!std::isfinite(currentIdepth))
+	{
+		printf("MAJOR ERROR! point idepth is nan after initialization (%f).\n", currentIdepth);
+		return (PointHessian*)((long)(-1));		// yeah I'm like 99% sure this is OK on 32bit systems.
+	}
+
+
+	int numGoodRes=0;
+	for(int i=0;i<nres;i++)
+		if(residuals[i].state_state == ResState::IN) numGoodRes++;
+
+	if(numGoodRes < minObs)
+	{
+		if(print) printf("OptPoint: OUTLIER!\n");
+		return (PointHessian*)((long)(-1));		// yeah I'm like 99% sure this is OK on 32bit systems.
+	}
+
+
+
+	PointHessian* p = new PointHessian(point, &Hcalib);
+	if(!std::isfinite(p->energyTH)) {delete p; return (PointHessian*)((long)(-1));}
+
+	p->lastResiduals[0].first = 0;
+	p->lastResiduals[0].second = ResState::OOB;
+	p->lastResiduals[1].first = 0;
+	p->lastResiduals[1].second = ResState::OOB;
+	p->setIdepthZero(currentIdepth);
+	p->setIdepth(currentIdepth);
+	p->setPointStatus(PointHessian::ACTIVE);
+
+	for(int i=0;i<nres;i++)
+		if(residuals[i].state_state == ResState::IN)
+		{
+			PointFrameResidual* r = new PointFrameResidual(p, p->host, residuals[i].target);
+			r->state_NewEnergy = r->state_energy = 0;
+			r->state_NewState = ResState::OUTLIER;
+			r->setState(ResState::IN);
+			p->residuals.push_back(r);
+
+			if(r->target == frameHessians.back())
+			{
+				p->lastResiduals[0].first = r;
+				p->lastResiduals[0].second = ResState::IN;
+			}
+			else if(r->target == (frameHessians.size()<2 ? 0 : frameHessians[frameHessians.size()-2]))
+			{
+				p->lastResiduals[1].first = r;
+				p->lastResiduals[1].second = ResState::IN;
+			}
+		}
+
+	if(print) printf("point activated!\n");
+
+	statistics_numActivatedPoints++;
+	return p;
+}
+
+
+
+}

src/FullSystem/FullSystemOptimize.cpp

@@ -0,0 +1,714 @@
+/**
+* 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 "FullSystem/FullSystem.h"
+ 
+#include "stdio.h"
+#include "util/globalFuncs.h"
+#include <Eigen/LU>
+#include <algorithm>
+#include "IOWrapper/ImageDisplay.h"
+#include "util/globalCalib.h"
+#include <Eigen/SVD>
+#include <Eigen/Eigenvalues>
+#include "FullSystem/ResidualProjections.h"
+
+#include "OptimizationBackend/EnergyFunctional.h"
+#include "OptimizationBackend/EnergyFunctionalStructs.h"
+
+#include <cmath>
+
+#include <algorithm>
+
+namespace dso
+{
+
+
+
+
+
+void FullSystem::linearizeAll_Reductor(bool fixLinearization, std::vector<PointFrameResidual*>* toRemove, int min, int max, Vec10* stats, int tid)
+{
+	for(int k=min;k<max;k++)
+	{
+		PointFrameResidual* r = activeResiduals[k];
+		(*stats)[0] += r->linearize(&Hcalib);
+
+		if(fixLinearization)
+		{
+			r->applyRes(true);
+
+			if(r->efResidual->isActive())
+			{
+				if(r->isNew)
+				{
+					PointHessian* p = r->point;
+					Vec3f ptp_inf = r->host->targetPrecalc[r->target->idx].PRE_KRKiTll * Vec3f(p->u,p->v, 1);	// projected point assuming infinite depth.
+					Vec3f ptp = ptp_inf + r->host->targetPrecalc[r->target->idx].PRE_KtTll*p->idepth_scaled;	// projected point with real depth.
+					float relBS = 0.01*((ptp_inf.head<2>() / ptp_inf[2])-(ptp.head<2>() / ptp[2])).norm();	// 0.01 = one pixel.
+
+
+					if(relBS > p->maxRelBaseline)
+						p->maxRelBaseline = relBS;
+
+					p->numGoodResiduals++;
+				}
+			}
+			else
+			{
+				toRemove[tid].push_back(activeResiduals[k]);
+			}
+		}
+	}
+}
+
+
+void FullSystem::applyRes_Reductor(bool copyJacobians, int min, int max, Vec10* stats, int tid)
+{
+	for(int k=min;k<max;k++)
+		activeResiduals[k]->applyRes(true);
+}
+void FullSystem::setNewFrameEnergyTH()
+{
+
+	// collect all residuals and make decision on TH.
+	allResVec.clear();
+	allResVec.reserve(activeResiduals.size()*2);
+	FrameHessian* newFrame = frameHessians.back();
+
+	for(PointFrameResidual* r : activeResiduals)
+		if(r->state_NewEnergyWithOutlier >= 0 && r->target == newFrame)
+		{
+			allResVec.push_back(r->state_NewEnergyWithOutlier);
+
+		}
+
+	if(allResVec.size()==0)
+	{
+		newFrame->frameEnergyTH = 12*12*patternNum;
+		return;		// should never happen, but lets make sure.
+	}
+
+
+	int nthIdx = setting_frameEnergyTHN*allResVec.size();
+
+	assert(nthIdx < (int)allResVec.size());
+	assert(setting_frameEnergyTHN < 1);
+
+	std::nth_element(allResVec.begin(), allResVec.begin()+nthIdx, allResVec.end());
+	float nthElement = sqrtf(allResVec[nthIdx]);
+
+
+
+
+
+
+    newFrame->frameEnergyTH = nthElement*setting_frameEnergyTHFacMedian;
+	newFrame->frameEnergyTH = 26.0f*setting_frameEnergyTHConstWeight + newFrame->frameEnergyTH*(1-setting_frameEnergyTHConstWeight);
+	newFrame->frameEnergyTH = newFrame->frameEnergyTH*newFrame->frameEnergyTH;
+	newFrame->frameEnergyTH *= setting_overallEnergyTHWeight*setting_overallEnergyTHWeight;
+
+
+
+//
+//	int good=0,bad=0;
+//	for(float f : allResVec) if(f<newFrame->frameEnergyTH) good++; else bad++;
+//	printf("EnergyTH: mean %f, median %f, result %f (in %d, out %d)! \n",
+//			meanElement, nthElement, sqrtf(newFrame->frameEnergyTH),
+//			good, bad);
+}
+Vec3 FullSystem::linearizeAll(bool fixLinearization)
+{
+	double lastEnergyP = 0;
+	double lastEnergyR = 0;
+	double num = 0;
+
+
+	std::vector<PointFrameResidual*> toRemove[NUM_THREADS];
+	for(int i=0;i<NUM_THREADS;i++) toRemove[i].clear();
+
+	if(multiThreading)
+	{
+		treadReduce.reduce(boost::bind(&FullSystem::linearizeAll_Reductor, this, fixLinearization, toRemove, _1, _2, _3, _4), 0, activeResiduals.size(), 0);
+		lastEnergyP = treadReduce.stats[0];
+	}
+	else
+	{
+		Vec10 stats;
+		linearizeAll_Reductor(fixLinearization, toRemove, 0,activeResiduals.size(),&stats,0);
+		lastEnergyP = stats[0];
+	}
+
+
+	setNewFrameEnergyTH();
+
+
+	if(fixLinearization)
+	{
+
+		for(PointFrameResidual* r : activeResiduals)
+		{
+			PointHessian* ph = r->point;
+			if(ph->lastResiduals[0].first == r)
+				ph->lastResiduals[0].second = r->state_state;
+			else if(ph->lastResiduals[1].first == r)
+				ph->lastResiduals[1].second = r->state_state;
+
+
+
+		}
+
+		int nResRemoved=0;
+		for(int i=0;i<NUM_THREADS;i++)
+		{
+			for(PointFrameResidual* r : toRemove[i])
+			{
+				PointHessian* ph = r->point;
+
+				if(ph->lastResiduals[0].first == r)
+					ph->lastResiduals[0].first=0;
+				else if(ph->lastResiduals[1].first == r)
+					ph->lastResiduals[1].first=0;
+
+				for(unsigned int k=0; k<ph->residuals.size();k++)
+					if(ph->residuals[k] == r)
+					{
+						ef->dropResidual(r->efResidual);
+						deleteOut<PointFrameResidual>(ph->residuals,k);
+						nResRemoved++;
+						break;
+					}
+			}
+		}
+		//printf("FINAL LINEARIZATION: removed %d / %d residuals!\n", nResRemoved, (int)activeResiduals.size());
+
+	}
+
+	return Vec3(lastEnergyP, lastEnergyR, num);
+}
+
+
+
+
+// applies step to linearization point.
+bool FullSystem::doStepFromBackup(float stepfacC,float stepfacT,float stepfacR,float stepfacA,float stepfacD)
+{
+//	float meanStepC=0,meanStepP=0,meanStepD=0;
+//	meanStepC += Hcalib.step.norm();
+
+	Vec10 pstepfac;
+	pstepfac.segment<3>(0).setConstant(stepfacT);
+	pstepfac.segment<3>(3).setConstant(stepfacR);
+	pstepfac.segment<4>(6).setConstant(stepfacA);
+
+
+	float sumA=0, sumB=0, sumT=0, sumR=0, sumID=0, numID=0;
+
+	float sumNID=0;
+
+	if(setting_solverMode & SOLVER_MOMENTUM)
+	{
+		Hcalib.setValue(Hcalib.value_backup + Hcalib.step);
+		for(FrameHessian* fh : frameHessians)
+		{
+			Vec10 step = fh->step;
+			step.head<6>() += 0.5f*(fh->step_backup.head<6>());
+
+			fh->setState(fh->state_backup + step);
+			sumA += step[6]*step[6];
+			sumB += step[7]*step[7];
+			sumT += step.segment<3>(0).squaredNorm();
+			sumR += step.segment<3>(3).squaredNorm();
+
+			for(PointHessian* ph : fh->pointHessians)
+			{
+				float step = ph->step+0.5f*(ph->step_backup);
+				ph->setIdepth(ph->idepth_backup + step);
+				sumID += step*step;
+				sumNID += fabsf(ph->idepth_backup);
+				numID++;
+
+                ph->setIdepthZero(ph->idepth_backup + step);
+			}
+		}
+	}
+	else
+	{
+		Hcalib.setValue(Hcalib.value_backup + stepfacC*Hcalib.step);
+		for(FrameHessian* fh : frameHessians)
+		{
+			fh->setState(fh->state_backup + pstepfac.cwiseProduct(fh->step));
+			sumA += fh->step[6]*fh->step[6];
+			sumB += fh->step[7]*fh->step[7];
+			sumT += fh->step.segment<3>(0).squaredNorm();
+			sumR += fh->step.segment<3>(3).squaredNorm();
+
+			for(PointHessian* ph : fh->pointHessians)
+			{
+				ph->setIdepth(ph->idepth_backup + stepfacD*ph->step);
+				sumID += ph->step*ph->step;
+				sumNID += fabsf(ph->idepth_backup);
+				numID++;
+
+                ph->setIdepthZero(ph->idepth_backup + stepfacD*ph->step);
+			}
+		}
+	}
+
+	sumA /= frameHessians.size();
+	sumB /= frameHessians.size();
+	sumR /= frameHessians.size();
+	sumT /= frameHessians.size();
+	sumID /= numID;
+	sumNID /= numID;
+
+
+
+    if(!setting_debugout_runquiet)
+        printf("STEPS: A %.1f; B %.1f; R %.1f; T %.1f. \t",
+                sqrtf(sumA) / (0.0005*setting_thOptIterations),
+                sqrtf(sumB) / (0.00005*setting_thOptIterations),
+                sqrtf(sumR) / (0.00005*setting_thOptIterations),
+                sqrtf(sumT)*sumNID / (0.00005*setting_thOptIterations));
+
+
+	EFDeltaValid=false;
+	setPrecalcValues();
+
+
+
+	return sqrtf(sumA) < 0.0005*setting_thOptIterations &&
+			sqrtf(sumB) < 0.00005*setting_thOptIterations &&
+			sqrtf(sumR) < 0.00005*setting_thOptIterations &&
+			sqrtf(sumT)*sumNID < 0.00005*setting_thOptIterations;
+//
+//	printf("mean steps: %f %f %f!\n",
+//			meanStepC, meanStepP, meanStepD);
+}
+
+
+
+// sets linearization point.
+void FullSystem::backupState(bool backupLastStep)
+{
+	if(setting_solverMode & SOLVER_MOMENTUM)
+	{
+		if(backupLastStep)
+		{
+			Hcalib.step_backup = Hcalib.step;
+			Hcalib.value_backup = Hcalib.value;
+			for(FrameHessian* fh : frameHessians)
+			{
+				fh->step_backup = fh->step;
+				fh->state_backup = fh->get_state();
+				for(PointHessian* ph : fh->pointHessians)
+				{
+					ph->idepth_backup = ph->idepth;
+					ph->step_backup = ph->step;
+				}
+			}
+		}
+		else
+		{
+			Hcalib.step_backup.setZero();
+			Hcalib.value_backup = Hcalib.value;
+			for(FrameHessian* fh : frameHessians)
+			{
+				fh->step_backup.setZero();
+				fh->state_backup = fh->get_state();
+				for(PointHessian* ph : fh->pointHessians)
+				{
+					ph->idepth_backup = ph->idepth;
+					ph->step_backup=0;
+				}
+			}
+		}
+	}
+	else
+	{
+		Hcalib.value_backup = Hcalib.value;
+		for(FrameHessian* fh : frameHessians)
+		{
+			fh->state_backup = fh->get_state();
+			for(PointHessian* ph : fh->pointHessians)
+				ph->idepth_backup = ph->idepth;
+		}
+	}
+}
+
+// sets linearization point.
+void FullSystem::loadSateBackup()
+{
+	Hcalib.setValue(Hcalib.value_backup);
+	for(FrameHessian* fh : frameHessians)
+	{
+		fh->setState(fh->state_backup);
+		for(PointHessian* ph : fh->pointHessians)
+		{
+			ph->setIdepth(ph->idepth_backup);
+
+            ph->setIdepthZero(ph->idepth_backup);
+		}
+
+	}
+
+
+	EFDeltaValid=false;
+	setPrecalcValues();
+}
+
+
+double FullSystem::calcMEnergy()
+{
+	if(setting_forceAceptStep) return 0;
+	// calculate (x-x0)^T * [2b + H * (x-x0)] for everything saved in L.
+	//ef->makeIDX();
+	//ef->setDeltaF(&Hcalib);
+	return ef->calcMEnergyF();
+
+}
+
+
+void FullSystem::printOptRes(const Vec3 &res, double resL, double resM, double resPrior, double LExact, float a, float b)
+{
+	printf("A(%f)=(AV %.3f). Num: A(%'d) + M(%'d); ab %f %f!\n",
+			res[0],
+			sqrtf((float)(res[0] / (patternNum*ef->resInA))),
+			ef->resInA,
+			ef->resInM,
+			a,
+			b
+	);
+
+}
+
+
+float FullSystem::optimize(int mnumOptIts)
+{
+
+	if(frameHessians.size() < 2) return 0;
+	if(frameHessians.size() < 3) mnumOptIts = 20;
+	if(frameHessians.size() < 4) mnumOptIts = 15;
+
+
+
+
+
+
+	// get statistics and active residuals.
+
+	activeResiduals.clear();
+	int numPoints = 0;
+	int numLRes = 0;
+	for(FrameHessian* fh : frameHessians)
+		for(PointHessian* ph : fh->pointHessians)
+		{
+			for(PointFrameResidual* r : ph->residuals)
+			{
+				if(!r->efResidual->isLinearized)
+				{
+					activeResiduals.push_back(r);
+					r->resetOOB();
+				}
+				else
+					numLRes++;
+			}
+			numPoints++;
+		}
+
+    if(!setting_debugout_runquiet)
+        printf("OPTIMIZE %d pts, %d active res, %d lin res!\n",ef->nPoints,(int)activeResiduals.size(), numLRes);
+
+
+	Vec3 lastEnergy = linearizeAll(false);
+	double lastEnergyL = calcLEnergy();
+	double lastEnergyM = calcMEnergy();
+
+
+
+
+
+	if(multiThreading)
+		treadReduce.reduce(boost::bind(&FullSystem::applyRes_Reductor, this, true, _1, _2, _3, _4), 0, activeResiduals.size(), 50);
+	else
+		applyRes_Reductor(true,0,activeResiduals.size(),0,0);
+
+
+    if(!setting_debugout_runquiet)
+    {
+        printf("Initial Error       \t");
+        printOptRes(lastEnergy, lastEnergyL, lastEnergyM, 0, 0, frameHessians.back()->aff_g2l().a, frameHessians.back()->aff_g2l().b);
+    }
+
+	debugPlotTracking();
+
+
+
+	double lambda = 1e-1;
+	float stepsize=1;
+	VecX previousX = VecX::Constant(CPARS+ 8*frameHessians.size(), NAN);
+	for(int iteration=0;iteration<mnumOptIts;iteration++)
+	{
+		// solve!
+		backupState(iteration!=0);
+		//solveSystemNew(0);
+		solveSystem(iteration, lambda);
+		double incDirChange = (1e-20 + previousX.dot(ef->lastX)) / (1e-20 + previousX.norm() * ef->lastX.norm());
+		previousX = ef->lastX;
+
+
+		if(std::isfinite(incDirChange) && (setting_solverMode & SOLVER_STEPMOMENTUM))
+		{
+			float newStepsize = exp(incDirChange*1.4);
+			if(incDirChange<0 && stepsize>1) stepsize=1;
+
+			stepsize = sqrtf(sqrtf(newStepsize*stepsize*stepsize*stepsize));
+			if(stepsize > 2) stepsize=2;
+			if(stepsize <0.25) stepsize=0.25;
+		}
+
+		bool canbreak = doStepFromBackup(stepsize,stepsize,stepsize,stepsize,stepsize);
+
+
+
+
+
+
+
+		// eval new energy!
+		Vec3 newEnergy = linearizeAll(false);
+		double newEnergyL = calcLEnergy();
+		double newEnergyM = calcMEnergy();
+
+
+
+
+        if(!setting_debugout_runquiet)
+        {
+            printf("%s %d (L %.2f, dir %.2f, ss %.1f): \t",
+				(newEnergy[0] +  newEnergy[1] +  newEnergyL + newEnergyM <
+						lastEnergy[0] + lastEnergy[1] + lastEnergyL + lastEnergyM) ? "ACCEPT" : "REJECT",
+				iteration,
+				log10(lambda),
+				incDirChange,
+				stepsize);
+            printOptRes(newEnergy, newEnergyL, newEnergyM , 0, 0, frameHessians.back()->aff_g2l().a, frameHessians.back()->aff_g2l().b);
+        }
+
+		if(setting_forceAceptStep || (newEnergy[0] +  newEnergy[1] +  newEnergyL + newEnergyM <
+				lastEnergy[0] + lastEnergy[1] + lastEnergyL + lastEnergyM))
+		{
+
+			if(multiThreading)
+				treadReduce.reduce(boost::bind(&FullSystem::applyRes_Reductor, this, true, _1, _2, _3, _4), 0, activeResiduals.size(), 50);
+			else
+				applyRes_Reductor(true,0,activeResiduals.size(),0,0);
+
+			lastEnergy = newEnergy;
+			lastEnergyL = newEnergyL;
+			lastEnergyM = newEnergyM;
+
+			lambda *= 0.25;
+		}
+		else
+		{
+			loadSateBackup();
+			lastEnergy = linearizeAll(false);
+			lastEnergyL = calcLEnergy();
+			lastEnergyM = calcMEnergy();
+			lambda *= 1e2;
+		}
+
+
+		if(canbreak && iteration >= setting_minOptIterations) break;
+	}
+
+
+
+	Vec10 newStateZero = Vec10::Zero();
+	newStateZero.segment<2>(6) = frameHessians.back()->get_state().segment<2>(6);
+
+	frameHessians.back()->setEvalPT(frameHessians.back()->PRE_worldToCam,
+			newStateZero);
+	EFDeltaValid=false;
+	EFAdjointsValid=false;
+	ef->setAdjointsF(&Hcalib);
+	setPrecalcValues();
+
+
+
+
+	lastEnergy = linearizeAll(true);
+
+
+
+
+	if(!std::isfinite((double)lastEnergy[0]) || !std::isfinite((double)lastEnergy[1]) || !std::isfinite((double)lastEnergy[2]))
+    {
+        printf("KF Tracking failed: LOST!\n");
+		isLost=true;
+    }
+
+
+	statistics_lastFineTrackRMSE = sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA)));
+
+	if(calibLog != 0)
+	{
+		(*calibLog) << Hcalib.value_scaled.transpose() <<
+				" " << frameHessians.back()->get_state_scaled().transpose() <<
+				" " << sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA))) <<
+				" " << ef->resInM << "\n";
+		calibLog->flush();
+	}
+
+	{
+		boost::unique_lock<boost::mutex> crlock(shellPoseMutex);
+		for(FrameHessian* fh : frameHessians)
+		{
+			fh->shell->camToWorld = fh->PRE_camToWorld;
+			fh->shell->aff_g2l = fh->aff_g2l();
+		}
+	}
+
+
+
+
+	debugPlotTracking();
+
+	return sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA)));
+
+}
+
+
+
+
+
+void FullSystem::solveSystem(int iteration, double lambda)
+{
+	ef->lastNullspaces_forLogging = getNullspaces(
+			ef->lastNullspaces_pose,
+			ef->lastNullspaces_scale,
+			ef->lastNullspaces_affA,
+			ef->lastNullspaces_affB);
+
+	ef->solveSystemF(iteration, lambda,&Hcalib);
+}
+
+
+
+double FullSystem::calcLEnergy()
+{
+	if(setting_forceAceptStep) return 0;
+
+	double Ef = ef->calcLEnergyF_MT();
+	return Ef;
+
+}
+
+
+void FullSystem::removeOutliers()
+{
+	int numPointsDropped=0;
+	for(FrameHessian* fh : frameHessians)
+	{
+		for(unsigned int i=0;i<fh->pointHessians.size();i++)
+		{
+			PointHessian* ph = fh->pointHessians[i];
+			if(ph==0) continue;
+
+			if(ph->residuals.size() == 0)
+			{
+				fh->pointHessiansOut.push_back(ph);
+				ph->efPoint->stateFlag = EFPointStatus::PS_DROP;
+				fh->pointHessians[i] = fh->pointHessians.back();
+				fh->pointHessians.pop_back();
+				i--;
+				numPointsDropped++;
+			}
+		}
+	}
+	ef->dropPointsF();
+}
+
+
+
+
+std::vector<VecX> FullSystem::getNullspaces(
+		std::vector<VecX> &nullspaces_pose,
+		std::vector<VecX> &nullspaces_scale,
+		std::vector<VecX> &nullspaces_affA,
+		std::vector<VecX> &nullspaces_affB)
+{
+	nullspaces_pose.clear();
+	nullspaces_scale.clear();
+	nullspaces_affA.clear();
+	nullspaces_affB.clear();
+
+
+	int n=CPARS+frameHessians.size()*8;
+	std::vector<VecX> nullspaces_x0_pre;
+	for(int i=0;i<6;i++)
+	{
+		VecX nullspace_x0(n);
+		nullspace_x0.setZero();
+		for(FrameHessian* fh : frameHessians)
+		{
+			nullspace_x0.segment<6>(CPARS+fh->idx*8) = fh->nullspaces_pose.col(i);
+			nullspace_x0.segment<3>(CPARS+fh->idx*8) *= SCALE_XI_TRANS_INVERSE;
+			nullspace_x0.segment<3>(CPARS+fh->idx*8+3) *= SCALE_XI_ROT_INVERSE;
+		}
+		nullspaces_x0_pre.push_back(nullspace_x0);
+		nullspaces_pose.push_back(nullspace_x0);
+	}
+	for(int i=0;i<2;i++)
+	{
+		VecX nullspace_x0(n);
+		nullspace_x0.setZero();
+		for(FrameHessian* fh : frameHessians)
+		{
+			nullspace_x0.segment<2>(CPARS+fh->idx*8+6) = fh->nullspaces_affine.col(i).head<2>();
+			nullspace_x0[CPARS+fh->idx*8+6] *= SCALE_A_INVERSE;
+			nullspace_x0[CPARS+fh->idx*8+7] *= SCALE_B_INVERSE;
+		}
+		nullspaces_x0_pre.push_back(nullspace_x0);
+		if(i==0) nullspaces_affA.push_back(nullspace_x0);
+		if(i==1) nullspaces_affB.push_back(nullspace_x0);
+	}
+
+	VecX nullspace_x0(n);
+	nullspace_x0.setZero();
+	for(FrameHessian* fh : frameHessians)
+	{
+		nullspace_x0.segment<6>(CPARS+fh->idx*8) = fh->nullspaces_scale;
+		nullspace_x0.segment<3>(CPARS+fh->idx*8) *= SCALE_XI_TRANS_INVERSE;
+		nullspace_x0.segment<3>(CPARS+fh->idx*8+3) *= SCALE_XI_ROT_INVERSE;
+	}
+	nullspaces_x0_pre.push_back(nullspace_x0);
+	nullspaces_scale.push_back(nullspace_x0);
+
+	return nullspaces_x0_pre;
+}
+
+}

src/FullSystem/HessianBlocks.cpp

@@ -0,0 +1,226 @@
+/**
+* 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 "FullSystem/HessianBlocks.h"
+#include "util/FrameShell.h"
+#include "FullSystem/ImmaturePoint.h"
+#include "OptimizationBackend/EnergyFunctionalStructs.h"
+
+namespace dso
+{
+
+
+PointHessian::PointHessian(const ImmaturePoint* const rawPoint, CalibHessian* Hcalib)
+{
+	instanceCounter++;
+	host = rawPoint->host;
+	hasDepthPrior=false;
+
+	idepth_hessian=0;
+	maxRelBaseline=0;
+	numGoodResiduals=0;
+
+	// set static values & initialization.
+	u = rawPoint->u;
+	v = rawPoint->v;
+	assert(std::isfinite(rawPoint->idepth_max));
+	//idepth_init = rawPoint->idepth_GT;
+
+	my_type = rawPoint->my_type;
+
+	setIdepthScaled((rawPoint->idepth_max + rawPoint->idepth_min)*0.5);
+	setPointStatus(PointHessian::INACTIVE);
+
+	int n = patternNum;
+	memcpy(color, rawPoint->color, sizeof(float)*n);
+	memcpy(weights, rawPoint->weights, sizeof(float)*n);
+	energyTH = rawPoint->energyTH;
+
+	efPoint=0;
+
+
+}
+
+
+void PointHessian::release()
+{
+	for(unsigned int i=0;i<residuals.size();i++) delete residuals[i];
+	residuals.clear();
+}
+
+
+void FrameHessian::setStateZero(const Vec10 &state_zero)
+{
+	assert(state_zero.head<6>().squaredNorm() < 1e-20);
+
+	this->state_zero = state_zero;
+
+
+	for(int i=0;i<6;i++)
+	{
+		Vec6 eps; eps.setZero(); eps[i] = 1e-3;
+		SE3 EepsP = Sophus::SE3::exp(eps);
+		SE3 EepsM = Sophus::SE3::exp(-eps);
+		SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT() * EepsP) * get_worldToCam_evalPT().inverse();
+		SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT() * EepsM) * get_worldToCam_evalPT().inverse();
+		nullspaces_pose.col(i) = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3);
+	}
+	//nullspaces_pose.topRows<3>() *= SCALE_XI_TRANS_INVERSE;
+	//nullspaces_pose.bottomRows<3>() *= SCALE_XI_ROT_INVERSE;
+
+	// scale change
+	SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT());
+	w2c_leftEps_P_x0.translation() *= 1.00001;
+	w2c_leftEps_P_x0 = w2c_leftEps_P_x0 * get_worldToCam_evalPT().inverse();
+	SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT());
+	w2c_leftEps_M_x0.translation() /= 1.00001;
+	w2c_leftEps_M_x0 = w2c_leftEps_M_x0 * get_worldToCam_evalPT().inverse();
+	nullspaces_scale = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3);
+
+
+	nullspaces_affine.setZero();
+	nullspaces_affine.topLeftCorner<2,1>()  = Vec2(1,0);
+	assert(ab_exposure > 0);
+	nullspaces_affine.topRightCorner<2,1>() = Vec2(0, expf(aff_g2l_0().a)*ab_exposure);
+};
+
+
+
+void FrameHessian::release()
+{
+	// DELETE POINT
+	// DELETE RESIDUAL
+	for(unsigned int i=0;i<pointHessians.size();i++) delete pointHessians[i];
+	for(unsigned int i=0;i<pointHessiansMarginalized.size();i++) delete pointHessiansMarginalized[i];
+	for(unsigned int i=0;i<pointHessiansOut.size();i++) delete pointHessiansOut[i];
+	for(unsigned int i=0;i<immaturePoints.size();i++) delete immaturePoints[i];
+
+
+	pointHessians.clear();
+	pointHessiansMarginalized.clear();
+	pointHessiansOut.clear();
+	immaturePoints.clear();
+}
+
+
+void FrameHessian::makeImages(float* color, CalibHessian* HCalib)
+{
+
+	for(int i=0;i<pyrLevelsUsed;i++)
+	{
+		dIp[i] = new Eigen::Vector3f[wG[i]*hG[i]];
+		absSquaredGrad[i] = new float[wG[i]*hG[i]];
+	}
+	dI = dIp[0];
+
+
+	// make d0
+	int w=wG[0];
+	int h=hG[0];
+	for(int i=0;i<w*h;i++)
+		dI[i][0] = color[i];
+
+	for(int lvl=0; lvl<pyrLevelsUsed; lvl++)
+	{
+		int wl = wG[lvl], hl = hG[lvl];
+		Eigen::Vector3f* dI_l = dIp[lvl];
+
+		float* dabs_l = absSquaredGrad[lvl];
+		if(lvl>0)
+		{
+			int lvlm1 = lvl-1;
+			int wlm1 = wG[lvlm1];
+			Eigen::Vector3f* dI_lm = dIp[lvlm1];
+
+
+
+			for(int y=0;y<hl;y++)
+				for(int x=0;x<wl;x++)
+				{
+					dI_l[x + y*wl][0] = 0.25f * (dI_lm[2*x   + 2*y*wlm1][0] +
+												dI_lm[2*x+1 + 2*y*wlm1][0] +
+												dI_lm[2*x   + 2*y*wlm1+wlm1][0] +
+												dI_lm[2*x+1 + 2*y*wlm1+wlm1][0]);
+				}
+		}
+
+		for(int idx=wl;idx < wl*(hl-1);idx++)
+		{
+			float dx = 0.5f*(dI_l[idx+1][0] - dI_l[idx-1][0]);
+			float dy = 0.5f*(dI_l[idx+wl][0] - dI_l[idx-wl][0]);
+
+
+			if(!std::isfinite(dx)) dx=0;
+			if(!std::isfinite(dy)) dy=0;
+
+			dI_l[idx][1] = dx;
+			dI_l[idx][2] = dy;
+
+
+			dabs_l[idx] = dx*dx+dy*dy;
+
+			if(setting_gammaWeightsPixelSelect==1 && HCalib!=0)
+			{
+				float gw = HCalib->getBGradOnly((float)(dI_l[idx][0]));
+				dabs_l[idx] *= gw*gw;	// convert to gradient of original color space (before removing response).
+			}
+		}
+	}
+}
+
+void FrameFramePrecalc::set(FrameHessian* host, FrameHessian* target, CalibHessian* HCalib )
+{
+	this->host = host;
+	this->target = target;
+
+	SE3 leftToLeft_0 = target->get_worldToCam_evalPT() * host->get_worldToCam_evalPT().inverse();
+	PRE_RTll_0 = (leftToLeft_0.rotationMatrix()).cast<float>();
+	PRE_tTll_0 = (leftToLeft_0.translation()).cast<float>();
+
+
+
+	SE3 leftToLeft = target->PRE_worldToCam * host->PRE_camToWorld;
+	PRE_RTll = (leftToLeft.rotationMatrix()).cast<float>();
+	PRE_tTll = (leftToLeft.translation()).cast<float>();
+	distanceLL = leftToLeft.translation().norm();
+
+
+	Mat33f K = Mat33f::Zero();
+	K(0,0) = HCalib->fxl();
+	K(1,1) = HCalib->fyl();
+	K(0,2) = HCalib->cxl();
+	K(1,2) = HCalib->cyl();
+	K(2,2) = 1;
+	PRE_KRKiTll = K * PRE_RTll * K.inverse();
+	PRE_RKiTll = PRE_RTll * K.inverse();
+	PRE_KtTll = K * PRE_tTll;
+
+
+	PRE_aff_mode = AffLight::fromToVecExposure(host->ab_exposure, target->ab_exposure, host->aff_g2l(), target->aff_g2l()).cast<float>();
+	PRE_b0_mode = host->aff_g2l_0().b;
+}
+
+}
+

src/FullSystem/HessianBlocks.h

@@ -0,0 +1,505 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+#define MAX_ACTIVE_FRAMES 100
+
+ 
+#include "util/globalCalib.h"
+#include "vector"
+ 
+#include <iostream>
+#include <fstream>
+#include "util/NumType.h"
+#include "FullSystem/Residuals.h"
+#include "util/ImageAndExposure.h"
+
+
+namespace dso
+{
+
+
+inline Vec2 affFromTo(const Vec2 &from, const Vec2 &to)	// contains affine parameters as XtoWorld.
+{
+	return Vec2(from[0] / to[0], (from[1] - to[1]) / to[0]);
+}
+
+
+struct FrameHessian;
+struct PointHessian;
+
+class ImmaturePoint;
+class FrameShell;
+
+class EFFrame;
+class EFPoint;
+
+#define SCALE_IDEPTH 1.0f		// scales internal value to idepth.
+#define SCALE_XI_ROT 1.0f
+#define SCALE_XI_TRANS 0.5f
+#define SCALE_F 50.0f
+#define SCALE_C 50.0f
+#define SCALE_W 1.0f
+#define SCALE_A 10.0f
+#define SCALE_B 1000.0f
+
+#define SCALE_IDEPTH_INVERSE (1.0f / SCALE_IDEPTH)
+#define SCALE_XI_ROT_INVERSE (1.0f / SCALE_XI_ROT)
+#define SCALE_XI_TRANS_INVERSE (1.0f / SCALE_XI_TRANS)
+#define SCALE_F_INVERSE (1.0f / SCALE_F)
+#define SCALE_C_INVERSE (1.0f / SCALE_C)
+#define SCALE_W_INVERSE (1.0f / SCALE_W)
+#define SCALE_A_INVERSE (1.0f / SCALE_A)
+#define SCALE_B_INVERSE (1.0f / SCALE_B)
+
+
+struct FrameFramePrecalc
+{
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	// static values
+	static int instanceCounter;
+	FrameHessian* host;	// defines row
+	FrameHessian* target;	// defines column
+
+	// precalc values
+	Mat33f PRE_RTll;
+	Mat33f PRE_KRKiTll;
+	Mat33f PRE_RKiTll;
+	Mat33f PRE_RTll_0;
+
+	Vec2f PRE_aff_mode;
+	float PRE_b0_mode;
+
+	Vec3f PRE_tTll;
+	Vec3f PRE_KtTll;
+	Vec3f PRE_tTll_0;
+
+	float distanceLL;
+
+
+    inline ~FrameFramePrecalc() {}
+    inline FrameFramePrecalc() {host=target=0;}
+	void set(FrameHessian* host, FrameHessian* target, CalibHessian* HCalib);
+};
+
+
+
+
+
+struct FrameHessian
+{
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	EFFrame* efFrame;
+
+	// constant info & pre-calculated values
+	//DepthImageWrap* frame;
+	FrameShell* shell;
+
+	Eigen::Vector3f* dI;				 // trace, fine tracking. Used for direction select (not for gradient histograms etc.)
+	Eigen::Vector3f* dIp[PYR_LEVELS];	 // coarse tracking / coarse initializer. NAN in [0] only.
+	float* absSquaredGrad[PYR_LEVELS];  // only used for pixel select (histograms etc.). no NAN.
+
+
+
+
+
+
+	int frameID;						// incremental ID for keyframes only!
+	static int instanceCounter;
+	int idx;
+
+	// Photometric Calibration Stuff
+	float frameEnergyTH;	// set dynamically depending on tracking residual
+	float ab_exposure;
+
+	bool flaggedForMarginalization;
+
+	std::vector<PointHessian*> pointHessians;				// contains all ACTIVE points.
+	std::vector<PointHessian*> pointHessiansMarginalized;	// contains all MARGINALIZED points (= fully marginalized, usually because point went OOB.)
+	std::vector<PointHessian*> pointHessiansOut;		// contains all OUTLIER points (= discarded.).
+	std::vector<ImmaturePoint*> immaturePoints;		// contains all OUTLIER points (= discarded.).
+
+
+	Mat66 nullspaces_pose;
+	Mat42 nullspaces_affine;
+	Vec6 nullspaces_scale;
+
+	// variable info.
+	SE3 worldToCam_evalPT;
+	Vec10 state_zero;
+	Vec10 state_scaled;
+	Vec10 state;	// [0-5: worldToCam-leftEps. 6-7: a,b]
+	Vec10 step;
+	Vec10 step_backup;
+	Vec10 state_backup;
+
+
+    EIGEN_STRONG_INLINE const SE3 &get_worldToCam_evalPT() const {return worldToCam_evalPT;}
+    EIGEN_STRONG_INLINE const Vec10 &get_state_zero() const {return state_zero;}
+    EIGEN_STRONG_INLINE const Vec10 &get_state() const {return state;}
+    EIGEN_STRONG_INLINE const Vec10 &get_state_scaled() const {return state_scaled;}
+    EIGEN_STRONG_INLINE const Vec10 get_state_minus_stateZero() const {return get_state() - get_state_zero();}
+
+
+	// precalc values
+	SE3 PRE_worldToCam;
+	SE3 PRE_camToWorld;
+	std::vector<FrameFramePrecalc,Eigen::aligned_allocator<FrameFramePrecalc>> targetPrecalc;
+	MinimalImageB3* debugImage;
+
+
+    inline Vec6 w2c_leftEps() const {return get_state_scaled().head<6>();}
+    inline AffLight aff_g2l() const {return AffLight(get_state_scaled()[6], get_state_scaled()[7]);}
+    inline AffLight aff_g2l_0() const {return AffLight(get_state_zero()[6]*SCALE_A, get_state_zero()[7]*SCALE_B);}
+
+
+
+	void setStateZero(const Vec10 &state_zero);
+	inline void setState(const Vec10 &state)
+	{
+
+		this->state = state;
+		state_scaled.segment<3>(0) = SCALE_XI_TRANS * state.segment<3>(0);
+		state_scaled.segment<3>(3) = SCALE_XI_ROT * state.segment<3>(3);
+		state_scaled[6] = SCALE_A * state[6];
+		state_scaled[7] = SCALE_B * state[7];
+		state_scaled[8] = SCALE_A * state[8];
+		state_scaled[9] = SCALE_B * state[9];
+
+		PRE_worldToCam = SE3::exp(w2c_leftEps()) * get_worldToCam_evalPT();
+		PRE_camToWorld = PRE_worldToCam.inverse();
+		//setCurrentNullspace();
+	};
+	inline void setStateScaled(const Vec10 &state_scaled)
+	{
+
+		this->state_scaled = state_scaled;
+		state.segment<3>(0) = SCALE_XI_TRANS_INVERSE * state_scaled.segment<3>(0);
+		state.segment<3>(3) = SCALE_XI_ROT_INVERSE * state_scaled.segment<3>(3);
+		state[6] = SCALE_A_INVERSE * state_scaled[6];
+		state[7] = SCALE_B_INVERSE * state_scaled[7];
+		state[8] = SCALE_A_INVERSE * state_scaled[8];
+		state[9] = SCALE_B_INVERSE * state_scaled[9];
+
+		PRE_worldToCam = SE3::exp(w2c_leftEps()) * get_worldToCam_evalPT();
+		PRE_camToWorld = PRE_worldToCam.inverse();
+		//setCurrentNullspace();
+	};
+	inline void setEvalPT(const SE3 &worldToCam_evalPT, const Vec10 &state)
+	{
+
+		this->worldToCam_evalPT = worldToCam_evalPT;
+		setState(state);
+		setStateZero(state);
+	};
+
+
+
+	inline void setEvalPT_scaled(const SE3 &worldToCam_evalPT, const AffLight &aff_g2l)
+	{
+		Vec10 initial_state = Vec10::Zero();
+		initial_state[6] = aff_g2l.a;
+		initial_state[7] = aff_g2l.b;
+		this->worldToCam_evalPT = worldToCam_evalPT;
+		setStateScaled(initial_state);
+		setStateZero(this->get_state());
+	};
+
+	void release();
+
+	inline ~FrameHessian()
+	{
+		assert(efFrame==0);
+		release(); instanceCounter--;
+		for(int i=0;i<pyrLevelsUsed;i++)
+		{
+			delete[] dIp[i];
+			delete[]  absSquaredGrad[i];
+
+		}
+
+
+
+		if(debugImage != 0) delete debugImage;
+	};
+	inline FrameHessian()
+	{
+		instanceCounter++;
+		flaggedForMarginalization=false;
+		frameID = -1;
+		efFrame = 0;
+		frameEnergyTH = 8*8*patternNum;
+
+
+
+		debugImage=0;
+	};
+
+
+    void makeImages(float* color, CalibHessian* HCalib);
+
+	inline Vec10 getPrior()
+	{
+		Vec10 p =  Vec10::Zero();
+		if(frameID==0)
+		{
+			p.head<3>() = Vec3::Constant(setting_initialTransPrior);
+			p.segment<3>(3) = Vec3::Constant(setting_initialRotPrior);
+			if(setting_solverMode & SOLVER_REMOVE_POSEPRIOR) p.head<6>().setZero();
+
+			p[6] = setting_initialAffAPrior;
+			p[7] = setting_initialAffBPrior;
+		}
+		else
+		{
+			if(setting_affineOptModeA < 0)
+				p[6] = setting_initialAffAPrior;
+			else
+				p[6] = setting_affineOptModeA;
+
+			if(setting_affineOptModeB < 0)
+				p[7] = setting_initialAffBPrior;
+			else
+				p[7] = setting_affineOptModeB;
+		}
+		p[8] = setting_initialAffAPrior;
+		p[9] = setting_initialAffBPrior;
+		return p;
+	}
+
+
+	inline Vec10 getPriorZero()
+	{
+		return Vec10::Zero();
+	}
+
+};
+
+struct CalibHessian
+{
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	static int instanceCounter;
+
+	VecC value_zero;
+	VecC value_scaled;
+	VecCf value_scaledf;
+	VecCf value_scaledi;
+	VecC value;
+	VecC step;
+	VecC step_backup;
+	VecC value_backup;
+	VecC value_minus_value_zero;
+
+    inline ~CalibHessian() {instanceCounter--;}
+	inline CalibHessian()
+	{
+
+		VecC initial_value = VecC::Zero();
+		initial_value[0] = fxG[0];
+		initial_value[1] = fyG[0];
+		initial_value[2] = cxG[0];
+		initial_value[3] = cyG[0];
+
+		setValueScaled(initial_value);
+		value_zero = value;
+		value_minus_value_zero.setZero();
+
+		instanceCounter++;
+		for(int i=0;i<256;i++)
+			Binv[i] = B[i] = i;		// set gamma function to identity
+	};
+
+
+	// normal mode: use the optimized parameters everywhere!
+    inline float& fxl() {return value_scaledf[0];}
+    inline float& fyl() {return value_scaledf[1];}
+    inline float& cxl() {return value_scaledf[2];}
+    inline float& cyl() {return value_scaledf[3];}
+    inline float& fxli() {return value_scaledi[0];}
+    inline float& fyli() {return value_scaledi[1];}
+    inline float& cxli() {return value_scaledi[2];}
+    inline float& cyli() {return value_scaledi[3];}
+
+
+
+	inline void setValue(const VecC &value)
+	{
+		// [0-3: Kl, 4-7: Kr, 8-12: l2r]
+		this->value = value;
+		value_scaled[0] = SCALE_F * value[0];
+		value_scaled[1] = SCALE_F * value[1];
+		value_scaled[2] = SCALE_C * value[2];
+		value_scaled[3] = SCALE_C * value[3];
+
+		this->value_scaledf = this->value_scaled.cast<float>();
+		this->value_scaledi[0] = 1.0f / this->value_scaledf[0];
+		this->value_scaledi[1] = 1.0f / this->value_scaledf[1];
+		this->value_scaledi[2] = - this->value_scaledf[2] / this->value_scaledf[0];
+		this->value_scaledi[3] = - this->value_scaledf[3] / this->value_scaledf[1];
+		this->value_minus_value_zero = this->value - this->value_zero;
+	};
+
+	inline void setValueScaled(const VecC &value_scaled)
+	{
+		this->value_scaled = value_scaled;
+		this->value_scaledf = this->value_scaled.cast<float>();
+		value[0] = SCALE_F_INVERSE * value_scaled[0];
+		value[1] = SCALE_F_INVERSE * value_scaled[1];
+		value[2] = SCALE_C_INVERSE * value_scaled[2];
+		value[3] = SCALE_C_INVERSE * value_scaled[3];
+
+		this->value_minus_value_zero = this->value - this->value_zero;
+		this->value_scaledi[0] = 1.0f / this->value_scaledf[0];
+		this->value_scaledi[1] = 1.0f / this->value_scaledf[1];
+		this->value_scaledi[2] = - this->value_scaledf[2] / this->value_scaledf[0];
+		this->value_scaledi[3] = - this->value_scaledf[3] / this->value_scaledf[1];
+	};
+
+
+	float Binv[256];
+	float B[256];
+
+
+	EIGEN_STRONG_INLINE float getBGradOnly(float color)
+	{
+		int c = color+0.5f;
+		if(c<5) c=5;
+		if(c>250) c=250;
+		return B[c+1]-B[c];
+	}
+
+	EIGEN_STRONG_INLINE float getBInvGradOnly(float color)
+	{
+		int c = color+0.5f;
+		if(c<5) c=5;
+		if(c>250) c=250;
+		return Binv[c+1]-Binv[c];
+	}
+};
+
+
+// hessian component associated with one point.
+struct PointHessian
+{
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	static int instanceCounter;
+	EFPoint* efPoint;
+
+	// static values
+	float color[MAX_RES_PER_POINT];			// colors in host frame
+	float weights[MAX_RES_PER_POINT];		// host-weights for respective residuals.
+
+
+
+	float u,v;
+	int idx;
+	float energyTH;
+	FrameHessian* host;
+	bool hasDepthPrior;
+
+	float my_type;
+
+	float idepth_scaled;
+	float idepth_zero_scaled;
+	float idepth_zero;
+	float idepth;
+	float step;
+	float step_backup;
+	float idepth_backup;
+
+	float nullspaces_scale;
+	float idepth_hessian;
+	float maxRelBaseline;
+	int numGoodResiduals;
+
+	enum PtStatus {ACTIVE=0, INACTIVE, OUTLIER, OOB, MARGINALIZED};
+	PtStatus status;
+
+    inline void setPointStatus(PtStatus s) {status=s;}
+
+
+	inline void setIdepth(float idepth) {
+		this->idepth = idepth;
+		this->idepth_scaled = SCALE_IDEPTH * idepth;
+    }
+	inline void setIdepthScaled(float idepth_scaled) {
+		this->idepth = SCALE_IDEPTH_INVERSE * idepth_scaled;
+		this->idepth_scaled = idepth_scaled;
+    }
+	inline void setIdepthZero(float idepth) {
+		idepth_zero = idepth;
+		idepth_zero_scaled = SCALE_IDEPTH * idepth;
+		nullspaces_scale = -(idepth*1.001 - idepth/1.001)*500;
+    }
+
+
+	std::vector<PointFrameResidual*> residuals;					// only contains good residuals (not OOB and not OUTLIER). Arbitrary order.
+	std::pair<PointFrameResidual*, ResState> lastResiduals[2]; 	// contains information about residuals to the last two (!) frames. ([0] = latest, [1] = the one before).
+
+
+	void release();
+	PointHessian(const ImmaturePoint* const rawPoint, CalibHessian* Hcalib);
+    inline ~PointHessian() {assert(efPoint==0); release(); instanceCounter--;}
+
+
+	inline bool isOOB(const std::vector<FrameHessian*>& toKeep, const std::vector<FrameHessian*>& toMarg) const
+	{
+
+		int visInToMarg = 0;
+		for(PointFrameResidual* r : residuals)
+		{
+			if(r->state_state != ResState::IN) continue;
+			for(FrameHessian* k : toMarg)
+				if(r->target == k) visInToMarg++;
+		}
+		if((int)residuals.size() >= setting_minGoodActiveResForMarg &&
+				numGoodResiduals > setting_minGoodResForMarg+10 &&
+				(int)residuals.size()-visInToMarg < setting_minGoodActiveResForMarg)
+			return true;
+
+
+
+
+
+		if(lastResiduals[0].second == ResState::OOB) return true;
+		if(residuals.size() < 2) return false;
+		if(lastResiduals[0].second == ResState::OUTLIER && lastResiduals[1].second == ResState::OUTLIER) return true;
+		return false;
+	}
+
+
+	inline bool isInlierNew()
+	{
+		return (int)residuals.size() >= setting_minGoodActiveResForMarg
+                    && numGoodResiduals >= setting_minGoodResForMarg;
+	}
+
+};
+
+
+
+
+
+}
+

src/FullSystem/ImmaturePoint.cpp

@@ -0,0 +1,547 @@
+/**
+* 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 "FullSystem/ImmaturePoint.h"
+#include "util/FrameShell.h"
+#include "FullSystem/ResidualProjections.h"
+
+namespace dso
+{
+ImmaturePoint::ImmaturePoint(int u_, int v_, FrameHessian* host_, float type, CalibHessian* HCalib)
+: u(u_), v(v_), host(host_), my_type(type), idepth_min(0), idepth_max(NAN), lastTraceStatus(IPS_UNINITIALIZED)
+{
+
+	gradH.setZero();
+
+	for(int idx=0;idx<patternNum;idx++)
+	{
+		int dx = patternP[idx][0];
+		int dy = patternP[idx][1];
+
+        Vec3f ptc = getInterpolatedElement33BiLin(host->dI, u+dx, v+dy,wG[0]);
+
+
+
+		color[idx] = ptc[0];
+		if(!std::isfinite(color[idx])) {energyTH=NAN; return;}
+
+
+		gradH += ptc.tail<2>()  * ptc.tail<2>().transpose();
+
+		weights[idx] = sqrtf(setting_outlierTHSumComponent / (setting_outlierTHSumComponent + ptc.tail<2>().squaredNorm()));
+	}
+
+	energyTH = patternNum*setting_outlierTH;
+	energyTH *= setting_overallEnergyTHWeight*setting_overallEnergyTHWeight;
+
+	idepth_GT=0;
+	quality=10000;
+}
+
+ImmaturePoint::~ImmaturePoint()
+{
+}
+
+
+
+/*
+ * returns
+ * * OOB -> point is optimized and marginalized
+ * * UPDATED -> point has been updated.
+ * * SKIP -> point has not been updated.
+ */
+ImmaturePointStatus ImmaturePoint::traceOn(FrameHessian* frame,const Mat33f &hostToFrame_KRKi, const Vec3f &hostToFrame_Kt, const Vec2f& hostToFrame_affine, CalibHessian* HCalib, bool debugPrint)
+{
+	if(lastTraceStatus == ImmaturePointStatus::IPS_OOB) return lastTraceStatus;
+
+
+	debugPrint = false;//rand()%100==0;
+	float maxPixSearch = (wG[0]+hG[0])*setting_maxPixSearch;
+
+	if(debugPrint)
+		printf("trace pt (%.1f %.1f) from frame %d to %d. Range %f -> %f. t %f %f %f!\n",
+				u,v,
+				host->shell->id, frame->shell->id,
+				idepth_min, idepth_max,
+				hostToFrame_Kt[0],hostToFrame_Kt[1],hostToFrame_Kt[2]);
+
+//	const float stepsize = 1.0;				// stepsize for initial discrete search.
+//	const int GNIterations = 3;				// max # GN iterations
+//	const float GNThreshold = 0.1;				// GN stop after this stepsize.
+//	const float extraSlackOnTH = 1.2;			// for energy-based outlier check, be slightly more relaxed by this factor.
+//	const float slackInterval = 0.8;			// if pixel-interval is smaller than this, leave it be.
+//	const float minImprovementFactor = 2;		// if pixel-interval is smaller than this, leave it be.
+	// ============== project min and max. return if one of them is OOB ===================
+	Vec3f pr = hostToFrame_KRKi * Vec3f(u,v, 1);
+	Vec3f ptpMin = pr + hostToFrame_Kt*idepth_min;
+	float uMin = ptpMin[0] / ptpMin[2];
+	float vMin = ptpMin[1] / ptpMin[2];
+
+	if(!(uMin > 4 && vMin > 4 && uMin < wG[0]-5 && vMin < hG[0]-5))
+	{
+		if(debugPrint) printf("OOB uMin %f %f - %f %f %f (id %f-%f)!\n",
+				u,v,uMin, vMin,  ptpMin[2], idepth_min, idepth_max);
+		lastTraceUV = Vec2f(-1,-1);
+		lastTracePixelInterval=0;
+		return lastTraceStatus = ImmaturePointStatus::IPS_OOB;
+	}
+
+	float dist;
+	float uMax;
+	float vMax;
+	Vec3f ptpMax;
+	if(std::isfinite(idepth_max))
+	{
+		ptpMax = pr + hostToFrame_Kt*idepth_max;
+		uMax = ptpMax[0] / ptpMax[2];
+		vMax = ptpMax[1] / ptpMax[2];
+
+
+		if(!(uMax > 4 && vMax > 4 && uMax < wG[0]-5 && vMax < hG[0]-5))
+		{
+			if(debugPrint) printf("OOB uMax  %f %f - %f %f!\n",u,v, uMax, vMax);
+			lastTraceUV = Vec2f(-1,-1);
+			lastTracePixelInterval=0;
+			return lastTraceStatus = ImmaturePointStatus::IPS_OOB;
+		}
+
+
+
+		// ============== check their distance. everything below 2px is OK (-> skip). ===================
+		dist = (uMin-uMax)*(uMin-uMax) + (vMin-vMax)*(vMin-vMax);
+		dist = sqrtf(dist);
+		if(dist < setting_trace_slackInterval)
+		{
+			if(debugPrint)
+				printf("TOO CERTAIN ALREADY (dist %f)!\n", dist);
+
+			lastTraceUV = Vec2f(uMax+uMin, vMax+vMin)*0.5;
+			lastTracePixelInterval=dist;
+			return lastTraceStatus = ImmaturePointStatus::IPS_SKIPPED;
+		}
+		assert(dist>0);
+	}
+	else
+	{
+		dist = maxPixSearch;
+
+		// project to arbitrary depth to get direction.
+		ptpMax = pr + hostToFrame_Kt*0.01;
+		uMax = ptpMax[0] / ptpMax[2];
+		vMax = ptpMax[1] / ptpMax[2];
+
+		// direction.
+		float dx = uMax-uMin;
+		float dy = vMax-vMin;
+		float d = 1.0f / sqrtf(dx*dx+dy*dy);
+
+		// set to [setting_maxPixSearch].
+		uMax = uMin + dist*dx*d;
+		vMax = vMin + dist*dy*d;
+
+		// may still be out!
+		if(!(uMax > 4 && vMax > 4 && uMax < wG[0]-5 && vMax < hG[0]-5))
+		{
+			if(debugPrint) printf("OOB uMax-coarse %f %f %f!\n", uMax, vMax,  ptpMax[2]);
+			lastTraceUV = Vec2f(-1,-1);
+			lastTracePixelInterval=0;
+			return lastTraceStatus = ImmaturePointStatus::IPS_OOB;
+		}
+		assert(dist>0);
+	}
+
+
+	// set OOB if scale change too big.
+	if(!(idepth_min<0 || (ptpMin[2]>0.75 && ptpMin[2]<1.5)))
+	{
+		if(debugPrint) printf("OOB SCALE %f %f %f!\n", uMax, vMax,  ptpMin[2]);
+		lastTraceUV = Vec2f(-1,-1);
+		lastTracePixelInterval=0;
+		return lastTraceStatus = ImmaturePointStatus::IPS_OOB;
+	}
+
+
+	// ============== compute error-bounds on result in pixel. if the new interval is not at least 1/2 of the old, SKIP ===================
+	float dx = setting_trace_stepsize*(uMax-uMin);
+	float dy = setting_trace_stepsize*(vMax-vMin);
+
+	float a = (Vec2f(dx,dy).transpose() * gradH * Vec2f(dx,dy));
+	float b = (Vec2f(dy,-dx).transpose() * gradH * Vec2f(dy,-dx));
+	float errorInPixel = 0.2f + 0.2f * (a+b) / a;
+
+	if(errorInPixel*setting_trace_minImprovementFactor > dist && std::isfinite(idepth_max))
+	{
+		if(debugPrint)
+			printf("NO SIGNIFICANT IMPROVMENT (%f)!\n", errorInPixel);
+		lastTraceUV = Vec2f(uMax+uMin, vMax+vMin)*0.5;
+		lastTracePixelInterval=dist;
+		return lastTraceStatus = ImmaturePointStatus::IPS_BADCONDITION;
+	}
+
+	if(errorInPixel >10) errorInPixel=10;
+
+
+
+	// ============== do the discrete search ===================
+	dx /= dist;
+	dy /= dist;
+
+	if(debugPrint)
+		printf("trace pt (%.1f %.1f) from frame %d to %d. Range %f (%.1f %.1f) -> %f (%.1f %.1f)! ErrorInPixel %.1f!\n",
+				u,v,
+				host->shell->id, frame->shell->id,
+				idepth_min, uMin, vMin,
+				idepth_max, uMax, vMax,
+				errorInPixel
+				);
+
+
+	if(dist>maxPixSearch)
+	{
+		uMax = uMin + maxPixSearch*dx;
+		vMax = vMin + maxPixSearch*dy;
+		dist = maxPixSearch;
+	}
+
+	int numSteps = 1.9999f + dist / setting_trace_stepsize;
+	Mat22f Rplane = hostToFrame_KRKi.topLeftCorner<2,2>();
+
+	float randShift = uMin*1000-floorf(uMin*1000);
+	float ptx = uMin-randShift*dx;
+	float pty = vMin-randShift*dy;
+
+
+	Vec2f rotatetPattern[MAX_RES_PER_POINT];
+	for(int idx=0;idx<patternNum;idx++)
+		rotatetPattern[idx] = Rplane * Vec2f(patternP[idx][0], patternP[idx][1]);
+
+
+
+
+	if(!std::isfinite(dx) || !std::isfinite(dy))
+	{
+		//printf("COUGHT INF / NAN dxdy (%f %f)!\n", dx, dx);
+
+		lastTracePixelInterval=0;
+		lastTraceUV = Vec2f(-1,-1);
+		return lastTraceStatus = ImmaturePointStatus::IPS_OOB;
+	}
+
+
+
+	float errors[100];
+	float bestU=0, bestV=0, bestEnergy=1e10;
+	int bestIdx=-1;
+	if(numSteps >= 100) numSteps = 99;
+
+	for(int i=0;i<numSteps;i++)
+	{
+		float energy=0;
+		for(int idx=0;idx<patternNum;idx++)
+		{
+			float hitColor = getInterpolatedElement31(frame->dI,
+										(float)(ptx+rotatetPattern[idx][0]),
+										(float)(pty+rotatetPattern[idx][1]),
+										wG[0]);
+
+			if(!std::isfinite(hitColor)) {energy+=1e5; continue;}
+			float residual = hitColor - (float)(hostToFrame_affine[0] * color[idx] + hostToFrame_affine[1]);
+			float hw = fabs(residual) < setting_huberTH ? 1 : setting_huberTH / fabs(residual);
+			energy += hw *residual*residual*(2-hw);
+		}
+
+		if(debugPrint)
+			printf("step %.1f %.1f (id %f): energy = %f!\n",
+					ptx, pty, 0.0f, energy);
+
+
+		errors[i] = energy;
+		if(energy < bestEnergy)
+		{
+			bestU = ptx; bestV = pty; bestEnergy = energy; bestIdx = i;
+		}
+
+		ptx+=dx;
+		pty+=dy;
+	}
+
+
+	// find best score outside a +-2px radius.
+	float secondBest=1e10;
+	for(int i=0;i<numSteps;i++)
+	{
+		if((i < bestIdx-setting_minTraceTestRadius || i > bestIdx+setting_minTraceTestRadius) && errors[i] < secondBest)
+			secondBest = errors[i];
+	}
+	float newQuality = secondBest / bestEnergy;
+	if(newQuality < quality || numSteps > 10) quality = newQuality;
+
+
+	// ============== do GN optimization ===================
+	float uBak=bestU, vBak=bestV, gnstepsize=1, stepBack=0;
+	if(setting_trace_GNIterations>0) bestEnergy = 1e5;
+	int gnStepsGood=0, gnStepsBad=0;
+	for(int it=0;it<setting_trace_GNIterations;it++)
+	{
+		float H = 1, b=0, energy=0;
+		for(int idx=0;idx<patternNum;idx++)
+		{
+			Vec3f hitColor = getInterpolatedElement33(frame->dI,
+					(float)(bestU+rotatetPattern[idx][0]),
+					(float)(bestV+rotatetPattern[idx][1]),wG[0]);
+
+			if(!std::isfinite((float)hitColor[0])) {energy+=1e5; continue;}
+			float residual = hitColor[0] - (hostToFrame_affine[0] * color[idx] + hostToFrame_affine[1]);
+			float dResdDist = dx*hitColor[1] + dy*hitColor[2];
+			float hw = fabs(residual) < setting_huberTH ? 1 : setting_huberTH / fabs(residual);
+
+			H += hw*dResdDist*dResdDist;
+			b += hw*residual*dResdDist;
+			energy += weights[idx]*weights[idx]*hw *residual*residual*(2-hw);
+		}
+
+
+		if(energy > bestEnergy)
+		{
+			gnStepsBad++;
+
+			// do a smaller step from old point.
+			stepBack*=0.5;
+			bestU = uBak + stepBack*dx;
+			bestV = vBak + stepBack*dy;
+			if(debugPrint)
+				printf("GN BACK %d: E %f, H %f, b %f. id-step %f. UV %f %f -> %f %f.\n",
+						it, energy, H, b, stepBack,
+						uBak, vBak, bestU, bestV);
+		}
+		else
+		{
+			gnStepsGood++;
+
+			float step = -gnstepsize*b/H;
+			if(step < -0.5) step = -0.5;
+			else if(step > 0.5) step=0.5;
+
+			if(!std::isfinite(step)) step=0;
+
+			uBak=bestU;
+			vBak=bestV;
+			stepBack=step;
+
+			bestU += step*dx;
+			bestV += step*dy;
+			bestEnergy = energy;
+
+			if(debugPrint)
+				printf("GN step %d: E %f, H %f, b %f. id-step %f. UV %f %f -> %f %f.\n",
+						it, energy, H, b, step,
+						uBak, vBak, bestU, bestV);
+		}
+
+		if(fabsf(stepBack) < setting_trace_GNThreshold) break;
+	}
+
+
+	// ============== detect energy-based outlier. ===================
+//	float absGrad0 = getInterpolatedElement(frame->absSquaredGrad[0],bestU, bestV, wG[0]);
+//	float absGrad1 = getInterpolatedElement(frame->absSquaredGrad[1],bestU*0.5-0.25, bestV*0.5-0.25, wG[1]);
+//	float absGrad2 = getInterpolatedElement(frame->absSquaredGrad[2],bestU*0.25-0.375, bestV*0.25-0.375, wG[2]);
+	if(!(bestEnergy < energyTH*setting_trace_extraSlackOnTH))
+//			|| (absGrad0*areaGradientSlackFactor < host->frameGradTH
+//		     && absGrad1*areaGradientSlackFactor < host->frameGradTH*0.75f
+//			 && absGrad2*areaGradientSlackFactor < host->frameGradTH*0.50f))
+	{
+		if(debugPrint)
+			printf("OUTLIER!\n");
+
+		lastTracePixelInterval=0;
+		lastTraceUV = Vec2f(-1,-1);
+		if(lastTraceStatus == ImmaturePointStatus::IPS_OUTLIER)
+			return lastTraceStatus = ImmaturePointStatus::IPS_OOB;
+		else
+			return lastTraceStatus = ImmaturePointStatus::IPS_OUTLIER;
+	}
+
+
+	// ============== set new interval ===================
+	if(dx*dx>dy*dy)
+	{
+		idepth_min = (pr[2]*(bestU-errorInPixel*dx) - pr[0]) / (hostToFrame_Kt[0] - hostToFrame_Kt[2]*(bestU-errorInPixel*dx));
+		idepth_max = (pr[2]*(bestU+errorInPixel*dx) - pr[0]) / (hostToFrame_Kt[0] - hostToFrame_Kt[2]*(bestU+errorInPixel*dx));
+	}
+	else
+	{
+		idepth_min = (pr[2]*(bestV-errorInPixel*dy) - pr[1]) / (hostToFrame_Kt[1] - hostToFrame_Kt[2]*(bestV-errorInPixel*dy));
+		idepth_max = (pr[2]*(bestV+errorInPixel*dy) - pr[1]) / (hostToFrame_Kt[1] - hostToFrame_Kt[2]*(bestV+errorInPixel*dy));
+	}
+	if(idepth_min > idepth_max) std::swap<float>(idepth_min, idepth_max);
+
+
+	if(!std::isfinite(idepth_min) || !std::isfinite(idepth_max) || (idepth_max<0))
+	{
+		//printf("COUGHT INF / NAN minmax depth (%f %f)!\n", idepth_min, idepth_max);
+
+		lastTracePixelInterval=0;
+		lastTraceUV = Vec2f(-1,-1);
+		return lastTraceStatus = ImmaturePointStatus::IPS_OUTLIER;
+	}
+
+	lastTracePixelInterval=2*errorInPixel;
+	lastTraceUV = Vec2f(bestU, bestV);
+	return lastTraceStatus = ImmaturePointStatus::IPS_GOOD;
+}
+
+
+float ImmaturePoint::getdPixdd(
+		CalibHessian *  HCalib,
+		ImmaturePointTemporaryResidual* tmpRes,
+		float idepth)
+{
+	FrameFramePrecalc* precalc = &(host->targetPrecalc[tmpRes->target->idx]);
+	const Vec3f &PRE_tTll = precalc->PRE_tTll;
+	float drescale, u=0, v=0, new_idepth;
+	float Ku, Kv;
+	Vec3f KliP;
+
+	projectPoint(this->u,this->v, idepth, 0, 0,HCalib,
+			precalc->PRE_RTll,PRE_tTll, drescale, u, v, Ku, Kv, KliP, new_idepth);
+
+	float dxdd = (PRE_tTll[0]-PRE_tTll[2]*u)*HCalib->fxl();
+	float dydd = (PRE_tTll[1]-PRE_tTll[2]*v)*HCalib->fyl();
+	return drescale*sqrtf(dxdd*dxdd + dydd*dydd);
+}
+
+
+float ImmaturePoint::calcResidual(
+		CalibHessian *  HCalib, const float outlierTHSlack,
+		ImmaturePointTemporaryResidual* tmpRes,
+		float idepth)
+{
+	FrameFramePrecalc* precalc = &(host->targetPrecalc[tmpRes->target->idx]);
+
+	float energyLeft=0;
+	const Eigen::Vector3f* dIl = tmpRes->target->dI;
+	const Mat33f &PRE_KRKiTll = precalc->PRE_KRKiTll;
+	const Vec3f &PRE_KtTll = precalc->PRE_KtTll;
+	Vec2f affLL = precalc->PRE_aff_mode;
+
+	for(int idx=0;idx<patternNum;idx++)
+	{
+		float Ku, Kv;
+		if(!projectPoint(this->u+patternP[idx][0], this->v+patternP[idx][1], idepth, PRE_KRKiTll, PRE_KtTll, Ku, Kv))
+			{return 1e10;}
+
+		Vec3f hitColor = (getInterpolatedElement33(dIl, Ku, Kv, wG[0]));
+		if(!std::isfinite((float)hitColor[0])) {return 1e10;}
+		//if(benchmarkSpecialOption==5) hitColor = (getInterpolatedElement13BiCub(tmpRes->target->I, Ku, Kv, wG[0]));
+
+		float residual = hitColor[0] - (affLL[0] * color[idx] + affLL[1]);
+
+		float hw = fabsf(residual) < setting_huberTH ? 1 : setting_huberTH / fabsf(residual);
+		energyLeft += weights[idx]*weights[idx]*hw *residual*residual*(2-hw);
+	}
+
+	if(energyLeft > energyTH*outlierTHSlack)
+	{
+		energyLeft = energyTH*outlierTHSlack;
+	}
+	return energyLeft;
+}
+
+
+
+
+double ImmaturePoint::linearizeResidual(
+		CalibHessian *  HCalib, const float outlierTHSlack,
+		ImmaturePointTemporaryResidual* tmpRes,
+		float &Hdd, float &bd,
+		float idepth)
+{
+	if(tmpRes->state_state == ResState::OOB)
+		{ tmpRes->state_NewState = ResState::OOB; return tmpRes->state_energy; }
+
+	FrameFramePrecalc* precalc = &(host->targetPrecalc[tmpRes->target->idx]);
+
+	// check OOB due to scale angle change.
+
+	float energyLeft=0;
+	const Eigen::Vector3f* dIl = tmpRes->target->dI;
+	const Mat33f &PRE_RTll = precalc->PRE_RTll;
+	const Vec3f &PRE_tTll = precalc->PRE_tTll;
+	//const float * const Il = tmpRes->target->I;
+
+	Vec2f affLL = precalc->PRE_aff_mode;
+
+	for(int idx=0;idx<patternNum;idx++)
+	{
+		int dx = patternP[idx][0];
+		int dy = patternP[idx][1];
+
+		float drescale, u, v, new_idepth;
+		float Ku, Kv;
+		Vec3f KliP;
+
+		if(!projectPoint(this->u,this->v, idepth, dx, dy,HCalib,
+				PRE_RTll,PRE_tTll, drescale, u, v, Ku, Kv, KliP, new_idepth))
+			{tmpRes->state_NewState = ResState::OOB; return tmpRes->state_energy;}
+
+
+		Vec3f hitColor = (getInterpolatedElement33(dIl, Ku, Kv, wG[0]));
+
+		if(!std::isfinite((float)hitColor[0])) {tmpRes->state_NewState = ResState::OOB; return tmpRes->state_energy;}
+		float residual = hitColor[0] - (affLL[0] * color[idx] + affLL[1]);
+
+		float hw = fabsf(residual) < setting_huberTH ? 1 : setting_huberTH / fabsf(residual);
+		energyLeft += weights[idx]*weights[idx]*hw *residual*residual*(2-hw);
+
+		// depth derivatives.
+		float dxInterp = hitColor[1]*HCalib->fxl();
+		float dyInterp = hitColor[2]*HCalib->fyl();
+		float d_idepth = derive_idepth(PRE_tTll, u, v, dx, dy, dxInterp, dyInterp, drescale);
+
+		hw *= weights[idx]*weights[idx];
+
+		Hdd += (hw*d_idepth)*d_idepth;
+		bd += (hw*residual)*d_idepth;
+	}
+
+
+	if(energyLeft > energyTH*outlierTHSlack)
+	{
+		energyLeft = energyTH*outlierTHSlack;
+		tmpRes->state_NewState = ResState::OUTLIER;
+	}
+	else
+	{
+		tmpRes->state_NewState = ResState::IN;
+	}
+
+	tmpRes->state_NewEnergy = energyLeft;
+	return energyLeft;
+}
+
+
+
+}

src/FullSystem/ImmaturePoint.h

@@ -0,0 +1,111 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+ 
+#include "util/NumType.h"
+ 
+#include "FullSystem/HessianBlocks.h"
+namespace dso
+{
+
+
+struct ImmaturePointTemporaryResidual
+{
+public:
+	ResState state_state;
+	double state_energy;
+	ResState state_NewState;
+	double state_NewEnergy;
+	FrameHessian* target;
+};
+
+
+enum ImmaturePointStatus {
+	IPS_GOOD=0,					// traced well and good
+	IPS_OOB,					// OOB: end tracking & marginalize!
+	IPS_OUTLIER,				// energy too high: if happens again: outlier!
+	IPS_SKIPPED,				// traced well and good (but not actually traced).
+	IPS_BADCONDITION,			// not traced because of bad condition.
+	IPS_UNINITIALIZED};			// not even traced once.
+
+
+class ImmaturePoint
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	// static values
+	float color[MAX_RES_PER_POINT];
+	float weights[MAX_RES_PER_POINT];
+
+
+
+
+
+	Mat22f gradH;
+	Vec2f gradH_ev;
+	Mat22f gradH_eig;
+	float energyTH;
+	float u,v;
+	FrameHessian* host;
+	int idxInImmaturePoints;
+
+	float quality;
+
+	float my_type;
+
+	float idepth_min;
+	float idepth_max;
+	ImmaturePoint(int u_, int v_, FrameHessian* host_, float type, CalibHessian* HCalib);
+	~ImmaturePoint();
+
+	ImmaturePointStatus traceOn(FrameHessian* frame, const Mat33f &hostToFrame_KRKi, const Vec3f &hostToFrame_Kt, const Vec2f &hostToFrame_affine, CalibHessian* HCalib, bool debugPrint=false);
+
+	ImmaturePointStatus lastTraceStatus;
+	Vec2f lastTraceUV;
+	float lastTracePixelInterval;
+
+	float idepth_GT;
+
+	double linearizeResidual(
+			CalibHessian *  HCalib, const float outlierTHSlack,
+			ImmaturePointTemporaryResidual* tmpRes,
+			float &Hdd, float &bd,
+			float idepth);
+	float getdPixdd(
+			CalibHessian *  HCalib,
+			ImmaturePointTemporaryResidual* tmpRes,
+			float idepth);
+
+	float calcResidual(
+			CalibHessian *  HCalib, const float outlierTHSlack,
+			ImmaturePointTemporaryResidual* tmpRes,
+			float idepth);
+
+private:
+};
+
+}
+

src/FullSystem/PixelSelector.h

@@ -0,0 +1,256 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+
+#include "util/NumType.h"
+
+
+ 
+
+namespace dso
+{
+
+
+const float minUseGrad_pixsel = 10;
+
+
+template<int pot>
+inline int gridMaxSelection(Eigen::Vector3f* grads, bool* map_out, int w, int h, float THFac)
+{
+
+	memset(map_out, 0, sizeof(bool)*w*h);
+
+	int numGood = 0;
+	for(int y=1;y<h-pot;y+=pot)
+	{
+		for(int x=1;x<w-pot;x+=pot)
+		{
+			int bestXXID = -1;
+			int bestYYID = -1;
+			int bestXYID = -1;
+			int bestYXID = -1;
+
+			float bestXX=0, bestYY=0, bestXY=0, bestYX=0;
+
+			Eigen::Vector3f* grads0 = grads+x+y*w;
+			for(int dx=0;dx<pot;dx++)
+				for(int dy=0;dy<pot;dy++)
+				{
+					int idx = dx+dy*w;
+					Eigen::Vector3f g=grads0[idx];
+					float sqgd = g.tail<2>().squaredNorm();
+					float TH = THFac*minUseGrad_pixsel * (0.75f);
+
+					if(sqgd > TH*TH)
+					{
+						float agx = fabs((float)g[1]);
+						if(agx > bestXX) {bestXX=agx; bestXXID=idx;}
+
+						float agy = fabs((float)g[2]);
+						if(agy > bestYY) {bestYY=agy; bestYYID=idx;}
+
+						float gxpy = fabs((float)(g[1]-g[2]));
+						if(gxpy > bestXY) {bestXY=gxpy; bestXYID=idx;}
+
+						float gxmy = fabs((float)(g[1]+g[2]));
+						if(gxmy > bestYX) {bestYX=gxmy; bestYXID=idx;}
+					}
+				}
+
+			bool* map0 = map_out+x+y*w;
+
+			if(bestXXID>=0)
+			{
+				if(!map0[bestXXID])
+					numGood++;
+				map0[bestXXID] = true;
+
+			}
+			if(bestYYID>=0)
+			{
+				if(!map0[bestYYID])
+					numGood++;
+				map0[bestYYID] = true;
+
+			}
+			if(bestXYID>=0)
+			{
+				if(!map0[bestXYID])
+					numGood++;
+				map0[bestXYID] = true;
+
+			}
+			if(bestYXID>=0)
+			{
+				if(!map0[bestYXID])
+					numGood++;
+				map0[bestYXID] = true;
+
+			}
+		}
+	}
+
+	return numGood;
+}
+
+
+inline int gridMaxSelection(Eigen::Vector3f* grads, bool* map_out, int w, int h, int pot, float THFac)
+{
+
+	memset(map_out, 0, sizeof(bool)*w*h);
+
+	int numGood = 0;
+	for(int y=1;y<h-pot;y+=pot)
+	{
+		for(int x=1;x<w-pot;x+=pot)
+		{
+			int bestXXID = -1;
+			int bestYYID = -1;
+			int bestXYID = -1;
+			int bestYXID = -1;
+
+			float bestXX=0, bestYY=0, bestXY=0, bestYX=0;
+
+			Eigen::Vector3f* grads0 = grads+x+y*w;
+			for(int dx=0;dx<pot;dx++)
+				for(int dy=0;dy<pot;dy++)
+				{
+					int idx = dx+dy*w;
+					Eigen::Vector3f g=grads0[idx];
+					float sqgd = g.tail<2>().squaredNorm();
+					float TH = THFac*minUseGrad_pixsel * (0.75f);
+
+					if(sqgd > TH*TH)
+					{
+						float agx = fabs((float)g[1]);
+						if(agx > bestXX) {bestXX=agx; bestXXID=idx;}
+
+						float agy = fabs((float)g[2]);
+						if(agy > bestYY) {bestYY=agy; bestYYID=idx;}
+
+						float gxpy = fabs((float)(g[1]-g[2]));
+						if(gxpy > bestXY) {bestXY=gxpy; bestXYID=idx;}
+
+						float gxmy = fabs((float)(g[1]+g[2]));
+						if(gxmy > bestYX) {bestYX=gxmy; bestYXID=idx;}
+					}
+				}
+
+			bool* map0 = map_out+x+y*w;
+
+			if(bestXXID>=0)
+			{
+				if(!map0[bestXXID])
+					numGood++;
+				map0[bestXXID] = true;
+
+			}
+			if(bestYYID>=0)
+			{
+				if(!map0[bestYYID])
+					numGood++;
+				map0[bestYYID] = true;
+
+			}
+			if(bestXYID>=0)
+			{
+				if(!map0[bestXYID])
+					numGood++;
+				map0[bestXYID] = true;
+
+			}
+			if(bestYXID>=0)
+			{
+				if(!map0[bestYXID])
+					numGood++;
+				map0[bestYXID] = true;
+
+			}
+		}
+	}
+
+	return numGood;
+}
+
+
+inline int makePixelStatus(Eigen::Vector3f* grads, bool* map, int w, int h, float desiredDensity, int recsLeft=5, float THFac = 1)
+{
+	if(sparsityFactor < 1) sparsityFactor = 1;
+
+	int numGoodPoints;
+
+
+	if(sparsityFactor==1) numGoodPoints = gridMaxSelection<1>(grads, map, w, h, THFac);
+	else if(sparsityFactor==2) numGoodPoints = gridMaxSelection<2>(grads, map, w, h, THFac);
+	else if(sparsityFactor==3) numGoodPoints = gridMaxSelection<3>(grads, map, w, h, THFac);
+	else if(sparsityFactor==4) numGoodPoints = gridMaxSelection<4>(grads, map, w, h, THFac);
+	else if(sparsityFactor==5) numGoodPoints = gridMaxSelection<5>(grads, map, w, h, THFac);
+	else if(sparsityFactor==6) numGoodPoints = gridMaxSelection<6>(grads, map, w, h, THFac);
+	else if(sparsityFactor==7) numGoodPoints = gridMaxSelection<7>(grads, map, w, h, THFac);
+	else if(sparsityFactor==8) numGoodPoints = gridMaxSelection<8>(grads, map, w, h, THFac);
+	else if(sparsityFactor==9) numGoodPoints = gridMaxSelection<9>(grads, map, w, h, THFac);
+	else if(sparsityFactor==10) numGoodPoints = gridMaxSelection<10>(grads, map, w, h, THFac);
+	else if(sparsityFactor==11) numGoodPoints = gridMaxSelection<11>(grads, map, w, h, THFac);
+	else numGoodPoints = gridMaxSelection(grads, map, w, h, sparsityFactor, THFac);
+
+
+	/*
+	 * #points is approximately proportional to sparsityFactor^2.
+	 */
+
+	float quotia = numGoodPoints / (float)(desiredDensity);
+
+	int newSparsity = (sparsityFactor * sqrtf(quotia))+0.7f;
+
+
+	if(newSparsity < 1) newSparsity=1;
+
+
+	float oldTHFac = THFac;
+	if(newSparsity==1 && sparsityFactor==1) THFac = 0.5;
+
+
+	if((abs(newSparsity-sparsityFactor) < 1 && THFac==oldTHFac) ||
+			( quotia > 0.8 &&  1.0f / quotia > 0.8) ||
+			recsLeft == 0)
+	{
+
+//		printf(" \n");
+		//all good
+		sparsityFactor = newSparsity;
+		return numGoodPoints;
+	}
+	else
+	{
+//		printf(" -> re-evaluate! \n");
+		// re-evaluate.
+		sparsityFactor = newSparsity;
+		return makePixelStatus(grads, map, w,h, desiredDensity, recsLeft-1, THFac);
+	}
+}
+
+}
+

src/FullSystem/PixelSelector2.cpp

@@ -0,0 +1,442 @@
+/**
+* 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 "FullSystem/PixelSelector2.h"
+ 
+// 
+
+
+
+#include "util/NumType.h"
+#include "IOWrapper/ImageDisplay.h"
+#include "util/globalCalib.h"
+#include "FullSystem/HessianBlocks.h"
+#include "util/globalFuncs.h"
+
+namespace dso
+{
+
+
+PixelSelector::PixelSelector(int w, int h)
+{
+	randomPattern = new unsigned char[w*h];
+	std::srand(3141592);	// want to be deterministic.
+	for(int i=0;i<w*h;i++) randomPattern[i] = rand() & 0xFF;
+
+	currentPotential=3;
+
+
+	gradHist = new int[100*(1+w/32)*(1+h/32)];
+	ths = new float[(w/32)*(h/32)+100];
+	thsSmoothed = new float[(w/32)*(h/32)+100];
+
+	allowFast=false;
+	gradHistFrame=0;
+}
+
+PixelSelector::~PixelSelector()
+{
+	delete[] randomPattern;
+	delete[] gradHist;
+	delete[] ths;
+	delete[] thsSmoothed;
+}
+
+int computeHistQuantil(int* hist, float below)
+{
+	int th = hist[0]*below+0.5f;
+	for(int i=0;i<90;i++)
+	{
+		th -= hist[i+1];
+		if(th<0) return i;
+	}
+	return 90;
+}
+
+
+void PixelSelector::makeHists(const FrameHessian* const fh)
+{
+	gradHistFrame = fh;
+	float * mapmax0 = fh->absSquaredGrad[0];
+
+	int w = wG[0];
+	int h = hG[0];
+
+	int w32 = w/32;
+	int h32 = h/32;
+	thsStep = w32;
+
+	for(int y=0;y<h32;y++)
+		for(int x=0;x<w32;x++)
+		{
+			float* map0 = mapmax0+32*x+32*y*w;
+			int* hist0 = gradHist;// + 50*(x+y*w32);
+			memset(hist0,0,sizeof(int)*50);
+
+			for(int j=0;j<32;j++) for(int i=0;i<32;i++)
+			{
+				int it = i+32*x;
+				int jt = j+32*y;
+				if(it>w-2 || jt>h-2 || it<1 || jt<1) continue;
+				int g = sqrtf(map0[i+j*w]);
+				if(g>48) g=48;
+				hist0[g+1]++;
+				hist0[0]++;
+			}
+
+			ths[x+y*w32] = computeHistQuantil(hist0,setting_minGradHistCut) + setting_minGradHistAdd;
+		}
+
+	for(int y=0;y<h32;y++)
+		for(int x=0;x<w32;x++)
+		{
+			float sum=0,num=0;
+			if(x>0)
+			{
+				if(y>0) 	{num++; 	sum+=ths[x-1+(y-1)*w32];}
+				if(y<h32-1) {num++; 	sum+=ths[x-1+(y+1)*w32];}
+				num++; sum+=ths[x-1+(y)*w32];
+			}
+
+			if(x<w32-1)
+			{
+				if(y>0) 	{num++; 	sum+=ths[x+1+(y-1)*w32];}
+				if(y<h32-1) {num++; 	sum+=ths[x+1+(y+1)*w32];}
+				num++; sum+=ths[x+1+(y)*w32];
+			}
+
+			if(y>0) 	{num++; 	sum+=ths[x+(y-1)*w32];}
+			if(y<h32-1) {num++; 	sum+=ths[x+(y+1)*w32];}
+			num++; sum+=ths[x+y*w32];
+
+			thsSmoothed[x+y*w32] = (sum/num) * (sum/num);
+
+		}
+
+
+
+
+
+}
+int PixelSelector::makeMaps(
+		const FrameHessian* const fh,
+		float* map_out, float density, int recursionsLeft, bool plot, float thFactor)
+{
+	float numHave=0;
+	float numWant=density;
+	float quotia;
+	int idealPotential = currentPotential;
+
+
+//	if(setting_pixelSelectionUseFast>0 && allowFast)
+//	{
+//		memset(map_out, 0, sizeof(float)*wG[0]*hG[0]);
+//		std::vector<cv::KeyPoint> pts;
+//		cv::Mat img8u(hG[0],wG[0],CV_8U);
+//		for(int i=0;i<wG[0]*hG[0];i++)
+//		{
+//			float v = fh->dI[i][0]*0.8;
+//			img8u.at<uchar>(i) = (!std::isfinite(v) || v>255) ? 255 : v;
+//		}
+//		cv::FAST(img8u, pts, setting_pixelSelectionUseFast, true);
+//		for(unsigned int i=0;i<pts.size();i++)
+//		{
+//			int x = pts[i].pt.x+0.5;
+//			int y = pts[i].pt.y+0.5;
+//			map_out[x+y*wG[0]]=1;
+//			numHave++;
+//		}
+//
+//		printf("FAST selection: got %f / %f!\n", numHave, numWant);
+//		quotia = numWant / numHave;
+//	}
+//	else
+	{
+
+
+
+
+		// the number of selected pixels behaves approximately as
+		// K / (pot+1)^2, where K is a scene-dependent constant.
+		// we will allow sub-selecting pixels by up to a quotia of 0.25, otherwise we will re-select.
+
+		if(fh != gradHistFrame) makeHists(fh);
+
+		// select!
+		Eigen::Vector3i n = this->select(fh, map_out,currentPotential, thFactor);
+
+		// sub-select!
+		numHave = n[0]+n[1]+n[2];
+		quotia = numWant / numHave;
+
+		// by default we want to over-sample by 40% just to be sure.
+		float K = numHave * (currentPotential+1) * (currentPotential+1);
+		idealPotential = sqrtf(K/numWant)-1;	// round down.
+		if(idealPotential<1) idealPotential=1;
+
+		if( recursionsLeft>0 && quotia > 1.25 && currentPotential>1)
+		{
+			//re-sample to get more points!
+			// potential needs to be smaller
+			if(idealPotential>=currentPotential)
+				idealPotential = currentPotential-1;
+
+	//		printf("PixelSelector: have %.2f%%, need %.2f%%. RESAMPLE with pot %d -> %d.\n",
+	//				100*numHave/(float)(wG[0]*hG[0]),
+	//				100*numWant/(float)(wG[0]*hG[0]),
+	//				currentPotential,
+	//				idealPotential);
+			currentPotential = idealPotential;
+			return makeMaps(fh,map_out, density, recursionsLeft-1, plot,thFactor);
+		}
+		else if(recursionsLeft>0 && quotia < 0.25)
+		{
+			// re-sample to get less points!
+
+			if(idealPotential<=currentPotential)
+				idealPotential = currentPotential+1;
+
+	//		printf("PixelSelector: have %.2f%%, need %.2f%%. RESAMPLE with pot %d -> %d.\n",
+	//				100*numHave/(float)(wG[0]*hG[0]),
+	//				100*numWant/(float)(wG[0]*hG[0]),
+	//				currentPotential,
+	//				idealPotential);
+			currentPotential = idealPotential;
+			return makeMaps(fh,map_out, density, recursionsLeft-1, plot,thFactor);
+
+		}
+	}
+
+	int numHaveSub = numHave;
+	if(quotia < 0.95)
+	{
+		int wh=wG[0]*hG[0];
+		int rn=0;
+		unsigned char charTH = 255*quotia;
+		for(int i=0;i<wh;i++)
+		{
+			if(map_out[i] != 0)
+			{
+				if(randomPattern[rn] > charTH )
+				{
+					map_out[i]=0;
+					numHaveSub--;
+				}
+				rn++;
+			}
+		}
+	}
+
+//	printf("PixelSelector: have %.2f%%, need %.2f%%. KEEPCURR with pot %d -> %d. Subsampled to %.2f%%\n",
+//			100*numHave/(float)(wG[0]*hG[0]),
+//			100*numWant/(float)(wG[0]*hG[0]),
+//			currentPotential,
+//			idealPotential,
+//			100*numHaveSub/(float)(wG[0]*hG[0]));
+	currentPotential = idealPotential;
+
+
+	if(plot)
+	{
+		int w = wG[0];
+		int h = hG[0];
+
+
+		MinimalImageB3 img(w,h);
+
+		for(int i=0;i<w*h;i++)
+		{
+			float c = fh->dI[i][0]*0.7;
+			if(c>255) c=255;
+			img.at(i) = Vec3b(c,c,c);
+		}
+		IOWrap::displayImage("Selector Image", &img);
+
+		for(int y=0; y<h;y++)
+			for(int x=0;x<w;x++)
+			{
+				int i=x+y*w;
+				if(map_out[i] == 1)
+					img.setPixelCirc(x,y,Vec3b(0,255,0));
+				else if(map_out[i] == 2)
+					img.setPixelCirc(x,y,Vec3b(255,0,0));
+				else if(map_out[i] == 4)
+					img.setPixelCirc(x,y,Vec3b(0,0,255));
+			}
+		IOWrap::displayImage("Selector Pixels", &img);
+	}
+
+	return numHaveSub;
+}
+
+
+
+Eigen::Vector3i PixelSelector::select(const FrameHessian* const fh,
+		float* map_out, int pot, float thFactor)
+{
+
+	Eigen::Vector3f const * const map0 = fh->dI;
+
+	float * mapmax0 = fh->absSquaredGrad[0];
+	float * mapmax1 = fh->absSquaredGrad[1];
+	float * mapmax2 = fh->absSquaredGrad[2];
+
+
+	int w = wG[0];
+	int w1 = wG[1];
+	int w2 = wG[2];
+	int h = hG[0];
+
+
+	const Vec2f directions[16] = {
+	         Vec2f(0,    1.0000),
+	         Vec2f(0.3827,    0.9239),
+	         Vec2f(0.1951,    0.9808),
+	         Vec2f(0.9239,    0.3827),
+	         Vec2f(0.7071,    0.7071),
+	         Vec2f(0.3827,   -0.9239),
+	         Vec2f(0.8315,    0.5556),
+	         Vec2f(0.8315,   -0.5556),
+	         Vec2f(0.5556,   -0.8315),
+	         Vec2f(0.9808,    0.1951),
+	         Vec2f(0.9239,   -0.3827),
+	         Vec2f(0.7071,   -0.7071),
+	         Vec2f(0.5556,    0.8315),
+	         Vec2f(0.9808,   -0.1951),
+	         Vec2f(1.0000,    0.0000),
+	         Vec2f(0.1951,   -0.9808)};
+
+	memset(map_out,0,w*h*sizeof(PixelSelectorStatus));
+
+
+
+	float dw1 = setting_gradDownweightPerLevel;
+	float dw2 = dw1*dw1;
+
+
+	int n3=0, n2=0, n4=0;
+	for(int y4=0;y4<h;y4+=(4*pot)) for(int x4=0;x4<w;x4+=(4*pot))
+	{
+		int my3 = std::min((4*pot), h-y4);
+		int mx3 = std::min((4*pot), w-x4);
+		int bestIdx4=-1; float bestVal4=0;
+		Vec2f dir4 = directions[randomPattern[n2] & 0xF];
+		for(int y3=0;y3<my3;y3+=(2*pot)) for(int x3=0;x3<mx3;x3+=(2*pot))
+		{
+			int x34 = x3+x4;
+			int y34 = y3+y4;
+			int my2 = std::min((2*pot), h-y34);
+			int mx2 = std::min((2*pot), w-x34);
+			int bestIdx3=-1; float bestVal3=0;
+			Vec2f dir3 = directions[randomPattern[n2] & 0xF];
+			for(int y2=0;y2<my2;y2+=pot) for(int x2=0;x2<mx2;x2+=pot)
+			{
+				int x234 = x2+x34;
+				int y234 = y2+y34;
+				int my1 = std::min(pot, h-y234);
+				int mx1 = std::min(pot, w-x234);
+				int bestIdx2=-1; float bestVal2=0;
+				Vec2f dir2 = directions[randomPattern[n2] & 0xF];
+				for(int y1=0;y1<my1;y1+=1) for(int x1=0;x1<mx1;x1+=1)
+				{
+					assert(x1+x234 < w);
+					assert(y1+y234 < h);
+					int idx = x1+x234 + w*(y1+y234);
+					int xf = x1+x234;
+					int yf = y1+y234;
+
+					if(xf<4 || xf>=w-5 || yf<4 || yf>h-4) continue;
+
+
+					float pixelTH0 = thsSmoothed[(xf>>5) + (yf>>5) * thsStep];
+					float pixelTH1 = pixelTH0*dw1;
+					float pixelTH2 = pixelTH1*dw2;
+
+
+					float ag0 = mapmax0[idx];
+					if(ag0 > pixelTH0*thFactor)
+					{
+						Vec2f ag0d = map0[idx].tail<2>();
+						float dirNorm = fabsf((float)(ag0d.dot(dir2)));
+						if(!setting_selectDirectionDistribution) dirNorm = ag0;
+
+						if(dirNorm > bestVal2)
+						{ bestVal2 = dirNorm; bestIdx2 = idx; bestIdx3 = -2; bestIdx4 = -2;}
+					}
+					if(bestIdx3==-2) continue;
+
+					float ag1 = mapmax1[(int)(xf*0.5f+0.25f) + (int)(yf*0.5f+0.25f)*w1];
+					if(ag1 > pixelTH1*thFactor)
+					{
+						Vec2f ag0d = map0[idx].tail<2>();
+						float dirNorm = fabsf((float)(ag0d.dot(dir3)));
+						if(!setting_selectDirectionDistribution) dirNorm = ag1;
+
+						if(dirNorm > bestVal3)
+						{ bestVal3 = dirNorm; bestIdx3 = idx; bestIdx4 = -2;}
+					}
+					if(bestIdx4==-2) continue;
+
+					float ag2 = mapmax2[(int)(xf*0.25f+0.125) + (int)(yf*0.25f+0.125)*w2];
+					if(ag2 > pixelTH2*thFactor)
+					{
+						Vec2f ag0d = map0[idx].tail<2>();
+						float dirNorm = fabsf((float)(ag0d.dot(dir4)));
+						if(!setting_selectDirectionDistribution) dirNorm = ag2;
+
+						if(dirNorm > bestVal4)
+						{ bestVal4 = dirNorm; bestIdx4 = idx; }
+					}
+				}
+
+				if(bestIdx2>0)
+				{
+					map_out[bestIdx2] = 1;
+					bestVal3 = 1e10;
+					n2++;
+				}
+			}
+
+			if(bestIdx3>0)
+			{
+				map_out[bestIdx3] = 2;
+				bestVal4 = 1e10;
+				n3++;
+			}
+		}
+
+		if(bestIdx4>0)
+		{
+			map_out[bestIdx4] = 4;
+			n4++;
+		}
+	}
+
+
+	return Eigen::Vector3i(n2,n3,n4);
+}
+
+
+}
+

src/FullSystem/PixelSelector2.h

@@ -0,0 +1,72 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+ 
+#include "util/NumType.h"
+
+namespace dso
+{
+
+enum PixelSelectorStatus {PIXSEL_VOID=0, PIXSEL_1, PIXSEL_2, PIXSEL_3};
+
+
+class FrameHessian;
+
+class PixelSelector
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	int makeMaps(
+			const FrameHessian* const fh,
+			float* map_out, float density, int recursionsLeft=1, bool plot=false, float thFactor=1);
+
+	PixelSelector(int w, int h);
+	~PixelSelector();
+	int currentPotential;
+
+
+	bool allowFast;
+	void makeHists(const FrameHessian* const fh);
+private:
+
+	Eigen::Vector3i select(const FrameHessian* const fh,
+			float* map_out, int pot, float thFactor=1);
+
+
+	unsigned char* randomPattern;
+
+
+	int* gradHist;
+	float* ths;
+	float* thsSmoothed;
+	int thsStep;
+	const FrameHessian* gradHistFrame;
+};
+
+
+
+
+}
+

src/FullSystem/ResidualProjections.h

@@ -0,0 +1,93 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+#include "util/NumType.h"
+#include "FullSystem/FullSystem.h"
+#include "FullSystem/HessianBlocks.h"
+#include "util/settings.h"
+
+namespace dso
+{
+
+
+EIGEN_STRONG_INLINE float derive_idepth(
+		const Vec3f &t, const float &u, const float &v,
+		const int &dx, const int &dy, const float &dxInterp,
+		const float &dyInterp, const float &drescale)
+{
+	return (dxInterp*drescale * (t[0]-t[2]*u)
+			+ dyInterp*drescale * (t[1]-t[2]*v))*SCALE_IDEPTH;
+}
+
+
+
+EIGEN_STRONG_INLINE bool projectPoint(
+		const float &u_pt,const float &v_pt,
+		const float &idepth,
+		const Mat33f &KRKi, const Vec3f &Kt,
+		float &Ku, float &Kv)
+{
+	Vec3f ptp = KRKi * Vec3f(u_pt,v_pt, 1) + Kt*idepth;
+	Ku = ptp[0] / ptp[2];
+	Kv = ptp[1] / ptp[2];
+	return Ku>1.1f && Kv>1.1f && Ku<wM3G && Kv<hM3G;
+}
+
+
+
+EIGEN_STRONG_INLINE bool projectPoint(
+		const float &u_pt,const float &v_pt,
+		const float &idepth,
+		const int &dx, const int &dy,
+		CalibHessian* const &HCalib,
+		const Mat33f &R, const Vec3f &t,
+		float &drescale, float &u, float &v,
+		float &Ku, float &Kv, Vec3f &KliP, float &new_idepth)
+{
+	KliP = Vec3f(
+			(u_pt+dx-HCalib->cxl())*HCalib->fxli(),
+			(v_pt+dy-HCalib->cyl())*HCalib->fyli(),
+			1);
+
+	Vec3f ptp = R * KliP + t*idepth;
+	drescale = 1.0f/ptp[2];
+	new_idepth = idepth*drescale;
+
+	if(!(drescale>0)) return false;
+
+	u = ptp[0] * drescale;
+	v = ptp[1] * drescale;
+	Ku = u*HCalib->fxl() + HCalib->cxl();
+	Kv = v*HCalib->fyl() + HCalib->cyl();
+
+	return Ku>1.1f && Kv>1.1f && Ku<wM3G && Kv<hM3G;
+}
+
+
+
+
+}
+

src/FullSystem/Residuals.cpp

@@ -0,0 +1,329 @@
+/**
+* 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/>.
+*/
+
+
+/*
+ * KFBuffer.cpp
+ *
+ *  Created on: Jan 7, 2014
+ *      Author: engelj
+ */
+
+#include "FullSystem/FullSystem.h"
+ 
+#include "stdio.h"
+#include "util/globalFuncs.h"
+#include <Eigen/LU>
+#include <algorithm>
+#include "IOWrapper/ImageDisplay.h"
+#include "util/globalCalib.h"
+#include <Eigen/SVD>
+#include <Eigen/Eigenvalues>
+
+#include "FullSystem/ResidualProjections.h"
+#include "OptimizationBackend/EnergyFunctional.h"
+#include "OptimizationBackend/EnergyFunctionalStructs.h"
+
+#include "FullSystem/HessianBlocks.h"
+
+namespace dso
+{
+int PointFrameResidual::instanceCounter = 0;
+
+
+long runningResID=0;
+
+
+PointFrameResidual::PointFrameResidual(){assert(false); instanceCounter++;}
+
+PointFrameResidual::~PointFrameResidual(){assert(efResidual==0); instanceCounter--; delete J;}
+
+PointFrameResidual::PointFrameResidual(PointHessian* point_, FrameHessian* host_, FrameHessian* target_) :
+	point(point_),
+	host(host_),
+	target(target_)
+{
+	efResidual=0;
+	instanceCounter++;
+	resetOOB();
+	J = new RawResidualJacobian();
+	assert(((long)J)%16==0);
+
+	isNew=true;
+}
+
+
+
+
+double PointFrameResidual::linearize(CalibHessian* HCalib)
+{
+	state_NewEnergyWithOutlier=-1;
+
+	if(state_state == ResState::OOB)
+		{ state_NewState = ResState::OOB; return state_energy; }
+
+	FrameFramePrecalc* precalc = &(host->targetPrecalc[target->idx]);
+	float energyLeft=0;
+	const Eigen::Vector3f* dIl = target->dI;
+	//const float* const Il = target->I;
+	const Mat33f &PRE_KRKiTll = precalc->PRE_KRKiTll;
+	const Vec3f &PRE_KtTll = precalc->PRE_KtTll;
+	const Mat33f &PRE_RTll_0 = precalc->PRE_RTll_0;
+	const Vec3f &PRE_tTll_0 = precalc->PRE_tTll_0;
+	const float * const color = point->color;
+	const float * const weights = point->weights;
+
+	Vec2f affLL = precalc->PRE_aff_mode;
+	float b0 = precalc->PRE_b0_mode;
+
+
+	Vec6f d_xi_x, d_xi_y;
+	Vec4f d_C_x, d_C_y;
+	float d_d_x, d_d_y;
+	{
+		float drescale, u, v, new_idepth;
+		float Ku, Kv;
+		Vec3f KliP;
+
+		if(!projectPoint(point->u, point->v, point->idepth_zero_scaled, 0, 0,HCalib,
+				PRE_RTll_0,PRE_tTll_0, drescale, u, v, Ku, Kv, KliP, new_idepth))
+			{ state_NewState = ResState::OOB; return state_energy; }
+
+		centerProjectedTo = Vec3f(Ku, Kv, new_idepth);
+
+
+		// diff d_idepth
+		d_d_x = drescale * (PRE_tTll_0[0]-PRE_tTll_0[2]*u)*SCALE_IDEPTH*HCalib->fxl();
+		d_d_y = drescale * (PRE_tTll_0[1]-PRE_tTll_0[2]*v)*SCALE_IDEPTH*HCalib->fyl();
+
+
+
+
+		// diff calib
+		d_C_x[2] = drescale*(PRE_RTll_0(2,0)*u-PRE_RTll_0(0,0));
+		d_C_x[3] = HCalib->fxl() * drescale*(PRE_RTll_0(2,1)*u-PRE_RTll_0(0,1)) * HCalib->fyli();
+		d_C_x[0] = KliP[0]*d_C_x[2];
+		d_C_x[1] = KliP[1]*d_C_x[3];
+
+		d_C_y[2] = HCalib->fyl() * drescale*(PRE_RTll_0(2,0)*v-PRE_RTll_0(1,0)) * HCalib->fxli();
+		d_C_y[3] = drescale*(PRE_RTll_0(2,1)*v-PRE_RTll_0(1,1));
+		d_C_y[0] = KliP[0]*d_C_y[2];
+		d_C_y[1] = KliP[1]*d_C_y[3];
+
+		d_C_x[0] = (d_C_x[0]+u)*SCALE_F;
+		d_C_x[1] *= SCALE_F;
+		d_C_x[2] = (d_C_x[2]+1)*SCALE_C;
+		d_C_x[3] *= SCALE_C;
+
+		d_C_y[0] *= SCALE_F;
+		d_C_y[1] = (d_C_y[1]+v)*SCALE_F;
+		d_C_y[2] *= SCALE_C;
+		d_C_y[3] = (d_C_y[3]+1)*SCALE_C;
+
+
+		d_xi_x[0] = new_idepth*HCalib->fxl();
+		d_xi_x[1] = 0;
+		d_xi_x[2] = -new_idepth*u*HCalib->fxl();
+		d_xi_x[3] = -u*v*HCalib->fxl();
+		d_xi_x[4] = (1+u*u)*HCalib->fxl();
+		d_xi_x[5] = -v*HCalib->fxl();
+
+		d_xi_y[0] = 0;
+		d_xi_y[1] = new_idepth*HCalib->fyl();
+		d_xi_y[2] = -new_idepth*v*HCalib->fyl();
+		d_xi_y[3] = -(1+v*v)*HCalib->fyl();
+		d_xi_y[4] = u*v*HCalib->fyl();
+		d_xi_y[5] = u*HCalib->fyl();
+	}
+
+
+	{
+		J->Jpdxi[0] = d_xi_x;
+		J->Jpdxi[1] = d_xi_y;
+
+		J->Jpdc[0] = d_C_x;
+		J->Jpdc[1] = d_C_y;
+
+		J->Jpdd[0] = d_d_x;
+		J->Jpdd[1] = d_d_y;
+
+	}
+
+
+
+
+
+
+	float JIdxJIdx_00=0, JIdxJIdx_11=0, JIdxJIdx_10=0;
+	float JabJIdx_00=0, JabJIdx_01=0, JabJIdx_10=0, JabJIdx_11=0;
+	float JabJab_00=0, JabJab_01=0, JabJab_11=0;
+
+	float wJI2_sum = 0;
+
+	for(int idx=0;idx<patternNum;idx++)
+	{
+		float Ku, Kv;
+		if(!projectPoint(point->u+patternP[idx][0], point->v+patternP[idx][1], point->idepth_scaled, PRE_KRKiTll, PRE_KtTll, Ku, Kv))
+			{ state_NewState = ResState::OOB; return state_energy; }
+
+		projectedTo[idx][0] = Ku;
+		projectedTo[idx][1] = Kv;
+
+
+        Vec3f hitColor = (getInterpolatedElement33(dIl, Ku, Kv, wG[0]));
+        float residual = hitColor[0] - (float)(affLL[0] * color[idx] + affLL[1]);
+
+
+
+		float drdA = (color[idx]-b0);
+		if(!std::isfinite((float)hitColor[0]))
+		{ state_NewState = ResState::OOB; return state_energy; }
+
+
+		float w = sqrtf(setting_outlierTHSumComponent / (setting_outlierTHSumComponent + hitColor.tail<2>().squaredNorm()));
+        w = 0.5f*(w + weights[idx]);
+
+
+
+		float hw = fabsf(residual) < setting_huberTH ? 1 : setting_huberTH / fabsf(residual);
+		energyLeft += w*w*hw *residual*residual*(2-hw);
+
+		{
+			if(hw < 1) hw = sqrtf(hw);
+			hw = hw*w;
+
+			hitColor[1]*=hw;
+			hitColor[2]*=hw;
+
+			J->resF[idx] = residual*hw;
+
+			J->JIdx[0][idx] = hitColor[1];
+			J->JIdx[1][idx] = hitColor[2];
+			J->JabF[0][idx] = drdA*hw;
+			J->JabF[1][idx] = hw;
+
+			JIdxJIdx_00+=hitColor[1]*hitColor[1];
+			JIdxJIdx_11+=hitColor[2]*hitColor[2];
+			JIdxJIdx_10+=hitColor[1]*hitColor[2];
+
+			JabJIdx_00+= drdA*hw * hitColor[1];
+			JabJIdx_01+= drdA*hw * hitColor[2];
+			JabJIdx_10+= hw * hitColor[1];
+			JabJIdx_11+= hw * hitColor[2];
+
+			JabJab_00+= drdA*drdA*hw*hw;
+			JabJab_01+= drdA*hw*hw;
+			JabJab_11+= hw*hw;
+
+
+			wJI2_sum += hw*hw*(hitColor[1]*hitColor[1]+hitColor[2]*hitColor[2]);
+
+			if(setting_affineOptModeA < 0) J->JabF[0][idx]=0;
+			if(setting_affineOptModeB < 0) J->JabF[1][idx]=0;
+
+		}
+	}
+
+	J->JIdx2(0,0) = JIdxJIdx_00;
+	J->JIdx2(0,1) = JIdxJIdx_10;
+	J->JIdx2(1,0) = JIdxJIdx_10;
+	J->JIdx2(1,1) = JIdxJIdx_11;
+	J->JabJIdx(0,0) = JabJIdx_00;
+	J->JabJIdx(0,1) = JabJIdx_01;
+	J->JabJIdx(1,0) = JabJIdx_10;
+	J->JabJIdx(1,1) = JabJIdx_11;
+	J->Jab2(0,0) = JabJab_00;
+	J->Jab2(0,1) = JabJab_01;
+	J->Jab2(1,0) = JabJab_01;
+	J->Jab2(1,1) = JabJab_11;
+
+	state_NewEnergyWithOutlier = energyLeft;
+
+	if(energyLeft > std::max<float>(host->frameEnergyTH, target->frameEnergyTH) || wJI2_sum < 2)
+	{
+		energyLeft = std::max<float>(host->frameEnergyTH, target->frameEnergyTH);
+		state_NewState = ResState::OUTLIER;
+	}
+	else
+	{
+		state_NewState = ResState::IN;
+	}
+
+	state_NewEnergy = energyLeft;
+	return energyLeft;
+}
+
+
+
+void PointFrameResidual::debugPlot()
+{
+	if(state_state==ResState::OOB) return;
+	Vec3b cT = Vec3b(0,0,0);
+
+	if(freeDebugParam5==0)
+	{
+		float rT = 20*sqrt(state_energy/9);
+		if(rT<0) rT=0; if(rT>255)rT=255;
+		cT = Vec3b(0,255-rT,rT);
+	}
+	else
+	{
+		if(state_state == ResState::IN) cT = Vec3b(255,0,0);
+		else if(state_state == ResState::OOB) cT = Vec3b(255,255,0);
+		else if(state_state == ResState::OUTLIER) cT = Vec3b(0,0,255);
+		else cT = Vec3b(255,255,255);
+	}
+
+	for(int i=0;i<patternNum;i++)
+	{
+		if((projectedTo[i][0] > 2 && projectedTo[i][1] > 2 && projectedTo[i][0] < wG[0]-3 && projectedTo[i][1] < hG[0]-3 ))
+			target->debugImage->setPixel1((float)projectedTo[i][0], (float)projectedTo[i][1],cT);
+	}
+}
+
+
+
+void PointFrameResidual::applyRes(bool copyJacobians)
+{
+	if(copyJacobians)
+	{
+		if(state_state == ResState::OOB)
+		{
+			assert(!efResidual->isActiveAndIsGoodNEW);
+			return;	// can never go back from OOB
+		}
+		if(state_NewState == ResState::IN)// && )
+		{
+			efResidual->isActiveAndIsGoodNEW=true;
+			efResidual->takeDataF();
+		}
+		else
+		{
+			efResidual->isActiveAndIsGoodNEW=false;
+		}
+	}
+
+	setState(state_NewState);
+	state_energy = state_NewEnergy;
+}
+}

src/FullSystem/Residuals.h

@@ -0,0 +1,106 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+ 
+#include "util/globalCalib.h"
+#include "vector"
+ 
+#include "util/NumType.h"
+#include <iostream>
+#include <fstream>
+#include "util/globalFuncs.h"
+#include "OptimizationBackend/RawResidualJacobian.h"
+
+namespace dso
+{
+class PointHessian;
+class FrameHessian;
+class CalibHessian;
+
+class EFResidual;
+
+
+enum ResLocation {ACTIVE=0, LINEARIZED, MARGINALIZED, NONE};
+enum ResState {IN=0, OOB, OUTLIER};
+
+struct FullJacRowT
+{
+	Eigen::Vector2f projectedTo[MAX_RES_PER_POINT];
+};
+
+class PointFrameResidual
+{
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+	EFResidual* efResidual;
+
+	static int instanceCounter;
+
+
+	ResState state_state;
+	double state_energy;
+	ResState state_NewState;
+	double state_NewEnergy;
+	double state_NewEnergyWithOutlier;
+
+
+	void setState(ResState s) {state_state = s;}
+
+
+	PointHessian* point;
+	FrameHessian* host;
+	FrameHessian* target;
+	RawResidualJacobian* J;
+
+
+	bool isNew;
+
+
+	Eigen::Vector2f projectedTo[MAX_RES_PER_POINT];
+	Vec3f centerProjectedTo;
+
+	~PointFrameResidual();
+	PointFrameResidual();
+	PointFrameResidual(PointHessian* point_, FrameHessian* host_, FrameHessian* target_);
+	double linearize(CalibHessian* HCalib);
+
+
+	void resetOOB()
+	{
+		state_NewEnergy = state_energy = 0;
+		state_NewState = ResState::OUTLIER;
+
+		setState(ResState::IN);
+	};
+	void applyRes( bool copyJacobians);
+
+	void debugPlot();
+
+	void printRows(std::vector<VecX> &v, VecX &r, int nFrames, int nPoints, int M, int res);
+};
+}
+

src/IOWrapper/ImageDisplay.h

@@ -0,0 +1,55 @@
+/**
+* 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/>.
+*/
+
+
+
+#pragma once
+#include <vector>
+#include "util/NumType.h"
+#include "util/MinimalImage.h"
+
+
+namespace dso
+{
+
+namespace IOWrap
+{
+
+void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize = false);
+void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize = false);
+void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize = false);
+void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize = false);
+void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize = false);
+
+
+void displayImageStitch(const char* windowName, const std::vector<MinimalImageB*> images, int cc=0, int rc=0);
+void displayImageStitch(const char* windowName, const std::vector<MinimalImageB3*> images, int cc=0, int rc=0);
+void displayImageStitch(const char* windowName, const std::vector<MinimalImageF*> images, int cc=0, int rc=0);
+void displayImageStitch(const char* windowName, const std::vector<MinimalImageF3*> images, int cc=0, int rc=0);
+
+int waitKey(int milliseconds);
+void closeAllWindows();
+
+}
+
+}

src/IOWrapper/ImageDisplay_dummy.cpp

@@ -0,0 +1,50 @@
+/**
+* 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 "IOWrapper/ImageDisplay.h"
+
+namespace dso
+{
+
+
+namespace IOWrap
+{
+void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize) {};
+void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize) {};
+void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize) {};
+void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize) {};
+void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize) {};
+
+
+void displayImageStitch(const char* windowName, const std::vector<MinimalImageB*> images, int cc, int rc) {};
+void displayImageStitch(const char* windowName, const std::vector<MinimalImageB3*> images, int cc, int rc) {};
+void displayImageStitch(const char* windowName, const std::vector<MinimalImageF*> images, int cc, int rc) {};
+void displayImageStitch(const char* windowName, const std::vector<MinimalImageF3*> images, int cc, int rc) {};
+
+int waitKey(int milliseconds) {return 0;};
+void closeAllWindows() {};
+}
+
+}

src/IOWrapper/ImageRW.h

@@ -0,0 +1,48 @@
+/**
+* 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/>.
+*/
+
+
+
+#pragma once
+#include "util/NumType.h"
+#include "util/MinimalImage.h"
+
+namespace dso
+{
+namespace IOWrap
+{
+
+MinimalImageB* readImageBW_8U(std::string filename);
+MinimalImageB3* readImageRGB_8U(std::string filename);
+MinimalImage<unsigned short>* readImageBW_16U(std::string filename);
+
+
+MinimalImageB* readStreamBW_8U(char* data, int numBytes);
+
+void writeImage(std::string filename, MinimalImageB* img);
+void writeImage(std::string filename, MinimalImageB3* img);
+void writeImage(std::string filename, MinimalImageF* img);
+void writeImage(std::string filename, MinimalImageF3* img);
+
+}
+}

src/IOWrapper/ImageRW_dummy.cpp

@@ -0,0 +1,46 @@
+/**
+* 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 "IOWrapper/ImageRW.h"
+
+namespace dso
+{
+
+
+namespace IOWrap
+{
+
+MinimalImageB* readImageBW_8U(std::string filename) {printf("not implemented. bye!\n"); return 0;};
+MinimalImageB3* readImageRGB_8U(std::string filename) {printf("not implemented. bye!\n"); return 0;};
+MinimalImage<unsigned short>* readImageBW_16U(std::string filename) {printf("not implemented. bye!\n"); return 0;};
+MinimalImageB* readStreamBW_8U(char* data, int numBytes) {printf("not implemented. bye!\n"); return 0;};
+void writeImage(std::string filename, MinimalImageB* img) {};
+void writeImage(std::string filename, MinimalImageB3* img) {};
+void writeImage(std::string filename, MinimalImageF* img) {};
+void writeImage(std::string filename, MinimalImageF3* img) {};
+
+}
+
+}

src/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp

@@ -0,0 +1,197 @@
+/**
+* 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 "IOWrapper/ImageDisplay.h"
+
+#include <opencv2/highgui/highgui.hpp>
+
+#include <string>
+#include <unordered_set>
+
+#include <boost/thread.hpp>
+
+#include "util/settings.h"
+
+namespace dso
+{
+
+
+namespace IOWrap
+{
+
+std::unordered_set<std::string> openWindows;
+boost::mutex openCVdisplayMutex;
+
+
+
+void displayImage(const char* windowName, const cv::Mat& image, bool autoSize)
+{
+	if(disableAllDisplay) return;
+
+	boost::unique_lock<boost::mutex> lock(openCVdisplayMutex);
+	if(!autoSize)
+	{
+		if(openWindows.find(windowName) == openWindows.end())
+		{
+			cv::namedWindow(windowName, cv::WINDOW_NORMAL);
+			cv::resizeWindow(windowName, image.cols, image.rows);
+			openWindows.insert(windowName);
+		}
+	}
+	cv::imshow(windowName, image);
+}
+
+
+void displayImageStitch(const char* windowName, const std::vector<cv::Mat*> images, int cc, int rc)
+{
+	if(disableAllDisplay) return;
+	if(images.size() == 0) return;
+
+	// get dimensions.
+	int w = images[0]->cols;
+	int h = images[0]->rows;
+
+	int num = std::max((int)setting_maxFrames, (int)images.size());
+
+	// get optimal dimensions.
+	int bestCC = 0;
+	float bestLoss = 1e10;
+	for(int cc=1;cc<10;cc++)
+	{
+		int ww = w * cc;
+		int hh = h * ((num+cc-1)/cc);
+
+
+		float wLoss = ww/16.0f;
+		float hLoss = hh/10.0f;
+		float loss = std::max(wLoss, hLoss);
+
+		if(loss < bestLoss)
+		{
+			bestLoss = loss;
+			bestCC = cc;
+		}
+	}
+
+	int bestRC = ((num+bestCC-1)/bestCC);
+	if(cc != 0)
+	{
+		bestCC = cc;
+		bestRC= rc;
+	}
+	cv::Mat stitch = cv::Mat(bestRC*h, bestCC*w, images[0]->type());
+	stitch.setTo(0);
+	for(int i=0;i<(int)images.size() && i < bestCC*bestRC;i++)
+	{
+		int c = i%bestCC;
+		int r = i/bestCC;
+
+		cv::Mat roi = stitch(cv::Rect(c*w, r*h, w,h));
+		images[i]->copyTo(roi);
+	}
+	displayImage(windowName, stitch, false);
+}
+
+
+
+void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize)
+{
+	displayImage(windowName, cv::Mat(img->h, img->w, CV_8U, img->data), autoSize);
+}
+void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize)
+{
+	displayImage(windowName, cv::Mat(img->h, img->w, CV_8UC3, img->data), autoSize);
+}
+void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize)
+{
+	displayImage(windowName, cv::Mat(img->h, img->w, CV_32F, img->data)*(1/254.0f), autoSize);
+}
+void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize)
+{
+	displayImage(windowName, cv::Mat(img->h, img->w, CV_32FC3, img->data)*(1/254.0f), autoSize);
+}
+void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize)
+{
+	displayImage(windowName, cv::Mat(img->h, img->w, CV_16U, img->data), autoSize);
+}
+
+
+void displayImageStitch(const char* windowName, const std::vector<MinimalImageB*> images, int cc, int rc)
+{
+	std::vector<cv::Mat*> imagesCV;
+    for(size_t i=0; i < images.size();i++)
+		imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_8U, images[i]->data));
+	displayImageStitch(windowName, imagesCV, cc, rc);
+    for(size_t i=0; i < images.size();i++)
+		delete imagesCV[i];
+}
+void displayImageStitch(const char* windowName, const std::vector<MinimalImageB3*> images, int cc, int rc)
+{
+	std::vector<cv::Mat*> imagesCV;
+    for(size_t i=0; i < images.size();i++)
+		imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_8UC3, images[i]->data));
+	displayImageStitch(windowName, imagesCV, cc, rc);
+    for(size_t i=0; i < images.size();i++)
+		delete imagesCV[i];
+}
+void displayImageStitch(const char* windowName, const std::vector<MinimalImageF*> images, int cc, int rc)
+{
+	std::vector<cv::Mat*> imagesCV;
+    for(size_t i=0; i < images.size();i++)
+		imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_32F, images[i]->data));
+	displayImageStitch(windowName, imagesCV, cc, rc);
+    for(size_t i=0; i < images.size();i++)
+		delete imagesCV[i];
+}
+void displayImageStitch(const char* windowName, const std::vector<MinimalImageF3*> images, int cc, int rc)
+{
+	std::vector<cv::Mat*> imagesCV;
+    for(size_t i=0; i < images.size();i++)
+		imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_32FC3, images[i]->data));
+	displayImageStitch(windowName, imagesCV, cc, rc);
+    for(size_t i=0; i < images.size();i++)
+		delete imagesCV[i];
+}
+
+
+
+int waitKey(int milliseconds)
+{
+	if(disableAllDisplay) return 0;
+
+	boost::unique_lock<boost::mutex> lock(openCVdisplayMutex);
+	return cv::waitKey(milliseconds);
+}
+
+void closeAllWindows()
+{
+	if(disableAllDisplay) return;
+	boost::unique_lock<boost::mutex> lock(openCVdisplayMutex);
+	cv::destroyAllWindows();
+	openWindows.clear();
+}
+}
+
+}

src/IOWrapper/OpenCV/ImageRW_OpenCV.cpp

@@ -0,0 +1,128 @@
+/**
+* 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 "IOWrapper/ImageRW.h"
+#include <opencv2/highgui/highgui.hpp>
+
+
+namespace dso
+{
+
+namespace IOWrap
+{
+MinimalImageB* readImageBW_8U(std::string filename)
+{
+	cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
+	if(m.rows*m.cols==0)
+	{
+		printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str());
+		return 0;
+	}
+	if(m.type() != CV_8U)
+	{
+		printf("cv::imread did something strange! this may segfault. \n");
+		return 0;
+	}
+	MinimalImageB* img = new MinimalImageB(m.cols, m.rows);
+	memcpy(img->data, m.data, m.rows*m.cols);
+	return img;
+}
+
+MinimalImageB3* readImageRGB_8U(std::string filename)
+{
+	cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_COLOR);
+	if(m.rows*m.cols==0)
+	{
+		printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str());
+		return 0;
+	}
+	if(m.type() != CV_8UC3)
+	{
+		printf("cv::imread did something strange! this may segfault. \n");
+		return 0;
+	}
+	MinimalImageB3* img = new MinimalImageB3(m.cols, m.rows);
+	memcpy(img->data, m.data, 3*m.rows*m.cols);
+	return img;
+}
+
+MinimalImage<unsigned short>* readImageBW_16U(std::string filename)
+{
+	cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_UNCHANGED);
+	if(m.rows*m.cols==0)
+	{
+		printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str());
+		return 0;
+	}
+	if(m.type() != CV_16U)
+	{
+		printf("readImageBW_16U called on image that is not a 16bit grayscale image. this may segfault. \n");
+		return 0;
+	}
+	MinimalImage<unsigned short>* img = new MinimalImage<unsigned short>(m.cols, m.rows);
+	memcpy(img->data, m.data, 2*m.rows*m.cols);
+	return img;
+}
+
+MinimalImageB* readStreamBW_8U(char* data, int numBytes)
+{
+	cv::Mat m = cv::imdecode(cv::Mat(numBytes,1,CV_8U, data), CV_LOAD_IMAGE_GRAYSCALE);
+	if(m.rows*m.cols==0)
+	{
+		printf("cv::imdecode could not read stream (%d bytes)! this may segfault. \n", numBytes);
+		return 0;
+	}
+	if(m.type() != CV_8U)
+	{
+		printf("cv::imdecode did something strange! this may segfault. \n");
+		return 0;
+	}
+	MinimalImageB* img = new MinimalImageB(m.cols, m.rows);
+	memcpy(img->data, m.data, m.rows*m.cols);
+	return img;
+}
+
+
+
+void writeImage(std::string filename, MinimalImageB* img)
+{
+	cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8U, img->data));
+}
+void writeImage(std::string filename, MinimalImageB3* img)
+{
+	cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8UC3, img->data));
+}
+void writeImage(std::string filename, MinimalImageF* img)
+{
+	cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32F, img->data));
+}
+void writeImage(std::string filename, MinimalImageF3* img)
+{
+	cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32FC3, img->data));
+}
+
+}
+
+}

src/IOWrapper/Output3DWrapper.h

@@ -0,0 +1,209 @@
+/**
+* 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/>.
+*/
+
+
+
+#pragma once
+#include <vector>
+#include <string>
+
+#include "util/NumType.h"
+#include "util/MinimalImage.h"
+#include "map"
+
+namespace cv {
+        class Mat;
+}
+
+namespace dso
+{
+
+class FrameHessian;
+class CalibHessian;
+class FrameShell;
+
+namespace IOWrap
+{
+
+/* ======================= Some typical usecases: ===============
+ *
+ * (1) always get the pose of the most recent frame:
+ *     -> Implement [publishCamPose].
+ *
+ * (2) always get the depthmap of the most recent keyframe
+ *     -> Implement [pushDepthImageFloat] (use inverse depth in [image], and pose / frame information from [KF]).
+ *
+ * (3) accumulate final model
+ *     -> Implement [publishKeyframes] (skip for final!=false), and accumulate frames.
+ *
+ * (4) get evolving model in real-time
+ *     -> Implement [publishKeyframes] (update all frames for final==false).
+ *
+ *
+ *
+ *
+ * ==================== How to use the structs: ===================
+ * [FrameShell]: minimal struct kept for each frame ever tracked.
+ *      ->camToWorld = camera to world transformation
+ *      ->poseValid = false if [camToWorld] is invalid (only happens for frames during initialization).
+ *      ->trackingRef = Shell of the frame this frame was tracked on.
+ *      ->id = ID of that frame, starting with 0 for the very first frame.
+ *
+ *      ->incoming_id = ID passed into [addActiveFrame( ImageAndExposure* image, int id )].
+ *	->timestamp = timestamp passed into [addActiveFrame( ImageAndExposure* image, int id )] as image.timestamp.
+ *
+ * [FrameHessian]
+ *      ->immaturePoints: contains points that have not been "activated" (they do however have a depth initialization).
+ *      ->pointHessians: contains active points.
+ *      ->pointHessiansMarginalized: contains marginalized points.
+ *      ->pointHessiansOut: contains outlier points.
+ *
+ *      ->frameID: incremental ID for keyframes only.
+ *      ->shell: corresponding [FrameShell] struct.
+ *
+ *
+ * [CalibHessian]
+ *      ->fxl(), fyl(), cxl(), cyl(): get optimized, most recent (pinhole) camera intrinsics.
+ *
+ *
+ * [PointHessian]
+ * 	->u,v: pixel-coordinates of point.
+ *      ->idepth_scaled: inverse depth of point.
+ *                       DO NOT USE [idepth], since it may be scaled with [SCALE_IDEPTH] ... however that is currently set to 1 so never mind.
+ *      ->host: pointer to host-frame of point.
+ *      ->status: current status of point (ACTIVE=0, INACTIVE, OUTLIER, OOB, MARGINALIZED)
+ *      ->numGoodResiduals: number of non-outlier residuals supporting this point (approximate).
+ *      ->maxRelBaseline: value roughly proportional to the relative baseline this point was observed with (0 = no baseline).
+ *                        points for which this value is low are badly contrained.
+ *      ->idepth_hessian: hessian value (inverse variance) of inverse depth.
+ *
+ * [ImmaturePoint]
+ * 	->u,v: pixel-coordinates of point.
+ *      ->idepth_min, idepth_max: the initialization sais that the inverse depth of this point is very likely
+ *        between these two thresholds (their mean being the best guess)
+ *      ->host: pointer to host-frame of point.
+ */
+
+
+
+
+
+
+
+class Output3DWrapper
+{
+public:
+        Output3DWrapper() {}
+        virtual ~Output3DWrapper() {}
+
+
+        /*  Usage:
+         *  Called once after each new Keyframe is inserted & optimized.
+         *  [connectivity] contains for each frame-frame pair the number of [0] active residuals in between them,
+         *  and [1] the number of marginalized reisduals between them.
+         *  frame-frame pairs are encoded as HASH_IDX = [(int)hostFrameKFID << 32 + (int)targetFrameKFID].
+         *  the [***frameKFID] used for hashing correspond to the [FrameHessian]->frameID.
+         *
+         *  Calling:
+         *  Always called, no overhead if not used.
+         */
+        virtual void publishGraph(const std::map<uint64_t,Eigen::Vector2i, std::less<uint64_t>, Eigen::aligned_allocator<std::pair<const uint64_t, Eigen::Vector2i> > > &connectivity) {}
+
+
+
+
+
+        /* Usage:
+         * Called after each new Keyframe is inserted & optimized, with all keyframes that were part of the active window during
+         * that optimization in [frames] (with final=false). Use to access the new frame pose and points.
+         * Also called just before a frame is marginalized (with final=true) with only that frame in [frames]; at that point,
+         * no further updates will ever occur to it's optimized values (pose & idepth values of it's points).
+         *
+         * If you want always all most recent values for all frames, just use the [final=false] calls.
+         * If you only want to get the final model, but don't care about it being delay-free, only use the
+         * [final=true] calls, to save compute.
+         *
+         * Calling:
+         * Always called, negligible overhead if not used.
+         */
+        virtual void publishKeyframes(std::vector<FrameHessian*> &frames, bool final, CalibHessian* HCalib) {}
+
+
+
+
+
+        /* Usage:
+         * Called once for each tracked frame, with the real-time, low-delay frame pose.
+         *
+         * Calling:
+         * Always called, no overhead if not used.
+         */
+        virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) {}
+
+
+
+
+
+        /* Usage:
+         * Called once for each new frame, before it is tracked (i.e., it doesn't have a pose yet).
+         *
+         * Calling:
+         * Always called, no overhead if not used.
+         */
+        virtual void pushLiveFrame(FrameHessian* image) {}
+
+
+
+
+        /* called once after a new keyframe is created, with the color-coded, forward-warped inverse depthmap for that keyframe,
+         * which is used for initial alignment of future frames. Meant for visualization.
+         *
+         * Calling:
+         * Needs to prepare the depth image, so it is only called if [needPushDepthImage()] returned true.
+         */
+        virtual void pushDepthImage(MinimalImageB3* image) {}
+        virtual bool needPushDepthImage() {return false;}
+
+
+
+        /* Usage:
+         * called once after a new keyframe is created, with the forward-warped inverse depthmap for that keyframe.
+         * (<= 0 for pixels without inv. depth value, >0 for pixels with inv. depth value)
+         *
+         * Calling:
+         * Always called, almost no overhead if not used.
+         */
+        virtual void pushDepthImageFloat(MinimalImageF* image, FrameHessian* KF ) {}
+
+
+
+        /* call on finish */
+        virtual void join() {}
+
+        /* call on reset */
+        virtual void reset() {}
+
+};
+
+}
+}

src/IOWrapper/OutputWrapper/SampleOutputWrapper.h

@@ -0,0 +1,158 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+#include "boost/thread.hpp"
+#include "util/MinimalImage.h"
+#include "IOWrapper/Output3DWrapper.h"
+
+
+
+#include "FullSystem/HessianBlocks.h"
+#include "util/FrameShell.h"
+
+namespace dso
+{
+
+class FrameHessian;
+class CalibHessian;
+class FrameShell;
+
+
+namespace IOWrap
+{
+
+class SampleOutputWrapper : public Output3DWrapper
+{
+public:
+        inline SampleOutputWrapper()
+        {
+            printf("OUT: Created SampleOutputWrapper\n");
+        }
+
+        virtual ~SampleOutputWrapper()
+        {
+            printf("OUT: Destroyed SampleOutputWrapper\n");
+        }
+
+        virtual void publishGraph(const std::map<uint64_t, Eigen::Vector2i, std::less<uint64_t>, Eigen::aligned_allocator<std::pair<const uint64_t, Eigen::Vector2i>>> &connectivity) override
+        {
+            printf("OUT: got graph with %d edges\n", (int)connectivity.size());
+
+            int maxWrite = 5;
+
+            for(const std::pair<uint64_t,Eigen::Vector2i> &p : connectivity)
+            {
+                int idHost = p.first>>32;
+                int idTarget = p.first & ((uint64_t)0xFFFFFFFF);
+                printf("OUT: Example Edge %d -> %d has %d active and %d marg residuals\n", idHost, idTarget, p.second[0], p.second[1]);
+                maxWrite--;
+                if(maxWrite==0) break;
+            }
+        }
+
+
+
+        virtual void publishKeyframes( std::vector<FrameHessian*> &frames, bool final, CalibHessian* HCalib) override
+        {
+            for(FrameHessian* f : frames)
+            {
+                printf("OUT: KF %d (%s) (id %d, tme %f): %d active, %d marginalized, %d immature points. CameraToWorld:\n",
+                       f->frameID,
+                       final ? "final" : "non-final",
+                       f->shell->incoming_id,
+                       f->shell->timestamp,
+                       (int)f->pointHessians.size(), (int)f->pointHessiansMarginalized.size(), (int)f->immaturePoints.size());
+                std::cout << f->shell->camToWorld.matrix3x4() << "\n";
+
+
+                int maxWrite = 5;
+                for(PointHessian* p : f->pointHessians)
+                {
+                    printf("OUT: Example Point x=%.1f, y=%.1f, idepth=%f, idepth std.dev. %f, %d inlier-residuals\n",
+                           p->u, p->v, p->idepth_scaled, sqrt(1.0f / p->idepth_hessian), p->numGoodResiduals );
+                    maxWrite--;
+                    if(maxWrite==0) break;
+                }
+            }
+        }
+
+        virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) override
+        {
+            printf("OUT: Current Frame %d (time %f, internal ID %d). CameraToWorld:\n",
+                   frame->incoming_id,
+                   frame->timestamp,
+                   frame->id);
+            std::cout << frame->camToWorld.matrix3x4() << "\n";
+        }
+
+
+        virtual void pushLiveFrame(FrameHessian* image) override
+        {
+            // can be used to get the raw image / intensity pyramid.
+        }
+
+        virtual void pushDepthImage(MinimalImageB3* image) override
+        {
+            // can be used to get the raw image with depth overlay.
+        }
+        virtual bool needPushDepthImage() override
+        {
+            return false;
+        }
+
+        virtual void pushDepthImageFloat(MinimalImageF* image, FrameHessian* KF ) override
+        {
+            printf("OUT: Predicted depth for KF %d (id %d, time %f, internal frame-ID %d). CameraToWorld:\n",
+                   KF->frameID,
+                   KF->shell->incoming_id,
+                   KF->shell->timestamp,
+                   KF->shell->id);
+            std::cout << KF->shell->camToWorld.matrix3x4() << "\n";
+
+            int maxWrite = 5;
+            for(int y=0;y<image->h;y++)
+            {
+                for(int x=0;x<image->w;x++)
+                {
+                    if(image->at(x,y) <= 0) continue;
+
+                    printf("OUT: Example Idepth at pixel (%d,%d): %f.\n", x,y,image->at(x,y));
+                    maxWrite--;
+                    if(maxWrite==0) break;
+                }
+                if(maxWrite==0) break;
+            }
+        }
+
+
+};
+
+
+
+}
+
+
+
+}

src/IOWrapper/Pangolin/KeyFrameDisplay.cpp

@@ -0,0 +1,403 @@
+/**
+* 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 <stdio.h>
+#include "util/settings.h"
+
+//#include <GL/glx.h>
+//#include <GL/gl.h>
+//#include <GL/glu.h>
+
+#include <pangolin/pangolin.h>
+#include "KeyFrameDisplay.h"
+#include "FullSystem/HessianBlocks.h"
+#include "FullSystem/ImmaturePoint.h"
+#include "util/FrameShell.h"
+
+
+
+namespace dso
+{
+namespace IOWrap
+{
+
+
+KeyFrameDisplay::KeyFrameDisplay()
+{
+	originalInputSparse = 0;
+	numSparseBufferSize=0;
+	numSparsePoints=0;
+
+	id = 0;
+	active= true;
+	camToWorld = SE3();
+
+	needRefresh=true;
+
+	my_scaledTH =1e10;
+	my_absTH = 1e10;
+	my_displayMode = 1;
+	my_minRelBS = 0;
+	my_sparsifyFactor = 1;
+
+	numGLBufferPoints=0;
+	bufferValid = false;
+}
+void KeyFrameDisplay::setFromF(FrameShell* frame, CalibHessian* HCalib)
+{
+	id = frame->id;
+	fx = HCalib->fxl();
+	fy = HCalib->fyl();
+	cx = HCalib->cxl();
+	cy = HCalib->cyl();
+	width = wG[0];
+	height = hG[0];
+	fxi = 1/fx;
+	fyi = 1/fy;
+	cxi = -cx / fx;
+	cyi = -cy / fy;
+	camToWorld = frame->camToWorld;
+	needRefresh=true;
+}
+
+void KeyFrameDisplay::setFromKF(FrameHessian* fh, CalibHessian* HCalib)
+{
+	setFromF(fh->shell, HCalib);
+
+	// add all traces, inlier and outlier points.
+	int npoints = 	fh->immaturePoints.size() +
+					fh->pointHessians.size() +
+					fh->pointHessiansMarginalized.size() +
+					fh->pointHessiansOut.size();
+
+	if(numSparseBufferSize < npoints)
+	{
+		if(originalInputSparse != 0) delete originalInputSparse;
+		numSparseBufferSize = npoints+100;
+        originalInputSparse = new InputPointSparse<MAX_RES_PER_POINT>[numSparseBufferSize];
+	}
+
+    InputPointSparse<MAX_RES_PER_POINT>* pc = originalInputSparse;
+	numSparsePoints=0;
+	for(ImmaturePoint* p : fh->immaturePoints)
+	{
+		for(int i=0;i<patternNum;i++)
+			pc[numSparsePoints].color[i] = p->color[i];
+
+		pc[numSparsePoints].u = p->u;
+		pc[numSparsePoints].v = p->v;
+		pc[numSparsePoints].idpeth = (p->idepth_max+p->idepth_min)*0.5f;
+		pc[numSparsePoints].idepth_hessian = 1000;
+		pc[numSparsePoints].relObsBaseline = 0;
+		pc[numSparsePoints].numGoodRes = 1;
+		pc[numSparsePoints].status = 0;
+		numSparsePoints++;
+	}
+
+	for(PointHessian* p : fh->pointHessians)
+	{
+		for(int i=0;i<patternNum;i++)
+			pc[numSparsePoints].color[i] = p->color[i];
+		pc[numSparsePoints].u = p->u;
+		pc[numSparsePoints].v = p->v;
+		pc[numSparsePoints].idpeth = p->idepth_scaled;
+		pc[numSparsePoints].relObsBaseline = p->maxRelBaseline;
+		pc[numSparsePoints].idepth_hessian = p->idepth_hessian;
+		pc[numSparsePoints].numGoodRes =  0;
+		pc[numSparsePoints].status=1;
+
+		numSparsePoints++;
+	}
+
+	for(PointHessian* p : fh->pointHessiansMarginalized)
+	{
+		for(int i=0;i<patternNum;i++)
+			pc[numSparsePoints].color[i] = p->color[i];
+		pc[numSparsePoints].u = p->u;
+		pc[numSparsePoints].v = p->v;
+		pc[numSparsePoints].idpeth = p->idepth_scaled;
+		pc[numSparsePoints].relObsBaseline = p->maxRelBaseline;
+		pc[numSparsePoints].idepth_hessian = p->idepth_hessian;
+		pc[numSparsePoints].numGoodRes =  0;
+		pc[numSparsePoints].status=2;
+		numSparsePoints++;
+	}
+
+	for(PointHessian* p : fh->pointHessiansOut)
+	{
+		for(int i=0;i<patternNum;i++)
+			pc[numSparsePoints].color[i] = p->color[i];
+		pc[numSparsePoints].u = p->u;
+		pc[numSparsePoints].v = p->v;
+		pc[numSparsePoints].idpeth = p->idepth_scaled;
+		pc[numSparsePoints].relObsBaseline = p->maxRelBaseline;
+		pc[numSparsePoints].idepth_hessian = p->idepth_hessian;
+		pc[numSparsePoints].numGoodRes =  0;
+		pc[numSparsePoints].status=3;
+		numSparsePoints++;
+	}
+	assert(numSparsePoints <= npoints);
+
+	camToWorld = fh->PRE_camToWorld;
+	needRefresh=true;
+}
+
+
+KeyFrameDisplay::~KeyFrameDisplay()
+{
+	if(originalInputSparse != 0)
+		delete[] originalInputSparse;
+}
+
+bool KeyFrameDisplay::refreshPC(bool canRefresh, float scaledTH, float absTH, int mode, float minBS, int sparsity)
+{
+	if(canRefresh)
+	{
+		needRefresh = needRefresh ||
+				my_scaledTH != scaledTH ||
+				my_absTH != absTH ||
+				my_displayMode != mode ||
+				my_minRelBS != minBS ||
+				my_sparsifyFactor != sparsity;
+	}
+
+	if(!needRefresh) return false;
+	needRefresh=false;
+
+	my_scaledTH = scaledTH;
+	my_absTH = absTH;
+	my_displayMode = mode;
+	my_minRelBS = minBS;
+	my_sparsifyFactor = sparsity;
+
+
+	// if there are no vertices, done!
+	if(numSparsePoints == 0)
+		return false;
+
+	// make data
+	Vec3f* tmpVertexBuffer = new Vec3f[numSparsePoints*patternNum];
+	Vec3b* tmpColorBuffer = new Vec3b[numSparsePoints*patternNum];
+	int vertexBufferNumPoints=0;
+
+	for(int i=0;i<numSparsePoints;i++)
+	{
+		/* display modes:
+		 * my_displayMode==0 - all pts, color-coded
+		 * my_displayMode==1 - normal points
+		 * my_displayMode==2 - active only
+		 * my_displayMode==3 - nothing
+		 */
+
+		if(my_displayMode==1 && originalInputSparse[i].status != 1 && originalInputSparse[i].status!= 2) continue;
+		if(my_displayMode==2 && originalInputSparse[i].status != 1) continue;
+		if(my_displayMode>2) continue;
+
+		if(originalInputSparse[i].idpeth < 0) continue;
+
+
+		float depth = 1.0f / originalInputSparse[i].idpeth;
+		float depth4 = depth*depth; depth4*= depth4;
+		float var = (1.0f / (originalInputSparse[i].idepth_hessian+0.01));
+
+		if(var * depth4 > my_scaledTH)
+			continue;
+
+		if(var > my_absTH)
+			continue;
+
+		if(originalInputSparse[i].relObsBaseline < my_minRelBS)
+			continue;
+
+
+		for(int pnt=0;pnt<patternNum;pnt++)
+		{
+
+			if(my_sparsifyFactor > 1 && rand()%my_sparsifyFactor != 0) continue;
+			int dx = patternP[pnt][0];
+			int dy = patternP[pnt][1];
+
+			tmpVertexBuffer[vertexBufferNumPoints][0] = ((originalInputSparse[i].u+dx)*fxi + cxi) * depth;
+			tmpVertexBuffer[vertexBufferNumPoints][1] = ((originalInputSparse[i].v+dy)*fyi + cyi) * depth;
+			tmpVertexBuffer[vertexBufferNumPoints][2] = depth*(1 + 2*fxi * (rand()/(float)RAND_MAX-0.5f));
+
+
+
+			if(my_displayMode==0)
+			{
+				if(originalInputSparse[i].status==0)
+				{
+					tmpColorBuffer[vertexBufferNumPoints][0] = 0;
+					tmpColorBuffer[vertexBufferNumPoints][1] = 255;
+					tmpColorBuffer[vertexBufferNumPoints][2] = 255;
+				}
+				else if(originalInputSparse[i].status==1)
+				{
+					tmpColorBuffer[vertexBufferNumPoints][0] = 0;
+					tmpColorBuffer[vertexBufferNumPoints][1] = 255;
+					tmpColorBuffer[vertexBufferNumPoints][2] = 0;
+				}
+				else if(originalInputSparse[i].status==2)
+				{
+					tmpColorBuffer[vertexBufferNumPoints][0] = 0;
+					tmpColorBuffer[vertexBufferNumPoints][1] = 0;
+					tmpColorBuffer[vertexBufferNumPoints][2] = 255;
+				}
+				else if(originalInputSparse[i].status==3)
+				{
+					tmpColorBuffer[vertexBufferNumPoints][0] = 255;
+					tmpColorBuffer[vertexBufferNumPoints][1] = 0;
+					tmpColorBuffer[vertexBufferNumPoints][2] = 0;
+				}
+				else
+				{
+					tmpColorBuffer[vertexBufferNumPoints][0] = 255;
+					tmpColorBuffer[vertexBufferNumPoints][1] = 255;
+					tmpColorBuffer[vertexBufferNumPoints][2] = 255;
+				}
+
+			}
+			else
+			{
+				tmpColorBuffer[vertexBufferNumPoints][0] = originalInputSparse[i].color[pnt];
+				tmpColorBuffer[vertexBufferNumPoints][1] = originalInputSparse[i].color[pnt];
+				tmpColorBuffer[vertexBufferNumPoints][2] = originalInputSparse[i].color[pnt];
+			}
+			vertexBufferNumPoints++;
+
+
+			assert(vertexBufferNumPoints <= numSparsePoints*patternNum);
+		}
+	}
+
+	if(vertexBufferNumPoints==0)
+	{
+		delete[] tmpColorBuffer;
+		delete[] tmpVertexBuffer;
+		return true;
+	}
+
+	numGLBufferGoodPoints = vertexBufferNumPoints;
+	if(numGLBufferGoodPoints > numGLBufferPoints)
+	{
+		numGLBufferPoints = vertexBufferNumPoints*1.3;
+		vertexBuffer.Reinitialise(pangolin::GlArrayBuffer, numGLBufferPoints, GL_FLOAT, 3, GL_DYNAMIC_DRAW );
+		colorBuffer.Reinitialise(pangolin::GlArrayBuffer, numGLBufferPoints, GL_UNSIGNED_BYTE, 3, GL_DYNAMIC_DRAW );
+	}
+	vertexBuffer.Upload(tmpVertexBuffer, sizeof(float)*3*numGLBufferGoodPoints, 0);
+	colorBuffer.Upload(tmpColorBuffer, sizeof(unsigned char)*3*numGLBufferGoodPoints, 0);
+	bufferValid=true;
+	delete[] tmpColorBuffer;
+	delete[] tmpVertexBuffer;
+
+
+	return true;
+}
+
+
+
+void KeyFrameDisplay::drawCam(float lineWidth, float* color, float sizeFactor)
+{
+	if(width == 0)
+		return;
+
+	float sz=sizeFactor;
+
+	glPushMatrix();
+
+		Sophus::Matrix4f m = camToWorld.matrix().cast<float>();
+		glMultMatrixf((GLfloat*)m.data());
+
+		if(color == 0)
+		{
+			glColor3f(1,0,0);
+		}
+		else
+			glColor3f(color[0],color[1],color[2]);
+
+		glLineWidth(lineWidth);
+		glBegin(GL_LINES);
+		glVertex3f(0,0,0);
+		glVertex3f(sz*(0-cx)/fx,sz*(0-cy)/fy,sz);
+		glVertex3f(0,0,0);
+		glVertex3f(sz*(0-cx)/fx,sz*(height-1-cy)/fy,sz);
+		glVertex3f(0,0,0);
+		glVertex3f(sz*(width-1-cx)/fx,sz*(height-1-cy)/fy,sz);
+		glVertex3f(0,0,0);
+		glVertex3f(sz*(width-1-cx)/fx,sz*(0-cy)/fy,sz);
+
+		glVertex3f(sz*(width-1-cx)/fx,sz*(0-cy)/fy,sz);
+		glVertex3f(sz*(width-1-cx)/fx,sz*(height-1-cy)/fy,sz);
+
+		glVertex3f(sz*(width-1-cx)/fx,sz*(height-1-cy)/fy,sz);
+		glVertex3f(sz*(0-cx)/fx,sz*(height-1-cy)/fy,sz);
+
+		glVertex3f(sz*(0-cx)/fx,sz*(height-1-cy)/fy,sz);
+		glVertex3f(sz*(0-cx)/fx,sz*(0-cy)/fy,sz);
+
+		glVertex3f(sz*(0-cx)/fx,sz*(0-cy)/fy,sz);
+		glVertex3f(sz*(width-1-cx)/fx,sz*(0-cy)/fy,sz);
+
+		glEnd();
+	glPopMatrix();
+}
+
+
+void KeyFrameDisplay::drawPC(float pointSize)
+{
+
+	if(!bufferValid || numGLBufferGoodPoints==0)
+		return;
+
+
+	glDisable(GL_LIGHTING);
+
+	glPushMatrix();
+
+		Sophus::Matrix4f m = camToWorld.matrix().cast<float>();
+		glMultMatrixf((GLfloat*)m.data());
+
+		glPointSize(pointSize);
+
+
+		colorBuffer.Bind();
+		glColorPointer(colorBuffer.count_per_element, colorBuffer.datatype, 0, 0);
+		glEnableClientState(GL_COLOR_ARRAY);
+
+		vertexBuffer.Bind();
+		glVertexPointer(vertexBuffer.count_per_element, vertexBuffer.datatype, 0, 0);
+		glEnableClientState(GL_VERTEX_ARRAY);
+		glDrawArrays(GL_POINTS, 0, numGLBufferGoodPoints);
+		glDisableClientState(GL_VERTEX_ARRAY);
+		vertexBuffer.Unbind();
+
+		glDisableClientState(GL_COLOR_ARRAY);
+		colorBuffer.Unbind();
+
+	glPopMatrix();
+}
+
+}
+}

src/IOWrapper/Pangolin/KeyFrameDisplay.h

@@ -0,0 +1,123 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+#undef Success
+#include <Eigen/Core>
+#include "util/NumType.h"
+#include <pangolin/pangolin.h>
+
+#include <sstream>
+#include <fstream>
+
+namespace dso
+{
+class CalibHessian;
+class FrameHessian;
+class FrameShell;
+
+namespace IOWrap
+{
+
+template<int ppp>
+struct InputPointSparse
+{
+	float u;
+	float v;
+	float idpeth;
+	float idepth_hessian;
+	float relObsBaseline;
+	int numGoodRes;
+	unsigned char color[ppp];
+	unsigned char status;
+};
+
+struct MyVertex
+{
+	float point[3];
+	unsigned char color[4];
+};
+
+// stores a pointcloud associated to a Keyframe.
+class KeyFrameDisplay
+{
+
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	KeyFrameDisplay();
+	~KeyFrameDisplay();
+
+	// copies points from KF over to internal buffer,
+	// keeping some additional information so we can render it differently.
+	void setFromKF(FrameHessian* fh, CalibHessian* HCalib);
+
+	// copies points from KF over to internal buffer,
+	// keeping some additional information so we can render it differently.
+	void setFromF(FrameShell* fs, CalibHessian* HCalib);
+
+	// copies & filters internal data to GL buffer for rendering. if nothing to do: does nothing.
+	bool refreshPC(bool canRefresh, float scaledTH, float absTH, int mode, float minBS, int sparsity);
+
+	// renders cam & pointcloud.
+	void drawCam(float lineWidth = 1, float* color = 0, float sizeFactor=1);
+	void drawPC(float pointSize);
+
+	int id;
+	bool active;
+	SE3 camToWorld;
+
+    inline bool operator < (const KeyFrameDisplay& other) const
+    {
+        return (id < other.id);
+    }
+
+
+private:
+	float fx,fy,cx,cy;
+	float fxi,fyi,cxi,cyi;
+	int width, height;
+
+	float my_scaledTH, my_absTH, my_scale;
+	int my_sparsifyFactor;
+	int my_displayMode;
+	float my_minRelBS;
+	bool needRefresh;
+
+
+	int numSparsePoints;
+	int numSparseBufferSize;
+    InputPointSparse<MAX_RES_PER_POINT>* originalInputSparse;
+
+
+	bool bufferValid;
+	int numGLBufferPoints;
+	int numGLBufferGoodPoints;
+	pangolin::GlBuffer vertexBuffer;
+	pangolin::GlBuffer colorBuffer;
+};
+
+}
+}
+

src/IOWrapper/Pangolin/PangolinDSOViewer.cpp

@@ -0,0 +1,767 @@
+/**
+* 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;
+}
+
+}
+}

src/IOWrapper/Pangolin/PangolinDSOViewer.h

@@ -0,0 +1,154 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+#include <pangolin/pangolin.h>
+#include "boost/thread.hpp"
+#include "util/MinimalImage.h"
+#include "IOWrapper/Output3DWrapper.h"
+#include <map>
+#include <deque>
+
+#include "opencv2/core/core.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+#include "opencv2/calib3d/calib3d.hpp"
+#include "opencv2/highgui/highgui.hpp"
+
+
+
+namespace dso
+{
+
+class FrameHessian;
+class CalibHessian;
+class FrameShell;
+
+
+namespace IOWrap
+{
+
+class KeyFrameDisplay;
+
+struct GraphConnection
+{
+	KeyFrameDisplay* from;
+	KeyFrameDisplay* to;
+	int fwdMarg, bwdMarg, fwdAct, bwdAct;
+};
+
+
+class PangolinDSOViewer : public Output3DWrapper
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    PangolinDSOViewer(int w, int h, bool startRunThread=true);
+	virtual ~PangolinDSOViewer();
+
+	void run();
+	void close();
+
+	void addImageToDisplay(std::string name, MinimalImageB3* image);
+	void clearAllImagesToDisplay();
+
+
+	// ==================== Output3DWrapper Functionality ======================
+    virtual void publishGraph(const std::map<uint64_t, Eigen::Vector2i, std::less<uint64_t>, Eigen::aligned_allocator<std::pair<const uint64_t, Eigen::Vector2i>>> &connectivity) override;
+    virtual void publishKeyframes( std::vector<FrameHessian*> &frames, bool final, CalibHessian* HCalib) override;
+    virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) override;
+
+
+    virtual void pushLiveFrame(FrameHessian* image) override;
+    virtual void pushDepthImage(MinimalImageB3* image) override;
+    virtual bool needPushDepthImage() override;
+
+    virtual void join() override;
+
+    virtual void reset() override;
+
+    //Append
+    FrameShell* lastPose = nullptr;
+    CalibHessian* Hcalib = nullptr;
+
+private:
+
+	bool needReset;
+	void reset_internal();
+	void drawConstraints();
+
+	boost::thread runThread;
+	bool running;
+	int w,h;
+
+
+
+	// images rendering
+	boost::mutex openImagesMutex;
+	MinimalImageB3* internalVideoImg;
+	MinimalImageB3* internalKFImg;
+	MinimalImageB3* internalResImg;
+	bool videoImgChanged, kfImgChanged, resImgChanged;
+
+
+
+	// 3D model rendering
+	boost::mutex model3DMutex;
+	KeyFrameDisplay* currentCam;
+	std::vector<KeyFrameDisplay*> keyframes;
+	std::vector<Vec3f,Eigen::aligned_allocator<Vec3f>> allFramePoses;
+	std::map<int, KeyFrameDisplay*> keyframesByKFID;
+	std::vector<GraphConnection,Eigen::aligned_allocator<GraphConnection>> connections;
+
+
+
+	// render settings
+	bool settings_showKFCameras;
+	bool settings_showCurrentCamera;
+	bool settings_showTrajectory;
+	bool settings_showFullTrajectory;
+	bool settings_showActiveConstraints;
+	bool settings_showAllConstraints;
+
+	float settings_scaledVarTH;
+	float settings_absVarTH;
+	int settings_pointCloudMode;
+	float settings_minRelBS;
+	int settings_sparsity;
+
+
+	// timings
+	struct timeval last_track;
+	struct timeval last_map;
+
+
+	std::deque<float> lastNTrackingMs;
+	std::deque<float> lastNMappingMs;
+};
+
+
+
+}
+
+
+
+}

src/OptimizationBackend/AccumulatedSCHessian.cpp

@@ -0,0 +1,221 @@
+/**
+* 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 "OptimizationBackend/AccumulatedSCHessian.h"
+#include "OptimizationBackend/EnergyFunctional.h"
+#include "OptimizationBackend/EnergyFunctionalStructs.h"
+
+#include "FullSystem/HessianBlocks.h"
+
+namespace dso
+{
+
+void AccumulatedSCHessianSSE::addPoint(EFPoint* p, bool shiftPriorToZero, int tid)
+{
+	int ngoodres = 0;
+	for(EFResidual* r : p->residualsAll) if(r->isActive()) ngoodres++;
+	if(ngoodres==0)
+	{
+		p->HdiF=0;
+		p->bdSumF=0;
+		p->data->idepth_hessian=0;
+		p->data->maxRelBaseline=0;
+		return;
+	}
+
+	float H = p->Hdd_accAF+p->Hdd_accLF+p->priorF;
+	if(H < 1e-10) H = 1e-10;
+
+	p->data->idepth_hessian=H;
+
+	p->HdiF = 1.0 / H;
+	p->bdSumF = p->bd_accAF + p->bd_accLF;
+	if(shiftPriorToZero) p->bdSumF += p->priorF*p->deltaF;
+	VecCf Hcd = p->Hcd_accAF + p->Hcd_accLF;
+	accHcc[tid].update(Hcd,Hcd,p->HdiF);
+	accbc[tid].update(Hcd, p->bdSumF * p->HdiF);
+
+	assert(std::isfinite((float)(p->HdiF)));
+
+	int nFrames2 = nframes[tid]*nframes[tid];
+	for(EFResidual* r1 : p->residualsAll)
+	{
+		if(!r1->isActive()) continue;
+		int r1ht = r1->hostIDX + r1->targetIDX*nframes[tid];
+
+		for(EFResidual* r2 : p->residualsAll)
+		{
+			if(!r2->isActive()) continue;
+
+			accD[tid][r1ht+r2->targetIDX*nFrames2].update(r1->JpJdF, r2->JpJdF, p->HdiF);
+		}
+
+		accE[tid][r1ht].update(r1->JpJdF, Hcd, p->HdiF);
+		accEB[tid][r1ht].update(r1->JpJdF,p->HdiF*p->bdSumF);
+	}
+}
+void AccumulatedSCHessianSSE::stitchDoubleInternal(
+		MatXX* H, VecX* b, EnergyFunctional const * const EF,
+		int min, int max, Vec10* stats, int tid)
+{
+	int toAggregate = NUM_THREADS;
+	if(tid == -1) { toAggregate = 1; tid = 0; }	// special case: if we dont do multithreading, dont aggregate.
+	if(min==max) return;
+
+
+	int nf = nframes[0];
+	int nframes2 = nf*nf;
+
+	for(int k=min;k<max;k++)
+	{
+		int i = k%nf;
+		int j = k/nf;
+
+		int iIdx = CPARS+i*8;
+		int jIdx = CPARS+j*8;
+		int ijIdx = i+nf*j;
+
+		Mat8C Hpc = Mat8C::Zero();
+		Vec8 bp = Vec8::Zero();
+
+		for(int tid2=0;tid2 < toAggregate;tid2++)
+		{
+			accE[tid2][ijIdx].finish();
+			accEB[tid2][ijIdx].finish();
+			Hpc += accE[tid2][ijIdx].A1m.cast<double>();
+			bp += accEB[tid2][ijIdx].A1m.cast<double>();
+		}
+
+		H[tid].block<8,CPARS>(iIdx,0) += EF->adHost[ijIdx] * Hpc;
+		H[tid].block<8,CPARS>(jIdx,0) += EF->adTarget[ijIdx] * Hpc;
+		b[tid].segment<8>(iIdx) += EF->adHost[ijIdx] * bp;
+		b[tid].segment<8>(jIdx) += EF->adTarget[ijIdx] * bp;
+
+
+
+		for(int k=0;k<nf;k++)
+		{
+			int kIdx = CPARS+k*8;
+			int ijkIdx = ijIdx + k*nframes2;
+			int ikIdx = i+nf*k;
+
+			Mat88 accDM = Mat88::Zero();
+
+			for(int tid2=0;tid2 < toAggregate;tid2++)
+			{
+				accD[tid2][ijkIdx].finish();
+				if(accD[tid2][ijkIdx].num == 0) continue;
+				accDM += accD[tid2][ijkIdx].A1m.cast<double>();
+			}
+
+			H[tid].block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose();
+			H[tid].block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose();
+			H[tid].block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose();
+			H[tid].block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose();
+		}
+	}
+
+	if(min==0)
+	{
+		for(int tid2=0;tid2 < toAggregate;tid2++)
+		{
+			accHcc[tid2].finish();
+			accbc[tid2].finish();
+			H[tid].topLeftCorner<CPARS,CPARS>() += accHcc[tid2].A1m.cast<double>();
+			b[tid].head<CPARS>() += accbc[tid2].A1m.cast<double>();
+		}
+	}
+
+
+//	// ----- new: copy transposed parts for calibration only.
+//	for(int h=0;h<nf;h++)
+//	{
+//		int hIdx = 4+h*8;
+//		H.block<4,8>(0,hIdx).noalias() = H.block<8,4>(hIdx,0).transpose();
+//	}
+}
+
+void AccumulatedSCHessianSSE::stitchDouble(MatXX &H, VecX &b, EnergyFunctional const * const EF, int tid)
+{
+
+	int nf = nframes[0];
+	int nframes2 = nf*nf;
+
+	H = MatXX::Zero(nf*8+CPARS, nf*8+CPARS);
+	b = VecX::Zero(nf*8+CPARS);
+
+
+	for(int i=0;i<nf;i++)
+		for(int j=0;j<nf;j++)
+		{
+			int iIdx = CPARS+i*8;
+			int jIdx = CPARS+j*8;
+			int ijIdx = i+nf*j;
+
+			accE[tid][ijIdx].finish();
+			accEB[tid][ijIdx].finish();
+
+			Mat8C accEM = accE[tid][ijIdx].A1m.cast<double>();
+			Vec8 accEBV = accEB[tid][ijIdx].A1m.cast<double>();
+
+			H.block<8,CPARS>(iIdx,0) += EF->adHost[ijIdx] * accEM;
+			H.block<8,CPARS>(jIdx,0) += EF->adTarget[ijIdx] * accEM;
+
+			b.segment<8>(iIdx) += EF->adHost[ijIdx] * accEBV;
+			b.segment<8>(jIdx) += EF->adTarget[ijIdx] * accEBV;
+
+			for(int k=0;k<nf;k++)
+			{
+				int kIdx = CPARS+k*8;
+				int ijkIdx = ijIdx + k*nframes2;
+				int ikIdx = i+nf*k;
+
+				accD[tid][ijkIdx].finish();
+				if(accD[tid][ijkIdx].num == 0) continue;
+				Mat88 accDM = accD[tid][ijkIdx].A1m.cast<double>();
+
+				H.block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose();
+
+				H.block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose();
+
+				H.block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose();
+
+				H.block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose();
+			}
+		}
+
+	accHcc[tid].finish();
+	accbc[tid].finish();
+	H.topLeftCorner<CPARS,CPARS>() = accHcc[tid].A1m.cast<double>();
+	b.head<CPARS>() = accbc[tid].A1m.cast<double>();
+
+	// ----- new: copy transposed parts for calibration only.
+	for(int h=0;h<nf;h++)
+	{
+		int hIdx = CPARS+h*8;
+		H.block<CPARS,8>(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose();
+	}
+}
+
+}

src/OptimizationBackend/AccumulatedSCHessian.h

@@ -0,0 +1,159 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+ 
+#include "util/NumType.h"
+#include "util/IndexThreadReduce.h"
+#include "OptimizationBackend/MatrixAccumulators.h"
+#include "vector"
+#include <math.h>
+
+namespace dso
+{
+
+class EFPoint;
+class EnergyFunctional;
+
+
+class AccumulatedSCHessianSSE
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	inline AccumulatedSCHessianSSE()
+	{
+		for(int i=0;i<NUM_THREADS;i++)
+		{
+			accE[i]=0;
+			accEB[i]=0;
+			accD[i]=0;
+			nframes[i]=0;
+		}
+	};
+	inline ~AccumulatedSCHessianSSE()
+	{
+		for(int i=0;i<NUM_THREADS;i++)
+		{
+			if(accE[i] != 0) delete[] accE[i];
+			if(accEB[i] != 0) delete[] accEB[i];
+			if(accD[i] != 0) delete[] accD[i];
+		}
+	};
+
+	inline void setZero(int n, int min=0, int max=1, Vec10* stats=0, int tid=0)
+	{
+		if(n != nframes[tid])
+		{
+			if(accE[tid] != 0) delete[] accE[tid];
+			if(accEB[tid] != 0) delete[] accEB[tid];
+			if(accD[tid] != 0) delete[] accD[tid];
+			accE[tid] = new AccumulatorXX<8,CPARS>[n*n];
+			accEB[tid] = new AccumulatorX<8>[n*n];
+			accD[tid] = new AccumulatorXX<8,8>[n*n*n];
+		}
+		accbc[tid].initialize();
+		accHcc[tid].initialize();
+
+		for(int i=0;i<n*n;i++)
+		{
+			accE[tid][i].initialize();
+			accEB[tid][i].initialize();
+
+			for(int j=0;j<n;j++)
+				accD[tid][i*n+j].initialize();
+		}
+		nframes[tid]=n;
+	}
+	void stitchDouble(MatXX &H_sc, VecX &b_sc, EnergyFunctional const * const EF, int tid=0);
+	void addPoint(EFPoint* p, bool shiftPriorToZero, int tid=0);
+
+
+	void stitchDoubleMT(IndexThreadReduce<Vec10>* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool MT)
+	{
+		// sum up, splitting by bock in square.
+		if(MT)
+		{
+			MatXX Hs[NUM_THREADS];
+			VecX bs[NUM_THREADS];
+			for(int i=0;i<NUM_THREADS;i++)
+			{
+				assert(nframes[0] == nframes[i]);
+				Hs[i] = MatXX::Zero(nframes[0]*8+CPARS, nframes[0]*8+CPARS);
+				bs[i] = VecX::Zero(nframes[0]*8+CPARS);
+			}
+
+			red->reduce(boost::bind(&AccumulatedSCHessianSSE::stitchDoubleInternal,
+				this,Hs, bs, EF,  _1, _2, _3, _4), 0, nframes[0]*nframes[0], 0);
+
+			// sum up results
+			H = Hs[0];
+			b = bs[0];
+
+			for(int i=1;i<NUM_THREADS;i++)
+			{
+				H.noalias() += Hs[i];
+				b.noalias() += bs[i];
+			}
+		}
+		else
+		{
+			H = MatXX::Zero(nframes[0]*8+CPARS, nframes[0]*8+CPARS);
+			b = VecX::Zero(nframes[0]*8+CPARS);
+			stitchDoubleInternal(&H, &b, EF,0,nframes[0]*nframes[0],0,-1);
+		}
+
+		// make diagonal by copying over parts.
+		for(int h=0;h<nframes[0];h++)
+		{
+			int hIdx = CPARS+h*8;
+			H.block<CPARS,8>(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose();
+		}
+	}
+
+
+	AccumulatorXX<8,CPARS>* accE[NUM_THREADS];
+	AccumulatorX<8>* accEB[NUM_THREADS];
+	AccumulatorXX<8,8>* accD[NUM_THREADS];
+	AccumulatorXX<CPARS,CPARS> accHcc[NUM_THREADS];
+	AccumulatorX<CPARS> accbc[NUM_THREADS];
+	int nframes[NUM_THREADS];
+
+
+	void addPointsInternal(
+			std::vector<EFPoint*>* points, bool shiftPriorToZero,
+			int min=0, int max=1, Vec10* stats=0, int tid=0)
+	{
+		for(int i=min;i<max;i++) addPoint((*points)[i],shiftPriorToZero,tid);
+	}
+
+private:
+
+	void stitchDoubleInternal(
+			MatXX* H, VecX* b, EnergyFunctional const * const EF,
+			int min, int max, Vec10* stats, int tid);
+};
+
+}
+

src/OptimizationBackend/AccumulatedTopHessian.cpp

@@ -0,0 +1,309 @@
+/**
+* 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 "OptimizationBackend/AccumulatedTopHessian.h"
+#include "OptimizationBackend/EnergyFunctional.h"
+#include "OptimizationBackend/EnergyFunctionalStructs.h"
+#include <iostream>
+
+#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__)
+#include "SSE2NEON.h"
+#endif
+
+namespace dso
+{
+
+
+
+template<int mode>
+void AccumulatedTopHessianSSE::addPoint(EFPoint* p, EnergyFunctional const * const ef, int tid)	// 0 = active, 1 = linearized, 2=marginalize
+{
+
+
+	assert(mode==0 || mode==1 || mode==2);
+
+	VecCf dc = ef->cDeltaF;
+	float dd = p->deltaF;
+
+	float bd_acc=0;
+	float Hdd_acc=0;
+	VecCf  Hcd_acc = VecCf::Zero();
+
+	for(EFResidual* r : p->residualsAll)
+	{
+		if(mode==0)
+		{
+			if(r->isLinearized || !r->isActive()) continue;
+		}
+		if(mode==1)
+		{
+			if(!r->isLinearized || !r->isActive()) continue;
+		}
+		if(mode==2)
+		{
+			if(!r->isActive()) continue;
+			assert(r->isLinearized);
+		}
+
+
+		RawResidualJacobian* rJ = r->J;
+		int htIDX = r->hostIDX + r->targetIDX*nframes[tid];
+		Mat18f dp = ef->adHTdeltaF[htIDX];
+
+
+
+		VecNRf resApprox;
+		if(mode==0)
+			resApprox = rJ->resF;
+		if(mode==2)
+			resApprox = r->res_toZeroF;
+		if(mode==1)
+		{
+			// compute Jp*delta
+			__m128 Jp_delta_x = _mm_set1_ps(rJ->Jpdxi[0].dot(dp.head<6>())+rJ->Jpdc[0].dot(dc)+rJ->Jpdd[0]*dd);
+			__m128 Jp_delta_y = _mm_set1_ps(rJ->Jpdxi[1].dot(dp.head<6>())+rJ->Jpdc[1].dot(dc)+rJ->Jpdd[1]*dd);
+			__m128 delta_a = _mm_set1_ps((float)(dp[6]));
+			__m128 delta_b = _mm_set1_ps((float)(dp[7]));
+
+			for(int i=0;i<patternNum;i+=4)
+			{
+				// PATTERN: rtz = resF - [JI*Jp Ja]*delta.
+				__m128 rtz = _mm_load_ps(((float*)&r->res_toZeroF)+i);
+				rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx))+i),Jp_delta_x));
+				rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx+1))+i),Jp_delta_y));
+				rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF))+i),delta_a));
+				rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF+1))+i),delta_b));
+				_mm_store_ps(((float*)&resApprox)+i, rtz);
+			}
+		}
+
+		// need to compute JI^T * r, and Jab^T * r. (both are 2-vectors).
+		Vec2f JI_r(0,0);
+		Vec2f Jab_r(0,0);
+		float rr=0;
+		for(int i=0;i<patternNum;i++)
+		{
+			JI_r[0] += resApprox[i] *rJ->JIdx[0][i];
+			JI_r[1] += resApprox[i] *rJ->JIdx[1][i];
+			Jab_r[0] += resApprox[i] *rJ->JabF[0][i];
+			Jab_r[1] += resApprox[i] *rJ->JabF[1][i];
+			rr += resApprox[i]*resApprox[i];
+		}
+
+
+		acc[tid][htIDX].update(
+				rJ->Jpdc[0].data(), rJ->Jpdxi[0].data(),
+				rJ->Jpdc[1].data(), rJ->Jpdxi[1].data(),
+				rJ->JIdx2(0,0),rJ->JIdx2(0,1),rJ->JIdx2(1,1));
+
+		acc[tid][htIDX].updateBotRight(
+				rJ->Jab2(0,0), rJ->Jab2(0,1), Jab_r[0],
+				rJ->Jab2(1,1), Jab_r[1],rr);
+
+		acc[tid][htIDX].updateTopRight(
+				rJ->Jpdc[0].data(), rJ->Jpdxi[0].data(),
+				rJ->Jpdc[1].data(), rJ->Jpdxi[1].data(),
+				rJ->JabJIdx(0,0), rJ->JabJIdx(0,1),
+				rJ->JabJIdx(1,0), rJ->JabJIdx(1,1),
+				JI_r[0], JI_r[1]);
+
+
+		Vec2f Ji2_Jpdd = rJ->JIdx2 * rJ->Jpdd;
+		bd_acc +=  JI_r[0]*rJ->Jpdd[0] + JI_r[1]*rJ->Jpdd[1];
+		Hdd_acc += Ji2_Jpdd.dot(rJ->Jpdd);
+		Hcd_acc += rJ->Jpdc[0]*Ji2_Jpdd[0] + rJ->Jpdc[1]*Ji2_Jpdd[1];
+
+		nres[tid]++;
+	}
+
+	if(mode==0)
+	{
+		p->Hdd_accAF = Hdd_acc;
+		p->bd_accAF = bd_acc;
+		p->Hcd_accAF = Hcd_acc;
+	}
+	if(mode==1 || mode==2)
+	{
+		p->Hdd_accLF = Hdd_acc;
+		p->bd_accLF = bd_acc;
+		p->Hcd_accLF = Hcd_acc;
+	}
+	if(mode==2)
+	{
+		p->Hcd_accAF.setZero();
+		p->Hdd_accAF = 0;
+		p->bd_accAF = 0;
+	}
+
+}
+template void AccumulatedTopHessianSSE::addPoint<0>(EFPoint* p, EnergyFunctional const * const ef, int tid);
+template void AccumulatedTopHessianSSE::addPoint<1>(EFPoint* p, EnergyFunctional const * const ef, int tid);
+template void AccumulatedTopHessianSSE::addPoint<2>(EFPoint* p, EnergyFunctional const * const ef, int tid);
+
+
+
+
+
+
+
+
+void AccumulatedTopHessianSSE::stitchDouble(MatXX &H, VecX &b, EnergyFunctional const * const EF, bool usePrior, bool useDelta, int tid)
+{
+	H = MatXX::Zero(nframes[tid]*8+CPARS, nframes[tid]*8+CPARS);
+	b = VecX::Zero(nframes[tid]*8+CPARS);
+
+
+	for(int h=0;h<nframes[tid];h++)
+		for(int t=0;t<nframes[tid];t++)
+		{
+			int hIdx = CPARS+h*8;
+			int tIdx = CPARS+t*8;
+			int aidx = h+nframes[tid]*t;
+
+
+
+			acc[tid][aidx].finish();
+			if(acc[tid][aidx].num==0) continue;
+
+			MatPCPC accH = acc[tid][aidx].H.cast<double>();
+
+
+			H.block<8,8>(hIdx, hIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adHost[aidx].transpose();
+
+			H.block<8,8>(tIdx, tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose();
+
+			H.block<8,8>(hIdx, tIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose();
+
+			H.block<8,CPARS>(hIdx,0).noalias() += EF->adHost[aidx] * accH.block<8,CPARS>(CPARS,0);
+
+			H.block<8,CPARS>(tIdx,0).noalias() += EF->adTarget[aidx] * accH.block<8,CPARS>(CPARS,0);
+
+			H.topLeftCorner<CPARS,CPARS>().noalias() += accH.block<CPARS,CPARS>(0,0);
+
+			b.segment<8>(hIdx).noalias() += EF->adHost[aidx] * accH.block<8,1>(CPARS,8+CPARS);
+
+			b.segment<8>(tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,1>(CPARS,8+CPARS);
+
+			b.head<CPARS>().noalias() += accH.block<CPARS,1>(0,8+CPARS);
+		}
+
+
+	// ----- new: copy transposed parts.
+	for(int h=0;h<nframes[tid];h++)
+	{
+		int hIdx = CPARS+h*8;
+		H.block<CPARS,8>(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose();
+
+		for(int t=h+1;t<nframes[tid];t++)
+		{
+			int tIdx = CPARS+t*8;
+			H.block<8,8>(hIdx, tIdx).noalias() += H.block<8,8>(tIdx, hIdx).transpose();
+			H.block<8,8>(tIdx, hIdx).noalias() = H.block<8,8>(hIdx, tIdx).transpose();
+		}
+	}
+
+
+	if(usePrior)
+	{
+		assert(useDelta);
+		H.diagonal().head<CPARS>() += EF->cPrior;
+		b.head<CPARS>() += EF->cPrior.cwiseProduct(EF->cDeltaF.cast<double>());
+		for(int h=0;h<nframes[tid];h++)
+		{
+            H.diagonal().segment<8>(CPARS+h*8) += EF->frames[h]->prior;
+            b.segment<8>(CPARS+h*8) += EF->frames[h]->prior.cwiseProduct(EF->frames[h]->delta_prior);
+		}
+	}
+}
+
+
+void AccumulatedTopHessianSSE::stitchDoubleInternal(
+		MatXX* H, VecX* b, EnergyFunctional const * const EF, bool usePrior,
+		int min, int max, Vec10* stats, int tid)
+{
+	int toAggregate = NUM_THREADS;
+	if(tid == -1) { toAggregate = 1; tid = 0; }	// special case: if we dont do multithreading, dont aggregate.
+	if(min==max) return;
+
+
+	for(int k=min;k<max;k++)
+	{
+		int h = k%nframes[0];
+		int t = k/nframes[0];
+
+		int hIdx = CPARS+h*8;
+		int tIdx = CPARS+t*8;
+		int aidx = h+nframes[0]*t;
+
+		assert(aidx == k);
+
+		MatPCPC accH = MatPCPC::Zero();
+
+		for(int tid2=0;tid2 < toAggregate;tid2++)
+		{
+			acc[tid2][aidx].finish();
+			if(acc[tid2][aidx].num==0) continue;
+			accH += acc[tid2][aidx].H.cast<double>();
+		}
+
+		H[tid].block<8,8>(hIdx, hIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adHost[aidx].transpose();
+
+		H[tid].block<8,8>(tIdx, tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose();
+
+		H[tid].block<8,8>(hIdx, tIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose();
+
+		H[tid].block<8,CPARS>(hIdx,0).noalias() += EF->adHost[aidx] * accH.block<8,CPARS>(CPARS,0);
+
+		H[tid].block<8,CPARS>(tIdx,0).noalias() += EF->adTarget[aidx] * accH.block<8,CPARS>(CPARS,0);
+
+		H[tid].topLeftCorner<CPARS,CPARS>().noalias() += accH.block<CPARS,CPARS>(0,0);
+
+		b[tid].segment<8>(hIdx).noalias() += EF->adHost[aidx] * accH.block<8,1>(CPARS,CPARS+8);
+
+		b[tid].segment<8>(tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,1>(CPARS,CPARS+8);
+
+		b[tid].head<CPARS>().noalias() += accH.block<CPARS,1>(0,CPARS+8);
+
+	}
+
+
+	// only do this on one thread.
+	if(min==0 && usePrior)
+	{
+		H[tid].diagonal().head<CPARS>() += EF->cPrior;
+		b[tid].head<CPARS>() += EF->cPrior.cwiseProduct(EF->cDeltaF.cast<double>());
+		for(int h=0;h<nframes[tid];h++)
+		{
+            H[tid].diagonal().segment<8>(CPARS+h*8) += EF->frames[h]->prior;
+            b[tid].segment<8>(CPARS+h*8) += EF->frames[h]->prior.cwiseProduct(EF->frames[h]->delta_prior);
+
+		}
+	}
+}
+
+
+
+}
+
+

src/OptimizationBackend/AccumulatedTopHessian.h

@@ -0,0 +1,168 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+ 
+#include "util/NumType.h"
+#include "OptimizationBackend/MatrixAccumulators.h"
+#include "vector"
+#include <math.h>
+#include "util/IndexThreadReduce.h"
+
+
+namespace dso
+{
+
+class EFPoint;
+class EnergyFunctional;
+
+
+
+class AccumulatedTopHessianSSE
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	inline AccumulatedTopHessianSSE()
+	{
+		for(int tid=0;tid < NUM_THREADS; tid++)
+		{
+			nres[tid]=0;
+			acc[tid]=0;
+			nframes[tid]=0;
+		}
+
+	};
+	inline ~AccumulatedTopHessianSSE()
+	{
+		for(int tid=0;tid < NUM_THREADS; tid++)
+		{
+			if(acc[tid] != 0) delete[] acc[tid];
+		}
+	};
+
+	inline void setZero(int nFrames, int min=0, int max=1, Vec10* stats=0, int tid=0)
+	{
+
+		if(nFrames != nframes[tid])
+		{
+			if(acc[tid] != 0) delete[] acc[tid];
+#if USE_XI_MODEL
+			acc[tid] = new Accumulator14[nFrames*nFrames];
+#else
+			acc[tid] = new AccumulatorApprox[nFrames*nFrames];
+#endif
+		}
+
+		for(int i=0;i<nFrames*nFrames;i++)
+		{ acc[tid][i].initialize(); }
+
+		nframes[tid]=nFrames;
+		nres[tid]=0;
+
+	}
+	void stitchDouble(MatXX &H, VecX &b, EnergyFunctional const * const EF, bool usePrior, bool useDelta, int tid=0);
+
+	template<int mode> void addPoint(EFPoint* p, EnergyFunctional const * const ef, int tid=0);
+
+
+
+	void stitchDoubleMT(IndexThreadReduce<Vec10>* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool usePrior, bool MT)
+	{
+		// sum up, splitting by bock in square.
+		if(MT)
+		{
+			MatXX Hs[NUM_THREADS];
+			VecX bs[NUM_THREADS];
+			for(int i=0;i<NUM_THREADS;i++)
+			{
+				assert(nframes[0] == nframes[i]);
+				Hs[i] = MatXX::Zero(nframes[0]*8+CPARS, nframes[0]*8+CPARS);
+				bs[i] = VecX::Zero(nframes[0]*8+CPARS);
+			}
+
+			red->reduce(boost::bind(&AccumulatedTopHessianSSE::stitchDoubleInternal,
+				this,Hs, bs, EF, usePrior,  _1, _2, _3, _4), 0, nframes[0]*nframes[0], 0);
+
+			// sum up results
+			H = Hs[0];
+			b = bs[0];
+
+			for(int i=1;i<NUM_THREADS;i++)
+			{
+				H.noalias() += Hs[i];
+				b.noalias() += bs[i];
+				nres[0] += nres[i];
+			}
+		}
+		else
+		{
+			H = MatXX::Zero(nframes[0]*8+CPARS, nframes[0]*8+CPARS);
+			b = VecX::Zero(nframes[0]*8+CPARS);
+			stitchDoubleInternal(&H, &b, EF, usePrior,0,nframes[0]*nframes[0],0,-1);
+		}
+
+		// make diagonal by copying over parts.
+		for(int h=0;h<nframes[0];h++)
+		{
+			int hIdx = CPARS+h*8;
+			H.block<CPARS,8>(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose();
+
+			for(int t=h+1;t<nframes[0];t++)
+			{
+				int tIdx = CPARS+t*8;
+				H.block<8,8>(hIdx, tIdx).noalias() += H.block<8,8>(tIdx, hIdx).transpose();
+				H.block<8,8>(tIdx, hIdx).noalias() = H.block<8,8>(hIdx, tIdx).transpose();
+			}
+		}
+	}
+
+
+
+
+	int nframes[NUM_THREADS];
+
+	EIGEN_ALIGN16 AccumulatorApprox* acc[NUM_THREADS];
+
+
+	int nres[NUM_THREADS];
+
+
+	template<int mode> void addPointsInternal(
+			std::vector<EFPoint*>* points, EnergyFunctional const * const ef,
+			int min=0, int max=1, Vec10* stats=0, int tid=0)
+	{
+		for(int i=min;i<max;i++) addPoint<mode>((*points)[i],ef,tid);
+	}
+
+
+
+private:
+
+	void stitchDoubleInternal(
+			MatXX* H, VecX* b, EnergyFunctional const * const EF, bool usePrior,
+			int min, int max, Vec10* stats, int tid);
+};
+}
+

src/OptimizationBackend/EnergyFunctional.cpp

@@ -0,0 +1,946 @@
+/**
+* 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 "OptimizationBackend/EnergyFunctional.h"
+#include "OptimizationBackend/EnergyFunctionalStructs.h"
+#include "FullSystem/FullSystem.h"
+#include "FullSystem/HessianBlocks.h"
+#include "FullSystem/Residuals.h"
+#include "OptimizationBackend/AccumulatedSCHessian.h"
+#include "OptimizationBackend/AccumulatedTopHessian.h"
+
+#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__)
+#include "SSE2NEON.h"
+#endif
+
+namespace dso
+{
+
+
+bool EFAdjointsValid = false;
+bool EFIndicesValid = false;
+bool EFDeltaValid = false;
+
+
+void EnergyFunctional::setAdjointsF(CalibHessian* Hcalib)
+{
+
+	if(adHost != 0) delete[] adHost;
+	if(adTarget != 0) delete[] adTarget;
+	adHost = new Mat88[nFrames*nFrames];
+	adTarget = new Mat88[nFrames*nFrames];
+
+	for(int h=0;h<nFrames;h++)
+		for(int t=0;t<nFrames;t++)
+		{
+			FrameHessian* host = frames[h]->data;
+			FrameHessian* target = frames[t]->data;
+
+			SE3 hostToTarget = target->get_worldToCam_evalPT() * host->get_worldToCam_evalPT().inverse();
+
+			Mat88 AH = Mat88::Identity();
+			Mat88 AT = Mat88::Identity();
+
+			AH.topLeftCorner<6,6>() = -hostToTarget.Adj().transpose();
+			AT.topLeftCorner<6,6>() = Mat66::Identity();
+
+
+			Vec2f affLL = AffLight::fromToVecExposure(host->ab_exposure, target->ab_exposure, host->aff_g2l_0(), target->aff_g2l_0()).cast<float>();
+			AT(6,6) = -affLL[0];
+			AH(6,6) = affLL[0];
+			AT(7,7) = -1;
+			AH(7,7) = affLL[0];
+
+			AH.block<3,8>(0,0) *= SCALE_XI_TRANS;
+			AH.block<3,8>(3,0) *= SCALE_XI_ROT;
+			AH.block<1,8>(6,0) *= SCALE_A;
+			AH.block<1,8>(7,0) *= SCALE_B;
+			AT.block<3,8>(0,0) *= SCALE_XI_TRANS;
+			AT.block<3,8>(3,0) *= SCALE_XI_ROT;
+			AT.block<1,8>(6,0) *= SCALE_A;
+			AT.block<1,8>(7,0) *= SCALE_B;
+
+			adHost[h+t*nFrames] = AH;
+			adTarget[h+t*nFrames] = AT;
+		}
+	cPrior = VecC::Constant(setting_initialCalibHessian);
+
+
+	if(adHostF != 0) delete[] adHostF;
+	if(adTargetF != 0) delete[] adTargetF;
+	adHostF = new Mat88f[nFrames*nFrames];
+	adTargetF = new Mat88f[nFrames*nFrames];
+
+	for(int h=0;h<nFrames;h++)
+		for(int t=0;t<nFrames;t++)
+		{
+			adHostF[h+t*nFrames] = adHost[h+t*nFrames].cast<float>();
+			adTargetF[h+t*nFrames] = adTarget[h+t*nFrames].cast<float>();
+		}
+
+	cPriorF = cPrior.cast<float>();
+
+
+	EFAdjointsValid = true;
+}
+
+
+
+EnergyFunctional::EnergyFunctional()
+{
+	adHost=0;
+	adTarget=0;
+
+
+	red=0;
+
+	adHostF=0;
+	adTargetF=0;
+	adHTdeltaF=0;
+
+	nFrames = nResiduals = nPoints = 0;
+
+	HM = MatXX::Zero(CPARS,CPARS);
+	bM = VecX::Zero(CPARS);
+
+
+	accSSE_top_L = new AccumulatedTopHessianSSE();
+	accSSE_top_A = new AccumulatedTopHessianSSE();
+	accSSE_bot = new AccumulatedSCHessianSSE();
+
+	resInA = resInL = resInM = 0;
+	currentLambda=0;
+}
+EnergyFunctional::~EnergyFunctional()
+{
+	for(EFFrame* f : frames)
+	{
+		for(EFPoint* p : f->points)
+		{
+			for(EFResidual* r : p->residualsAll)
+			{
+				r->data->efResidual=0;
+				delete r;
+			}
+			p->data->efPoint=0;
+			delete p;
+		}
+		f->data->efFrame=0;
+		delete f;
+	}
+
+	if(adHost != 0) delete[] adHost;
+	if(adTarget != 0) delete[] adTarget;
+
+
+	if(adHostF != 0) delete[] adHostF;
+	if(adTargetF != 0) delete[] adTargetF;
+	if(adHTdeltaF != 0) delete[] adHTdeltaF;
+
+
+
+	delete accSSE_top_L;
+	delete accSSE_top_A;
+	delete accSSE_bot;
+}
+
+
+
+
+void EnergyFunctional::setDeltaF(CalibHessian* HCalib)
+{
+	if(adHTdeltaF != 0) delete[] adHTdeltaF;
+	adHTdeltaF = new Mat18f[nFrames*nFrames];
+	for(int h=0;h<nFrames;h++)
+		for(int t=0;t<nFrames;t++)
+		{
+			int idx = h+t*nFrames;
+			adHTdeltaF[idx] = frames[h]->data->get_state_minus_stateZero().head<8>().cast<float>().transpose() * adHostF[idx]
+					        +frames[t]->data->get_state_minus_stateZero().head<8>().cast<float>().transpose() * adTargetF[idx];
+		}
+
+	cDeltaF = HCalib->value_minus_value_zero.cast<float>();
+	for(EFFrame* f : frames)
+	{
+		f->delta = f->data->get_state_minus_stateZero().head<8>();
+		f->delta_prior = (f->data->get_state() - f->data->getPriorZero()).head<8>();
+
+		for(EFPoint* p : f->points)
+			p->deltaF = p->data->idepth-p->data->idepth_zero;
+	}
+
+	EFDeltaValid = true;
+}
+
+// accumulates & shifts L.
+void EnergyFunctional::accumulateAF_MT(MatXX &H, VecX &b, bool MT)
+{
+	if(MT)
+	{
+		red->reduce(boost::bind(&AccumulatedTopHessianSSE::setZero, accSSE_top_A, nFrames,  _1, _2, _3, _4), 0, 0, 0);
+		red->reduce(boost::bind(&AccumulatedTopHessianSSE::addPointsInternal<0>,
+				accSSE_top_A, &allPoints, this,  _1, _2, _3, _4), 0, allPoints.size(), 50);
+		accSSE_top_A->stitchDoubleMT(red,H,b,this,false,true);
+		resInA = accSSE_top_A->nres[0];
+	}
+	else
+	{
+		accSSE_top_A->setZero(nFrames);
+		for(EFFrame* f : frames)
+			for(EFPoint* p : f->points)
+				accSSE_top_A->addPoint<0>(p,this);
+		accSSE_top_A->stitchDoubleMT(red,H,b,this,false,false);
+		resInA = accSSE_top_A->nres[0];
+	}
+}
+
+// accumulates & shifts L.
+void EnergyFunctional::accumulateLF_MT(MatXX &H, VecX &b, bool MT)
+{
+	if(MT)
+	{
+		red->reduce(boost::bind(&AccumulatedTopHessianSSE::setZero, accSSE_top_L, nFrames,  _1, _2, _3, _4), 0, 0, 0);
+		red->reduce(boost::bind(&AccumulatedTopHessianSSE::addPointsInternal<1>,
+				accSSE_top_L, &allPoints, this,  _1, _2, _3, _4), 0, allPoints.size(), 50);
+		accSSE_top_L->stitchDoubleMT(red,H,b,this,true,true);
+		resInL = accSSE_top_L->nres[0];
+	}
+	else
+	{
+		accSSE_top_L->setZero(nFrames);
+		for(EFFrame* f : frames)
+			for(EFPoint* p : f->points)
+				accSSE_top_L->addPoint<1>(p,this);
+		accSSE_top_L->stitchDoubleMT(red,H,b,this,true,false);
+		resInL = accSSE_top_L->nres[0];
+	}
+}
+
+
+
+
+
+void EnergyFunctional::accumulateSCF_MT(MatXX &H, VecX &b, bool MT)
+{
+	if(MT)
+	{
+		red->reduce(boost::bind(&AccumulatedSCHessianSSE::setZero, accSSE_bot, nFrames,  _1, _2, _3, _4), 0, 0, 0);
+		red->reduce(boost::bind(&AccumulatedSCHessianSSE::addPointsInternal,
+				accSSE_bot, &allPoints, true,  _1, _2, _3, _4), 0, allPoints.size(), 50);
+		accSSE_bot->stitchDoubleMT(red,H,b,this,true);
+	}
+	else
+	{
+		accSSE_bot->setZero(nFrames);
+		for(EFFrame* f : frames)
+			for(EFPoint* p : f->points)
+				accSSE_bot->addPoint(p, true);
+		accSSE_bot->stitchDoubleMT(red, H, b,this,false);
+	}
+}
+
+void EnergyFunctional::resubstituteF_MT(VecX x, CalibHessian* HCalib, bool MT)
+{
+	assert(x.size() == CPARS+nFrames*8);
+
+	VecXf xF = x.cast<float>();
+	HCalib->step = - x.head<CPARS>();
+
+	Mat18f* xAd = new Mat18f[nFrames*nFrames];
+	VecCf cstep = xF.head<CPARS>();
+	for(EFFrame* h : frames)
+	{
+		h->data->step.head<8>() = - x.segment<8>(CPARS+8*h->idx);
+		h->data->step.tail<2>().setZero();
+
+		for(EFFrame* t : frames)
+			xAd[nFrames*h->idx + t->idx] = xF.segment<8>(CPARS+8*h->idx).transpose() *   adHostF[h->idx+nFrames*t->idx]
+			            + xF.segment<8>(CPARS+8*t->idx).transpose() * adTargetF[h->idx+nFrames*t->idx];
+	}
+
+	if(MT)
+		red->reduce(boost::bind(&EnergyFunctional::resubstituteFPt,
+						this, cstep, xAd,  _1, _2, _3, _4), 0, allPoints.size(), 50);
+	else
+		resubstituteFPt(cstep, xAd, 0, allPoints.size(), 0,0);
+
+	delete[] xAd;
+}
+
+void EnergyFunctional::resubstituteFPt(
+        const VecCf &xc, Mat18f* xAd, int min, int max, Vec10* stats, int tid)
+{
+	for(int k=min;k<max;k++)
+	{
+		EFPoint* p = allPoints[k];
+
+		int ngoodres = 0;
+		for(EFResidual* r : p->residualsAll) if(r->isActive()) ngoodres++;
+		if(ngoodres==0)
+		{
+			p->data->step = 0;
+			continue;
+		}
+		float b = p->bdSumF;
+		b -= xc.dot(p->Hcd_accAF + p->Hcd_accLF);
+
+		for(EFResidual* r : p->residualsAll)
+		{
+			if(!r->isActive()) continue;
+			b -= xAd[r->hostIDX*nFrames + r->targetIDX] * r->JpJdF;
+		}
+
+		p->data->step = - b*p->HdiF;
+		assert(std::isfinite(p->data->step));
+	}
+}
+
+
+double EnergyFunctional::calcMEnergyF()
+{
+
+	assert(EFDeltaValid);
+	assert(EFAdjointsValid);
+	assert(EFIndicesValid);
+
+	VecX delta = getStitchedDeltaF();
+	return delta.dot(2*bM + HM*delta);
+}
+
+
+void EnergyFunctional::calcLEnergyPt(int min, int max, Vec10* stats, int tid)
+{
+
+	Accumulator11 E;
+	E.initialize();
+	VecCf dc = cDeltaF;
+
+	for(int i=min;i<max;i++)
+	{
+		EFPoint* p = allPoints[i];
+		float dd = p->deltaF;
+
+		for(EFResidual* r : p->residualsAll)
+		{
+			if(!r->isLinearized || !r->isActive()) continue;
+
+			Mat18f dp = adHTdeltaF[r->hostIDX+nFrames*r->targetIDX];
+			RawResidualJacobian* rJ = r->J;
+
+
+
+			// compute Jp*delta
+			float Jp_delta_x_1 =  rJ->Jpdxi[0].dot(dp.head<6>())
+						   +rJ->Jpdc[0].dot(dc)
+						   +rJ->Jpdd[0]*dd;
+
+			float Jp_delta_y_1 =  rJ->Jpdxi[1].dot(dp.head<6>())
+						   +rJ->Jpdc[1].dot(dc)
+						   +rJ->Jpdd[1]*dd;
+
+			__m128 Jp_delta_x = _mm_set1_ps(Jp_delta_x_1);
+			__m128 Jp_delta_y = _mm_set1_ps(Jp_delta_y_1);
+			__m128 delta_a = _mm_set1_ps((float)(dp[6]));
+			__m128 delta_b = _mm_set1_ps((float)(dp[7]));
+
+			for(int i=0;i+3<patternNum;i+=4)
+			{
+				// PATTERN: E = (2*res_toZeroF + J*delta) * J*delta.
+				__m128 Jdelta =            _mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx))+i),Jp_delta_x);
+				Jdelta = _mm_add_ps(Jdelta,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx+1))+i),Jp_delta_y));
+				Jdelta = _mm_add_ps(Jdelta,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF))+i),delta_a));
+				Jdelta = _mm_add_ps(Jdelta,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF+1))+i),delta_b));
+
+				__m128 r0 = _mm_load_ps(((float*)&r->res_toZeroF)+i);
+				r0 = _mm_add_ps(r0,r0);
+				r0 = _mm_add_ps(r0,Jdelta);
+				Jdelta = _mm_mul_ps(Jdelta,r0);
+				E.updateSSENoShift(Jdelta);
+			}
+			for(int i=((patternNum>>2)<<2); i < patternNum; i++)
+			{
+				float Jdelta = rJ->JIdx[0][i]*Jp_delta_x_1 + rJ->JIdx[1][i]*Jp_delta_y_1 +
+								rJ->JabF[0][i]*dp[6] + rJ->JabF[1][i]*dp[7];
+				E.updateSingleNoShift((float)(Jdelta * (Jdelta + 2*r->res_toZeroF[i])));
+			}
+		}
+		E.updateSingle(p->deltaF*p->deltaF*p->priorF);
+	}
+	E.finish();
+	(*stats)[0] += E.A;
+}
+
+
+
+
+double EnergyFunctional::calcLEnergyF_MT()
+{
+	assert(EFDeltaValid);
+	assert(EFAdjointsValid);
+	assert(EFIndicesValid);
+
+	double E = 0;
+	for(EFFrame* f : frames)
+        E += f->delta_prior.cwiseProduct(f->prior).dot(f->delta_prior);
+
+	E += cDeltaF.cwiseProduct(cPriorF).dot(cDeltaF);
+
+	red->reduce(boost::bind(&EnergyFunctional::calcLEnergyPt,
+			this, _1, _2, _3, _4), 0, allPoints.size(), 50);
+
+	return E+red->stats[0];
+}
+
+
+
+EFResidual* EnergyFunctional::insertResidual(PointFrameResidual* r)
+{
+	EFResidual* efr = new EFResidual(r, r->point->efPoint, r->host->efFrame, r->target->efFrame);
+	efr->idxInAll = r->point->efPoint->residualsAll.size();
+	r->point->efPoint->residualsAll.push_back(efr);
+
+    connectivityMap[(((uint64_t)efr->host->frameID) << 32) + ((uint64_t)efr->target->frameID)][0]++;
+
+	nResiduals++;
+	r->efResidual = efr;
+	return efr;
+}
+EFFrame* EnergyFunctional::insertFrame(FrameHessian* fh, CalibHessian* Hcalib)
+{
+	EFFrame* eff = new EFFrame(fh);
+	eff->idx = frames.size();
+	frames.push_back(eff);
+
+	nFrames++;
+	fh->efFrame = eff;
+
+	assert(HM.cols() == 8*nFrames+CPARS-8);
+	bM.conservativeResize(8*nFrames+CPARS);
+	HM.conservativeResize(8*nFrames+CPARS,8*nFrames+CPARS);
+	bM.tail<8>().setZero();
+	HM.rightCols<8>().setZero();
+	HM.bottomRows<8>().setZero();
+
+	EFIndicesValid = false;
+	EFAdjointsValid=false;
+	EFDeltaValid=false;
+
+	setAdjointsF(Hcalib);
+	makeIDX();
+
+
+	for(EFFrame* fh2 : frames)
+	{
+        connectivityMap[(((uint64_t)eff->frameID) << 32) + ((uint64_t)fh2->frameID)] = Eigen::Vector2i(0,0);
+		if(fh2 != eff)
+            connectivityMap[(((uint64_t)fh2->frameID) << 32) + ((uint64_t)eff->frameID)] = Eigen::Vector2i(0,0);
+	}
+
+	return eff;
+}
+EFPoint* EnergyFunctional::insertPoint(PointHessian* ph)
+{
+	EFPoint* efp = new EFPoint(ph, ph->host->efFrame);
+	efp->idxInPoints = ph->host->efFrame->points.size();
+	ph->host->efFrame->points.push_back(efp);
+
+	nPoints++;
+	ph->efPoint = efp;
+
+	EFIndicesValid = false;
+
+	return efp;
+}
+
+
+void EnergyFunctional::dropResidual(EFResidual* r)
+{
+	EFPoint* p = r->point;
+	assert(r == p->residualsAll[r->idxInAll]);
+
+	p->residualsAll[r->idxInAll] = p->residualsAll.back();
+	p->residualsAll[r->idxInAll]->idxInAll = r->idxInAll;
+	p->residualsAll.pop_back();
+
+
+	if(r->isActive())
+		r->host->data->shell->statistics_goodResOnThis++;
+	else
+		r->host->data->shell->statistics_outlierResOnThis++;
+
+
+    connectivityMap[(((uint64_t)r->host->frameID) << 32) + ((uint64_t)r->target->frameID)][0]--;
+	nResiduals--;
+	r->data->efResidual=0;
+	delete r;
+}
+void EnergyFunctional::marginalizeFrame(EFFrame* fh)
+{
+
+	assert(EFDeltaValid);
+	assert(EFAdjointsValid);
+	assert(EFIndicesValid);
+
+	assert((int)fh->points.size()==0);
+	int ndim = nFrames*8+CPARS-8;// new dimension
+	int odim = nFrames*8+CPARS;// old dimension
+
+
+//	VecX eigenvaluesPre = HM.eigenvalues().real();
+//	std::sort(eigenvaluesPre.data(), eigenvaluesPre.data()+eigenvaluesPre.size());
+//
+
+
+
+	if((int)fh->idx != (int)frames.size()-1)
+	{
+		int io = fh->idx*8+CPARS;	// index of frame to move to end
+		int ntail = 8*(nFrames-fh->idx-1);
+		assert((io+8+ntail) == nFrames*8+CPARS);
+
+		Vec8 bTmp = bM.segment<8>(io);
+		VecX tailTMP = bM.tail(ntail);
+		bM.segment(io,ntail) = tailTMP;
+		bM.tail<8>() = bTmp;
+
+		MatXX HtmpCol = HM.block(0,io,odim,8);
+		MatXX rightColsTmp = HM.rightCols(ntail);
+		HM.block(0,io,odim,ntail) = rightColsTmp;
+		HM.rightCols(8) = HtmpCol;
+
+		MatXX HtmpRow = HM.block(io,0,8,odim);
+		MatXX botRowsTmp = HM.bottomRows(ntail);
+		HM.block(io,0,ntail,odim) = botRowsTmp;
+		HM.bottomRows(8) = HtmpRow;
+	}
+
+
+//	// marginalize. First add prior here, instead of to active.
+    HM.bottomRightCorner<8,8>().diagonal() += fh->prior;
+    bM.tail<8>() += fh->prior.cwiseProduct(fh->delta_prior);
+
+
+
+//	std::cout << std::setprecision(16) << "HMPre:\n" << HM << "\n\n";
+
+
+	VecX SVec = (HM.diagonal().cwiseAbs()+VecX::Constant(HM.cols(), 10)).cwiseSqrt();
+	VecX SVecI = SVec.cwiseInverse();
+
+
+//	std::cout << std::setprecision(16) << "SVec: " << SVec.transpose() << "\n\n";
+//	std::cout << std::setprecision(16) << "SVecI: " << SVecI.transpose() << "\n\n";
+
+	// scale!
+	MatXX HMScaled = SVecI.asDiagonal() * HM * SVecI.asDiagonal();
+	VecX bMScaled =  SVecI.asDiagonal() * bM;
+
+	// invert bottom part!
+	Mat88 hpi = HMScaled.bottomRightCorner<8,8>();
+	hpi = 0.5f*(hpi+hpi);
+	hpi = hpi.inverse();
+	hpi = 0.5f*(hpi+hpi);
+
+	// schur-complement!
+	MatXX bli = HMScaled.bottomLeftCorner(8,ndim).transpose() * hpi;
+	HMScaled.topLeftCorner(ndim,ndim).noalias() -= bli * HMScaled.bottomLeftCorner(8,ndim);
+	bMScaled.head(ndim).noalias() -= bli*bMScaled.tail<8>();
+
+	//unscale!
+	HMScaled = SVec.asDiagonal() * HMScaled * SVec.asDiagonal();
+	bMScaled = SVec.asDiagonal() * bMScaled;
+
+	// set.
+	HM = 0.5*(HMScaled.topLeftCorner(ndim,ndim) + HMScaled.topLeftCorner(ndim,ndim).transpose());
+	bM = bMScaled.head(ndim);
+
+	// remove from vector, without changing the order!
+	for(unsigned int i=fh->idx; i+1<frames.size();i++)
+	{
+		frames[i] = frames[i+1];
+		frames[i]->idx = i;
+	}
+	frames.pop_back();
+	nFrames--;
+	fh->data->efFrame=0;
+
+	assert((int)frames.size()*8+CPARS == (int)HM.rows());
+	assert((int)frames.size()*8+CPARS == (int)HM.cols());
+	assert((int)frames.size()*8+CPARS == (int)bM.size());
+	assert((int)frames.size() == (int)nFrames);
+
+
+
+
+//	VecX eigenvaluesPost = HM.eigenvalues().real();
+//	std::sort(eigenvaluesPost.data(), eigenvaluesPost.data()+eigenvaluesPost.size());
+
+//	std::cout << std::setprecision(16) << "HMPost:\n" << HM << "\n\n";
+
+//	std::cout << "EigPre:: " << eigenvaluesPre.transpose() << "\n";
+//	std::cout << "EigPost: " << eigenvaluesPost.transpose() << "\n";
+
+	EFIndicesValid = false;
+	EFAdjointsValid=false;
+	EFDeltaValid=false;
+
+	makeIDX();
+	delete fh;
+}
+
+
+
+
+void EnergyFunctional::marginalizePointsF()
+{
+	assert(EFDeltaValid);
+	assert(EFAdjointsValid);
+	assert(EFIndicesValid);
+
+
+	allPointsToMarg.clear();
+	for(EFFrame* f : frames)
+	{
+		for(int i=0;i<(int)f->points.size();i++)
+		{
+			EFPoint* p = f->points[i];
+			if(p->stateFlag == EFPointStatus::PS_MARGINALIZE)
+			{
+				p->priorF *= setting_idepthFixPriorMargFac;
+				for(EFResidual* r : p->residualsAll)
+					if(r->isActive())
+                        connectivityMap[(((uint64_t)r->host->frameID) << 32) + ((uint64_t)r->target->frameID)][1]++;
+				allPointsToMarg.push_back(p);
+			}
+		}
+	}
+
+	accSSE_bot->setZero(nFrames);
+	accSSE_top_A->setZero(nFrames);
+	for(EFPoint* p : allPointsToMarg)
+	{
+		accSSE_top_A->addPoint<2>(p,this);
+		accSSE_bot->addPoint(p,false);
+		removePoint(p);
+	}
+	MatXX M, Msc;
+	VecX Mb, Mbsc;
+	accSSE_top_A->stitchDouble(M,Mb,this,false,false);
+	accSSE_bot->stitchDouble(Msc,Mbsc,this);
+
+	resInM+= accSSE_top_A->nres[0];
+
+	MatXX H =  M-Msc;
+    VecX b =  Mb-Mbsc;
+
+	if(setting_solverMode & SOLVER_ORTHOGONALIZE_POINTMARG)
+	{
+		// have a look if prior is there.
+		bool haveFirstFrame = false;
+		for(EFFrame* f : frames) if(f->frameID==0) haveFirstFrame=true;
+
+		if(!haveFirstFrame)
+			orthogonalize(&b, &H);
+
+	}
+
+	HM += setting_margWeightFac*H;
+	bM += setting_margWeightFac*b;
+
+	if(setting_solverMode & SOLVER_ORTHOGONALIZE_FULL)
+		orthogonalize(&bM, &HM);
+
+	EFIndicesValid = false;
+	makeIDX();
+}
+
+void EnergyFunctional::dropPointsF()
+{
+
+
+	for(EFFrame* f : frames)
+	{
+		for(int i=0;i<(int)f->points.size();i++)
+		{
+			EFPoint* p = f->points[i];
+			if(p->stateFlag == EFPointStatus::PS_DROP)
+			{
+				removePoint(p);
+				i--;
+			}
+		}
+	}
+
+	EFIndicesValid = false;
+	makeIDX();
+}
+
+
+void EnergyFunctional::removePoint(EFPoint* p)
+{
+	for(EFResidual* r : p->residualsAll)
+		dropResidual(r);
+
+	EFFrame* h = p->host;
+	h->points[p->idxInPoints] = h->points.back();
+	h->points[p->idxInPoints]->idxInPoints = p->idxInPoints;
+	h->points.pop_back();
+
+	nPoints--;
+	p->data->efPoint = 0;
+
+	EFIndicesValid = false;
+
+	delete p;
+}
+
+void EnergyFunctional::orthogonalize(VecX* b, MatXX* H)
+{
+//	VecX eigenvaluesPre = H.eigenvalues().real();
+//	std::sort(eigenvaluesPre.data(), eigenvaluesPre.data()+eigenvaluesPre.size());
+//	std::cout << "EigPre:: " << eigenvaluesPre.transpose() << "\n";
+
+
+	// decide to which nullspaces to orthogonalize.
+	std::vector<VecX> ns;
+	ns.insert(ns.end(), lastNullspaces_pose.begin(), lastNullspaces_pose.end());
+	ns.insert(ns.end(), lastNullspaces_scale.begin(), lastNullspaces_scale.end());
+//	if(setting_affineOptModeA <= 0)
+//		ns.insert(ns.end(), lastNullspaces_affA.begin(), lastNullspaces_affA.end());
+//	if(setting_affineOptModeB <= 0)
+//		ns.insert(ns.end(), lastNullspaces_affB.begin(), lastNullspaces_affB.end());
+
+
+
+
+
+	// make Nullspaces matrix
+	MatXX N(ns[0].rows(), ns.size());
+	for(unsigned int i=0;i<ns.size();i++)
+		N.col(i) = ns[i].normalized();
+
+
+
+	// compute Npi := N * (N' * N)^-1 = pseudo inverse of N.
+	Eigen::JacobiSVD<MatXX> svdNN(N, Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+	VecX SNN = svdNN.singularValues();
+	double minSv = 1e10, maxSv = 0;
+	for(int i=0;i<SNN.size();i++)
+	{
+		if(SNN[i] < minSv) minSv = SNN[i];
+		if(SNN[i] > maxSv) maxSv = SNN[i];
+	}
+	for(int i=0;i<SNN.size();i++)
+		{ if(SNN[i] > setting_solverModeDelta*maxSv) SNN[i] = 1.0 / SNN[i]; else SNN[i] = 0; }
+
+	MatXX Npi = svdNN.matrixU() * SNN.asDiagonal() * svdNN.matrixV().transpose(); 	// [dim] x 9.
+	MatXX NNpiT = N*Npi.transpose(); 	// [dim] x [dim].
+	MatXX NNpiTS = 0.5*(NNpiT + NNpiT.transpose());	// = N * (N' * N)^-1 * N'.
+
+	if(b!=0) *b -= NNpiTS * *b;
+	if(H!=0) *H -= NNpiTS * *H * NNpiTS;
+
+
+//	std::cout << std::setprecision(16) << "Orth SV: " << SNN.reverse().transpose() << "\n";
+
+//	VecX eigenvaluesPost = H.eigenvalues().real();
+//	std::sort(eigenvaluesPost.data(), eigenvaluesPost.data()+eigenvaluesPost.size());
+//	std::cout << "EigPost:: " << eigenvaluesPost.transpose() << "\n";
+
+}
+
+
+void EnergyFunctional::solveSystemF(int iteration, double lambda, CalibHessian* HCalib)
+{
+	if(setting_solverMode & SOLVER_USE_GN) lambda=0;
+	if(setting_solverMode & SOLVER_FIX_LAMBDA) lambda = 1e-5;
+
+	assert(EFDeltaValid);
+	assert(EFAdjointsValid);
+	assert(EFIndicesValid);
+
+	MatXX HL_top, HA_top, H_sc;
+	VecX  bL_top, bA_top, bM_top, b_sc;
+
+	accumulateAF_MT(HA_top, bA_top,multiThreading);
+
+
+	accumulateLF_MT(HL_top, bL_top,multiThreading);
+
+
+
+	accumulateSCF_MT(H_sc, b_sc,multiThreading);
+
+
+
+	bM_top = (bM+ HM * getStitchedDeltaF());
+
+
+
+
+
+
+
+	MatXX HFinal_top;
+	VecX bFinal_top;
+
+	if(setting_solverMode & SOLVER_ORTHOGONALIZE_SYSTEM)
+	{
+		// have a look if prior is there.
+		bool haveFirstFrame = false;
+		for(EFFrame* f : frames) if(f->frameID==0) haveFirstFrame=true;
+
+
+
+
+		MatXX HT_act =  HL_top + HA_top - H_sc;
+		VecX bT_act =   bL_top + bA_top - b_sc;
+
+
+		if(!haveFirstFrame)
+			orthogonalize(&bT_act, &HT_act);
+
+		HFinal_top = HT_act + HM;
+		bFinal_top = bT_act + bM_top;
+
+
+
+
+
+		lastHS = HFinal_top;
+		lastbS = bFinal_top;
+
+		for(int i=0;i<8*nFrames+CPARS;i++) HFinal_top(i,i) *= (1+lambda);
+
+	}
+	else
+	{
+
+
+		HFinal_top = HL_top + HM + HA_top;
+		bFinal_top = bL_top + bM_top + bA_top - b_sc;
+
+		lastHS = HFinal_top - H_sc;
+		lastbS = bFinal_top;
+
+		for(int i=0;i<8*nFrames+CPARS;i++) HFinal_top(i,i) *= (1+lambda);
+		HFinal_top -= H_sc * (1.0f/(1+lambda));
+	}
+
+
+
+
+
+
+	VecX x;
+	if(setting_solverMode & SOLVER_SVD)
+	{
+		VecX SVecI = HFinal_top.diagonal().cwiseSqrt().cwiseInverse();
+		MatXX HFinalScaled = SVecI.asDiagonal() * HFinal_top * SVecI.asDiagonal();
+		VecX bFinalScaled  = SVecI.asDiagonal() * bFinal_top;
+		Eigen::JacobiSVD<MatXX> svd(HFinalScaled, Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+		VecX S = svd.singularValues();
+		double minSv = 1e10, maxSv = 0;
+		for(int i=0;i<S.size();i++)
+		{
+			if(S[i] < minSv) minSv = S[i];
+			if(S[i] > maxSv) maxSv = S[i];
+		}
+
+		VecX Ub = svd.matrixU().transpose()*bFinalScaled;
+		int setZero=0;
+		for(int i=0;i<Ub.size();i++)
+		{
+			if(S[i] < setting_solverModeDelta*maxSv)
+			{ Ub[i] = 0; setZero++; }
+
+			if((setting_solverMode & SOLVER_SVD_CUT7) && (i >= Ub.size()-7))
+			{ Ub[i] = 0; setZero++; }
+
+			else Ub[i] /= S[i];
+		}
+		x = SVecI.asDiagonal() * svd.matrixV() * Ub;
+
+	}
+	else
+	{
+		VecX SVecI = (HFinal_top.diagonal()+VecX::Constant(HFinal_top.cols(), 10)).cwiseSqrt().cwiseInverse();
+		MatXX HFinalScaled = SVecI.asDiagonal() * HFinal_top * SVecI.asDiagonal();
+		x = SVecI.asDiagonal() * HFinalScaled.ldlt().solve(SVecI.asDiagonal() * bFinal_top);//  SVec.asDiagonal() * svd.matrixV() * Ub;
+	}
+
+
+
+	if((setting_solverMode & SOLVER_ORTHOGONALIZE_X) || (iteration >= 2 && (setting_solverMode & SOLVER_ORTHOGONALIZE_X_LATER)))
+	{
+		VecX xOld = x;
+		orthogonalize(&x, 0);
+	}
+
+
+	lastX = x;
+
+
+	//resubstituteF(x, HCalib);
+	currentLambda= lambda;
+	resubstituteF_MT(x, HCalib,multiThreading);
+	currentLambda=0;
+
+
+}
+void EnergyFunctional::makeIDX()
+{
+	for(unsigned int idx=0;idx<frames.size();idx++)
+		frames[idx]->idx = idx;
+
+	allPoints.clear();
+
+	for(EFFrame* f : frames)
+		for(EFPoint* p : f->points)
+		{
+			allPoints.push_back(p);
+			for(EFResidual* r : p->residualsAll)
+			{
+				r->hostIDX = r->host->idx;
+				r->targetIDX = r->target->idx;
+			}
+		}
+
+
+	EFIndicesValid=true;
+}
+
+
+VecX EnergyFunctional::getStitchedDeltaF() const
+{
+	VecX d = VecX(CPARS+nFrames*8); d.head<CPARS>() = cDeltaF.cast<double>();
+	for(int h=0;h<nFrames;h++) d.segment<8>(CPARS+8*h) = frames[h]->delta;
+	return d;
+}
+
+
+
+}

src/OptimizationBackend/EnergyFunctional.h

@@ -0,0 +1,162 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+ 
+#include "util/NumType.h"
+#include "util/IndexThreadReduce.h"
+#include "vector"
+#include <math.h>
+#include "map"
+
+
+namespace dso
+{
+
+class PointFrameResidual;
+class CalibHessian;
+class FrameHessian;
+class PointHessian;
+
+
+class EFResidual;
+class EFPoint;
+class EFFrame;
+class EnergyFunctional;
+class AccumulatedTopHessian;
+class AccumulatedTopHessianSSE;
+class AccumulatedSCHessian;
+class AccumulatedSCHessianSSE;
+
+
+extern bool EFAdjointsValid;
+extern bool EFIndicesValid;
+extern bool EFDeltaValid;
+
+
+
+class EnergyFunctional {
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	friend class EFFrame;
+	friend class EFPoint;
+	friend class EFResidual;
+	friend class AccumulatedTopHessian;
+	friend class AccumulatedTopHessianSSE;
+	friend class AccumulatedSCHessian;
+	friend class AccumulatedSCHessianSSE;
+
+	EnergyFunctional();
+	~EnergyFunctional();
+
+
+	EFResidual* insertResidual(PointFrameResidual* r);
+	EFFrame* insertFrame(FrameHessian* fh, CalibHessian* Hcalib);
+	EFPoint* insertPoint(PointHessian* ph);
+
+	void dropResidual(EFResidual* r);
+	void marginalizeFrame(EFFrame* fh);
+	void removePoint(EFPoint* ph);
+
+
+
+	void marginalizePointsF();
+	void dropPointsF();
+	void solveSystemF(int iteration, double lambda, CalibHessian* HCalib);
+	double calcMEnergyF();
+	double calcLEnergyF_MT();
+
+
+	void makeIDX();
+
+	void setDeltaF(CalibHessian* HCalib);
+
+	void setAdjointsF(CalibHessian* Hcalib);
+
+	std::vector<EFFrame*> frames;
+	int nPoints, nFrames, nResiduals;
+
+	MatXX HM;
+	VecX bM;
+
+	int resInA, resInL, resInM;
+	MatXX lastHS;
+	VecX lastbS;
+	VecX lastX;
+	std::vector<VecX> lastNullspaces_forLogging;
+	std::vector<VecX> lastNullspaces_pose;
+	std::vector<VecX> lastNullspaces_scale;
+	std::vector<VecX> lastNullspaces_affA;
+	std::vector<VecX> lastNullspaces_affB;
+
+	IndexThreadReduce<Vec10>* red;
+
+
+	std::map<uint64_t,
+	  Eigen::Vector2i,
+	  std::less<uint64_t>,
+	  Eigen::aligned_allocator<std::pair<const uint64_t, Eigen::Vector2i>>
+	  > connectivityMap;
+
+private:
+
+	VecX getStitchedDeltaF() const;
+
+	void resubstituteF_MT(VecX x, CalibHessian* HCalib, bool MT);
+    void resubstituteFPt(const VecCf &xc, Mat18f* xAd, int min, int max, Vec10* stats, int tid);
+
+	void accumulateAF_MT(MatXX &H, VecX &b, bool MT);
+	void accumulateLF_MT(MatXX &H, VecX &b, bool MT);
+	void accumulateSCF_MT(MatXX &H, VecX &b, bool MT);
+
+	void calcLEnergyPt(int min, int max, Vec10* stats, int tid);
+
+	void orthogonalize(VecX* b, MatXX* H);
+	Mat18f* adHTdeltaF;
+
+	Mat88* adHost;
+	Mat88* adTarget;
+
+	Mat88f* adHostF;
+	Mat88f* adTargetF;
+
+
+	VecC cPrior;
+	VecCf cDeltaF;
+	VecCf cPriorF;
+
+	AccumulatedTopHessianSSE* accSSE_top_L;
+	AccumulatedTopHessianSSE* accSSE_top_A;
+
+
+	AccumulatedSCHessianSSE* accSSE_bot;
+
+	std::vector<EFPoint*> allPoints;
+	std::vector<EFPoint*> allPointsToMarg;
+
+	float currentLambda;
+};
+}
+

src/OptimizationBackend/EnergyFunctionalStructs.cpp

@@ -0,0 +1,116 @@
+/**
+* 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 "OptimizationBackend/EnergyFunctionalStructs.h"
+#include "OptimizationBackend/EnergyFunctional.h"
+#include "FullSystem/FullSystem.h"
+#include "FullSystem/HessianBlocks.h"
+#include "FullSystem/Residuals.h"
+
+#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__)
+#include "SSE2NEON.h"
+#endif
+
+namespace dso
+{
+
+
+void EFResidual::takeDataF()
+{
+	std::swap<RawResidualJacobian*>(J, data->J);
+
+	Vec2f JI_JI_Jd = J->JIdx2 * J->Jpdd;
+
+	for(int i=0;i<6;i++)
+		JpJdF[i] = J->Jpdxi[0][i]*JI_JI_Jd[0] + J->Jpdxi[1][i] * JI_JI_Jd[1];
+
+	JpJdF.segment<2>(6) = J->JabJIdx*J->Jpdd;
+}
+
+
+void EFFrame::takeData()
+{
+	prior = data->getPrior().head<8>();
+	delta = data->get_state_minus_stateZero().head<8>();
+	delta_prior =  (data->get_state() - data->getPriorZero()).head<8>();
+
+
+
+//	Vec10 state_zero =  data->get_state_zero();
+//	state_zero.segment<3>(0) = SCALE_XI_TRANS * state_zero.segment<3>(0);
+//	state_zero.segment<3>(3) = SCALE_XI_ROT * state_zero.segment<3>(3);
+//	state_zero[6] = SCALE_A * state_zero[6];
+//	state_zero[7] = SCALE_B * state_zero[7];
+//	state_zero[8] = SCALE_A * state_zero[8];
+//	state_zero[9] = SCALE_B * state_zero[9];
+//
+//	std::cout << "state_zero: " << state_zero.transpose() << "\n";
+
+
+	assert(data->frameID != -1);
+
+	frameID = data->frameID;
+}
+
+
+
+
+void EFPoint::takeData()
+{
+	priorF = data->hasDepthPrior ? setting_idepthFixPrior*SCALE_IDEPTH*SCALE_IDEPTH : 0;
+	if(setting_solverMode & SOLVER_REMOVE_POSEPRIOR) priorF=0;
+
+	deltaF = data->idepth-data->idepth_zero;
+}
+
+
+void EFResidual::fixLinearizationF(EnergyFunctional* ef)
+{
+	Vec8f dp = ef->adHTdeltaF[hostIDX+ef->nFrames*targetIDX];
+
+	// compute Jp*delta
+	__m128 Jp_delta_x = _mm_set1_ps(J->Jpdxi[0].dot(dp.head<6>())
+								   +J->Jpdc[0].dot(ef->cDeltaF)
+								   +J->Jpdd[0]*point->deltaF);
+	__m128 Jp_delta_y = _mm_set1_ps(J->Jpdxi[1].dot(dp.head<6>())
+								   +J->Jpdc[1].dot(ef->cDeltaF)
+								   +J->Jpdd[1]*point->deltaF);
+	__m128 delta_a = _mm_set1_ps((float)(dp[6]));
+	__m128 delta_b = _mm_set1_ps((float)(dp[7]));
+
+	for(int i=0;i<patternNum;i+=4)
+	{
+		// PATTERN: rtz = resF - [JI*Jp Ja]*delta.
+		__m128 rtz = _mm_load_ps(((float*)&J->resF)+i);
+		rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx))+i),Jp_delta_x));
+		rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx+1))+i),Jp_delta_y));
+		rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF))+i),delta_a));
+		rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF+1))+i),delta_b));
+		_mm_store_ps(((float*)&res_toZeroF)+i, rtz);
+	}
+
+	isLinearized = true;
+}
+
+}

src/OptimizationBackend/EnergyFunctionalStructs.h

@@ -0,0 +1,168 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+ 
+#include "util/NumType.h"
+#include "vector"
+#include <math.h>
+#include "OptimizationBackend/RawResidualJacobian.h"
+
+namespace dso
+{
+
+class PointFrameResidual;
+class CalibHessian;
+class FrameHessian;
+class PointHessian;
+
+class EFResidual;
+class EFPoint;
+class EFFrame;
+class EnergyFunctional;
+
+
+
+
+
+
+class EFResidual
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+	inline EFResidual(PointFrameResidual* org, EFPoint* point_, EFFrame* host_, EFFrame* target_) :
+		data(org), point(point_), host(host_), target(target_)
+	{
+		isLinearized=false;
+		isActiveAndIsGoodNEW=false;
+		J = new RawResidualJacobian();
+		assert(((long)this)%16==0);
+		assert(((long)J)%16==0);
+	}
+	inline ~EFResidual()
+	{
+		delete J;
+	}
+
+
+	void takeDataF();
+
+
+	void fixLinearizationF(EnergyFunctional* ef);
+
+
+	// structural pointers
+	PointFrameResidual* data;
+	int hostIDX, targetIDX;
+	EFPoint* point;
+	EFFrame* host;
+	EFFrame* target;
+	int idxInAll;
+
+	RawResidualJacobian* J;
+
+	VecNRf res_toZeroF;
+	Vec8f JpJdF;
+
+
+	// status.
+	bool isLinearized;
+
+	// if residual is not OOB & not OUTLIER & should be used during accumulations
+	bool isActiveAndIsGoodNEW;
+	inline const bool &isActive() const {return isActiveAndIsGoodNEW;}
+};
+
+
+enum EFPointStatus {PS_GOOD=0, PS_MARGINALIZE, PS_DROP};
+
+class EFPoint
+{
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	EFPoint(PointHessian* d, EFFrame* host_) : data(d),host(host_)
+	{
+		takeData();
+		stateFlag=EFPointStatus::PS_GOOD;
+	}
+	void takeData();
+
+	PointHessian* data;
+
+
+
+	float priorF;
+	float deltaF;
+
+
+	// constant info (never changes in-between).
+	int idxInPoints;
+	EFFrame* host;
+
+	// contains all residuals.
+	std::vector<EFResidual*> residualsAll;
+
+	float bdSumF;
+	float HdiF;
+	float Hdd_accLF;
+	VecCf Hcd_accLF;
+	float bd_accLF;
+	float Hdd_accAF;
+	VecCf Hcd_accAF;
+	float bd_accAF;
+
+
+	EFPointStatus stateFlag;
+};
+
+
+
+class EFFrame
+{
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	EFFrame(FrameHessian* d) : data(d)
+	{
+		takeData();
+	}
+	void takeData();
+
+
+	Vec8 prior;				// prior hessian (diagonal)
+	Vec8 delta_prior;		// = state-state_prior (E_prior = (delta_prior)' * diag(prior) * (delta_prior)
+	Vec8 delta;				// state - state_zero.
+
+
+
+	std::vector<EFPoint*> points;
+	FrameHessian* data;
+	int idx;	// idx in frames.
+
+	int frameID;
+};
+
+}
+

src/OptimizationBackend/RawResidualJacobian.h

@@ -0,0 +1,63 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+ 
+#include "util/NumType.h"
+
+namespace dso
+{
+struct RawResidualJacobian
+{
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	// ================== new structure: save independently =============.
+	VecNRf resF;
+
+	// the two rows of d[x,y]/d[xi].
+	Vec6f Jpdxi[2];			// 2x6
+
+	// the two rows of d[x,y]/d[C].
+	VecCf Jpdc[2];			// 2x4
+
+	// the two rows of d[x,y]/d[idepth].
+	Vec2f Jpdd;				// 2x1
+
+	// the two columns of d[r]/d[x,y].
+	VecNRf JIdx[2];			// 9x2
+
+	// = the two columns of d[r] / d[ab]
+	VecNRf JabF[2];			// 9x2
+
+
+	// = JIdx^T * JIdx (inner product). Only as a shorthand.
+	Mat22f JIdx2;				// 2x2
+	// = Jab^T * JIdx (inner product). Only as a shorthand.
+	Mat22f JabJIdx;			// 2x2
+	// = Jab^T * Jab (inner product). Only as a shorthand.
+	Mat22f Jab2;			// 2x2
+
+};
+}
+

src/main_dso_pangolin.cpp

@@ -0,0 +1,590 @@
+/**
+* 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 <fstream>
+
+
+std::string vignette = "";
+std::string gammaCalib = "";
+std::string source = "";
+std::string calib = "";
+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();
+}
+
+
+
+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;
+	}
+
+	printf("==============================================\n");
+}
+
+
+
+
+
+
+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;
+    }
+
+    printf("could not parse argument \"%s\"!!!!\n", arg);
+}
+
+int main( int argc, char** argv )
+{
+	//setlocale(LC_ALL, "");
+	for(int i=1; i<argc;i++)
+		parseArgument(argv[i]);
+
+	// hook crtl+C.
+	boost::thread exThread = boost::thread(exitThread);
+
+
+	ImageFolderReader* reader = new ImageFolderReader(source,calib, gammaCalib, vignette);
+	reader->setGlobalCalibration();
+
+    // Appended code here
+    std::ofstream online_results;
+    online_results.open("result_online.txt");
+//    online_results << std::setprecision(15);
+//    online_results << "time[s] tx ty tz qx qy qz qw" << std::endl;
+    online_results << std::fixed;
+
+
+
+
+	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);
+	}
+
+
+
+
+	int lstart=start;
+	int lend = end;
+	int linc = 1;
+	if(reverse)
+	{
+		printf("REVERSE!!!!");
+		lstart=end-1;
+		if(lstart >= reader->getNumImages())
+			lstart = reader->getNumImages()-1;
+		lend = start;
+		linc = -1;
+	}
+
+
+
+	FullSystem* fullSystem = new FullSystem();
+	fullSystem->setGammaFunction(reader->getPhotometricGamma());
+	fullSystem->linearizeOperation = (playbackSpeed==0);
+
+
+
+
+
+
+
+    IOWrap::PangolinDSOViewer* viewer = 0;
+	if(!disableAllDisplay)
+    {
+        viewer = new IOWrap::PangolinDSOViewer(wG[0],hG[0], false);
+        fullSystem->outputWrapper.push_back(viewer);
+    }
+
+
+
+    if(useSampleOutput)
+        fullSystem->outputWrapper.push_back(new IOWrap::SampleOutputWrapper());
+
+
+
+
+    // 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;
+        for(int i=lstart;i>= 0 && i< reader->getNumImages() && linc*i < linc*lend;i+=linc)
+        {
+            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");
+            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;
+
+
+        for(int ii=0;ii<(int)idsToPlay.size(); ii++)
+        {
+            if(!fullSystem->initialized)	// if not initialized: reset start time.
+            {
+                gettimeofday(&tv_start, NULL);
+                started = clock();
+                sInitializerOffset = timesToPlayAt[ii];
+            }
+
+            int i = idsToPlay[ii];
+
+
+            ImageAndExposure* img;
+            if(preload)
+                img = preloadedImages[ii];
+            else
+                img = reader->getImage(i);
+
+
+
+            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;
+                }
+            }
+
+
+
+            if(!skipFrame) fullSystem->addActiveFrame(img, i);
+
+            auto s = fullSystem->getLastPose();
+
+            // Fill the viewer for the point projection.
+            viewer->lastPose = s;
+            viewer->Hcalib = fullSystem->getHCalib();
+
+            bool write = true;
+            if(!s->poseValid) {
+                write=false;
+            }
+
+//            if(setting_onlyLogKFPoses && s->marginalizedAt == s->id) {
+//                write=false;
+//            }
+
+            std::cout << "writing to the file? " << write << std::endl;
+
+            if(write){
+                online_results << s->timestamp <<
+                               " " << s->camToWorld.translation().transpose()<<
+                               " " << s->camToWorld.so3().unit_quaternion().x()<<
+                               " " << s->camToWorld.so3().unit_quaternion().y()<<
+                               " " << s->camToWorld.so3().unit_quaternion().z()<<
+                               " " << s->camToWorld.so3().unit_quaternion().w() << "\n";
+            }
+
+
+
+            delete img;
+
+            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;
+
+                    setting_fullResetRequested=false;
+                }
+            }
+
+            if(fullSystem->isLost)
+            {
+                    printf("LOST!!\n");
+                    break;
+            }
+
+        }
+        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();
+        }
+
+    });
+
+
+    if(viewer != 0)
+        viewer->run();
+
+    runthread.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;
+}

src/util/DatasetReader.h

@@ -0,0 +1,379 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+#include "util/settings.h"
+#include "util/globalFuncs.h"
+#include "util/globalCalib.h"
+
+#include <sstream>
+#include <fstream>
+#include <dirent.h>
+#include <algorithm>
+
+#include "util/Undistort.h"
+#include "IOWrapper/ImageRW.h"
+
+#if HAS_ZIPLIB
+	#include "zip.h"
+#endif
+
+#include <boost/thread.hpp>
+
+using namespace dso;
+
+
+
+inline int getdir (std::string dir, std::vector<std::string> &files)
+{
+    DIR *dp;
+    struct dirent *dirp;
+    if((dp  = opendir(dir.c_str())) == NULL)
+    {
+        return -1;
+    }
+
+    while ((dirp = readdir(dp)) != NULL) {
+    	std::string name = std::string(dirp->d_name);
+
+    	if(name != "." && name != "..")
+    		files.push_back(name);
+    }
+    closedir(dp);
+
+
+    std::sort(files.begin(), files.end());
+
+    if(dir.at( dir.length() - 1 ) != '/') dir = dir+"/";
+	for(unsigned int i=0;i<files.size();i++)
+	{
+		if(files[i].at(0) != '/')
+			files[i] = dir + files[i];
+	}
+
+    return files.size();
+}
+
+
+struct PrepImageItem
+{
+	int id;
+	bool isQueud;
+	ImageAndExposure* pt;
+
+	inline PrepImageItem(int _id)
+	{
+		id=_id;
+		isQueud = false;
+		pt=0;
+	}
+
+	inline void release()
+	{
+		if(pt!=0) delete pt;
+		pt=0;
+	}
+};
+
+
+
+
+class ImageFolderReader
+{
+public:
+	ImageFolderReader(std::string path, std::string calibFile, std::string gammaFile, std::string vignetteFile)
+	{
+		this->path = path;
+		this->calibfile = calibFile;
+
+#if HAS_ZIPLIB
+		ziparchive=0;
+		databuffer=0;
+#endif
+
+		isZipped = (path.length()>4 && path.substr(path.length()-4) == ".zip");
+
+
+
+
+
+		if(isZipped)
+		{
+#if HAS_ZIPLIB
+			int ziperror=0;
+			ziparchive = zip_open(path.c_str(),  ZIP_RDONLY, &ziperror);
+			if(ziperror!=0)
+			{
+				printf("ERROR %d reading archive %s!\n", ziperror, path.c_str());
+				exit(1);
+			}
+
+			files.clear();
+			int numEntries = zip_get_num_entries(ziparchive, 0);
+			for(int k=0;k<numEntries;k++)
+			{
+				const char* name = zip_get_name(ziparchive, k,  ZIP_FL_ENC_STRICT);
+				std::string nstr = std::string(name);
+				if(nstr == "." || nstr == "..") continue;
+				files.push_back(name);
+			}
+
+			printf("got %d entries and %d files!\n", numEntries, (int)files.size());
+			std::sort(files.begin(), files.end());
+#else
+			printf("ERROR: cannot read .zip archive, as compile without ziplib!\n");
+			exit(1);
+#endif
+		}
+		else
+			getdir (path, files);
+
+
+		undistort = Undistort::getUndistorterForFile(calibFile, gammaFile, vignetteFile);
+
+
+		widthOrg = undistort->getOriginalSize()[0];
+		heightOrg = undistort->getOriginalSize()[1];
+		width=undistort->getSize()[0];
+		height=undistort->getSize()[1];
+
+
+		// load timestamps if possible.
+		loadTimestamps();
+		printf("ImageFolderReader: got %d files in %s!\n", (int)files.size(), path.c_str());
+
+	}
+	~ImageFolderReader()
+	{
+#if HAS_ZIPLIB
+		if(ziparchive!=0) zip_close(ziparchive);
+		if(databuffer!=0) delete databuffer;
+#endif
+
+
+		delete undistort;
+	};
+
+	Eigen::VectorXf getOriginalCalib()
+	{
+		return undistort->getOriginalParameter().cast<float>();
+	}
+	Eigen::Vector2i getOriginalDimensions()
+	{
+		return  undistort->getOriginalSize();
+	}
+
+	void getCalibMono(Eigen::Matrix3f &K, int &w, int &h)
+	{
+		K = undistort->getK().cast<float>();
+		w = undistort->getSize()[0];
+		h = undistort->getSize()[1];
+	}
+
+	void setGlobalCalibration()
+	{
+		int w_out, h_out;
+		Eigen::Matrix3f K;
+		getCalibMono(K, w_out, h_out);
+		setGlobalCalib(w_out, h_out, K);
+	}
+
+	int getNumImages()
+	{
+		return files.size();
+	}
+
+	double getTimestamp(int id)
+	{
+		if(timestamps.size()==0) return id*0.1f;
+		if(id >= (int)timestamps.size()) return 0;
+		if(id < 0) return 0;
+		return timestamps[id];
+	}
+
+
+	void prepImage(int id, bool as8U=false)
+	{
+
+	}
+
+
+	MinimalImageB* getImageRaw(int id)
+	{
+			return getImageRaw_internal(id,0);
+	}
+
+	ImageAndExposure* getImage(int id, bool forceLoadDirectly=false)
+	{
+		return getImage_internal(id, 0);
+	}
+
+
+	inline float* getPhotometricGamma()
+	{
+		if(undistort==0 || undistort->photometricUndist==0) return 0;
+		return undistort->photometricUndist->getG();
+	}
+
+
+	// undistorter. [0] always exists, [1-2] only when MT is enabled.
+	Undistort* undistort;
+private:
+
+
+	MinimalImageB* getImageRaw_internal(int id, int unused)
+	{
+		if(!isZipped)
+		{
+			// CHANGE FOR ZIP FILE
+			return IOWrap::readImageBW_8U(files[id]);
+		}
+		else
+		{
+#if HAS_ZIPLIB
+			if(databuffer==0) databuffer = new char[widthOrg*heightOrg*6+10000];
+			zip_file_t* fle = zip_fopen(ziparchive, files[id].c_str(), 0);
+			long readbytes = zip_fread(fle, databuffer, (long)widthOrg*heightOrg*6+10000);
+
+			if(readbytes > (long)widthOrg*heightOrg*6)
+			{
+				printf("read %ld/%ld bytes for file %s. increase buffer!!\n", readbytes,(long)widthOrg*heightOrg*6+10000, files[id].c_str());
+				delete[] databuffer;
+				databuffer = new char[(long)widthOrg*heightOrg*30];
+				fle = zip_fopen(ziparchive, files[id].c_str(), 0);
+				readbytes = zip_fread(fle, databuffer, (long)widthOrg*heightOrg*30+10000);
+
+				if(readbytes > (long)widthOrg*heightOrg*30)
+				{
+					printf("buffer still to small (read %ld/%ld). abort.\n", readbytes,(long)widthOrg*heightOrg*30+10000);
+					exit(1);
+				}
+			}
+
+			return IOWrap::readStreamBW_8U(databuffer, readbytes);
+#else
+			printf("ERROR: cannot read .zip archive, as compile without ziplib!\n");
+			exit(1);
+#endif
+		}
+	}
+
+
+	ImageAndExposure* getImage_internal(int id, int unused)
+	{
+		MinimalImageB* minimg = getImageRaw_internal(id, 0);
+		ImageAndExposure* ret2 = undistort->undistort<unsigned char>(
+				minimg,
+				(exposures.size() == 0 ? 1.0f : exposures[id]),
+				(timestamps.size() == 0 ? 0.0 : timestamps[id]));
+		delete minimg;
+		return ret2;
+	}
+
+	inline void loadTimestamps()
+	{
+		std::ifstream tr;
+		std::string timesFile = path.substr(0,path.find_last_of('/')) + "/times.txt";
+		tr.open(timesFile.c_str());
+		while(!tr.eof() && tr.good())
+		{
+			std::string line;
+			char buf[1000];
+			tr.getline(buf, 1000);
+
+			int id;
+			double stamp;
+			float exposure = 0;
+
+			if(3 == sscanf(buf, "%d %lf %f", &id, &stamp, &exposure))
+			{
+				timestamps.push_back(stamp);
+				exposures.push_back(exposure);
+			}
+
+			else if(2 == sscanf(buf, "%d %lf", &id, &stamp))
+			{
+				timestamps.push_back(stamp);
+				exposures.push_back(exposure);
+			}
+		}
+		tr.close();
+
+		// check if exposures are correct, (possibly skip)
+		bool exposuresGood = ((int)exposures.size()==(int)getNumImages()) ;
+		for(int i=0;i<(int)exposures.size();i++)
+		{
+			if(exposures[i] == 0)
+			{
+				// fix!
+				float sum=0,num=0;
+				if(i>0 && exposures[i-1] > 0) {sum += exposures[i-1]; num++;}
+				if(i+1<(int)exposures.size() && exposures[i+1] > 0) {sum += exposures[i+1]; num++;}
+
+				if(num>0)
+					exposures[i] = sum/num;
+			}
+
+			if(exposures[i] == 0) exposuresGood=false;
+		}
+
+
+		if((int)getNumImages() != (int)timestamps.size())
+		{
+			printf("set timestamps and exposures to zero!\n");
+			exposures.clear();
+			timestamps.clear();
+		}
+
+		if((int)getNumImages() != (int)exposures.size() || !exposuresGood)
+		{
+			printf("set EXPOSURES to zero!\n");
+			exposures.clear();
+		}
+
+		printf("got %d images and %d timestamps and %d exposures.!\n", (int)getNumImages(), (int)timestamps.size(), (int)exposures.size());
+	}
+
+
+
+
+	std::vector<ImageAndExposure*> preloadedImages;
+	std::vector<std::string> files;
+	std::vector<double> timestamps;
+	std::vector<float> exposures;
+
+	int width, height;
+	int widthOrg, heightOrg;
+
+	std::string path;
+	std::string calibfile;
+
+	bool isZipped;
+
+#if HAS_ZIPLIB
+	zip_t* ziparchive;
+	char* databuffer;
+#endif
+};
+

src/util/FrameShell.h

@@ -0,0 +1,74 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+#include "util/NumType.h"
+#include "algorithm"
+
+namespace dso
+{
+
+
+class FrameShell
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	int id; 			// INTERNAL ID, starting at zero.
+	int incoming_id;	// ID passed into DSO
+	double timestamp;		// timestamp passed into DSO.
+
+	// set once after tracking
+	SE3 camToTrackingRef;
+	FrameShell* trackingRef;
+
+	// constantly adapted.
+	SE3 camToWorld;				// Write: TRACKING, while frame is still fresh; MAPPING: only when locked [shellPoseMutex].
+	AffLight aff_g2l;
+	bool poseValid;
+
+	// statisitcs
+	int statistics_outlierResOnThis;
+	int statistics_goodResOnThis;
+	int marginalizedAt;
+	double movedByOpt;
+
+
+	inline FrameShell()
+	{
+		id=0;
+		poseValid=true;
+		camToWorld = SE3();
+		timestamp=0;
+		marginalizedAt=-1;
+		movedByOpt=0;
+		statistics_outlierResOnThis=statistics_goodResOnThis=0;
+		trackingRef=0;
+		camToTrackingRef = SE3();
+	}
+};
+
+
+}
+

src/util/ImageAndExposure.h

@@ -0,0 +1,67 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+#include <cstring>
+#include <iostream>
+
+
+namespace dso
+{
+
+
+class ImageAndExposure
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	float* image;			// irradiance. between 0 and 256
+	int w,h;				// width and height;
+	double timestamp;
+	float exposure_time;	// exposure time in ms.
+	inline ImageAndExposure(int w_, int h_, double timestamp_=0) : w(w_), h(h_), timestamp(timestamp_)
+	{
+		image = new float[w*h];
+		exposure_time=1;
+	}
+	inline ~ImageAndExposure()
+	{
+		delete[] image;
+	}
+
+	inline void copyMetaTo(ImageAndExposure &other)
+	{
+		other.exposure_time = exposure_time;
+	}
+
+	inline ImageAndExposure* getDeepCopy()
+	{
+		ImageAndExposure* img = new ImageAndExposure(w,h,timestamp);
+		img->exposure_time = exposure_time;
+		memcpy(img->image, image, w*h*sizeof(float));
+		return img;
+	}
+};
+
+
+}

src/util/IndexThreadReduce.h

@@ -0,0 +1,216 @@
+/**
+* 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/>.
+*/
+
+
+
+#pragma once
+#include "util/settings.h"
+#include "boost/thread.hpp"
+#include <stdio.h>
+#include <iostream>
+
+
+
+namespace dso
+{
+
+template<typename Running>
+class IndexThreadReduce
+{
+
+public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+	inline IndexThreadReduce()
+	{
+		nextIndex = 0;
+		maxIndex = 0;
+		stepSize = 1;
+		callPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4);
+
+		running = true;
+		for(int i=0;i<NUM_THREADS;i++)
+		{
+			isDone[i] = false;
+			gotOne[i] = true;
+			workerThreads[i] = boost::thread(&IndexThreadReduce::workerLoop, this, i);
+		}
+
+	}
+	inline ~IndexThreadReduce()
+	{
+		running = false;
+
+		exMutex.lock();
+		todo_signal.notify_all();
+		exMutex.unlock();
+
+		for(int i=0;i<NUM_THREADS;i++)
+			workerThreads[i].join();
+
+
+		printf("destroyed ThreadReduce\n");
+
+	}
+
+	inline void reduce(boost::function<void(int,int,Running*,int)> callPerIndex, int first, int end, int stepSize = 0)
+	{
+
+		memset(&stats, 0, sizeof(Running));
+
+//		if(!multiThreading)
+//		{
+//			callPerIndex(first, end, &stats, 0);
+//			return;
+//		}
+
+
+
+		if(stepSize == 0)
+			stepSize = ((end-first)+NUM_THREADS-1)/NUM_THREADS;
+
+
+		//printf("reduce called\n");
+
+		boost::unique_lock<boost::mutex> lock(exMutex);
+
+		// save
+		this->callPerIndex = callPerIndex;
+		nextIndex = first;
+		maxIndex = end;
+		this->stepSize = stepSize;
+
+		// go worker threads!
+		for(int i=0;i<NUM_THREADS;i++)
+		{
+			isDone[i] = false;
+			gotOne[i] = false;
+		}
+
+		// let them start!
+		todo_signal.notify_all();
+
+
+		//printf("reduce waiting for threads to finish\n");
+		// wait for all worker threads to signal they are done.
+		while(true)
+		{
+			// wait for at least one to finish
+			done_signal.wait(lock);
+			//printf("thread finished!\n");
+
+			// check if actually all are finished.
+			bool allDone = true;
+			for(int i=0;i<NUM_THREADS;i++)
+				allDone = allDone && isDone[i];
+
+			// all are finished! exit.
+			if(allDone)
+				break;
+		}
+
+		nextIndex = 0;
+		maxIndex = 0;
+		this->callPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4);
+
+		//printf("reduce done (all threads finished)\n");
+	}
+
+	Running stats;
+
+private:
+	boost::thread workerThreads[NUM_THREADS];
+	bool isDone[NUM_THREADS];
+	bool gotOne[NUM_THREADS];
+
+	boost::mutex exMutex;
+	boost::condition_variable todo_signal;
+	boost::condition_variable done_signal;
+
+	int nextIndex;
+	int maxIndex;
+	int stepSize;
+
+	bool running;
+
+	boost::function<void(int,int,Running*,int)> callPerIndex;
+
+	void callPerIndexDefault(int i, int j,Running* k, int tid)
+	{
+		printf("ERROR: should never be called....\n");
+		assert(false);
+	}
+
+	void workerLoop(int idx)
+	{
+		boost::unique_lock<boost::mutex> lock(exMutex);
+
+		while(running)
+		{
+			// try to get something to do.
+			int todo = 0;
+			bool gotSomething = false;
+			if(nextIndex < maxIndex)
+			{
+				// got something!
+				todo = nextIndex;
+				nextIndex+=stepSize;
+				gotSomething = true;
+			}
+
+			// if got something: do it (unlock in the meantime)
+			if(gotSomething)
+			{
+				lock.unlock();
+
+				assert(callPerIndex != 0);
+
+				Running s; memset(&s, 0, sizeof(Running));
+				callPerIndex(todo, std::min(todo+stepSize, maxIndex), &s, idx);
+				gotOne[idx] = true;
+				lock.lock();
+				stats += s;
+			}
+
+			// otherwise wait on signal, releasing lock in the meantime.
+			else
+			{
+				if(!gotOne[idx])
+				{
+					lock.unlock();
+					assert(callPerIndex != 0);
+					Running s; memset(&s, 0, sizeof(Running));
+					callPerIndex(0, 0, &s, idx);
+					gotOne[idx] = true;
+					lock.lock();
+					stats += s;
+				}
+				isDone[idx] = true;
+				//printf("worker %d waiting..\n", idx);
+				done_signal.notify_all();
+				todo_signal.wait(lock);
+			}
+		}
+	}
+};
+}

src/util/MinimalImage.h

@@ -0,0 +1,167 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+#include "util/NumType.h"
+#include "algorithm"
+
+namespace dso
+{
+
+template<typename T>
+class MinimalImage
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	int w;
+	int h;
+	T* data;
+
+	/*
+	 * creates minimal image with own memory
+	 */
+	inline MinimalImage(int w_, int h_) : w(w_), h(h_)
+	{
+		data = new T[w*h];
+		ownData=true;
+	}
+
+	/*
+	 * creates minimal image wrapping around existing memory
+	 */
+	inline MinimalImage(int w_, int h_, T* data_) : w(w_), h(h_)
+	{
+		data = data_;
+		ownData=false;
+	}
+
+	inline ~MinimalImage()
+	{
+		if(ownData) delete [] data;
+	}
+
+	inline MinimalImage* getClone()
+	{
+		MinimalImage* clone = new MinimalImage(w,h);
+		memcpy(clone->data, data, sizeof(T)*w*h);
+		return clone;
+	}
+
+
+	inline T& at(int x, int y) {return data[(int)x+((int)y)*w];}
+	inline T& at(int i) {return data[i];}
+
+	inline void setBlack()
+	{
+		memset(data, 0, sizeof(T)*w*h);
+	}
+
+	inline void setConst(T val)
+	{
+		for(int i=0;i<w*h;i++) data[i] = val;
+	}
+
+	inline void setPixel1(const float &u, const float &v, T val)
+	{
+		at(u+0.5f,v+0.5f) = val;
+	}
+
+	inline void setPixel4(const float &u, const float &v, T val)
+	{
+		at(u+1.0f,v+1.0f) = val;
+		at(u+1.0f,v) = val;
+		at(u,v+1.0f) = val;
+		at(u,v) = val;
+	}
+
+	inline void setPixel9(const int &u, const int &v, T val)
+	{
+		at(u+1,v-1) = val;
+		at(u+1,v) = val;
+		at(u+1,v+1) = val;
+		at(u,v-1) = val;
+		at(u,v) = val;
+		at(u,v+1) = val;
+		at(u-1,v-1) = val;
+		at(u-1,v) = val;
+		at(u-1,v+1) = val;
+	}
+
+	inline void setPixelCirc(const int &u, const int &v, T val)
+	{
+		for(int i=-3;i<=3;i++)
+		{
+			at(u+3,v+i) = val;
+			at(u-3,v+i) = val;
+			at(u+2,v+i) = val;
+			at(u-2,v+i) = val;
+
+			at(u+i,v-3) = val;
+			at(u+i,v+3) = val;
+			at(u+i,v-2) = val;
+			at(u+i,v+2) = val;
+		}
+	}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+private:
+	bool ownData;
+};
+
+typedef Eigen::Matrix<unsigned char,3,1> Vec3b;
+typedef MinimalImage<float> MinimalImageF;
+typedef MinimalImage<Vec3f> MinimalImageF3;
+typedef MinimalImage<unsigned char> MinimalImageB;
+typedef MinimalImage<Vec3b> MinimalImageB3;
+typedef MinimalImage<unsigned short> MinimalImageB16;
+
+}
+

src/util/NumType.h

@@ -0,0 +1,195 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+#include "Eigen/Core"
+#include "sophus/sim3.hpp"
+#include "sophus/se3.hpp"
+
+
+namespace dso
+{
+
+// CAMERA MODEL TO USE
+
+
+#define SSEE(val,idx) (*(((float*)&val)+idx))
+
+
+#define MAX_RES_PER_POINT 8
+#define NUM_THREADS 6
+
+
+#define todouble(x) (x).cast<double>()
+
+
+typedef Sophus::SE3d SE3;
+typedef Sophus::Sim3d Sim3;
+typedef Sophus::SO3d SO3;
+
+
+
+#define CPARS 4
+
+
+typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> MatXX;
+typedef Eigen::Matrix<double,CPARS,CPARS> MatCC;
+#define MatToDynamic(x) MatXX(x)
+
+
+
+typedef Eigen::Matrix<double,CPARS,10> MatC10;
+typedef Eigen::Matrix<double,10,10> Mat1010;
+typedef Eigen::Matrix<double,13,13> Mat1313;
+
+typedef Eigen::Matrix<double,8,10> Mat810;
+typedef Eigen::Matrix<double,8,3> Mat83;
+typedef Eigen::Matrix<double,6,6> Mat66;
+typedef Eigen::Matrix<double,5,3> Mat53;
+typedef Eigen::Matrix<double,4,3> Mat43;
+typedef Eigen::Matrix<double,4,2> Mat42;
+typedef Eigen::Matrix<double,3,3> Mat33;
+typedef Eigen::Matrix<double,2,2> Mat22;
+typedef Eigen::Matrix<double,8,CPARS> Mat8C;
+typedef Eigen::Matrix<double,CPARS,8> MatC8;
+typedef Eigen::Matrix<float,8,CPARS> Mat8Cf;
+typedef Eigen::Matrix<float,CPARS,8> MatC8f;
+
+typedef Eigen::Matrix<double,8,8> Mat88;
+typedef Eigen::Matrix<double,7,7> Mat77;
+
+typedef Eigen::Matrix<double,CPARS,1> VecC;
+typedef Eigen::Matrix<float,CPARS,1> VecCf;
+typedef Eigen::Matrix<double,13,1> Vec13;
+typedef Eigen::Matrix<double,10,1> Vec10;
+typedef Eigen::Matrix<double,9,1> Vec9;
+typedef Eigen::Matrix<double,8,1> Vec8;
+typedef Eigen::Matrix<double,7,1> Vec7;
+typedef Eigen::Matrix<double,6,1> Vec6;
+typedef Eigen::Matrix<double,5,1> Vec5;
+typedef Eigen::Matrix<double,4,1> Vec4;
+typedef Eigen::Matrix<double,3,1> Vec3;
+typedef Eigen::Matrix<double,2,1> Vec2;
+typedef Eigen::Matrix<double,Eigen::Dynamic,1> VecX;
+
+typedef Eigen::Matrix<float,3,3> Mat33f;
+typedef Eigen::Matrix<float,10,3> Mat103f;
+typedef Eigen::Matrix<float,2,2> Mat22f;
+typedef Eigen::Matrix<float,3,1> Vec3f;
+typedef Eigen::Matrix<float,2,1> Vec2f;
+typedef Eigen::Matrix<float,6,1> Vec6f;
+
+
+
+typedef Eigen::Matrix<double,4,9> Mat49;
+typedef Eigen::Matrix<double,8,9> Mat89;
+
+typedef Eigen::Matrix<double,9,4> Mat94;
+typedef Eigen::Matrix<double,9,8> Mat98;
+
+typedef Eigen::Matrix<double,8,1> Mat81;
+typedef Eigen::Matrix<double,1,8> Mat18;
+typedef Eigen::Matrix<double,9,1> Mat91;
+typedef Eigen::Matrix<double,1,9> Mat19;
+
+
+typedef Eigen::Matrix<double,8,4> Mat84;
+typedef Eigen::Matrix<double,4,8> Mat48;
+typedef Eigen::Matrix<double,4,4> Mat44;
+
+
+typedef Eigen::Matrix<float,MAX_RES_PER_POINT,1> VecNRf;
+typedef Eigen::Matrix<float,12,1> Vec12f;
+typedef Eigen::Matrix<float,1,8> Mat18f;
+typedef Eigen::Matrix<float,6,6> Mat66f;
+typedef Eigen::Matrix<float,8,8> Mat88f;
+typedef Eigen::Matrix<float,8,4> Mat84f;
+typedef Eigen::Matrix<float,8,1> Vec8f;
+typedef Eigen::Matrix<float,10,1> Vec10f;
+typedef Eigen::Matrix<float,6,6> Mat66f;
+typedef Eigen::Matrix<float,4,1> Vec4f;
+typedef Eigen::Matrix<float,4,4> Mat44f;
+typedef Eigen::Matrix<float,12,12> Mat1212f;
+typedef Eigen::Matrix<float,12,1> Vec12f;
+typedef Eigen::Matrix<float,13,13> Mat1313f;
+typedef Eigen::Matrix<float,10,10> Mat1010f;
+typedef Eigen::Matrix<float,13,1> Vec13f;
+typedef Eigen::Matrix<float,9,9> Mat99f;
+typedef Eigen::Matrix<float,9,1> Vec9f;
+
+typedef Eigen::Matrix<float,4,2> Mat42f;
+typedef Eigen::Matrix<float,6,2> Mat62f;
+typedef Eigen::Matrix<float,1,2> Mat12f;
+
+typedef Eigen::Matrix<float,Eigen::Dynamic,1> VecXf;
+typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic> MatXXf;
+
+
+typedef Eigen::Matrix<double,8+CPARS+1,8+CPARS+1> MatPCPC;
+typedef Eigen::Matrix<float,8+CPARS+1,8+CPARS+1> MatPCPCf;
+typedef Eigen::Matrix<double,8+CPARS+1,1> VecPC;
+typedef Eigen::Matrix<float,8+CPARS+1,1> VecPCf;
+
+typedef Eigen::Matrix<float,14,14> Mat1414f;
+typedef Eigen::Matrix<float,14,1> Vec14f;
+typedef Eigen::Matrix<double,14,14> Mat1414;
+typedef Eigen::Matrix<double,14,1> Vec14;
+
+
+
+
+
+
+// transforms points from one frame to another.
+struct AffLight
+{
+	AffLight(double a_, double b_) : a(a_), b(b_) {};
+	AffLight() : a(0), b(0) {};
+
+	// Affine Parameters:
+	double a,b;	// I_frame = exp(a)*I_global + b. // I_global = exp(-a)*(I_frame - b).
+
+	static Vec2 fromToVecExposure(float exposureF, float exposureT, AffLight g2F, AffLight g2T)
+	{
+		if(exposureF==0 || exposureT==0)
+		{
+			exposureT = exposureF = 1;
+			//printf("got exposure value of 0! please choose the correct model.\n");
+			//assert(setting_brightnessTransferFunc < 2);
+		}
+
+		double a = exp(g2T.a-g2F.a) * exposureT / exposureF;
+		double b = g2T.b - a*g2F.b;
+		return Vec2(a,b);
+	}
+
+	Vec2 vec()
+	{
+		return Vec2(a,b);
+	}
+};
+
+}
+

src/util/Undistort.h

@@ -0,0 +1,163 @@
+/**
+* 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/>.
+*/
+
+
+#pragma once
+
+#include "util/ImageAndExposure.h"
+#include "util/MinimalImage.h"
+#include "util/NumType.h"
+#include "Eigen/Core"
+
+
+
+
+
+namespace dso
+{
+
+
+class PhotometricUndistorter
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	PhotometricUndistorter(std::string file, std::string noiseImage, std::string vignetteImage, int w_, int h_);
+	~PhotometricUndistorter();
+
+	// removes readout noise, and converts to irradiance.
+	// affine normalizes values to 0 <= I < 256.
+	// raw irradiance = a*I + b.
+	// output will be written in [output].
+	template<typename T> void processFrame(T* image_in, float exposure_time, float factor=1);
+	void unMapFloatImage(float* image);
+
+	ImageAndExposure* output;
+
+	float* getG() {if(!valid) return 0; else return G;};
+private:
+    float G[256*256];
+    int GDepth;
+	float* vignetteMap;
+	float* vignetteMapInv;
+	int w,h;
+	bool valid;
+};
+
+
+class Undistort
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+	virtual ~Undistort();
+
+	virtual void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const = 0;
+
+	
+	inline const Mat33 getK() const {return K;};
+	inline const Eigen::Vector2i getSize() const {return Eigen::Vector2i(w,h);};
+	inline const VecX getOriginalParameter() const {return parsOrg;};
+	inline const Eigen::Vector2i getOriginalSize() {return Eigen::Vector2i(wOrg,hOrg);};
+	inline bool isValid() {return valid;};
+
+	template<typename T>
+	ImageAndExposure* undistort(const MinimalImage<T>* image_raw, float exposure=0, double timestamp=0, float factor=1) const;
+	static Undistort* getUndistorterForFile(std::string configFilename, std::string gammaFilename, std::string vignetteFilename);
+
+	void loadPhotometricCalibration(std::string file, std::string noiseImage, std::string vignetteImage);
+
+	PhotometricUndistorter* photometricUndist;
+
+protected:
+    int w, h, wOrg, hOrg, wUp, hUp;
+    int upsampleUndistFactor;
+	Mat33 K;
+	VecX parsOrg;
+	bool valid;
+	bool passthrough;
+
+	float* remapX;
+	float* remapY;
+
+	void applyBlurNoise(float* img) const;
+
+	void makeOptimalK_crop();
+	void makeOptimalK_full();
+
+	void readFromFile(const char* configFileName, int nPars, std::string prefix = "");
+};
+
+class UndistortFOV : public Undistort
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+    UndistortFOV(const char* configFileName, bool noprefix);
+	~UndistortFOV();
+	void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const;
+
+};
+
+class UndistortRadTan : public Undistort
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    UndistortRadTan(const char* configFileName, bool noprefix);
+    ~UndistortRadTan();
+    void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const;
+
+};
+
+class UndistortEquidistant : public Undistort
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    UndistortEquidistant(const char* configFileName, bool noprefix);
+    ~UndistortEquidistant();
+    void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const;
+
+};
+
+class UndistortPinhole : public Undistort
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    UndistortPinhole(const char* configFileName, bool noprefix);
+	~UndistortPinhole();
+	void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const;
+
+private:
+	float inputCalibration[8];
+};
+
+class UndistortKB : public Undistort
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    UndistortKB(const char* configFileName, bool noprefix);
+	~UndistortKB();
+	void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const;
+
+};
+
+}
+

src/util/globalCalib.cpp

@@ -0,0 +1,108 @@
+/**
+* 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 "util/globalCalib.h"
+#include "stdio.h"
+#include <iostream>
+
+namespace dso
+{
+	int wG[PYR_LEVELS], hG[PYR_LEVELS];
+	float fxG[PYR_LEVELS], fyG[PYR_LEVELS],
+		  cxG[PYR_LEVELS], cyG[PYR_LEVELS];
+
+	float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS],
+		  cxiG[PYR_LEVELS], cyiG[PYR_LEVELS];
+
+	Eigen::Matrix3f KG[PYR_LEVELS], KiG[PYR_LEVELS];
+
+
+	float wM3G;
+	float hM3G;
+
+	void setGlobalCalib(int w, int h,const Eigen::Matrix3f &K)
+	{
+		int wlvl=w;
+		int hlvl=h;
+		pyrLevelsUsed=1;
+		while(wlvl%2==0 && hlvl%2==0 && wlvl*hlvl > 5000 && pyrLevelsUsed < PYR_LEVELS)
+		{
+			wlvl /=2;
+			hlvl /=2;
+			pyrLevelsUsed++;
+		}
+		printf("using pyramid levels 0 to %d. coarsest resolution: %d x %d!\n",
+				pyrLevelsUsed-1, wlvl, hlvl);
+		if(wlvl>100 && hlvl > 100)
+		{
+			printf("\n\n===============WARNING!===================\n "
+					"using not enough pyramid levels.\n"
+					"Consider scaling to a resolution that is a multiple of a power of 2.\n");
+		}
+		if(pyrLevelsUsed < 3)
+		{
+			printf("\n\n===============WARNING!===================\n "
+					"I need higher resolution.\n"
+					"I will probably segfault.\n");
+		}
+
+		wM3G = w-3;
+		hM3G = h-3;
+
+		wG[0] = w;
+		hG[0] = h;
+		KG[0] = K;
+		fxG[0] = K(0,0);
+		fyG[0] = K(1,1);
+		cxG[0] = K(0,2);
+		cyG[0] = K(1,2);
+		KiG[0] = KG[0].inverse();
+		fxiG[0] = KiG[0](0,0);
+		fyiG[0] = KiG[0](1,1);
+		cxiG[0] = KiG[0](0,2);
+		cyiG[0] = KiG[0](1,2);
+
+		for (int level = 1; level < pyrLevelsUsed; ++ level)
+		{
+			wG[level] = w >> level;
+			hG[level] = h >> level;
+
+			fxG[level] = fxG[level-1] * 0.5;
+			fyG[level] = fyG[level-1] * 0.5;
+			cxG[level] = (cxG[0] + 0.5) / ((int)1<<level) - 0.5;
+			cyG[level] = (cyG[0] + 0.5) / ((int)1<<level) - 0.5;
+
+			KG[level]  << fxG[level], 0.0, cxG[level], 0.0, fyG[level], cyG[level], 0.0, 0.0, 1.0;	// synthetic
+			KiG[level] = KG[level].inverse();
+
+			fxiG[level] = KiG[level](0,0);
+			fyiG[level] = KiG[level](1,1);
+			cxiG[level] = KiG[level](0,2);
+			cyiG[level] = KiG[level](1,2);
+		}
+	}
+
+
+}

src/util/globalCalib.h

@@ -0,0 +1,45 @@
+/**
+* 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/>.
+*/
+
+
+
+#pragma once
+#include "util/settings.h"
+#include "util/NumType.h"
+
+namespace dso
+{
+	extern int wG[PYR_LEVELS], hG[PYR_LEVELS];
+	extern float fxG[PYR_LEVELS], fyG[PYR_LEVELS],
+		  cxG[PYR_LEVELS], cyG[PYR_LEVELS];
+
+	extern float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS],
+		  cxiG[PYR_LEVELS], cyiG[PYR_LEVELS];
+
+	extern Eigen::Matrix3f KG[PYR_LEVELS],KiG[PYR_LEVELS];
+
+	extern float wM3G;
+	extern float hM3G;
+
+	void setGlobalCalib(int w, int h, const Eigen::Matrix3f &K );
+}

src/util/globalFuncs.h

@@ -0,0 +1,382 @@
+/**
+* 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/>.
+*/
+
+
+
+#pragma once
+#include "util/settings.h"
+#include "util/NumType.h"
+#include "IOWrapper/ImageDisplay.h"
+#include "fstream"
+
+namespace dso
+{
+
+
+
+// reads interpolated element from a uchar* array
+// SSE2 optimization possible
+EIGEN_ALWAYS_INLINE float getInterpolatedElement(const float* const mat, const float x, const float y, const int width)
+{
+	//stats.num_pixelInterpolations++;
+
+	int ix = (int)x;
+	int iy = (int)y;
+	float dx = x - ix;
+	float dy = y - iy;
+	float dxdy = dx*dy;
+	const float* bp = mat +ix+iy*width;
+
+
+	float res =   dxdy * bp[1+width]
+				+ (dy-dxdy) * bp[width]
+				+ (dx-dxdy) * bp[1]
+				+ (1-dx-dy+dxdy) * bp[0];
+
+	return res;
+}
+
+EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement43(const Eigen::Vector4f* const mat, const float x, const float y, const int width)
+{
+	int ix = (int)x;
+	int iy = (int)y;
+	float dx = x - ix;
+	float dy = y - iy;
+	float dxdy = dx*dy;
+	const Eigen::Vector4f* bp = mat +ix+iy*width;
+
+
+	return dxdy * *(const Eigen::Vector3f*)(bp+1+width)
+	        + (dy-dxdy) * *(const Eigen::Vector3f*)(bp+width)
+	        + (dx-dxdy) * *(const Eigen::Vector3f*)(bp+1)
+			+ (1-dx-dy+dxdy) * *(const Eigen::Vector3f*)(bp);
+}
+
+EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33(const Eigen::Vector3f* const mat, const float x, const float y, const int width)
+{
+	int ix = (int)x;
+	int iy = (int)y;
+	float dx = x - ix;
+	float dy = y - iy;
+	float dxdy = dx*dy;
+	const Eigen::Vector3f* bp = mat +ix+iy*width;
+
+
+	return dxdy * *(const Eigen::Vector3f*)(bp+1+width)
+	        + (dy-dxdy) * *(const Eigen::Vector3f*)(bp+width)
+	        + (dx-dxdy) * *(const Eigen::Vector3f*)(bp+1)
+			+ (1-dx-dy+dxdy) * *(const Eigen::Vector3f*)(bp);
+}
+
+EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33OverAnd(const Eigen::Vector3f* const mat, const bool* overMat, const float x, const float y, const int width, bool& over_out)
+{
+	int ix = (int)x;
+	int iy = (int)y;
+	float dx = x - ix;
+	float dy = y - iy;
+	float dxdy = dx*dy;
+	const Eigen::Vector3f* bp = mat +ix+iy*width;
+
+	const bool* bbp = overMat +ix+iy*width;
+	over_out = bbp[1+width] && bbp[1] && bbp[width] && bbp[0];
+
+	return dxdy * *(const Eigen::Vector3f*)(bp+1+width)
+	        + (dy-dxdy) * *(const Eigen::Vector3f*)(bp+width)
+	        + (dx-dxdy) * *(const Eigen::Vector3f*)(bp+1)
+			+ (1-dx-dy+dxdy) * *(const Eigen::Vector3f*)(bp);
+}
+EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33OverOr(const Eigen::Vector3f* const mat, const bool* overMat, const float x, const float y, const int width, bool& over_out)
+{
+	int ix = (int)x;
+	int iy = (int)y;
+	float dx = x - ix;
+	float dy = y - iy;
+	float dxdy = dx*dy;
+	const Eigen::Vector3f* bp = mat +ix+iy*width;
+
+	const bool* bbp = overMat +ix+iy*width;
+	over_out = bbp[1+width] || bbp[1] || bbp[width] || bbp[0];
+
+	return dxdy * *(const Eigen::Vector3f*)(bp+1+width)
+	        + (dy-dxdy) * *(const Eigen::Vector3f*)(bp+width)
+	        + (dx-dxdy) * *(const Eigen::Vector3f*)(bp+1)
+			+ (1-dx-dy+dxdy) * *(const Eigen::Vector3f*)(bp);
+}
+
+EIGEN_ALWAYS_INLINE float getInterpolatedElement31(const Eigen::Vector3f* const mat, const float x, const float y, const int width)
+{
+	int ix = (int)x;
+	int iy = (int)y;
+	float dx = x - ix;
+	float dy = y - iy;
+	float dxdy = dx*dy;
+	const Eigen::Vector3f* bp = mat +ix+iy*width;
+
+
+	return dxdy * (*(const Eigen::Vector3f*)(bp+1+width))[0]
+	        + (dy-dxdy) * (*(const Eigen::Vector3f*)(bp+width))[0]
+	        + (dx-dxdy) * (*(const Eigen::Vector3f*)(bp+1))[0]
+			+ (1-dx-dy+dxdy) * (*(const Eigen::Vector3f*)(bp))[0];
+}
+
+EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement13BiLin(const float* const mat, const float x, const float y, const int width)
+{
+	int ix = (int)x;
+	int iy = (int)y;
+	const float* bp = mat +ix+iy*width;
+
+	float tl = *(bp);
+	float tr = *(bp+1);
+	float bl = *(bp+width);
+	float br = *(bp+width+1);
+
+	float dx = x - ix;
+	float dy = y - iy;
+	float topInt = dx * tr + (1-dx) * tl;
+	float botInt = dx * br + (1-dx) * bl;
+	float leftInt = dy * bl + (1-dy) * tl;
+	float rightInt = dy * br + (1-dy) * tr;
+
+	return Eigen::Vector3f(
+			dx * rightInt + (1-dx) * leftInt,
+			rightInt-leftInt,
+			botInt-topInt);
+}
+
+EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33BiLin(const Eigen::Vector3f* const mat, const float x, const float y, const int width)
+{
+	int ix = (int)x;
+	int iy = (int)y;
+	const Eigen::Vector3f* bp = mat +ix+iy*width;
+
+	float tl = (*(bp))[0];
+	float tr = (*(bp+1))[0];
+	float bl = (*(bp+width))[0];
+	float br = (*(bp+width+1))[0];
+
+	float dx = x - ix;
+	float dy = y - iy;
+	float topInt = dx * tr + (1-dx) * tl;
+	float botInt = dx * br + (1-dx) * bl;
+	float leftInt = dy * bl + (1-dy) * tl;
+	float rightInt = dy * br + (1-dy) * tr;
+
+	return Eigen::Vector3f(
+			dx * rightInt + (1-dx) * leftInt,
+			rightInt-leftInt,
+			botInt-topInt);
+}
+EIGEN_ALWAYS_INLINE float getInterpolatedElement11Cub(const float* const p, const float x)	// for x=0, this returns p[1].
+{
+	return p[1] + 0.5f * x*(p[2] - p[0] + x*(2.0f*p[0] - 5.0f*p[1] + 4.0f*p[2] - p[3] + x*(3.0f*(p[1] - p[2]) + p[3] - p[0])));
+}
+
+EIGEN_ALWAYS_INLINE Eigen::Vector2f getInterpolatedElement12Cub(const float* const p, const float x)	// for x=0, this returns p[1].
+{
+	float c1 = 0.5f * (p[2] - p[0]);
+	float c2 = p[0]-2.5f*p[1]+2*p[2]-0.5f*p[3];
+	float c3 = 0.5f*(3.0f*(p[1]-p[2])+p[3]-p[0]);
+	float xx = x*x;
+	float xxx = xx*x;
+	return Eigen::Vector2f(p[1] + x*c1 + xx*c2 + xxx*c3, c1 + x*2.0f*c2 + xx*3.0f*c3);
+}
+EIGEN_ALWAYS_INLINE Eigen::Vector2f getInterpolatedElement32Cub(const Eigen::Vector3f* const p, const float x)	// for x=0, this returns p[1].
+{
+	float c1 = 0.5f * (p[2][0] - p[0][0]);
+	float c2 = p[0][0]-2.5f*p[1][0]+2*p[2][0]-0.5f*p[3][0];
+	float c3 = 0.5f*(3.0f*(p[1][0]-p[2][0])+p[3][0]-p[0][0]);
+	float xx = x*x;
+	float xxx = xx*x;
+	return Eigen::Vector2f(p[1][0] + x*c1 + xx*c2 + xxx*c3, c1 + x*2.0f*c2 + xx*3.0f*c3);
+}
+
+EIGEN_ALWAYS_INLINE float getInterpolatedElement11BiCub(const float* const mat, const float x, const float y, const int width)
+{
+	int ix = (int)x;
+	int iy = (int)y;
+	float dx = x - ix;
+	const float* bp = mat +ix+iy*width;
+
+	float val[4];
+	val[0] =getInterpolatedElement11Cub(bp-width-1, dx);
+	val[1] =getInterpolatedElement11Cub(bp-1, dx);
+	val[2] =getInterpolatedElement11Cub(bp+width-1, dx);
+	val[3] =getInterpolatedElement11Cub(bp+2*width-1, dx);
+
+	float dy = y - iy;
+	return getInterpolatedElement11Cub(val, dy);
+}
+EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement13BiCub(const float* const mat, const float x, const float y, const int width)
+{
+	int ix = (int)x;
+	int iy = (int)y;
+	float dx = x - ix;
+	const float* bp = mat +ix+iy*width;
+
+	float val[4];
+	float grad[4];
+	Eigen::Vector2f v = getInterpolatedElement12Cub(bp-width-1, dx);
+	val[0] = v[0]; grad[0] = v[1];
+
+	v = getInterpolatedElement12Cub(bp-1, dx);
+	val[1] = v[0]; grad[1] = v[1];
+
+	v = getInterpolatedElement12Cub(bp+width-1, dx);
+	val[2] = v[0]; grad[2] = v[1];
+
+	v = getInterpolatedElement12Cub(bp+2*width-1, dx);
+	val[3] = v[0]; grad[3] = v[1];
+
+	float dy = y - iy;
+	v = getInterpolatedElement12Cub(val, dy);
+
+	return Eigen::Vector3f(v[0], getInterpolatedElement11Cub(grad, dy), v[1]);
+}
+
+EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33BiCub(const Eigen::Vector3f* const mat, const float x, const float y, const int width)
+{
+	int ix = (int)x;
+	int iy = (int)y;
+	float dx = x - ix;
+	const Eigen::Vector3f* bp = mat +ix+iy*width;
+
+	float val[4];
+	float grad[4];
+	Eigen::Vector2f v = getInterpolatedElement32Cub(bp-width-1, dx);
+	val[0] = v[0]; grad[0] = v[1];
+
+	v = getInterpolatedElement32Cub(bp-1, dx);
+	val[1] = v[0]; grad[1] = v[1];
+
+	v = getInterpolatedElement32Cub(bp+width-1, dx);
+	val[2] = v[0]; grad[2] = v[1];
+
+	v = getInterpolatedElement32Cub(bp+2*width-1, dx);
+	val[3] = v[0]; grad[3] = v[1];
+
+	float dy = y - iy;
+	v = getInterpolatedElement12Cub(val, dy);
+
+	return Eigen::Vector3f(v[0], getInterpolatedElement11Cub(grad, dy), v[1]);
+}
+
+EIGEN_ALWAYS_INLINE Eigen::Vector4f getInterpolatedElement44(const Eigen::Vector4f* const mat, const float x, const float y, const int width)
+{
+	int ix = (int)x;
+	int iy = (int)y;
+	float dx = x - ix;
+	float dy = y - iy;
+	float dxdy = dx*dy;
+	const Eigen::Vector4f* bp = mat +ix+iy*width;
+
+
+	return dxdy * *(bp+1+width)
+	        + (dy-dxdy) * *(bp+width)
+	        + (dx-dxdy) * *(bp+1)
+			+ (1-dx-dy+dxdy) * *(bp);
+}
+
+EIGEN_ALWAYS_INLINE Eigen::Vector2f getInterpolatedElement42(const Eigen::Vector4f* const mat, const float x, const float y, const int width)
+{
+	int ix = (int)x;
+	int iy = (int)y;
+	float dx = x - ix;
+	float dy = y - iy;
+	float dxdy = dx*dy;
+	const Eigen::Vector4f* bp = mat +ix+iy*width;
+
+
+	return dxdy * *(const Eigen::Vector2f*)(bp+1+width)
+	        + (dy-dxdy) * *(const Eigen::Vector2f*)(bp+width)
+	        + (dx-dxdy) * *(const Eigen::Vector2f*)(bp+1)
+			+ (1-dx-dy+dxdy) * *(const Eigen::Vector2f*)(bp);
+}
+
+
+
+inline Vec3f makeRainbowf3F(float id)
+{
+	id *= freeDebugParam3;
+	if(id < 0)
+		return Vec3f(1,1,1);
+
+	int icP = id;
+	float ifP = id-icP;
+	icP = icP%3;
+
+	if(icP == 0) return Vec3f((1-ifP), ifP,     0);
+	if(icP == 1) return Vec3f(0,           (1-ifP), ifP);
+	if(icP == 2) return Vec3f(ifP,     0,           (1-ifP));
+	assert(false);
+	return Vec3f(1,1,1);
+}
+
+inline Vec3b makeRainbow3B(float id)
+{
+	id *= freeDebugParam3;
+	if(!(id > 0))
+		return Vec3b(255,255,255);
+
+	int icP = id;
+	float ifP = id-icP;
+	icP = icP%3;
+
+	if(icP == 0) return Vec3b(255*(1-ifP), 255*ifP,     0);
+	if(icP == 1) return Vec3b(0,           255*(1-ifP), 255*ifP);
+	if(icP == 2) return Vec3b(255*ifP,     0,           255*(1-ifP));
+	return Vec3b(255,255,255);
+}
+
+inline Vec3b makeJet3B(float id)
+{
+	if(id <= 0) return Vec3b(128,0,0);
+	if(id >= 1) return Vec3b(0,0,128);
+
+	int icP = (id*8);
+	float ifP = (id*8)-icP;
+
+	if(icP == 0) return Vec3b(255*(0.5+0.5*ifP), 		    		  0,     					0);
+	if(icP == 1) return Vec3b(255, 					  255*(0.5*ifP),     					0);
+	if(icP == 2) return Vec3b(255, 				  255*(0.5+0.5*ifP),     					0);
+	if(icP == 3) return Vec3b(255*(1-0.5*ifP), 					255,     					255*(0.5*ifP));
+	if(icP == 4) return Vec3b(255*(0.5-0.5*ifP), 					255,     					255*(0.5+0.5*ifP));
+	if(icP == 5) return Vec3b(0, 						255*(1-0.5*ifP),     					255);
+	if(icP == 6) return Vec3b(0, 						255*(0.5-0.5*ifP),     					255);
+	if(icP == 7) return Vec3b(0, 					  				  0,     					255*(1-0.5*ifP));
+	return Vec3b(255,255,255);
+}
+
+inline Vec3b makeRedGreen3B(float val)	// 0 = red, 1=green, 0.5=yellow.
+{
+	if(val < 0) return Vec3b(0,0,255);
+	else if(val < 0.5) return Vec3b(0,255*2*val,255);
+	else if(val < 1) return Vec3b(0,255,255-255*2*(val-0.5));
+	else return Vec3b(0,255,0);
+
+}
+
+
+
+
+
+}

src/util/settings.cpp

@@ -0,0 +1,314 @@
+/**
+* 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 "util/settings.h"
+#include <boost/bind.hpp>
+
+
+namespace dso
+{
+int pyrLevelsUsed = PYR_LEVELS;
+
+
+/* Parameters controlling when KF's are taken */
+float setting_keyframesPerSecond = 0;   // if !=0, takes a fixed number of KF per second.
+bool setting_realTimeMaxKF = false;   // if true, takes as many KF's as possible (will break the system if the camera stays stationary)
+float setting_maxShiftWeightT= 0.04f * (640+480);
+float setting_maxShiftWeightR= 0.0f * (640+480);
+float setting_maxShiftWeightRT= 0.02f * (640+480);
+float setting_kfGlobalWeight = 1;   // general weight on threshold, the larger the more KF's are taken (e.g., 2 = double the amount of KF's).
+float setting_maxAffineWeight= 2;
+
+
+/* initial hessian values to fix unobservable dimensions / priors on affine lighting parameters.
+ */
+float setting_idepthFixPrior = 50*50;
+float setting_idepthFixPriorMargFac = 600*600;
+float setting_initialRotPrior = 1e11;
+float setting_initialTransPrior = 1e10;
+float setting_initialAffBPrior = 1e14;
+float setting_initialAffAPrior = 1e14;
+float setting_initialCalibHessian = 5e9;
+
+
+
+
+
+/* some modes for solving the resulting linear system (e.g. orthogonalize wrt. unobservable dimensions) */
+int setting_solverMode = SOLVER_FIX_LAMBDA | SOLVER_ORTHOGONALIZE_X_LATER;
+double setting_solverModeDelta = 0.00001;
+bool setting_forceAceptStep = true;
+
+
+
+/* some thresholds on when to activate / marginalize points */
+float setting_minIdepthH_act = 100;
+float setting_minIdepthH_marg = 50;
+
+
+
+float setting_desiredImmatureDensity = 1500; // immature points per frame
+float setting_desiredPointDensity = 2000; // aimed total points in the active window.
+float setting_minPointsRemaining = 0.05;  // marg a frame if less than X% points remain.
+float setting_maxLogAffFacInWindow = 0.7; // marg a frame if factor between intensities to current frame is larger than 1/X or X.
+
+
+int   setting_minFrames = 5; // min frames in window.
+int   setting_maxFrames = 7; // max frames in window.
+int   setting_minFrameAge = 1;
+int   setting_maxOptIterations=6; // max GN iterations.
+int   setting_minOptIterations=1; // min GN iterations.
+float setting_thOptIterations=1.2; // factor on break threshold for GN iteration (larger = break earlier)
+
+
+
+
+
+/* Outlier Threshold on photometric energy */
+float setting_outlierTH = 12*12;					// higher -> less strict
+float setting_outlierTHSumComponent = 50*50; 		// higher -> less strong gradient-based reweighting .
+
+
+
+
+int setting_pattern = 8;						// point pattern used. DISABLED.
+float setting_margWeightFac = 0.5*0.5;          // factor on hessian when marginalizing, to account for inaccurate linearization points.
+
+
+/* when to re-track a frame */
+float setting_reTrackThreshold = 1.5; // (larger = re-track more often)
+
+
+
+/* require some minimum number of residuals for a point to become valid */
+int   setting_minGoodActiveResForMarg=3;
+int   setting_minGoodResForMarg=4;
+
+
+
+
+
+
+// 0 = nothing.
+// 1 = apply inv. response.
+// 2 = apply inv. response & remove V.
+int setting_photometricCalibration = 2;
+bool setting_useExposure = true;
+float setting_affineOptModeA = 1e12; //-1: fix. >=0: optimize (with prior, if > 0).
+float setting_affineOptModeB = 1e8; //-1: fix. >=0: optimize (with prior, if > 0).
+
+int setting_gammaWeightsPixelSelect = 1; // 1 = use original intensity for pixel selection; 0 = use gamma-corrected intensity.
+
+
+
+
+float setting_huberTH = 9; // Huber Threshold
+
+
+
+
+
+// parameters controlling adaptive energy threshold computation.
+float setting_frameEnergyTHConstWeight = 0.5;
+float setting_frameEnergyTHN = 0.7f;
+float setting_frameEnergyTHFacMedian = 1.5;
+float setting_overallEnergyTHWeight = 1;
+float setting_coarseCutoffTH = 20;
+
+
+
+
+
+// parameters controlling pixel selection
+float setting_minGradHistCut = 0.5;
+float setting_minGradHistAdd = 7;
+float setting_gradDownweightPerLevel = 0.75;
+bool  setting_selectDirectionDistribution = true;
+
+
+
+
+
+
+/* settings controling initial immature point tracking */
+float setting_maxPixSearch = 0.027; // max length of the ep. line segment searched during immature point tracking. relative to image resolution.
+float setting_minTraceQuality = 3;
+int setting_minTraceTestRadius = 2;
+int setting_GNItsOnPointActivation = 3;
+float setting_trace_stepsize = 1.0;				// stepsize for initial discrete search.
+int setting_trace_GNIterations = 3;				// max # GN iterations
+float setting_trace_GNThreshold = 0.1;				// GN stop after this stepsize.
+float setting_trace_extraSlackOnTH = 1.2;			// for energy-based outlier check, be slightly more relaxed by this factor.
+float setting_trace_slackInterval = 1.5;			// if pixel-interval is smaller than this, leave it be.
+float setting_trace_minImprovementFactor = 2;		// if pixel-interval is smaller than this, leave it be.
+
+
+
+
+// for benchmarking different undistortion settings
+float benchmarkSetting_fxfyfac = 0;
+int benchmarkSetting_width = 0;
+int benchmarkSetting_height = 0;
+float benchmark_varNoise = 0;
+float benchmark_varBlurNoise = 0;
+float benchmark_initializerSlackFactor = 1;
+int benchmark_noiseGridsize = 3;
+
+
+float freeDebugParam1 = 1;
+float freeDebugParam2 = 1;
+float freeDebugParam3 = 1;
+float freeDebugParam4 = 1;
+float freeDebugParam5 = 1;
+
+
+
+bool disableReconfigure=false;
+bool debugSaveImages = false;
+bool multiThreading = true;
+bool disableAllDisplay = false;
+bool setting_onlyLogKFPoses = true;
+bool setting_logStuff = true;
+
+
+
+bool goStepByStep = false;
+
+
+bool setting_render_displayCoarseTrackingFull=false;
+bool setting_render_renderWindowFrames=true;
+bool setting_render_plotTrackingFull = false;
+bool setting_render_display3D = true;
+bool setting_render_displayResidual = true;
+bool setting_render_displayVideo = true;
+bool setting_render_displayDepth = true;
+
+bool setting_fullResetRequested = false;
+
+bool setting_debugout_runquiet = false;
+
+int sparsityFactor = 5;	// not actually a setting, only some legacy stuff for coarse initializer.
+
+
+void handleKey(char k)
+{
+	char kkk = k;
+	switch(kkk)
+	{
+	case 'd': case 'D':
+		freeDebugParam5 = ((int)(freeDebugParam5+1))%10;
+		printf("new freeDebugParam5: %f!\n", freeDebugParam5);
+		break;
+	case 's': case 'S':
+		freeDebugParam5 = ((int)(freeDebugParam5-1+10))%10;
+		printf("new freeDebugParam5: %f!\n", freeDebugParam5);
+		break;
+	}
+
+}
+
+
+
+
+int staticPattern[10][40][2] = {
+		{{0,0}, 	  {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},	// .
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}},
+
+		{{0,-1},	  {-1,0},	   {0,0},	    {1,0},	     {0,1}, 	  {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},	// +
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}},
+
+		{{-1,-1},	  {1,1},	   {0,0},	    {-1,1},	     {1,-1}, 	  {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},	// x
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}},
+
+		{{-1,-1},	  {-1,0},	   {-1,1},		{-1,0},		 {0,0},		  {0,1},	   {1,-1},		{1,0},		 {1,1},       {-100,-100},	// full-tight
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}},
+
+		{{0,-2},	  {-1,-1},	   {1,-1},		{-2,0},		 {0,0},		  {2,0},	   {-1,1},		{1,1},		 {0,2},       {-100,-100},	// full-spread-9
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}},
+
+		{{0,-2},	  {-1,-1},	   {1,-1},		{-2,0},		 {0,0},		  {2,0},	   {-1,1},		{1,1},		 {0,2},       {-2,-2},   // full-spread-13
+		 {-2,2},      {2,-2},      {2,2},       {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}},
+
+		{{-2,-2},     {-2,-1}, {-2,-0}, {-2,1}, {-2,2}, {-1,-2}, {-1,-1}, {-1,-0}, {-1,1}, {-1,2}, 										// full-25
+		 {-0,-2},     {-0,-1}, {-0,-0}, {-0,1}, {-0,2}, {+1,-2}, {+1,-1}, {+1,-0}, {+1,1}, {+1,2},
+		 {+2,-2}, 	  {+2,-1}, {+2,-0}, {+2,1}, {+2,2}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}},
+
+		{{0,-2},	  {-1,-1},	   {1,-1},		{-2,0},		 {0,0},		  {2,0},	   {-1,1},		{1,1},		 {0,2},       {-2,-2},   // full-spread-21
+		 {-2,2},      {2,-2},      {2,2},       {-3,-1},     {-3,1},      {3,-1}, 	   {3,1},       {1,-3},      {-1,-3},     {1,3},
+		 {-1,3},      {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}},
+
+		{{0,-2},	  {-1,-1},	   {1,-1},		{-2,0},		 {0,0},		  {2,0},	   {-1,1},		{0,2},		 {-100,-100}, {-100,-100},	// 8 for SSE efficiency
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100},
+		 {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}, {-100,-100}},
+
+		{{-4,-4},     {-4,-2}, {-4,-0}, {-4,2}, {-4,4}, {-2,-4}, {-2,-2}, {-2,-0}, {-2,2}, {-2,4}, 										// full-45-SPREAD
+		 {-0,-4},     {-0,-2}, {-0,-0}, {-0,2}, {-0,4}, {+2,-4}, {+2,-2}, {+2,-0}, {+2,2}, {+2,4},
+		 {+4,-4}, 	  {+4,-2}, {+4,-0}, {+4,2}, {+4,4}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200},
+		 {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}, {-200,-200}},
+};
+
+int staticPatternNum[10] = {
+		1,
+		5,
+		5,
+		9,
+		9,
+		13,
+		25,
+		21,
+		8,
+		25
+};
+
+int staticPatternPadding[10] = {
+		1,
+		1,
+		1,
+		1,
+		2,
+		2,
+		2,
+		3,
+		2,
+		4
+};
+
+
+}

src/util/settings.h

@@ -0,0 +1,231 @@
+/**
+* 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/>.
+*/
+
+
+
+#pragma once
+
+#include <string.h>
+#include <string>
+#include <cmath>
+
+
+namespace dso
+{
+#define SOLVER_SVD (int)1
+#define SOLVER_ORTHOGONALIZE_SYSTEM (int)2
+#define SOLVER_ORTHOGONALIZE_POINTMARG (int)4
+#define SOLVER_ORTHOGONALIZE_FULL (int)8
+#define SOLVER_SVD_CUT7 (int)16
+#define SOLVER_REMOVE_POSEPRIOR (int)32
+#define SOLVER_USE_GN (int)64
+#define SOLVER_FIX_LAMBDA (int)128
+#define SOLVER_ORTHOGONALIZE_X (int)256
+#define SOLVER_MOMENTUM (int)512
+#define SOLVER_STEPMOMENTUM (int)1024
+#define SOLVER_ORTHOGONALIZE_X_LATER (int)2048
+
+
+// ============== PARAMETERS TO BE DECIDED ON COMPILE TIME =================
+#define PYR_LEVELS 6
+extern int pyrLevelsUsed;
+
+
+
+extern float setting_keyframesPerSecond;
+extern bool setting_realTimeMaxKF;
+extern float setting_maxShiftWeightT;
+extern float setting_maxShiftWeightR;
+extern float setting_maxShiftWeightRT;
+extern float setting_maxAffineWeight;
+extern float setting_kfGlobalWeight;
+
+
+
+extern float setting_idepthFixPrior;
+extern float setting_idepthFixPriorMargFac;
+extern float setting_initialRotPrior;
+extern float setting_initialTransPrior;
+extern float setting_initialAffBPrior;
+extern float setting_initialAffAPrior;
+extern float setting_initialCalibHessian;
+
+extern int setting_solverMode;
+extern double setting_solverModeDelta;
+
+
+extern float setting_minIdepthH_act;
+extern float setting_minIdepthH_marg;
+
+
+
+extern float setting_maxIdepth;
+extern float setting_maxPixSearch;
+extern float setting_desiredImmatureDensity;			// done
+extern float setting_desiredPointDensity;			// done
+extern float setting_minPointsRemaining;
+extern float setting_maxLogAffFacInWindow;
+extern int setting_minFrames;
+extern int setting_maxFrames;
+extern int setting_minFrameAge;
+extern int setting_maxOptIterations;
+extern int setting_minOptIterations;
+extern float setting_thOptIterations;
+extern float setting_outlierTH;
+extern float setting_outlierTHSumComponent;
+
+
+
+extern int setting_pattern;
+extern float setting_margWeightFac;
+extern int setting_GNItsOnPointActivation;
+
+
+extern float setting_minTraceQuality;
+extern int setting_minTraceTestRadius;
+extern float setting_reTrackThreshold;
+
+
+extern int   setting_minGoodActiveResForMarg;
+extern int   setting_minGoodResForMarg;
+extern int   setting_minInlierVotesForMarg;
+
+
+
+
+extern int setting_photometricCalibration;
+extern bool setting_useExposure;
+extern float setting_affineOptModeA;
+extern float setting_affineOptModeB;
+extern int setting_gammaWeightsPixelSelect;
+
+
+
+extern bool setting_forceAceptStep;
+
+
+
+extern float setting_huberTH;
+
+
+extern bool setting_logStuff;
+extern float benchmarkSetting_fxfyfac;
+extern int benchmarkSetting_width;
+extern int benchmarkSetting_height;
+extern float benchmark_varNoise;
+extern float benchmark_varBlurNoise;
+extern int benchmark_noiseGridsize;
+extern float benchmark_initializerSlackFactor;
+
+extern float setting_frameEnergyTHConstWeight;
+extern float setting_frameEnergyTHN;
+
+extern float setting_frameEnergyTHFacMedian;
+extern float setting_overallEnergyTHWeight;
+extern float setting_coarseCutoffTH;
+
+extern float setting_minGradHistCut;
+extern float setting_minGradHistAdd;
+extern float setting_gradDownweightPerLevel;
+extern bool  setting_selectDirectionDistribution;
+
+
+
+extern float setting_trace_stepsize;
+extern int setting_trace_GNIterations;
+extern float setting_trace_GNThreshold;
+extern float setting_trace_extraSlackOnTH;
+extern float setting_trace_slackInterval;
+extern float setting_trace_minImprovementFactor;
+
+
+extern bool setting_render_displayCoarseTrackingFull;
+extern bool setting_render_renderWindowFrames;
+extern bool setting_render_plotTrackingFull;
+extern bool setting_render_display3D;
+extern bool setting_render_displayResidual;
+extern bool setting_render_displayVideo;
+extern bool setting_render_displayDepth;
+
+extern bool setting_fullResetRequested;
+
+extern bool setting_debugout_runquiet;
+
+extern bool disableAllDisplay;
+extern bool disableReconfigure;
+
+
+extern bool setting_onlyLogKFPoses;
+
+
+
+
+extern bool debugSaveImages;
+
+
+extern int sparsityFactor;
+extern bool goStepByStep;
+extern bool plotStereoImages;
+extern bool multiThreading;
+
+extern float freeDebugParam1;
+extern float freeDebugParam2;
+extern float freeDebugParam3;
+extern float freeDebugParam4;
+extern float freeDebugParam5;
+
+
+void handleKey(char k);
+
+
+
+
+extern int staticPattern[10][40][2];
+extern int staticPatternNum[10];
+extern int staticPatternPadding[10];
+
+
+
+
+//#define patternNum staticPatternNum[setting_pattern]
+//#define patternP staticPattern[setting_pattern]
+//#define patternPadding staticPatternPadding[setting_pattern]
+
+//
+#define patternNum 8
+#define patternP staticPattern[8]
+#define patternPadding 2
+
+
+
+
+
+
+
+
+
+
+
+
+
+}