diff --git a/cv-node/include/ftl/algorithms/fixstars_sgm.hpp b/cv-node/include/ftl/algorithms/fixstars_sgm.hpp index 1afa4dc46ef1233d84a32fecc00689ca8f75c41c..9d71c1b939f6c43769fcd83c32f80ba60d2545b1 100644 --- a/cv-node/include/ftl/algorithms/fixstars_sgm.hpp +++ b/cv-node/include/ftl/algorithms/fixstars_sgm.hpp @@ -1,3 +1,7 @@ +/* + * Copyright 2019 Nicolas Pope + */ + #ifndef _FTL_ALGORITHMS_FIXSTARS_SGM_HPP_ #define _FTL_ALGORITHMS_FIXSTARS_SGM_HPP_ @@ -8,19 +12,30 @@ namespace ftl { namespace algorithms { + +/** + * Fixstars libSGM stereo matcher. + * @see https://github.com/fixstars/libSGM + * + * NOTE: We are using a modified version that supports disparity of 256. + * @see https://github.com/knicos/libSGM + */ class FixstarsSGM : public ftl::Disparity { public: - FixstarsSGM(nlohmann::json &config); - + explicit FixstarsSGM(nlohmann::json &config); + void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp); - static inline Disparity *create(nlohmann::json &config) { return new FixstarsSGM(config); } - + /* Factory creator */ + static inline Disparity *create(nlohmann::json &config) { + return new FixstarsSGM(config); + } + private: sgm::StereoSGM *ssgm_; }; }; }; -#endif // _FTL_ALGORITHMS_FIXSTARS_SGM_HPP_ +#endif // _FTL_ALGORITHMS_FIXSTARS_SGM_HPP_ diff --git a/cv-node/include/ftl/algorithms/np_attention.hpp b/cv-node/include/ftl/algorithms/np_attention.hpp new file mode 100644 index 0000000000000000000000000000000000000000..0b5111b3b072ce167137bea473cb76658c8953d2 --- /dev/null +++ b/cv-node/include/ftl/algorithms/np_attention.hpp @@ -0,0 +1,31 @@ +#ifndef _FTL_ALGORITHMS_NP_ATTENTION_HPP_ +#define _FTL_ALGORITHMS_NP_ATTENTION_HPP_ + +namespace ftl { +namespace gpu { + +struct AttentionItem { + float factor; + HisteresisTexture<float> source; +} + +class Attention { + public: + Attention(); + + void source(float factor, const HisteresisTexture<float> &ht); + + void update(); + + cudaTextureObject_t cudaTexture(); + TextureObject<float> texture(); + + private: + std::vector<Component> components_; +}; + +} +} + +#endif // _FTL_ALGORITHMS_NP_ATTENTION_HPP_ + diff --git a/cv-node/include/ftl/algorithms/opencv_bm.hpp b/cv-node/include/ftl/algorithms/opencv_bm.hpp index defa3a105b2b11cb54bd5f8903ad3d7b61258610..3a83ae6d8b5d775132d211a5810be5c1c95dbaa4 100644 --- a/cv-node/include/ftl/algorithms/opencv_bm.hpp +++ b/cv-node/include/ftl/algorithms/opencv_bm.hpp @@ -1,3 +1,7 @@ +/* + * Copyright 2019 Nicolas Pope + */ + #ifndef _FTL_ALGORITHMS_OPENCV_BM_HPP_ #define _FTL_ALGORITHMS_OPENCV_BM_HPP_ @@ -9,13 +13,19 @@ namespace ftl { namespace algorithms { + +/** + * OpenCV Block Matching algorithm. + */ class OpenCVBM : public ftl::Disparity { public: - OpenCVBM(nlohmann::json &config); + explicit OpenCVBM(nlohmann::json &config); void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp); - static inline Disparity *create(nlohmann::json &config) { return new OpenCVBM(config); } + static inline Disparity *create(nlohmann::json &config) { + return new OpenCVBM(config); + } private: cv::Ptr<cv::StereoBM> left_matcher_; @@ -25,5 +35,5 @@ class OpenCVBM : public ftl::Disparity { }; }; -#endif // _FTL_ALGORITHMS_OPENCV_SGBM_HPP_ +#endif // _FTL_ALGORITHMS_OPENCV_SGBM_HPP_ diff --git a/cv-node/include/ftl/algorithms/opencv_cuda_bm.hpp b/cv-node/include/ftl/algorithms/opencv_cuda_bm.hpp index 87561c9217e79ed6aa26ae00e3fb16e4bf6e2ca3..405cf97cd44423327b93498093f33caef0fefa2f 100644 --- a/cv-node/include/ftl/algorithms/opencv_cuda_bm.hpp +++ b/cv-node/include/ftl/algorithms/opencv_cuda_bm.hpp @@ -1,3 +1,7 @@ +/* + * Copyright 2019 Nicolas Pope + */ + #ifndef _FTL_ALGORITHMS_OPENCV_CUDA_BM_HPP_ #define _FTL_ALGORITHMS_OPENCV_CUDA_BM_HPP_ @@ -8,13 +12,19 @@ namespace ftl { namespace algorithms { + +/** + * OpenCV CUDA Block Matching algorithm. + */ class OpenCVCudaBM : public ftl::Disparity { public: - OpenCVCudaBM(nlohmann::json &config); + explicit OpenCVCudaBM(nlohmann::json &config); void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp); - static inline Disparity *create(nlohmann::json &config) { return new OpenCVCudaBM(config); } + static inline Disparity *create(nlohmann::json &config) { + return new OpenCVCudaBM(config); + } private: cv::Ptr<cv::cuda::StereoBM> matcher_; @@ -27,5 +37,5 @@ class OpenCVCudaBM : public ftl::Disparity { }; }; -#endif // _FTL_ALGORITHMS_OPENCV_CUDA_BM_HPP_ +#endif // _FTL_ALGORITHMS_OPENCV_CUDA_BM_HPP_ diff --git a/cv-node/include/ftl/algorithms/opencv_cuda_bp.hpp b/cv-node/include/ftl/algorithms/opencv_cuda_bp.hpp index 190c3812a5270447a792b8897ea8cf5e464e37bf..a89d56df40c65488f4fca09222bbae0f960857e9 100644 --- a/cv-node/include/ftl/algorithms/opencv_cuda_bp.hpp +++ b/cv-node/include/ftl/algorithms/opencv_cuda_bp.hpp @@ -1,3 +1,7 @@ +/* + * Copyright 2019 Nicolas Pope + */ + #ifndef _FTL_ALGORITHMS_OPENCV_CUDA_BP_HPP_ #define _FTL_ALGORITHMS_OPENCV_CUDA_BP_HPP_ @@ -8,13 +12,19 @@ namespace ftl { namespace algorithms { + +/** + * OpenCV CUDA Belief Propagation algorithm. + */ class OpenCVCudaBP : public ftl::Disparity { public: - OpenCVCudaBP(nlohmann::json &config); + explicit OpenCVCudaBP(nlohmann::json &config); void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp); - static inline Disparity *create(nlohmann::json &config) { return new OpenCVCudaBP(config); } + static inline Disparity *create(nlohmann::json &config) { + return new OpenCVCudaBP(config); + } private: cv::Ptr<cv::cuda::StereoBeliefPropagation> matcher_; @@ -25,5 +35,5 @@ class OpenCVCudaBP : public ftl::Disparity { }; }; -#endif // _FTL_ALGORITHMS_OPENCV_CUDA_BP_HPP_ +#endif // _FTL_ALGORITHMS_OPENCV_CUDA_BP_HPP_ diff --git a/cv-node/include/ftl/algorithms/opencv_sgbm.hpp b/cv-node/include/ftl/algorithms/opencv_sgbm.hpp index 51e368cc5d9e5e6bca56178ea1a69d9a732fce8a..d36966a1d660b6d7d0b4d21f96d25602de63ab6b 100644 --- a/cv-node/include/ftl/algorithms/opencv_sgbm.hpp +++ b/cv-node/include/ftl/algorithms/opencv_sgbm.hpp @@ -1,3 +1,7 @@ +/* + * Copyright 2019 Nicolas Pope + */ + #ifndef _FTL_ALGORITHMS_OPENCV_SGBM_HPP_ #define _FTL_ALGORITHMS_OPENCV_SGBM_HPP_ @@ -9,9 +13,13 @@ namespace ftl { namespace algorithms { + +/** + * OpenCV Semi Global Matching algorithm. + */ class OpenCVSGBM : public ftl::Disparity { public: - OpenCVSGBM(nlohmann::json &config); + explicit OpenCVSGBM(nlohmann::json &config); void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp); @@ -25,5 +33,5 @@ class OpenCVSGBM : public ftl::Disparity { }; }; -#endif // _FTL_ALGORITHMS_OPENCV_SGBM_HPP_ +#endif // _FTL_ALGORITHMS_OPENCV_SGBM_HPP_ diff --git a/cv-node/include/ftl/algorithms/rtcensus.hpp b/cv-node/include/ftl/algorithms/rtcensus.hpp index aed1b122b86a69673bec711855c399d2d4594ebb..0a7d382b6531e113c37d86e5d0920b2851579d45 100644 --- a/cv-node/include/ftl/algorithms/rtcensus.hpp +++ b/cv-node/include/ftl/algorithms/rtcensus.hpp @@ -1,3 +1,7 @@ +/* + * Copyright 2019 Nicolas Pope + */ + #ifndef _FTL_ALGORITHMS_RTCENSUS_HPP_ #define _FTL_ALGORITHMS_RTCENSUS_HPP_ @@ -12,16 +16,22 @@ namespace ftl { namespace algorithms { + +/** + * Real-time Sparse Census disparity algorithm. + */ class RTCensus : public ftl::Disparity { public: - RTCensus(nlohmann::json &config); + explicit RTCensus(nlohmann::json &config); void setGamma(float gamma) { gamma_ = gamma; } void setTau(float tau) { tau_ = tau; } void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp); - static inline Disparity *create(nlohmann::json &config) { return new RTCensus(config); } + static inline Disparity *create(nlohmann::json &config) { + return new RTCensus(config); + } private: float gamma_; @@ -47,5 +57,5 @@ class RTCensus : public ftl::Disparity { }; }; -#endif // _FTL_ALGORITHMS_RTCENSUS_HPP_ +#endif // _FTL_ALGORITHMS_RTCENSUS_HPP_ diff --git a/cv-node/include/ftl/algorithms/rtcensus_sgm.hpp b/cv-node/include/ftl/algorithms/rtcensus_sgm.hpp index 83344affffd04217d0368a91062a616c5eacec7d..e2da5d8d5d2c357054c3b7a2a9654e72511c72d0 100644 --- a/cv-node/include/ftl/algorithms/rtcensus_sgm.hpp +++ b/cv-node/include/ftl/algorithms/rtcensus_sgm.hpp @@ -1,3 +1,7 @@ +/* + * Copyright 2019 Nicolas Pope + */ + #ifndef _FTL_ALGORITHMS_RTCENSUSSGM_HPP_ #define _FTL_ALGORITHMS_RTCENSUSSGM_HPP_ @@ -12,16 +16,22 @@ namespace ftl { namespace algorithms { + +/** + * WORK IN PROGRESS + */ class RTCensusSGM : public ftl::Disparity { public: - RTCensusSGM(nlohmann::json &config); + explicit RTCensusSGM(nlohmann::json &config); void setGamma(float gamma) { gamma_ = gamma; } void setTau(float tau) { tau_ = tau; } void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp); - static inline Disparity *create(nlohmann::json &config) { return new RTCensusSGM(config); } + static inline Disparity *create(nlohmann::json &config) { + return new RTCensusSGM(config); + } private: float gamma_; @@ -45,5 +55,5 @@ class RTCensusSGM : public ftl::Disparity { }; }; -#endif // _FTL_ALGORITHMS_RTCENSUSSGM_HPP_ +#endif // _FTL_ALGORITHMS_RTCENSUSSGM_HPP_ diff --git a/cv-node/include/ftl/calibrate.hpp b/cv-node/include/ftl/calibrate.hpp index 99d6908473f3ddb4e18c14473e8290819622a329..79fb310b348db16bbebde7f259e95db17c83b9f8 100644 --- a/cv-node/include/ftl/calibrate.hpp +++ b/cv-node/include/ftl/calibrate.hpp @@ -1,3 +1,7 @@ +/* + * Copyright 2019 Nicolas Pope + */ + #ifndef _FTL_CALIBRATION_HPP_ #define _FTL_CALIBRATION_HPP_ @@ -13,8 +17,16 @@ class FileNode; }; namespace ftl { + +/** + * Manage both performing and applying camera calibration. It will attempt to + * load any existing cached camera calibration unless explicitely told to + * redo the calibration. + */ class Calibrate { public: + + // TODO(nick) replace or remove this class. class Settings { public: Settings() : goodInput(false) {} @@ -68,15 +80,28 @@ class Calibrate { public: Calibrate(ftl::LocalSource *s, nlohmann::json &config); + /** + * Perform a new camera calibration. Ignore and replace any existing + * cached calibration. + */ bool recalibrate(); bool undistort(cv::Mat &l, cv::Mat &r); - bool rectified(cv::Mat &l, cv::Mat &r); + /** + * Get both left and right images from local source, but with intrinsic + * and extrinsic stereo calibration already applied. + */ + bool rectified(cv::Mat &l, cv::Mat &r); + bool isCalibrated(); - + + /** + * Get the camera matrix. Used to convert disparity map back to depth and + * a 3D point cloud. + */ const cv::Mat &getQ() const { return Q_; } - + private: bool _recalibrate(std::vector<std::vector<cv::Point2f>> *imagePoints, cv::Mat *cameraMatrix, cv::Mat *distCoeffs, cv::Size *imageSize); diff --git a/cv-node/include/ftl/cuda_algorithms.hpp b/cv-node/include/ftl/cuda_algorithms.hpp index a7f3412390d9561cfceb05547105ae705991b90c..1a70018de8c2f0e9ad3b629ec03530453cc0dccf 100644 --- a/cv-node/include/ftl/cuda_algorithms.hpp +++ b/cv-node/include/ftl/cuda_algorithms.hpp @@ -1,3 +1,7 @@ +/* + * Copyright 2019 Nicolas Pope + */ + #ifndef _FTL_CUDA_ALGORITHMS_HPP_ #define _FTL_CUDA_ALGORITHMS_HPP_ @@ -6,15 +10,27 @@ namespace ftl { namespace cuda { + /** + * Disparity consistency algorithm. + */ void consistency(const TextureObject<float> &dl, const TextureObject<float> &dr, TextureObject<float> &disp); - + + /** + * Calculate the sparse census 16x16 of two stereo images. + */ void sparse_census(const TextureObject<uchar4> &l, const TextureObject<uchar4> &r, TextureObject<uint2> &cl, TextureObject<uint2> &cr); + /** + * Filter a disparity image by a texture complexity threshold. + */ void texture_filter(const TextureObject<uchar4> &t, const TextureObject<float> &d, TextureObject<float> &f, int num_disp, double thresh); - + + /** + * Obtain a texture map from a colour image. + */ void texture_map(const TextureObject<uchar4> &t, TextureObject<float> &f); diff --git a/cv-node/include/ftl/disparity.hpp b/cv-node/include/ftl/disparity.hpp index cfd2aab48d8644293b3c63de70e9818f5e6e9fea..ed7f6b70fcacfefa76f5dd05aa081c9569507fc7 100644 --- a/cv-node/include/ftl/disparity.hpp +++ b/cv-node/include/ftl/disparity.hpp @@ -1,3 +1,7 @@ +/* + * Copyright 2019 Nicolas Pope + */ + #ifndef _FTL_DISPARITY_HPP_ #define _FTL_DISPARITY_HPP_ @@ -5,15 +9,29 @@ #include <nlohmann/json.hpp> namespace ftl { + +/** + * Virtual base class for disparity algorithms. An automatic factory is used + * to construct instances of specific algorithms that implement this + * interface, for this to work a static instance of the Register class must + * be created in the algorithms cpp file. + */ class Disparity { public: - Disparity(nlohmann::json &config); + explicit Disparity(nlohmann::json &config); virtual void setMinDisparity(size_t min) { min_disp_ = min; } virtual void setMaxDisparity(size_t max) { max_disp_ = max; } + /** + * Pure virtual function representing the actual computation of + * disparity from left and right images to be implemented. + */ virtual void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp)=0; + /** + * Factory registration class. + */ class Register { public: Register(const std::string &n, std::function<Disparity*(nlohmann::json&)> f) { @@ -21,6 +39,10 @@ class Disparity { }; }; + /** + * Factory instance creator where config contains an "algorithm" property + * used as the instance name to construct. + */ static Disparity *create(nlohmann::json &config); protected: diff --git a/cv-node/include/ftl/display.hpp b/cv-node/include/ftl/display.hpp index e0706a7bfcaf62a55a6187892f7e5676f5f7c9ea..930f698d1dcf1e1db3f9d16bdd842fda511f7800 100644 --- a/cv-node/include/ftl/display.hpp +++ b/cv-node/include/ftl/display.hpp @@ -1,33 +1,42 @@ +/* + * Copyright 2019 Nicolas Pope + */ + #ifndef _FTL_DISPLAY_HPP_ #define _FTL_DISPLAY_HPP_ +#include <ftl/config.h> + #include <nlohmann/json.hpp> #include <opencv2/opencv.hpp> #include <ftl/calibrate.hpp> -#include <ftl/config.h> #include "opencv2/highgui.hpp" namespace ftl { - class Display { - public: - Display(const Calibrate &cal, nlohmann::json &config); - ~Display(); - bool render(const cv::Mat &rgb, const cv::Mat &depth); +/** + * Multiple local display options for disparity or point clouds. + */ +class Display { + public: + Display(const Calibrate &cal, nlohmann::json &config); + ~Display(); + + bool render(const cv::Mat &rgb, const cv::Mat &depth); - bool active() const; + bool active() const; - private: - const ftl::Calibrate &calibrate_; - nlohmann::json config_; + private: + const ftl::Calibrate &calibrate_; + nlohmann::json config_; - #if defined HAVE_VIZ - cv::viz::Viz3d *window_; - #endif // HAVE_VIZ + #if defined HAVE_VIZ + cv::viz::Viz3d *window_; + #endif // HAVE_VIZ - bool active_; - }; + bool active_; +}; }; -#endif // _FTL_DISPLAY_HPP_ +#endif // _FTL_DISPLAY_HPP_ diff --git a/cv-node/src/algorithms/opencv_sgbm.cpp b/cv-node/src/algorithms/opencv_sgbm.cpp index fda8ff13eda864ab4dbd1d7138d7ea851e58c8e0..1f9a5aa1e8d5a3f03f669c0672c3244a76e61fb2 100644 --- a/cv-node/src/algorithms/opencv_sgbm.cpp +++ b/cv-node/src/algorithms/opencv_sgbm.cpp @@ -1,8 +1,11 @@ +/* + * Copyright 2019 Nicolas Pope + */ + #include <ftl/algorithms/opencv_sgbm.hpp> using ftl::algorithms::OpenCVSGBM; -using namespace cv::ximgproc; -using namespace cv; +using cv::Mat; static ftl::Disparity::Register opencvsgbm("sgbm", OpenCVSGBM::create); @@ -11,15 +14,15 @@ OpenCVSGBM::OpenCVSGBM(nlohmann::json &config) : Disparity(config) { float sigma = config.value("sigma", 1.5); float lambda = config.value("lambda", 8000.0); - left_matcher_ = StereoSGBM::create(min_disp_,max_disp_,wsize); + left_matcher_ = cv::StereoSGBM::create(min_disp_, max_disp_, wsize); left_matcher_->setP1(24*wsize*wsize); left_matcher_->setP2(96*wsize*wsize); left_matcher_->setPreFilterCap(63); - left_matcher_->setMode(StereoSGBM::MODE_SGBM_3WAY); - left_matcher_->setMinDisparity(config.value("minimum",0)); - wls_filter_ = createDisparityWLSFilter(left_matcher_); - right_matcher_ = createRightMatcher(left_matcher_); - + left_matcher_->setMode(cv::StereoSGBM::MODE_SGBM_3WAY); + left_matcher_->setMinDisparity(config.value("minimum", 0)); + wls_filter_ = cv::ximgproc::createDisparityWLSFilter(left_matcher_); + right_matcher_ = cv::ximgproc::createRightMatcher(left_matcher_); + wls_filter_->setLambda(lambda); wls_filter_->setSigmaColor(sigma); } @@ -28,15 +31,13 @@ void OpenCVSGBM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) { Mat lbw, rbw; Mat left_disp; Mat right_disp; - cv::cvtColor(l, lbw, COLOR_BGR2GRAY); - cv::cvtColor(r, rbw, COLOR_BGR2GRAY); - left_matcher_-> compute(lbw, rbw,left_disp); - right_matcher_->compute(rbw,lbw, right_disp); - wls_filter_->filter(left_disp,l,disp,right_disp); - + cv::cvtColor(l, lbw, cv::COLOR_BGR2GRAY); + cv::cvtColor(r, rbw, cv::COLOR_BGR2GRAY); + left_matcher_-> compute(lbw, rbw, left_disp); + right_matcher_->compute(rbw, lbw, right_disp); + wls_filter_->filter(left_disp, l, disp, right_disp); + // WHY 12!!!!!! disp.convertTo(disp, CV_32F, 12.0 / max_disp_); } - - diff --git a/cv-node/src/algorithms/rtcensus.cpp b/cv-node/src/algorithms/rtcensus.cpp index c0c98eb19c3c0c5ed1b6ef93e1411c1cda3805d5..d1aeba173fabb8764b3d78eddf0b761bd74aa1b3 100644 --- a/cv-node/src/algorithms/rtcensus.cpp +++ b/cv-node/src/algorithms/rtcensus.cpp @@ -14,17 +14,16 @@ * Equation numbering uses [1] unless otherwise stated */ - -#include <ftl/algorithms/rtcensus.hpp> - -#include <cmath> #include <glog/logging.h> +#include <cmath> #include <vector> #include <tuple> #include <bitset> #include <chrono> +#include <ftl/algorithms/rtcensus.hpp> + using ftl::algorithms::RTCensus; using std::vector; using cv::Mat; @@ -100,7 +99,7 @@ static void dsi_ca(vector<uint16_t> &result, const vector<uint64_t> &census_R, for (size_t d=0; d < ds; d++) { const auto d_ = d * sign; auto l = census_L[v_+(u_+d_)]; - result[ix+d] += bitset<64>(r^l).count(); // Hamming distance + result[ix+d] += bitset<64>(r^l).count(); // Hamming distance } } }