Add auto road estimation

multilayer_stixel_world.cpp

@@ -7,6 +7,128 @@
 #include <omp.h>
 #endif
 
+// Implementation of road disparity estimation by RANSAC
+class RoadEstimation
+{
+public:
+
+	using CameraParameters = MultiLayerStixelWrold::CameraParameters;
+
+	struct Parameters
+	{
+		int samplingStep;      //!< pixel step of disparity sampling
+		int minDisparity;      //!< minimum disparity used for RANSAC
+
+		int maxIterations;     //!< number of iterations of RANSAC
+		float inlierRadius;    //!< inlier radius of RANSAC
+		float maxCameraHeight; //!< maximum acceptable camera height
+
+		// default settings
+		Parameters()
+		{
+			samplingStep = 2;
+			minDisparity = 10;
+
+			maxIterations = 32;
+			inlierRadius = 1;
+			maxCameraHeight = 5;
+		}
+	};
+
+	RoadEstimation(const Parameters& param = Parameters()) : param_(param)
+	{
+	}
+
+	cv::Vec2f compute(const cv::Mat1f& disparity, const CameraParameters& camera)
+	{
+		CV_Assert(disparity.type() == CV_32F);
+
+		const int w = disparity.rows;
+		const int h = disparity.cols;
+		const int dmin = param_.minDisparity;
+
+		// sample v-disparity points
+		points_.reserve(h * w);
+		points_.clear();
+		for (int u = 0; u < w; u += param_.samplingStep)
+		{
+			for (int v = 0; v < h; v += param_.samplingStep)
+			{
+				const float d = disparity.ptr<float>(u)[v];
+				if (d >= dmin)
+					points_.push_back(cv::Point2f(static_cast<float>(h - 1 - v), d));
+			}
+		}
+		if (points_.empty())
+			return cv::Vec2f(0, 0);
+
+		// estimate line by RANSAC
+		const int npoints = static_cast<int>(points_.size());
+		cv::RNG random;
+		cv::Vec2f bestLine(0, 0);
+		int maxInliers = 0;
+		for (int iter = 0; iter < param_.maxIterations; iter++)
+		{
+			const cv::Point2f& pt1 = points_[random.next() % npoints];
+			const cv::Point2f& pt2 = points_[random.next() % npoints];
+			const float a = (pt2.y - pt1.y) / (pt2.x - pt1.x);
+			const float b = -a * pt1.x + pt1.y;
+
+			// estimate camera tilt and height
+			const float tilt = atanf((a * camera.v0 + b) / (camera.fu * a));
+			const float height = camera.baseline * cosf(tilt) / a;
+
+			// skip if not within valid range
+			if (height <= 0.f || height > param_.maxCameraHeight)
+				continue;
+
+			// count inliers within a radius and update the best line
+			int inliers = 0;
+			for (int i = 0; i < npoints; i++)
+			{
+				const float y = points_[i].x;
+				const float x = points_[i].y;
+				const float yhat = a * y + b;
+				if (fabs(yhat - x) <= param_.inlierRadius)
+					inliers++;
+			}
+
+			if (inliers > maxInliers)
+			{
+				maxInliers = inliers;
+				bestLine = cv::Vec2f(a, b);
+			}
+		}
+
+		// apply least squares fitting using inliers around the best line
+		double sx = 0, sy = 0, sxx = 0, syy = 0, sxy = 0;
+		int n = 0;
+		for (int i = 0; i < npoints; i++)
+		{
+			const float x = points_[i].x;
+			const float y = points_[i].y;
+			const float yhat = bestLine[0] * x + bestLine[1];
+			if (fabs(yhat - y) <= param_.inlierRadius)
+			{
+				sx += x;
+				sy += y;
+				sxx += x * x;
+				syy += y * y;
+				sxy += x * y;
+				n++;
+			}
+		}
+
+		const float a = static_cast<float>((n * sxy - sx * sy) / (n * sxx - sx * sx));
+		const float b = static_cast<float>((sxx * sy - sxy * sx) / (n * sxx - sx * sx));
+		return cv::Vec2f(a, b);
+	}
+
+private:
+	Parameters param_;
+	std::vector<cv::Point2f> points_;
+};
+
 MultiLayerStixelWrold::MultiLayerStixelWrold(const Parameters& param) : param_(param)
 {
 }
@@ -40,15 +162,34 @@ std::vector<Stixel> MultiLayerStixelWrold::compute(const cv::Mat& disparity)
 
 	// get camera parameters
 	const CameraParameters& camera = param_.camera;
