From 1f9cd1fc0bc3752adefe516976ee51884f0c81da Mon Sep 17 00:00:00 2001 From: gishi523 Date: Fri, 14 Apr 2017 23:29:28 +0900 Subject: [PATCH] Add sources --- CMakeLists.txt | 20 +++ README.md | 51 ++++++ camera.xml | 10 ++ cost_function.h | 314 ++++++++++++++++++++++++++++++++++++ main.cpp | 158 ++++++++++++++++++ matrix.h | 61 +++++++ multilayer_stixel_world.cpp | 225 ++++++++++++++++++++++++++ multilayer_stixel_world.h | 124 ++++++++++++++ 8 files changed, 963 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 camera.xml create mode 100644 cost_function.h create mode 100644 main.cpp create mode 100644 matrix.h create mode 100644 multilayer_stixel_world.cpp create mode 100644 multilayer_stixel_world.h diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..22f1e8c --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 2.8) + +find_package(OpenCV REQUIRED) +find_package(OpenMP) + +include_directories(${OpenCV_INCLUDE_DIRS}) + +if (CMAKE_COMPILER_IS_GNUCXX) + set(CMAKE_CXX_FLAGS "-std=c++11 -Wall -O3") +endif (CMAKE_COMPILER_IS_GNUCXX) + +if (OPENMP_FOUND) + set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + +file(GLOB srcs *.cpp *.h*) + +add_executable(stixelworld ${srcs}) +target_link_libraries(stixelworld ${OpenCV_LIBS}) diff --git a/README.md b/README.md new file mode 100644 index 0000000..3aba8fa --- /dev/null +++ b/README.md @@ -0,0 +1,51 @@ +# multilayer-stixel-world +An implementation of multi-layered stixel computation + +==== + +![stixel-world]() + +## Description +- An implementation of the Multi-Layered Stixel computation based on [1]. +- Extracts the static Stixels from the input disparity map. +- Not a dynamic Stixel. It means that tracking and estimating motion of each Stixel is not supported. + +## References +- [1] [The Stixel World - A Compact Medium-level Representation for Efficiently Modeling Three-dimensional Environments](https://www.mydlt.de/david/page/publications.html) + +## Requirement +- OpenCV +- OpenMP (optional) + +## How to build +``` +$ git clone https://github.com/gishi523/multilayer-stixel-world.git +$ cd multilayer-stixel-world +$ mkdir build +$ cd build +$ cmake ../ +$ make +``` + +## How to use +``` +./stixelworld left-image-format right-image-format camera.xml +``` +- left-image-format + - the left image sequence +- right-image-format + - the right image sequence +- camera.xml + - the camera intrinsic and extrinsic parameters + +### Example + ``` +./stixelworld images/img_c0_%09d.pgm images/img_c1_%09d.pgm ../camera.xml +``` + +### Data +- I tested this work using the Daimler Ground Truth Stixel Dataset +- http://www.6d-vision.com/ground-truth-stixel-dataset + +## Author +gishi523 \ No newline at end of file diff --git a/camera.xml b/camera.xml new file mode 100644 index 0000000..32d1606 --- /dev/null +++ b/camera.xml @@ -0,0 +1,10 @@ + + +1267.485352 +1224.548950 +472.735474 +175.787781 +0.214382 +1.170000 +0.081276 + \ No newline at end of file diff --git a/cost_function.h b/cost_function.h new file mode 100644 index 0000000..a4531d3 --- /dev/null +++ b/cost_function.h @@ -0,0 +1,314 @@ +#ifndef __COST_FUNCTION_H__ +#define __COST_FUNCTION_H__ + +#include "matrix.h" + +#include +#include +#include +#define _USE_MATH_DEFINES +#include + +////////////////////////////////////////////////////////////////////////////// +// data cost functions +////////////////////////////////////////////////////////////////////////////// + +static const float PI = static_cast(M_PI); +static const float SQRT2 = static_cast(M_SQRT2); + +struct NegativeLogDataTermGrd +{ + NegativeLogDataTermGrd(float dmax, float dmin, float sigma, float pOut, float pInv, const std::vector& groundDisparity) + { + init(dmax, dmin, sigma, pOut, pInv, groundDisparity); + } + + inline float operator()(float d, int v) const + { + if (d < 0.f) + return 0.f; + + return std::min(nLogPUniform_, nLogPGaussian_[v] + cquad_ * (d - fn_[v]) * (d - fn_[v])); + } + + // pre-compute constant terms + void init(float dmax, float dmin, float sigma, float pOut, float pInv, const std::vector& groundDisparity) + { + // uniform distribution term + nLogPUniform_ = logf(dmax - dmin) - logf(pOut); + + // Gaussian distribution term + const int h = static_cast(groundDisparity.size()); + nLogPGaussian_.resize(h); + fn_.resize(h); + for (int v = 0; v < h; v++) + { + const float fn = groundDisparity[v]; + 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; + } + + // coefficient of quadratic part + cquad_ = 1.f / (2.f * sigma * sigma); + } + + float nLogPUniform_, cquad_; + std::vector nLogPGaussian_, fn_; +}; + +struct NegativeLogDataTermObj +{ + NegativeLogDataTermObj(float dmax, float dmin, float sigma, float pOut, float pInv) + { + init(dmax, dmin, sigma, pOut, pInv); + } + + inline float operator()(float d, int fn) const + { + if (d < 0.f) + return 0.f; + + return std::min(nLogPUniform_, nLogPGaussian_[fn] + cquad_ * (d - fn) * (d - fn)); + } + + // pre-compute constant terms + void init(float dmax, float dmin, float sigma, float pOut, float pInv) + { + // uniform distribution term + nLogPUniform_ = logf(dmax - dmin) - logf(pOut); + + // Gaussian distribution term + const int fnmax = static_cast(dmax); + nLogPGaussian_.resize(fnmax); + for (int fn = 0; fn < fnmax; fn++) + { + const float ANorm = 0.5f * (erff((dmax - fn) / (SQRT2 * sigma)) - erff((dmin - fn) / (SQRT2 * sigma))); + nLogPGaussian_[fn] = logf(ANorm) + logf(sigma * sqrtf(2.f * PI)) - logf(1.f - pOut); + } + + // coefficient of quadratic part + cquad_ = 1.f / (2.f * sigma * sigma); + } + + float nLogPUniform_, cquad_; + std::vector nLogPGaussian_; +}; + +struct NegativeLogDataTermSky +{ + NegativeLogDataTermSky(float dmax, float dmin, float sigma, float pOut, float pInv, float fn = 0.f) : fn_(fn) + { + init(dmax, dmin, sigma, pOut, pInv, fn); + } + + inline float operator()(float d) const + { + if (d < 0.f) + return 0.f; + + return std::min(nLogPUniform_, nLogPGaussian_ + cquad_ * (d - fn_) * (d - fn_)); + } + + // pre-compute constant terms + void init(float dmax, float dmin, float sigma, float pOut, float pInv, float fn) + { + // uniform distribution term + nLogPUniform_ = logf(dmax - dmin) - logf(pOut); + + // Gaussian distribution term + const float ANorm = 0.5f * (erff((dmax - fn) / (SQRT2 * sigma)) - erff((dmin - fn) / (SQRT2 * sigma))); + nLogPGaussian_ = logf(ANorm) + logf(sigma * sqrtf(2.f * PI)) - logf(1.f - pOut); + + // coefficient of quadratic part + cquad_ = 1.f / (2.f * sigma * sigma); + } + + float nLogPUniform_, cquad_, nLogPGaussian_, fn_; +}; + +////////////////////////////////////////////////////////////////////////////// +// prior cost functions +////////////////////////////////////////////////////////////////////////////// + +static const float N_LOG_0_3 = -static_cast(log(0.3)); +static const float N_LOG_0_5 = -static_cast(log(0.5)); +static const float N_LOG_0_7 = -static_cast(log(0.7)); +static const float N_LOG_0_0 = std::numeric_limits::infinity(); +static const float N_LOG_1_0 = 0.f; + +struct NegativeLogPriorTerm +{ + static const int G = 0; + static const int O = 1; + static const int S = 2; + + NegativeLogPriorTerm(int h, float vhor, float dmax, float dmin, float b, float fu, float deltaz, float eps, + float pOrd, float pGrav, float pBlg, const std::vector& groundDisparity) + { + init(h, vhor, dmax, dmin, b, fu, deltaz, eps, pOrd, pGrav, pBlg, groundDisparity); + } + + inline float O0(int vT) const + { + return costs0_(vT, O); + } + inline float G0(int vT) const + { + return costs0_(vT, G); + } + inline float S0(int vT) const + { + return N_LOG_0_0; + } + + inline float OO(int vB, int d1, int d2) const + { + return costs1_(vB, O, O) + costs2_O_O_(d2, d1); + } + inline float OG(int vB, int d1, int d2) const + { + return costs1_(vB, O, G) + costs2_O_G_(vB - 1, d1); + } + inline float OS(int vB, int d1, int d2) const + { + return costs1_(vB, O, S) + costs2_O_S_(d1); + } + + inline float GO(int vB, int d1, int d2) const + { + return costs1_(vB, G, O); + } + inline float GG(int vB, int d1, int d2) const + { + return costs1_(vB, G, G); + } + inline float GS(int vB, int d1, int d2) const + { + return N_LOG_0_0; + } + + inline float SO(int vB, int d1, int d2) const + { + return costs1_(vB, S, O) + costs2_S_O_(d2, d1); + } + inline float SG(int vB, int d1, int d2) const + { + return N_LOG_0_0; + } + inline float SS(int vB, int d1, int d2) const + { + return N_LOG_0_0; + } + + void init(int h, float vhor, float dmax, float dmin, float b, float fu, float deltaz, float eps, + float pOrd, float pGrav, float pBlg, const std::vector& groundDisparity) + { + const int fnmax = static_cast(dmax); + + costs0_.create(h, 2); + costs1_.create(h, 3, 3); + costs2_O_O_.create(fnmax, fnmax); + costs2_O_S_.create(1, fnmax); + costs2_O_G_.create(h, fnmax); + costs2_S_O_.create(fnmax, fnmax); + + for (int vT = 0; vT < h; vT++) + { + const float P1 = N_LOG_1_0; + const float P2 = -logf(1.f / h); + const float P3_O = vT > vhor ? N_LOG_1_0 : N_LOG_0_5; + const float P3_G = vT > vhor ? N_LOG_0_0 : N_LOG_0_5; + const float P4_O = -logf(1.f / (dmax - dmin)); + const float P4_G = N_LOG_1_0; + + costs0_(vT, O) = P1 + P2 + P3_O + P4_O; + costs0_(vT, G) = P1 + P2 + P3_G + P4_G; + } + + for (int vB = 0; vB < h; vB++) + { + const float P1 = N_LOG_1_0; + const float P2 = -logf(1.f / (h - vB)); + + const float P3_O_O = vB - 1 < vhor ? N_LOG_0_7 : N_LOG_0_5; + const float P3_G_O = vB - 1 < vhor ? N_LOG_0_3 : N_LOG_0_0; + const float P3_S_O = vB - 1 < vhor ? N_LOG_0_0 : N_LOG_0_5; + + const float P3_O_G = vB - 1 < vhor ? N_LOG_0_7 : N_LOG_0_0; + const float P3_G_G = vB - 1 < vhor ? N_LOG_0_3 : N_LOG_0_0; + const float P3_S_G = vB - 1 < vhor ? N_LOG_0_0 : N_LOG_0_0; + + const float P3_O_S = vB - 1 < vhor ? N_LOG_0_0 : N_LOG_1_0; + const float P3_G_S = vB - 1 < vhor ? N_LOG_0_0 : N_LOG_0_0; + const float P3_S_S = vB - 1 < vhor ? N_LOG_0_0 : N_LOG_0_0; + + costs1_(vB, O, O) = P1 + P2 + P3_O_O; + costs1_(vB, G, O) = P1 + P2 + P3_G_O; + costs1_(vB, S, O) = P1 + P2 + P3_S_O; + + costs1_(vB, O, G) = P1 + P2 + P3_O_G; + costs1_(vB, G, G) = P1 + P2 + P3_G_G; + costs1_(vB, S, G) = P1 + P2 + P3_S_G; + + costs1_(vB, O, S) = P1 + P2 + P3_O_S; + costs1_(vB, G, S) = P1 + P2 + P3_G_S; + costs1_(vB, S, S) = P1 + P2 + P3_S_S; + } + + for (int d1 = 0; d1 < fnmax; d1++) + costs2_O_O_(0, d1) = N_LOG_0_0; + + for (int d2 = 1; d2 < fnmax; d2++) + { + const float z = b * fu / d2; + const float deltad = d2 - b * fu / (z + deltaz); + for (int d1 = 0; d1 < fnmax; d1++) + { + if (d1 > d2 + deltad) + costs2_O_O_(d2, d1) = -logf(pOrd / (d2 - deltad)); + else if (d1 <= d2 + deltad) + costs2_O_O_(d2, d1) = -logf((1.f - pOrd) / (dmax - d2 - deltad)); + else + costs2_O_O_(d2, d1) = N_LOG_0_0; + } + } + + for (int v = 0; v < h; v++) + { + const float fn = groundDisparity[v]; + for (int d1 = 0; d1 < fnmax; d1++) + { + if (d1 > fn + eps) + costs2_O_G_(v, d1) = -logf(pGrav / (dmax - fn - eps)); + else if (d1 > fn + eps) + costs2_O_G_(v, d1) = -logf(pBlg / (fn - eps - dmin)); + else + costs2_O_G_(v, d1) = -logf((1.f - pGrav - pBlg) / (2.f * eps)); + } + } + + for (int d1 = 0; d1 < fnmax; d1++) + { + costs2_O_S_(d1) = d1 > eps ? -logf((1.f / (dmax - dmin)) / (1.f - eps / (dmax - dmin))) : N_LOG_0_0; + } + + for (int d2 = 0; d2 < fnmax; d2++) + { + for (int d1 = 0; d1 < fnmax; d1++) + { + if (d2 < eps) + costs2_S_O_(d2, d1) = N_LOG_0_0; + else if (d1 <= 0) + costs2_S_O_(d2, d1) = N_LOG_1_0; + else + costs2_S_O_(d2, d1) = N_LOG_0_0; + } + } + } + + Matrixf costs0_, costs1_; + Matrixf costs2_O_O_, costs2_O_G_, costs2_O_S_, costs2_S_O_; +}; + +#endif // !__COST_FUNCTION_H__ \ No newline at end of file diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..956d344 --- /dev/null +++ b/main.cpp @@ -0,0 +1,158 @@ +#include +#include +#include + +#include "multilayer_stixel_world.h" + +static cv::Scalar computeColor(float val) +{ + const float hscale = 6.f; + float h = 0.6f * (1.f - val), s = 1.f, v = 1.f; + float r, g, b; + + static const int sector_data[][3] = + { { 1,3,0 },{ 1,0,2 },{ 3,0,1 },{ 0,2,1 },{ 0,1,3 },{ 2,1,0 } }; + float tab[4]; + int sector; + h *= hscale; + if (h < 0) + do h += 6; while (h < 0); + else if (h >= 6) + do h -= 6; while (h >= 6); + sector = cvFloor(h); + h -= sector; + if ((unsigned)sector >= 6u) + { + sector = 0; + h = 0.f; + } + + tab[0] = v; + tab[1] = v*(1.f - s); + tab[2] = v*(1.f - s*h); + tab[3] = v*(1.f - s*(1.f - h)); + + b = tab[sector_data[sector][0]]; + g = tab[sector_data[sector][1]]; + r = tab[sector_data[sector][2]]; + return 255 * cv::Scalar(b, g, r); +} + +static cv::Scalar dispToColor(float disp, float maxdisp) +{ + if (disp < 0) + return cv::Scalar(128, 128, 128); + return computeColor(std::min(disp, maxdisp) / maxdisp); +} + +static void drawStixel(cv::Mat& img, const Stixel& stixel, cv::Scalar color) +{ + const int radius = std::max(stixel.width / 2, 1); + const cv::Point tl(stixel.u - radius, stixel.vT); + const cv::Point br(stixel.u + radius, stixel.vB); + cv::rectangle(img, cv::Rect(tl, br), color, -1); + cv::rectangle(img, cv::Rect(tl, br), cv::Scalar(255, 255, 255), 1); +} + +int main(int argc, char* argv[]) +{ + if (argc < 4) + { + std::cout << "usage: " << argv[0] << " left-image-format right-image-format camera.xml" << std::endl; + return -1; + } + + // stereo sgbm + const int wsize = 11; + const int numDisparities = 64; + const int P1 = 8 * wsize * wsize; + const int P2 = 32 * wsize * wsize; + cv::Ptr ssgbm = cv::StereoSGBM::create(0, numDisparities, wsize, P1, P2, + 0, 0, 0, 0, 0, cv::StereoSGBM::MODE_SGBM_3WAY); + + // read camera parameters + const cv::FileStorage cvfs(argv[3], CV_STORAGE_READ); + CV_Assert(cvfs.isOpened()); + const cv::FileNode node(cvfs.fs, NULL); + + // input parameters + MultiLayerStixelWrold::Parameters param; + param.camera.fu = node["FocalLengthX"]; + param.camera.fv = node["FocalLengthY"]; + param.camera.u0 = node["CenterX"]; + param.camera.v0 = node["CenterY"]; + param.camera.baseline = node["BaseLine"]; + param.camera.height = node["Height"]; + param.camera.tilt = node["Tilt"]; + param.dmax = numDisparities; + + MultiLayerStixelWrold stixelWorld(param); + + for (int frameno = 1;; frameno++) + { + char bufl[256], bufr[256]; + sprintf(bufl, argv[1], frameno); + sprintf(bufr, argv[2], frameno); + + cv::Mat left = cv::imread(bufl, -1); + cv::Mat right = cv::imread(bufr, -1); + + if (left.empty() || right.empty()) + { + std::cerr << "imread failed." << std::endl; + break; + } + + CV_Assert(left.size() == right.size() && left.type() == right.type()); + + switch (left.type()) + { + case CV_8U: + // nothing to do + break; + case CV_16U: + // conver to CV_8U + double maxVal; + cv::minMaxLoc(left, NULL, &maxVal); + left.convertTo(left, CV_8U, 255 / maxVal); + right.convertTo(right, CV_8U, 255 / maxVal); + break; + default: + std::cerr << "unsupported image type." << std::endl; + return -1; + } + + // compute dispaliry + cv::Mat disparity; + ssgbm->compute(left, right, disparity); + disparity.convertTo(disparity, CV_32F, 1.0 / 16); + + // compute stixels + const auto t1 = std::chrono::system_clock::now(); + + const std::vector stixels = stixelWorld.compute(disparity); + + const auto t2 = std::chrono::system_clock::now(); + const auto duration = std::chrono::duration_cast(t2 - t1).count(); + std::cout << "stixel computation time: " << duration << "[msec]" << std::endl; + + // draw stixels + cv::Mat draw; + cv::cvtColor(left, draw, cv::COLOR_GRAY2BGRA); + + cv::Mat stixelImg = cv::Mat::zeros(left.size(), draw.type()); + for (const auto& stixel : stixels) + drawStixel(stixelImg, stixel, dispToColor(stixel.disp, 64)); + + draw = draw + 0.5 * stixelImg; + + cv::imshow("disparity", disparity / 64); + cv::imshow("stixels", draw); + + const char c = cv::waitKey(1); + if (c == 27) + break; + if (c == 'p') + cv::waitKey(0); + } +} diff --git a/matrix.h b/matrix.h new file mode 100644 index 0000000..e9c3ddc --- /dev/null +++ b/matrix.h @@ -0,0 +1,61 @@ +#ifndef __MATRIX_H__ +#define __MATRIX_H__ + +#include +#include + +template +class Matrix : public cv::Mat_ +{ +public: + Matrix() : cv::Mat_() {} + Matrix(int size1, int size2) { create(size1, size2); } + Matrix(int size1, int size2, int size3) { create(size1, size2, size3); } + + void create(int size1, int size2) + { + cv::Mat_::create(size1, size2); + this->size1 = size1; + this->size2 = size2; + this->size3 = 1; + } + + void create(int size1, int size2, int size3) + { + cv::Mat_::create(3, std::array{size1, size2, size3}.data()); + this->size1 = size1; + this->size2 = size2; + this->size3 = size3; + } + + inline T& operator()(int i) + { + return *((T*)data + i); + } + inline const T& operator()(int i) const + { + return *((T*)data + i); + } + inline T& operator()(int i, int j) + { + return *((T*)data + i * size2 + j); + } + inline const T& operator()(int i, int j) const + { + return *((T*)data + i * size2 + j); + } + inline T& operator()(int i, int j, int k) + { + return *((T*)data + size3 * (i * size2 + j) + k); + } + inline const T& operator()(int i, int j, int k) const + { + return *((T*)data + size3 * (i * size2 + j) + k); + } + int size1, size2, size3; +}; + +using Matrixf = Matrix; +using Matrixi = Matrix; + +#endif // !__MATRIX_H__ \ No newline at end of file diff --git a/multilayer_stixel_world.cpp b/multilayer_stixel_world.cpp new file mode 100644 index 0000000..83098b5 --- /dev/null +++ b/multilayer_stixel_world.cpp @@ -0,0 +1,225 @@ +#include "multilayer_stixel_world.h" +#include "matrix.h" +#include "cost_function.h" + +#include +#ifdef _OPENMP +#include +#endif + +MultiLayerStixelWrold::MultiLayerStixelWrold(const Parameters& param) : param_(param) +{ +} + +std::vector MultiLayerStixelWrold::compute(const cv::Mat& disparity) +{ + 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(param_.dmax); + + // compute horizontal median of each column + Matrixf columns(w, h); + for (int v = 0; v < h; v++) + { + for (int u = 0; u < w; u++) + { + // compute horizontal median + std::vector buf(stixelWidth); + for (int du = 0; du < stixelWidth; du++) + buf[du] = disparity.at(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; + } + } + + // 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[v] = std::max((camera.baseline / camera.height) * (camera.fu * sinTilt + (v - camera.v0) * cosTilt), 1.f); + std::reverse(std::begin(groundDisparity), std::end(groundDisparity)); + const float 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, groundDisparity); + NegativeLogDataTermObj dataTermO(param_.dmax, param_.dmin, param_.sigmaO, param_.pOutO, param_.pInvO); + NegativeLogDataTermSky dataTermS(param_.dmax, param_.dmin, param_.sigmaS, param_.pOutS, param_.pInvS); + + // create prior cost function of each segment + const int G = NegativeLogPriorTerm::G; + const int O = NegativeLogPriorTerm::O; + 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 indexTable(w, h, 3); + + // process each column + int u; +#pragma omp parallel for + for (u = 0; u < w; u++) + { + ////////////////////////////////////////////////////////////////////////////// + // pre-computate LUT + ////////////////////////////////////////////////////////////////////////////// + float tmpSumG = 0.f; + float tmpSumS = 0.f; + std::vector tmpSumO(fnmax, 0.f); + + float tmpSum = 0.f; + int tmpValid = 0; + + for (int v = 0; v < h; v++) + { + // measured disparity + const float d = columns(u, v); + + // pre-computation for ground costs + tmpSumG += dataTermG(d, v); + costsG(u, v) = tmpSumG; + + // pre-computation for sky costs + tmpSumS += dataTermS(d); + costsS(u, v) = tmpSumS; + + // pre-computation for object costs + for (int fn = 0; fn < fnmax; fn++) + { + tmpSumO[fn] += dataTermO(d, fn); + costsO(u, v, fn) = tmpSumO[fn]; + } + + // pre-computation for mean disparity of stixel + if (d >= 0.f) + { + tmpSum += d; + tmpValid++; + } + sum(u, v) = tmpSum; + valid(u, v) = tmpValid; + } + + ////////////////////////////////////////////////////////////////////////////// + // compute cost tables + ////////////////////////////////////////////////////////////////////////////// + for (int vT = 0; vT < h; vT++) + { + float minCostG, minCostO, minCostS; + float minDispG, minDispO, minDispS; + cv::Point minPosG(0, 0), minPosO(1, 0), minPosS(2, 0); + + // process vB = 0 + { + // compute mean disparity within the range of vB to vT + const float d1 = sum(u, vT) / std::max(valid(u, vT), 1); + const int fn = cvRound(d1); + + // initialize minimum costs + minCostG = costsG(u, vT) + priorTerm.G0(vT); + minCostO = costsO(u, vT, fn) + priorTerm.O0(vT); + minCostS = costsS(u, vT) + priorTerm.S0(vT); + minDispG = minDispO = minDispS = d1; + } + + for (int vB = 1; vB <= vT; vB++) + { + // compute mean disparity within the range of vB to vT + const float d1 = (sum(u, vT) - sum(u, vB - 1)) / std::max(valid(u, vT) - valid(u, vB - 1), 1); + const int fn = cvRound(d1); + + // compute data terms costs + const float dataCostG = vT < vhor ? costsG(u, vT) - costsG(u, vB - 1) : N_LOG_0_0; + const float dataCostO = costsO(u, vT, fn) - costsO(u, vB - 1, fn); + const float dataCostS = vT < vhor ? N_LOG_0_0 : costsS(u, vT) - costsS(u, vB - 1); + + // compute priors costs and update costs + const float d2 = dispTable(u, vB - 1, 1); + +#define UPDATE_COST(C1, C2) \ + const float cost##C1##C2 = dataCost##C1 + priorTerm.##C1##C2(vB, cvRound(d1), cvRound(d2)) + costTable(u, vB - 1, ##C2); \ + if (cost##C1##C2 < minCost##C1) \ + { \ + minCost##C1 = cost##C1##C2; \ + minDisp##C1 = d1; \ + minPos##C1 = cv::Point(##C2, vB - 1); \ + } \ + + UPDATE_COST(G, G); + UPDATE_COST(G, O); + UPDATE_COST(G, S); + UPDATE_COST(O, G); + UPDATE_COST(O, O); + UPDATE_COST(O, S); + UPDATE_COST(S, G); + UPDATE_COST(S, O); + UPDATE_COST(S, S); + } + + costTable(u, vT, 0) = minCostG; + costTable(u, vT, 1) = minCostO; + costTable(u, vT, 2) = minCostS; + + dispTable(u, vT, 0) = minDispG; + dispTable(u, vT, 1) = minDispO; + dispTable(u, vT, 2) = minDispS; + + indexTable(u, vT, 0) = minPosG; + indexTable(u, vT, 1) = minPosO; + indexTable(u, vT, 2) = minPosS; + } + } + + ////////////////////////////////////////////////////////////////////////////// + // backtracking step + ////////////////////////////////////////////////////////////////////////////// + std::vector stixels; + for (int u = 0; u < w; u++) + { + float minCost = std::numeric_limits::max(); + cv::Point minPos; + for (int c = 0; c < 3; c++) + { + const float cost = costTable(u, h - 1, c); + if (cost < minCost) + { + minCost = cost; + minPos = cv::Point(c, h - 1); + } + } + + while (minPos.y > 0) + { + const cv::Point p1 = minPos; + const cv::Point p2 = indexTable(u, p1.y, p1.x); + if (p1.x == 1) // object + { + Stixel stixel; + stixel.u = stixelWidth * u + stixelWidth / 2; + stixel.vT = h - 1 - p1.y; + stixel.vB = h - 1 - (p2.y + 1); + stixel.width = stixelWidth; + stixel.disp = dispTable(u, p1.y, p1.x); + stixels.push_back(stixel); + } + minPos = p2; + } + } + + return stixels; +} \ No newline at end of file diff --git a/multilayer_stixel_world.h b/multilayer_stixel_world.h new file mode 100644 index 0000000..f4e2c0c --- /dev/null +++ b/multilayer_stixel_world.h @@ -0,0 +1,124 @@ +#ifndef __MULTILAYER_STIXEL_WORLD_H__ +#define __MULTILAYER_STIXEL_WORLD_H__ + +#include + +struct Stixel +{ + int u; + int vT; + int vB; + int width; + float disp; +}; + +class MultiLayerStixelWrold +{ +public: + + struct CameraParameters + { + float fu; + float fv; + float u0; + float v0; + float baseline; + float height; + float tilt; + + // default settings + CameraParameters() + { + fu = 1.f; + fv = 1.f; + u0 = 0.f; + v0 = 0.f; + baseline = 0.2f; + height = 1.f; + tilt = 0.f; + } + }; + + struct Parameters + { + // stixel width + int stixelWidth; + + // disparity range + float dmin; + float dmax; + + // disparity measurement uncertainty + float sigmaG; + float sigmaO; + float sigmaS; + + // outlier rate + float pOutG; + float pOutO; + float pOutS; + + // probability of invalid disparity + float pInvG; + float pInvO; + float pInvS; + + // probability for regularization + float pOrd; + float pGrav; + float pBlg; + + float deltaz; + float eps; + + // camera parameters + CameraParameters camera; + + // default settings + Parameters() + { + // stixel width + stixelWidth = 7; + + // disparity range + dmin = 0; + dmax = 64; + + // disparity measurement uncertainty + sigmaG = 1.5f; + sigmaO = 1.5f; + sigmaS = 1.2f; + + // outlier rate + pOutG = 0.15f; + pOutO = 0.15f; + pOutS = 0.4f; + + // probability of invalid disparity + pInvG = 0.34f; + pInvO = 0.3f; + pInvS = 0.36f; + + // probability for regularization + pOrd = 0.1f; + pGrav = 0.1f; + pBlg = 0.001f; + + deltaz = 3.f; + eps = 1.f; + + // camera parameters + camera = CameraParameters(); + } + }; + + MultiLayerStixelWrold(const Parameters& param); + + std::vector compute(const cv::Mat& disparity); + +private: + + Parameters param_; +}; + +#endif // !__STIXEL_WORLD_H__ \ No newline at end of file