From b9d9d8b3f11a5ec0a8a8c13ad15b0d83ed84f587 Mon Sep 17 00:00:00 2001
From: Rayope <chabaud.morgan@gmail.com>
Date: Wed, 26 Jul 2017 10:28:25 +0200
Subject: [PATCH] Corrected a bug (segfault) occuring when no positive idepth
 is available

---
 src/FullSystem/CoarseTracker.cpp | 86 ++++++++++++++++----------------
 src/FullSystem/FullSystem.cpp    |  9 ++--
 2 files changed, 47 insertions(+), 48 deletions(-)

diff --git a/src/FullSystem/CoarseTracker.cpp b/src/FullSystem/CoarseTracker.cpp
index e56662586..e128cf0c4 100644
--- a/src/FullSystem/CoarseTracker.cpp
+++ b/src/FullSystem/CoarseTracker.cpp
@@ -721,27 +721,39 @@ bool CoarseTracker::trackNewestCoarse(
 
 void CoarseTracker::debugPlotIDepthMap(float* minID_pt, float* maxID_pt, std::vector<IOWrap::Output3DWrapper*> &wraps)
 {
-    if(w[1] == 0) return;
+	if(w[1] == 0) return;
 
+	const int lvl = 0;
+	const int wl = w[lvl];
+	const int hl = h[lvl];
 
-	int lvl = 0;
+	std::vector<float> allID;
+	for(int i=0; i < (hl*wl); i++)
+	{
+		if(idepth[lvl][i] > 0)
+			allID.push_back(idepth[lvl][i]);
+	}
 
+	// Initialise debug image
+	MinimalImageB3 mf(wl, hl);
+	mf.setBlack();
+	for(int i=0; i < (hl*wl); i++)
+	{
+		int c = lastRef->dIp[lvl][i][0]*0.9f;
+		if(c>255) c=255;
+		mf.at(i) = Vec3b(c,c,c);
+	}
+		
+	// Check in case all idepths are 0
+	if(!allID.empty())
 	{
-		std::vector<float> allID;
-		for(int i=0;i<h[lvl]*w[lvl];i++)
-		{
-			if(idepth[lvl][i] > 0)
-				allID.push_back(idepth[lvl][i]);
-		}
 		std::sort(allID.begin(), allID.end());
-		int n = allID.size()-1;
 
-		float minID_new = allID[(int)(n*0.05)];
-		float maxID_new = allID[(int)(n*0.95)];
+		const int n = allID.size()-1;
 
-		float minID, maxID;
-		minID = minID_new;
-		maxID = maxID_new;
+		float	minID = allID[(int)(n * 0.05)],
+			maxID = allID[(int)(n * 0.95)];
+		
 		if(minID_pt!=0 && maxID_pt!=0)
 		{
 			if(*minID_pt < 0 || *maxID_pt < 0)
@@ -751,38 +763,26 @@ void CoarseTracker::debugPlotIDepthMap(float* minID_pt, float* maxID_pt, std::ve
 			}
 			else
 			{
-
 				// slowly adapt: change by maximum 10% of old span.
-				float maxChange = 0.3*(*maxID_pt - *minID_pt);
-
+				const float maxChange = 0.3*(*maxID_pt - *minID_pt);
+	
 				if(minID < *minID_pt - maxChange)
 					minID = *minID_pt - maxChange;
 				if(minID > *minID_pt + maxChange)
 					minID = *minID_pt + maxChange;
-
-
+	
 				if(maxID < *maxID_pt - maxChange)
 					maxID = *maxID_pt - maxChange;
 				if(maxID > *maxID_pt + maxChange)
 					maxID = *maxID_pt + maxChange;
-
+	
 				*maxID_pt = maxID;
 				*minID_pt = minID;
 			}
 		}
 
-
-		MinimalImageB3 mf(w[lvl], h[lvl]);
-		mf.setBlack();
-		for(int i=0;i<h[lvl]*w[lvl];i++)
-		{
-			int c = lastRef->dIp[lvl][i][0]*0.9f;
-			if(c>255) c=255;
-			mf.at(i) = Vec3b(c,c,c);
-		}
-		int wl = w[lvl];
-		for(int y=3;y<h[lvl]-3;y++)
-			for(int x=3;x<wl-3;x++)
+		for(int y=3; y<(hl-3); y++)
+			for(int x=3; x<(wl-3); x++)
 			{
 				int idx=x+y*wl;
 				float sid=0, nid=0;
@@ -796,24 +796,22 @@ void CoarseTracker::debugPlotIDepthMap(float* minID_pt, float* maxID_pt, std::ve
 
 				if(bp[0] > 0 || nid >= 3)
 				{
-					float id = ((sid / nid)-minID) / ((maxID-minID));
+					const float id = ((sid / nid)-minID) / (maxID-minID);
 					mf.setPixelCirc(x,y,makeJet3B(id));
 					//mf.at(idx) = makeJet3B(id);
 				}
 			}
-        //IOWrap::displayImage("coarseDepth LVL0", &mf, false);
-
+	}
+	//IOWrap::displayImage("coarseDepth LVL0", &mf, false);
 
-        for(IOWrap::Output3DWrapper* ow : wraps)
-            ow->pushDepthImage(&mf);
-
-		if(debugSaveImages)
-		{
-			char buf[1000];
-			snprintf(buf, 1000, "images_out/predicted_%05d_%05d.png", lastRef->shell->id, refFrameID);
-			IOWrap::writeImage(buf,&mf);
-		}
+	for(IOWrap::Output3DWrapper* ow : wraps)
+		ow->pushDepthImage(&mf);
 
+	if(debugSaveImages)
+	{
+		char buf[1000];
+		snprintf(buf, 1000, "images_out/predicted_%05d_%05d.png", lastRef->shell->id, refFrameID);
+		IOWrap::writeImage(buf,&mf);
 	}
 }
 
diff --git a/src/FullSystem/FullSystem.cpp b/src/FullSystem/FullSystem.cpp
index 516f6ccec..31bd54899 100644
--- a/src/FullSystem/FullSystem.cpp
+++ b/src/FullSystem/FullSystem.cpp
@@ -1141,10 +1141,11 @@ void FullSystem::makeKeyFrame( FrameHessian* fh)
 		coarseTracker_forNewKF->makeK(&Hcalib);
 		coarseTracker_forNewKF->setCoarseTrackingRef(frameHessians);
 
-
-
-        coarseTracker_forNewKF->debugPlotIDepthMap(&minIdJetVisTracker, &maxIdJetVisTracker, outputWrapper);
-        coarseTracker_forNewKF->debugPlotIDepthMapFloat(outputWrapper);
+		if(!outputWrapper.empty() || debugSaveImages)
+		{
+			coarseTracker_forNewKF->debugPlotIDepthMap(&minIdJetVisTracker, &maxIdJetVisTracker, outputWrapper);
+			coarseTracker_forNewKF->debugPlotIDepthMapFloat(outputWrapper);
+		}
 	}