From 2bdcd857be9b6ea3d0d80ed1857029066bc3e5ed Mon Sep 17 00:00:00 2001 From: gishi523 Date: Mon, 9 Apr 2018 21:57:59 +0900 Subject: [PATCH] Add auto road estimation --- multilayer_stixel_world.cpp | 161 +++++++++++++++++++++++++++++++++--- multilayer_stixel_world.h | 12 +++ 2 files changed, 163 insertions(+), 10 deletions(-) diff --git a/multilayer_stixel_world.cpp b/multilayer_stixel_world.cpp index dfae518..3c1bd8e 100644 --- a/multilayer_stixel_world.cpp +++ b/multilayer_stixel_world.cpp @@ -7,6 +7,128 @@ #include #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(u)[v]; + if (d >= dmin) + points_.push_back(cv::Point2f(static_cast(h - 1 - v), d)); + } + } + if (points_.empty()) + return cv::Vec2f(0, 0); + + // estimate line by RANSAC + const int npoints = static_cast(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((n * sxy - sx * sy) / (n * sxx - sx * sx)); + const float b = static_cast((sxx * sy - sxy * sx) / (n * sxx - sx * sx)); + return cv::Vec2f(a, b); + } + +private: + Parameters param_; + std::vector points_; +}; + MultiLayerStixelWrold::MultiLayerStixelWrold(const Parameters& param) : param_(param) { } @@ -40,15 +162,34 @@ std::vector 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 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,7 +202,7 @@ std::vector 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); @@ -69,7 +210,7 @@ std::vector MultiLayerStixelWrold::compute(const cv::Mat& disparity) // cost table Matrixf costTable(w, h, 3), dispTable(w, h, 3); Matrix indexTable(w, h, 3); - + // process each column int u; #pragma omp parallel for @@ -97,7 +238,7 @@ std::vector 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 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 diff --git a/multilayer_stixel_world.h b/multilayer_stixel_world.h index 4ead3a5..3fbba79 100644 --- a/multilayer_stixel_world.h +++ b/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(); }