-	const float sinTilt = sinf(camera.tilt);
-	const float cosTilt = cosf(camera.tilt);
 
 	// compute expected ground disparity
 	std::vector<float> groundDisparity(h);
-	for (int v = 0; v < h; v++)
-		groundDisparity[h - 1 - v] = std::max((camera.baseline / camera.height) * (camera.fu * sinTilt + (v - camera.v0) * cosTilt), 0.f);
-	const float vhor = h - 1 - (camera.v0 * cosTilt - camera.fu * sinTilt) / cosTilt;
-
+	float vhor = 0.f;
+	if (param_.roadEstimation == ROAD_ESTIMATION_AUTO)
+	{
+		// estimate from v-disparity
+		RoadEstimation roadEstimation;
+		const cv::Vec2f line = roadEstimation.compute(columns, camera);
+		const float a = line[0];
+		const float b = line[1];
+		for (int v = 0; v < h; v++)
+			groundDisparity[h - 1 - v] = std::max(a * v + b, 0.f);
+
+		vhor = h - 1 + b / a;
+	}
+	else
+	{
+		const float sinTilt = sinf(camera.tilt);
+		const float cosTilt = cosf(camera.tilt);
+
+		// estimate from camera tilt and height
+		for (int v = 0; v < h; v++)
+			groundDisparity[h - 1 - v] = std::max((camera.baseline / camera.height) * (camera.fu * sinTilt + (v - camera.v0) * cosTilt), 0.f);
+
+		vhor = h - 1 - (camera.v0 * cosTilt - camera.fu * sinTilt) / cosTilt;
+	}
+
 	// create data cost function of each segment
 	NegativeLogDataTermGrd dataTermG(param_.dmax, param_.dmin, param_.sigmaG, param_.pOutG, param_.pInvG, camera,
 		groundDisparity, vhor, param_.sigmaH, param_.sigmaA);
@@ -61,15 +202,15 @@ std::vector<Stixel> MultiLayerStixelWrold::compute(const cv::Mat& disparity)
 	const int S = NegativeLogPriorTerm::S;
 	NegativeLogPriorTerm priorTerm(h, vhor, param_.dmax, param_.dmin, camera.baseline, camera.fu, param_.deltaz,
 		param_.eps, param_.pOrd, param_.pGrav, param_.pBlg, groundDisparity);
-	
+
 	// data cost LUT
 	Matrixf costsG(w, h), costsO(w, h, fnmax), costsS(w, h), sum(w, h);
 	Matrixi valid(w, h);
 
 	// cost table
 	Matrixf costTable(w, h, 3), dispTable(w, h, 3);
 	Matrix<cv::Point> indexTable(w, h, 3);
-	
+
 	// process each column
 	int u;
 #pragma omp parallel for
@@ -97,7 +238,7 @@ std::vector<Stixel> MultiLayerStixelWrold::compute(const cv::Mat& disparity)
 			// pre-computation for sky costs
 			tmpSumS += dataTermS(d);
 			costsS(u, v) = tmpSumS;
-			
+
 			// pre-computation for object costs
 			for (int fn = 0; fn < fnmax; fn++)
 			{
@@ -136,7 +277,7 @@ std::vector<Stixel> MultiLayerStixelWrold::compute(const cv::Mat& disparity)
 				minCostS = costsS(u, vT) + priorTerm.getS0(vT);
 				minDispG = minDispO = minDispS = d1;
 			}
-			
+
 			for (int vB = 1; vB <= vT; vB++)
 			{
 				// compute mean disparity within the range of vB to vT

multilayer_stixel_world.h

@@ -16,6 +16,12 @@ class MultiLayerStixelWrold
 {
 public:
 
+	enum
+	{
+		ROAD_ESTIMATION_AUTO = 0,
+		ROAD_ESTIMATION_CAMERA
+	};
+
 	struct CameraParameters
 	{
 		float fu;
@@ -75,6 +81,9 @@ class MultiLayerStixelWrold
 		float deltaz;
 		float eps;
 
+		// road disparity estimation
+		int roadEstimation;
+
 		// camera parameters
 		CameraParameters camera;
 
@@ -115,6 +124,9 @@ class MultiLayerStixelWrold
 			deltaz = 3.f;
 			eps = 1.f;
 
+			// road disparity estimation
+			roadEstimation = ROAD_ESTIMATION_AUTO;
+
 			// camera parameters
 			camera = CameraParameters();
 		}