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);
+};
+}
+