[Bugfix] Handle vertical scale down effect

gishi523 · Jan 13, 2019 · cec920b · cec920b
1 parent 749825c
commit cec920b
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 5 deletions.
diff --git a/cost_function.h b/cost_function.h
@@ -22,9 +22,9 @@ struct NegativeLogDataTermGrd
 	using CameraParameters = MultiLayerStixelWorld::CameraParameters;
 
 	NegativeLogDataTermGrd(float dmax, float dmin, float sigmaD, float pOut, float pInvC, float pInvD, const CameraParameters& camera,
-		const std::vector<float>& groundDisparity, float vhor, float sigmaH, float sigmaA)
+		const std::vector<float>& groundDisparity, float vhor, float sigmaH, float sigmaA, float vscale = 1.f)
 	{
-		init(dmax, dmin, sigmaD, pOut, pInvC, pInvD, camera, groundDisparity, vhor, sigmaH, sigmaA);
+		init(dmax, dmin, sigmaD, pOut, pInvC, pInvD, camera, groundDisparity, vhor, sigmaH, sigmaA, vscale);
 	}
 
 	inline float operator()(float d, int v) const
@@ -40,12 +40,12 @@ struct NegativeLogDataTermGrd
 
 	// pre-compute constant terms
 	void init(float dmax, float dmin, float sigmaD, float pOut, float pInvC, float pInvD, const CameraParameters& camera,
-		const std::vector<float>& groundDisparity, float vhor, float sigmaH, float sigmaA)
+		const std::vector<float>& groundDisparity, float vhor, float sigmaH, float sigmaA, float vscale)
 	{
 		// uniform distribution term
 		nLogPUniform_ = logf(dmax - dmin) - logf(pOut);
 
-		const float cf = camera.fu * camera.baseline / camera.height;
+		const float cf = camera.fu * camera.baseline / (vscale * camera.height);
 
 		// Gaussian distribution term
 		const int h = static_cast<int>(groundDisparity.size());

diff --git a/multilayer_stixel_world.cpp b/multilayer_stixel_world.cpp
@@ -141,6 +141,14 @@ static void computeColumns(const cv::Mat1f& src, cv::Mat1f& dst, int stixelWidth
 
 MultiLayerStixelWorld::MultiLayerStixelWorld(const Parameters& param) : param_(param)
 {
+	if (param.verticalScaleDown > 1.f)
+	{
+		// scale camera parameters
+		const float invScale = 1.f / param.verticalScaleDown;
+		param_.camera.v0 *= invScale;
+		param_.camera.tilt *= invScale;
+		param_.camera.height *= invScale;
+	}
 }
 
 void MultiLayerStixelWorld::compute(const cv::Mat& disparity, std::vector<Stixel>& stixels)
@@ -191,7 +199,7 @@ void MultiLayerStixelWorld::compute(const cv::Mat& disparity, std::vector<Stixel
 
 	// create data cost function of each segment
 	NegativeLogDataTermGrd dataTermG(param_.dmax, param_.dmin, param_.sigmaG, param_.pOutG, param_.pInvG, param_.pInvD,
-		camera, groundDisparity, vhor, param_.sigmaH, param_.sigmaA);
+		camera, groundDisparity, vhor, param_.sigmaH, param_.sigmaA, verticalScaleDown);
 	NegativeLogDataTermObj dataTermO(param_.dmax, param_.dmin, param_.sigmaO, param_.pOutO, param_.pInvO, param_.pInvD,
 		camera, param_.deltaz);
 	NegativeLogDataTermSky dataTermS(param_.dmax, param_.dmin, param_.sigmaS, param_.pOutS, param_.pInvS, param_.pInvD);