Scale down image in height

cost_function.h

@@ -58,7 +58,7 @@ struct NegativeLogDataTermGrd
 			const float sigmaR2 = cf * cf * (tmp * tmp * sigmaH * sigmaH + sigmaA * sigmaA);
 			const float sigma = sqrtf(sigmaD * sigmaD + sigmaR2);
 
-			const float fn = groundDisparity[v];
+			const float fn = std::max(groundDisparity[v], 0.f);
 			const float ANorm = 0.5f * (erff((dmax - fn) / (SQRT2 * sigma)) - erff((dmin - fn) / (SQRT2 * sigma)));
 			nLogPGaussian_[v] = logf(ANorm) + logf(sigma * sqrtf(2.f * PI)) - logf(1.f - pOut);
 			fn_[v] = fn;

multilayer_stixel_world.cpp

@@ -107,37 +107,54 @@ static Line calcRoadModelVD(const cv::Mat1f& disparity, const CameraParameters&
 	return Line(a, b);
 }
 
-MultiLayerStixelWorld::MultiLayerStixelWorld(const Parameters& param) : param_(param)
+static void computeColumns(const cv::Mat1f& src, cv::Mat1f& dst, int stixelWidth, float verticalScaleDown = -1)
 {
-}
-
-void MultiLayerStixelWorld::compute(const cv::Mat& disparity, std::vector<Stixel>& stixels)
-{
-	CV_Assert(disparity.type() == CV_32F);
-
-	const int stixelWidth = param_.stixelWidth;
-	const int w = disparity.cols / stixelWidth;
-	const int h = disparity.rows;
-	const int fnmax = static_cast<int>(param_.dmax);
+	const int w = src.cols / stixelWidth;
+	const int h = src.rows;
 
 	// compute horizontal median of each column
-	Matrixf columns(w, h);
+	dst.create(w, h);
 	std::vector<float> buf(stixelWidth);
 	for (int v = 0; v < h; v++)
 	{
 		for (int u = 0; u < w; u++)
 		{
 			// compute horizontal median
 			for (int du = 0; du < stixelWidth; du++)
-				buf[du] = disparity.at<float>(v, u * stixelWidth + du);
+				buf[du] = src(v, u * stixelWidth + du);
 			std::sort(std::begin(buf), std::end(buf));
 			const float m = buf[stixelWidth / 2];
 
-			// reverse order of data so that v = 0 points the bottom
-			columns(u, h - 1 - v) = m;
+			// disparities are stored in reverse order so that v = 0 points the bottom
+			// and transposed for memory efficiency
+			dst(u, h - 1 - v) = m;
 		}
 	}
 
+	// scale down the image in height
+	if (verticalScaleDown > 1.f)
+		cv::resize(dst, dst, cv::Size(), 1. / verticalScaleDown, 1., cv::INTER_NEAREST);
+}
+
+MultiLayerStixelWorld::MultiLayerStixelWorld(const Parameters& param) : param_(param)
+{
+}
+
+void MultiLayerStixelWorld::compute(const cv::Mat& disparity, std::vector<Stixel>& stixels)
+{
+	CV_Assert(disparity.type() == CV_32F);
+
+	const int stixelWidth = param_.stixelWidth;
+	const int fnmax = static_cast<int>(param_.dmax);
+	const float verticalScaleDown = param_.verticalScaleDown;
+
+	// reduce and reorder disparity map
+	cv::Mat1f columns;
+	computeColumns(disparity, columns, stixelWidth, verticalScaleDown);
+
+	const int w = columns.rows;
+	const int h = columns.cols;
+
 	// get camera parameters
 	CameraParameters camera = param_.camera;
 
@@ -340,6 +357,13 @@ void MultiLayerStixelWorld::compute(const cv::Mat& disparity, std::vector<Stixel
 				stixel.vB = h - 1 - (p2.y + 1);
 				stixel.width = stixelWidth;
 				stixel.disp = dispTable(u, p1.y, p1.x);
+
+				if (verticalScaleDown > 1.f)
+				{
+					stixel.vT = cvRound(verticalScaleDown * stixel.vT);
+					stixel.vB = cvRound(verticalScaleDown * stixel.vB);
+				}
+
 				stixels.push_back(stixel);
 			}
 			minPos = p2;

multilayer_stixel_world.h

@@ -88,6 +88,9 @@ class MultiLayerStixelWorld
 		// camera parameters
 		CameraParameters camera;
 
+		// scale down factor in height
+		float verticalScaleDown;
+
 		// default settings
 		Parameters()
 		{
@@ -104,8 +107,8 @@ class MultiLayerStixelWorld
 			sigmaS = 0.2f;
 
 			// camera height and tilt uncertainty
-			sigmaH = 0.05f;
-			sigmaA = 0.05f * static_cast<float>(CV_PI) / 180.f;
+			sigmaH = 0.01f;
+			sigmaA = 0.01f * static_cast<float>(CV_PI) / 180.f;
 
 			// outlier rate
 			pOutG = 0.15f;
@@ -131,6 +134,9 @@ class MultiLayerStixelWorld
 
 			// camera parameters
 			camera = CameraParameters();
+
+			// scale down factor in height
+			verticalScaleDown = 2.f;
 		}
 	};
 
@@ -143,4 +149,4 @@ class MultiLayerStixelWorld
 	Parameters param_;
 };
 
-#endif // !__STIXEL_WORLD_H__
+#endif // !__STIXEL_WORLD_H__