diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index dabc327c01d0fcddee74fd606f13ef7ff91b7aaf..35916ad801b4afcb7b33d993d9ef5cf7f9cdd59e 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -20,7 +20,7 @@ linux:
# before_script:
# - export DEBIAN_FRONTEND=noninteractive
# - apt-get update -qq && apt-get install -y -qq g++ cmake git
-# - apt-get install -y -qq libopencv-dev libgoogle-glog-dev liburiparser-dev libreadline-dev libmsgpack-dev uuid-dev libpcl-dev
+# - apt-get install -y -qq libopencv-dev libgoogle-glog-dev liburiparser-dev libreadline-dev libmsgpack-dev uuid-dev
script:
- mkdir build
- cd build
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6640f5d7ee46c3511254604ae0840e49cb9e91d6..584d3256e42acd4fd09280094158c6eef5484a80 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,6 @@ include(GNUInstallDirs)
include(CTest)
enable_testing()
-option(WITH_PCL "Use PCL if available" OFF)
option(WITH_NVPIPE "Use NvPipe for compression if available" ON)
option(WITH_OPTFLOW "Use NVIDIA Optical Flow if available" OFF)
option(WITH_OPENVR "Build with OpenVR support" OFF)
@@ -68,17 +67,9 @@ if(${CMAKE_VERSION} VERSION_GREATER "3.12.0")
cmake_policy(SET CMP0074 NEW)
endif()
-if (WITH_PCL)
- find_package( PCL QUIET COMPONENTS io common visualization registration segmentation)
-endif()
-
set(CMAKE_CXX_STANDARD 17) # For PCL/VTK https://github.com/PointCloudLibrary/pcl/issues/2686
set(HAVE_OPENCV TRUE)
-if (PCL_FOUND)
- set(HAVE_PCL TRUE)
-endif()
-
# Readline library is not required on Windows
# May also entirely remove dependence on this... it should be optional at least.
if (NOT WIN32)
@@ -250,10 +241,6 @@ if (BUILD_CALIBRATION)
add_subdirectory(applications/calibration-multi)
endif()
-if (HAVE_PCL)
- #add_subdirectory(applications/registration)
-endif()
-
if (BUILD_RECONSTRUCT)
add_subdirectory(applications/reconstruct)
endif()
diff --git a/README.md b/README.md
index 2269fe68c8b670690a14759aa463f343faaf5d0c..f439aa9098cc3a45a4cefaa695fc1ffcc3aecc48 100644
--- a/README.md
+++ b/README.md
@@ -47,5 +47,4 @@ to configure the project.
You will need to have OpenCV and glog installed. CUDA and LibSGM are optional
but recommended also. OpenCV should have cuda stereo modules compiled, along
with the viz module if local point cloud display is required. These are contrib
-modules. PCL is also required to build the reconstruction components and app.
-
+modules.
diff --git a/applications/registration/CMakeLists.txt b/applications/registration/CMakeLists.txt
deleted file mode 100644
index 8b27dcb38f13a92f2988d963e250c1028b6712f8..0000000000000000000000000000000000000000
--- a/applications/registration/CMakeLists.txt
+++ /dev/null
@@ -1,15 +0,0 @@
-set(REGSRC
- src/main.cpp
- src/manual.cpp
- src/correspondances.cpp
- src/sfm.cpp
- src/aruco.cpp
- src/common.cpp
-)
-
-add_executable(ftl-register ${REGSRC})
-
-target_include_directories(ftl-register PRIVATE src)
-
-target_link_libraries(ftl-register ftlcommon ftlnet ftlrgbd Threads::Threads ${OpenCV_LIBS})
-
diff --git a/applications/registration/src/aruco.cpp b/applications/registration/src/aruco.cpp
deleted file mode 100644
index 99d3552d9693cb548384043cef741703f115ce05..0000000000000000000000000000000000000000
--- a/applications/registration/src/aruco.cpp
+++ /dev/null
@@ -1,213 +0,0 @@
-#include "aruco.hpp"
-#include "common.hpp"
-#include "correspondances.hpp"
-
-#include <ftl/net/universe.hpp>
-#include <ftl/rgbd/source.hpp>
-
-#include <opencv2/aruco.hpp>
-
-using ftl::registration::aruco;
-using ftl::registration::Correspondances;
-using std::map;
-using std::string;
-using std::vector;
-using ftl::Configurable;
-using ftl::net::Universe;
-using ftl::rgbd::Source;
-using std::pair;
-using std::make_pair;
-
-void ftl::registration::aruco(ftl::Configurable *root) {
- using namespace cv;
-
- Universe *net = ftl::create<Universe>(root, "net");
-
- net->start();
- net->waitConnections();
-
- auto sources = ftl::createArray<Source>(root, "sources", net);
-
- int curtarget = 0;
- bool freeze = false;
- bool depthtoggle = false;
- double lastScore = 1000.0;
-
- if (sources.size() == 0) return;
-
- for (auto *src : sources) src->setChannel(ftl::rgbd::kChanDepth);
-
- cv::namedWindow("Target", cv::WINDOW_KEEPRATIO);
- cv::namedWindow("Source", cv::WINDOW_KEEPRATIO);
-
- map<string, Eigen::Matrix4d> oldTransforms;
- ftl::registration::loadTransformations(root->value("output", string("./test.json")), oldTransforms);
-
- //Correspondances c(sources[targsrc], sources[cursrc]);
- map<string, Correspondances*> corrs;
- ftl::registration::build_correspondances(sources, corrs, root->value("origin", 0), oldTransforms);
-
- Correspondances *current = corrs[sources[curtarget]->getURI()];
-
- map<int, vector<Point2f>> targ_corners;
- map<int, vector<Point2f>> src_corners;
- map<int, pair<Vec3d,Vec3d>> targ_trans;
- map<int, pair<Vec3d,Vec3d>> src_trans;
-
- while (ftl::running) {
- if (!freeze) {
- // Grab the latest frames from sources
- for (int i=0; i<sources.size(); i++) {
- if (sources[i]->isReady()) {
- sources[i]->grab();
- }
- }
- }
-
- // Get the raw rgb and depth frames from sources
- Mat ttrgb, trgb, tdepth, ttdepth;
- Mat tsrgb, srgb, sdepth, tsdepth;
-
- if (current == nullptr) {
- srgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
- trgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
-
- putText(srgb, string("No correspondance for ") + sources[curtarget]->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-
- if (!trgb.empty()) imshow("Target", trgb);
- if (!srgb.empty()) imshow("Source", srgb);
- } else if (current->source()->isReady() && current->target()->isReady()) {
- current->source()->getFrames(tsrgb, tsdepth);
- current->target()->getFrames(ttrgb, ttdepth);
-
- tsrgb.copyTo(srgb);
- ttrgb.copyTo(trgb);
- ttdepth.convertTo(tdepth, CV_8U, 255.0f / 10.0f);
- applyColorMap(tdepth, tdepth, cv::COLORMAP_JET);
- tsdepth.convertTo(sdepth, CV_8U, 255.0f / 10.0f);
- applyColorMap(sdepth, sdepth, cv::COLORMAP_JET);
-
- vector<int> mids_targ, mids_src;
- vector<vector<Point2f>> mpoints_targ, mpoints_src;
- Ptr<aruco::DetectorParameters> parameters(new aruco::DetectorParameters);
- auto dictionary = aruco::getPredefinedDictionary(aruco::DICT_4X4_50);
-
- aruco::detectMarkers(trgb, dictionary, mpoints_targ, mids_targ, parameters);
- aruco::drawDetectedMarkers(trgb, mpoints_targ, mids_targ);
-
- // Cache the marker positions
- for (size_t i=0; i<mids_targ.size(); i++) {
- targ_corners[mids_targ[i]] = mpoints_targ[i];
- }
-
- vector<Vec3d> rvecs, tvecs;
- cv::Mat distCoef(cv::Size(14,1), CV_64F, cv::Scalar(0.0));
- cv::Mat targ_cam = current->target()->cameraMatrix();
- aruco::estimatePoseSingleMarkers(mpoints_targ, 0.1, targ_cam, distCoef, rvecs, tvecs);
- for (size_t i=0; i<rvecs.size(); i++) {
- targ_trans[mids_targ[i]] = make_pair(tvecs[i], rvecs[i]);
- aruco::drawAxis(trgb, current->target()->cameraMatrix(), distCoef, rvecs[i], tvecs[i], 0.1);
- }
-
- aruco::detectMarkers(srgb, dictionary, mpoints_src, mids_src, parameters);
- aruco::drawDetectedMarkers(srgb, mpoints_src, mids_src);
-
- rvecs.clear();
- tvecs.clear();
- cv::Mat src_cam = current->source()->cameraMatrix();
- aruco::estimatePoseSingleMarkers(mpoints_src, 0.1, src_cam, distCoef, rvecs, tvecs);
- for (size_t i=0; i<rvecs.size(); i++) {
- src_trans[mids_src[i]] = make_pair(tvecs[i], rvecs[i]);
- aruco::drawAxis(srgb, current->source()->cameraMatrix(), distCoef, rvecs[i], tvecs[i], 0.1);
- }
-
- // Cache the marker positions
- for (size_t i=0; i<mids_src.size(); i++) {
- src_corners[mids_src[i]] = mpoints_src[i];
- }
-
- current->drawTarget(trgb);
- current->drawTarget(tdepth);
- current->drawSource(srgb);
- current->drawSource(sdepth);
-
- putText(srgb, string("Source: ") + current->source()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
- putText(trgb, string("Target: ") + current->target()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
- putText(srgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
- putText(trgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
- if (freeze) putText(srgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
- if (freeze) putText(trgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-
- if (!trgb.empty()) imshow("Target", (depthtoggle) ? tdepth : trgb);
- if (!srgb.empty()) imshow("Source", (depthtoggle) ? sdepth : srgb);
- }
-
- int key = cv::waitKey(20);
- if (key == 27) break;
- else if (key >= 48 && key <= 57) {
- curtarget = key-48;
- if (curtarget >= sources.size()) curtarget = 0;
- current = corrs[sources[curtarget]->getURI()];
- //lastScore = 1000.0;
- } else if (key == 'd') {
- depthtoggle = !depthtoggle;
- } else if (key == 'i') {
- current->icp();
- } else if (key == 'e') {
- vector<Vec3d> targfeat;
- vector<Vec3d> srcfeat;
-
- for (auto i : targ_trans) {
- auto si = src_trans.find(i.first);
- if (si != src_trans.end()) {
- //Vec3d dt = std::get<0>(si->second) - std::get<0>(i.second);
- targfeat.push_back(std::get<0>(i.second));
- srcfeat.push_back(std::get<0>(si->second));
- }
- }
-
- Eigen::Matrix4d t;
- lastScore = current->estimateTransform(t, srcfeat, targfeat, true);
- current->setTransform(t);
- //lastScore = current->icp();
- } else if (key == 'f') {
- Mat f1,f2;
- current->capture(f1,f2);
-
- LOG(INFO) << "Captured frames";
-
- // Convert matching marker corners into correspondence points
- for (auto i : targ_corners) {
- auto si = src_corners.find(i.first);
- if (si != src_corners.end()) {
- for (size_t j=0; j<4; ++j) {
- current->add(i.second[j].x, i.second[j].y, si->second[j].x, si->second[j].y);
- }
- }
- }
-
- LOG(INFO) << "Estimating transform...";
-
- Eigen::Matrix4d t;
- lastScore = current->estimateTransform(t);
- current->setTransform(t);
- lastScore = current->icp();
- } else if (key == 's') {
- // Save
- map<string, Eigen::Matrix4d> transforms;
- //transforms[sources[targsrc]->getURI()] = Eigen::Matrix4f().setIdentity();
- //transforms[sources[cursrc]->getURI()] = c.transform();
-
- for (auto x : corrs) {
- if (x.second == nullptr) {
- transforms[x.first] = Eigen::Matrix4d().setIdentity();
- } else {
- transforms[x.first] = x.second->transform();
- }
- }
-
- saveTransformations(root->value("output", string("./test.json")), transforms);
- LOG(INFO) << "Saved!";
- } else if (key == 32) freeze = !freeze;
- }
-}
diff --git a/applications/registration/src/aruco.hpp b/applications/registration/src/aruco.hpp
deleted file mode 100644
index 88438eac1bbae457c763bddb21c5259622bd1faa..0000000000000000000000000000000000000000
--- a/applications/registration/src/aruco.hpp
+++ /dev/null
@@ -1,14 +0,0 @@
-#ifndef _FTL_REGISTRATION_ARUCO_HPP_
-#define _FTL_REGISTRATION_ARUCO_HPP_
-
-#include <ftl/configuration.hpp>
-
-namespace ftl {
-namespace registration {
-
-void aruco(ftl::Configurable *root);
-
-}
-}
-
-#endif // _FTL_REGISTRATION_ARUCO_HPP_
diff --git a/applications/registration/src/common.cpp b/applications/registration/src/common.cpp
deleted file mode 100644
index d225c636734f0f8899e719b53ff1ea79f91c96e3..0000000000000000000000000000000000000000
--- a/applications/registration/src/common.cpp
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "common.hpp"
-
-using std::string;
-using std::map;
-
-void ftl::registration::from_json(nlohmann::json &json, map<string, Eigen::Matrix4d> &transformations) {
- for (auto it = json.begin(); it != json.end(); ++it) {
- Eigen::Matrix4d m;
- auto data = m.data();
- for(size_t i = 0; i < 16; i++) { data[i] = it.value()[i]; }
- transformations[it.key()] = m;
- }
-}
-
-void ftl::registration::to_json(nlohmann::json &json, map<string, Eigen::Matrix4d> &transformations) {
- for (auto &item : transformations) {
- auto val = nlohmann::json::array();
- for(size_t i = 0; i < 16; i++) { val.push_back((float) item.second.data()[i]); }
- json[item.first] = val;
- }
-}
-
-bool ftl::registration::saveTransformations(const string &path, map<string, Eigen::Matrix4d> &data) {
- nlohmann::json data_json;
- to_json(data_json, data);
- std::ofstream file(path);
-
- if (!file.is_open()) {
- LOG(ERROR) << "Error writing transformations to file " << path;
- return false;
- }
-
- file << std::setw(4) << data_json;
- return true;
-}
-
-bool ftl::registration::loadTransformations(const string &path, map<string, Eigen::Matrix4d> &data) {
- std::ifstream file(path);
- if (!file.is_open()) {
- LOG(ERROR) << "Error loading transformations from file " << path;
- return false;
- }
-
- nlohmann::json json_registration;
- file >> json_registration;
- from_json(json_registration, data);
- return true;
-}
-
diff --git a/applications/registration/src/common.hpp b/applications/registration/src/common.hpp
deleted file mode 100644
index bbb4e3749fd5c4ec3651291b01663667e406677e..0000000000000000000000000000000000000000
--- a/applications/registration/src/common.hpp
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef _FTL_REGISTRATION_COMMON_HPP_
-#define _FTL_REGISTRATION_COMMON_HPP_
-
-#include <ftl/rgbd/source.hpp>
-#include <nlohmann/json.hpp>
-
-#include <string>
-#include <map>
-
-namespace ftl {
-namespace registration {
-
-bool loadTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4d> &data);
-bool saveTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4d> &data);
-void from_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
-void to_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
-
-}
-}
-
-#endif // _FTL_REGISTRATION_COMMON_HPP_
diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
deleted file mode 100644
index ce662756a7901be5b5b1a3662eeb082585b3839a..0000000000000000000000000000000000000000
--- a/applications/registration/src/correspondances.cpp
+++ /dev/null
@@ -1,545 +0,0 @@
-#include "correspondances.hpp"
-#include <nlohmann/json.hpp>
-#include <random>
-#include <chrono>
-#include <thread>
-#include <opencv2/xphoto.hpp>
-#include <pcl/registration/icp.h>
-
-using ftl::registration::Correspondances;
-using std::string;
-using std::map;
-using std::vector;
-using ftl::rgbd::Source;
-using pcl::PointCloud;
-using pcl::PointXYZ;
-using pcl::PointXYZRGB;
-using nlohmann::json;
-using std::tuple;
-using std::make_tuple;
-using cv::Mat;
-
-void ftl::registration::build_correspondances(const vector<Source*> &sources, map<string, Correspondances*> &cs, int origin, map<string, Eigen::Matrix4d> &old) {
- Correspondances *last = nullptr;
-
- cs[sources[origin]->getURI()] = nullptr;
-
- for (int i=origin-1; i>=0; i--) {
- if (last == nullptr) {
- auto *c = new Correspondances(sources[i], sources[origin]);
- last = c;
- cs[sources[i]->getURI()] = c;
- if (old.find(sources[i]->getURI()) != old.end()) {
- c->setTransform(old[sources[i]->getURI()]);
- }
- } else {
- auto *c = new Correspondances(last, sources[i]);
- last = c;
- cs[sources[i]->getURI()] = c;
- if (old.find(sources[i]->getURI()) != old.end()) {
- c->setTransform(old[sources[i]->getURI()]);
- }
- }
- }
-
- last = nullptr;
-
- for (int i=origin+1; i<sources.size(); i++) {
- if (last == nullptr) {
- auto *c = new Correspondances(sources[i], sources[origin]);
- last = c;
- cs[sources[i]->getURI()] = c;
- } else {
- auto *c = new Correspondances(last, sources[i]);
- last = c;
- cs[sources[i]->getURI()] = c;
- }
- }
-}
-
-
-Correspondances::Correspondances(Source *src, Source *targ)
- : parent_(nullptr), targ_(targ), src_(src),
- targ_cloud_(new PointCloud<PointXYZ>),
- src_cloud_(new PointCloud<PointXYZ>), uptodate_(true) {
- transform_.setIdentity();
-}
-
-Correspondances::Correspondances(Correspondances *parent, Source *src)
- : parent_(parent), targ_(parent_->source()), src_(src),
- targ_cloud_(new PointCloud<PointXYZ>),
- src_cloud_(new PointCloud<PointXYZ>), uptodate_(true) {
- transform_.setIdentity();
-}
-
-static void averageDepth(vector<Mat> &in, Mat &out, float varThresh) {
- const int rows = in[0].rows;
- const int cols = in[0].cols;
- float varThresh2 = varThresh*varThresh;
-
- // todo: create new only if out is empty (or wrong shape/type)
- out = Mat(rows, cols, CV_32FC1);
-
- for (int i = 0; i < rows * cols; ++i) {
- double sum = 0;
- int good_values = 0;
-
- // Calculate mean
- for (int i_in = 0; i_in < in.size(); ++i_in) {
- double d = in[i_in].at<float>(i);
- if (ftl::rgbd::isValidDepth(d)) {
- good_values++;
- sum += d;
- }
- }
-
- if (good_values > 0) {
- sum /= (double)good_values;
-
- // Calculate variance
- double var = 0.0;
- for (int i_in = 0; i_in < in.size(); ++i_in) {
- double d = in[i_in].at<float>(i);
- if (d < 40.0) {
- double delta = (d - sum);
- var += delta*delta;
-
- //LOG(INFO) << "VAR " << delta;
- }
- }
- if (good_values > 1) var /= (double)(good_values-1);
- else var = 0.0;
-
- if (var < varThresh2) {
- out.at<float>(i) = (float)sum;
- } else {
- out.at<float>(i) = 0.0f;
- }
- } else {
- out.at<float>(i) = 0.0f;
- }
- }
-}
-
-static PointXYZ makePCL(Source *s, int x, int y) {
- Eigen::Vector4d p1 = s->point(x,y);
- PointXYZ pcl_p1;
- pcl_p1.x = p1[0];
- pcl_p1.y = p1[1];
- pcl_p1.z = p1[2];
- return pcl_p1;
-}
-
-void Correspondances::drawTarget(cv::Mat &img) {
- using namespace cv;
-
- for (size_t i=0; i<log_.size(); i++) {
- //for (auto &p : points) {
- auto [tx,ty,sx,sy] = log_[i];
- drawMarker(img, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
- }
-
- vector<Eigen::Vector2i> tpoints;
- getTransformedFeatures2D(tpoints);
- for (size_t i=0; i<tpoints.size(); i++) {
- Eigen::Vector2i p = tpoints[i];
- drawMarker(img, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
- }
-}
-
-void Correspondances::drawSource(cv::Mat &img) {
- using namespace cv;
-
- for (size_t i=0; i<log_.size(); i++) {
- //for (auto &p : points) {
- auto [tx,ty,sx,sy] = log_[i];
- drawMarker(img, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
- }
-
- /*vector<Eigen::Vector2i> tpoints;
- getTransformedFeatures2D(tpoints);
- for (size_t i=0; i<tpoints.size(); i++) {
- Eigen::Vector2i p = tpoints[i];
- drawMarker(img, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
- }*/
-}
-
-void Correspondances::clear() {
- targ_cloud_->clear();
- src_cloud_->clear();
- src_index_ = cv::Mat(cv::Size(src_->parameters().width, src_->parameters().height), CV_32SC1, cv::Scalar(-1));
- targ_index_ = cv::Mat(cv::Size(targ_->parameters().width, targ_->parameters().height), CV_32SC1, cv::Scalar(-1));
- src_feat_.clear();
- targ_feat_.clear();
- log_.clear();
-}
-
-void Correspondances::clearCorrespondances() {
- src_feat_.clear();
- targ_feat_.clear();
- log_.clear();
- uptodate_ = false;
-}
-
-bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
- Mat d1, d2;
- size_t buffer_size = 10;
- size_t buffer_i = 0;
- vector<vector<Mat>> buffer(2, vector<Mat>(buffer_size));
-
- for (size_t i = 0; i < buffer_size; ++i) {
- src_->grab();
- targ_->grab();
- src_->getFrames(rgb1, d1);
- targ_->getFrames(rgb2, d2);
-
- d1.copyTo(buffer[0][i]);
- d2.copyTo(buffer[1][i]);
-
- std::this_thread::sleep_for(std::chrono::milliseconds(50));
- }
- averageDepth(buffer[0], d1, 0.02f);
- averageDepth(buffer[1], d2, 0.02f);
- Mat d1_v, d2_v;
- d1.convertTo(d1_v, CV_8U, 255.0f / 10.0f);
- d2.convertTo(d2_v, CV_8U, 255.0f / 10.0f);
- applyColorMap(d1_v, d1_v, cv::COLORMAP_JET);
- applyColorMap(d2_v, d2_v, cv::COLORMAP_JET);
-
- cv::imshow("smooth d1", d1_v);
- cv::imshow("smooth d2", d2_v);
-
- // Should be done in RGB-Depth source class
- cv::Ptr<cv::xphoto::WhiteBalancer> wb;
- wb = cv::xphoto::createSimpleWB();
- wb->balanceWhite(rgb1, rgb1);
- wb->balanceWhite(rgb2, rgb2);
-
- // Build point clouds
- clear();
- int six=0;
- int tix=0;
-
- for (int y=0; y<rgb1.rows; y++) {
- //unsigned char *rgb1ptr = rgb1.ptr(y);
- //unsigned char *rgb2ptr = rgb2.ptr(y);
- float *d1ptr = (float*)d1.ptr(y);
- float *d2ptr = (float*)d2.ptr(y);
-
- for (int x=0; x<rgb1.cols; x++) {
- float d1_value = d1ptr[x];
- float d2_value = d2ptr[x];
-
- if (d1_value > 0.1f && d1_value < 39.0f) {
- // Make PCL points with specific depth value
- pcl::PointXYZ p1;
- Eigen::Vector4d p1e = src_->point(x,y,d1_value);
- p1.x = p1e[0];
- p1.y = p1e[1];
- p1.z = p1e[2];
- src_cloud_->push_back(p1);
- src_index_.at<int>(y,x) = six;
- six++;
- }
-
- if (d2_value > 0.1f && d2_value < 39.0f) {
- // Make PCL points with specific depth value
- pcl::PointXYZ p2;
- Eigen::Vector4d p2e = targ_->point(x,y,d2_value);
- p2.x = p2e[0];
- p2.y = p2e[1];
- p2.z = p2e[2];
- targ_cloud_->push_back(p2);
- targ_index_.at<int>(y,x) = tix;
- tix++;
- }
- }
- }
-
- return true; // TODO: return statement was missing; is true correct?
-}
-
-bool Correspondances::add(int tx, int ty, int sx, int sy) {
- LOG(INFO) << "Adding...";
- int tix = targ_index_.at<int>(ty,tx);
- int six = src_index_.at<int>(sy,sx);
-
- // Validate feature
- if (tix == -1 || six == -1) {
- LOG(WARNING) << "Bad point";
- return false;
- }
-
- log_.push_back(make_tuple(tx,ty,sx,sy));
-
- // Record correspondance point cloud point indexes
- src_feat_.push_back(six);
- targ_feat_.push_back(tix);
-
- uptodate_ = false;
- LOG(INFO) << "Point added: " << tx << "," << ty << " and " << sx << "," << sy;
- return true;
-}
-
-void Correspondances::removeLast() {
- uptodate_ = false;
- targ_feat_.erase(targ_feat_.end()-1);
- src_feat_.erase(src_feat_.end()-1);
- log_.pop_back();
-}
-
-double Correspondances::estimateTransform(Eigen::Matrix4d &T) {
- pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ, double> validate;
- pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ, double> svd;
-
- //validate.setMaxRange(0.1);
-
- pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-
- vector<int> idx;
-
- for (int i=0; i<src_feat_.size(); i++) {
- idx.push_back(i);
- src_cloud->push_back(src_cloud_->at(src_feat_[i]));
- targ_cloud->push_back(targ_cloud_->at(targ_feat_[i]));
- }
-
- pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
-
- svd.estimateRigidTransformation(*src_cloud, idx, *targ_cloud, idx, T);
- return validate.validateTransformation(src_cloud, targ_cloud, T);
-}
-
-double Correspondances::estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat, bool doicp) {
- pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ, double> validate;
- pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ, double> svd;
-
- pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-
- vector<int> idx;
-
- for (int i=0; i<src_feat.size(); i++) {
- pcl::PointXYZ ps,pt;
-
- ps.x = src_feat[i][0];
- ps.y = src_feat[i][1];
- ps.z = src_feat[i][2];
- pt.x = targ_feat[i][0];
- pt.y = targ_feat[i][1];
- pt.z = targ_feat[i][2];
-
- idx.push_back(i);
- src_cloud->push_back(ps);
- targ_cloud->push_back(pt);
- }
-
- pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
-
- svd.estimateRigidTransformation(*tsrc_cloud, idx, *targ_cloud, idx, T);
-
- if (doicp) {
- pcl::transformPointCloud(*tsrc_cloud, *src_cloud, T);
-
- pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ, double> icp;
-
- icp.setInputSource(src_cloud);
- icp.setInputTarget(targ_cloud);
- icp.align(*final_cloud);
- LOG(INFO) << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore();
- //transform_ *= icp.getFinalTransformation();
-
- T = icp.getFinalTransformation() * T * transform_;
- //uptodate_ = true;
-
- return icp.getFitnessScore();
- } else {
- return validate.validateTransformation(src_cloud, targ_cloud, T);
- }
-}
-
-double Correspondances::estimateTransform(Eigen::Matrix4d &T, const vector<int> &src_feat, const vector<int> &targ_feat) {
- pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ, double> validate;
- pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ, double> svd;
-
- pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-
- vector<int> idx;
-
- for (int i=0; i<src_feat.size(); i++) {
- idx.push_back(i);
- src_cloud->push_back(src_cloud_->at(src_feat[i]));
- targ_cloud->push_back(targ_cloud_->at(targ_feat[i]));
- }
-
- pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
-
- validate.setMaxRange(0.1);
-
- svd.estimateRigidTransformation(*tsrc_cloud, idx, *targ_cloud, idx, T);
- T = T * transform_;
- uptodate_ = true;
- float score = validate.validateTransformation(src_cloud, targ_cloud, T);
- return score;
-}
-
-double Correspondances::icp() {
- //pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-
- pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-
- vector<int> idx;
-
- for (int i=0; i<src_feat_.size(); i++) {
- idx.push_back(i);
- src_cloud->push_back(src_cloud_->at(src_feat_[i]));
- targ_cloud->push_back(targ_cloud_->at(targ_feat_[i]));
- }
-
- pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
-
- pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ, double> icp;
- icp.setInputSource(tsrc_cloud);
- icp.setInputTarget(targ_cloud);
- icp.align(*final_cloud);
- LOG(INFO) << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore();
- transform_ *= icp.getFinalTransformation();
- return icp.getFitnessScore();
-}
-
-double Correspondances::icp(const Eigen::Matrix4d &T_in, Eigen::Matrix4d &T_out, const vector<int> &idx) {
- //pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-
- pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-
- for (int i=0; i<idx.size(); i++) {
- src_cloud->push_back(src_cloud_->at(src_feat_[idx[i]]));
- targ_cloud->push_back(targ_cloud_->at(targ_feat_[idx[i]]));
- }
-
- pcl::transformPointCloud(*src_cloud, *tsrc_cloud, T_in);
-
- pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ, double> icp;
- icp.setInputSource(tsrc_cloud);
- icp.setInputTarget(targ_cloud);
- icp.align(*final_cloud);
- LOG(INFO) << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore();
- T_out = T_in * icp.getFinalTransformation();
- return icp.getFitnessScore();
-}
-
-static std::default_random_engine generator;
-
-void Correspondances::convertToPCL(const std::vector<std::tuple<int,int,int,int>> &p, std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &pp) {
- for (size_t i=0; i<p.size(); i++) {
- auto [sx,sy,tx,ty] = p[i];
- //LOG(INFO) << "POINT " << sx << "," << sy;
- auto p1 = makePCL(src_, sx, sy);
- auto p2 = makePCL(targ_, tx, ty);
- pp.push_back(std::make_pair(p1,p2));
- }
-}
-
-void Correspondances::getFeatures3D(std::vector<Eigen::Vector4d> &f) {
- for (int i=0; i<src_feat_.size(); i++) {
- Eigen::Vector4d p;
- const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
- p[0] = pp.x;
- p[1] = pp.y;
- p[2] = pp.z;
- p[3] = 1.0;
- f.push_back(p);
- }
-}
-
-void Correspondances::getTransformedFeatures(std::vector<Eigen::Vector4d> &f) {
- for (int i=0; i<src_feat_.size(); i++) {
- Eigen::Vector4d p;
- const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
- p[0] = pp.x;
- p[1] = pp.y;
- p[2] = pp.z;
- p[3] = 1.0;
- f.push_back(transform_ * p);
- }
-}
-
-void Correspondances::getTransformedFeatures2D(std::vector<Eigen::Vector2i> &f) {
- for (int i=0; i<src_feat_.size(); i++) {
- Eigen::Vector4d p;
- const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
- p[0] = pp.x;
- p[1] = pp.y;
- p[2] = pp.z;
- p[3] = 1.0;
- f.push_back(src_->point(transform_ * p));
- }
-}
-
-double Correspondances::findBestSubset(Eigen::Matrix4d &tr, int K, int N) {
- double score = 10000.0f;
- vector<int> best;
- Eigen::Matrix4d bestT;
-
- std::mt19937 rng(std::chrono::steady_clock::now().time_since_epoch().count());
- std::uniform_int_distribution<int> distribution(0,src_feat_.size()-1);
- //int dice_roll = distribution(generator);
- auto dice = std::bind ( distribution, rng );
-
- vector<int> sidx(K);
- vector<int> tidx(K);
- //for (int i = 0; i < K; i++) { idx[i] = i; }
-
- // 1. Build complete point clouds using filtered and smoothed depth maps
- // 2. Build a full index map of x,y to point cloud index.
- // 3. Generate random subsets of features using index map
- // 4. Find minimum
-
- for (int n=0; n<N; n++) {
-
- sidx.clear();
- tidx.clear();
-
- vector<int> idx;
- for (int k=0; k<K; k++) {
- int r = dice();
- idx.push_back(r);
- sidx.push_back(src_feat_[r]);
- tidx.push_back(targ_feat_[r]);
- }
-
- Eigen::Matrix4d T;
- float scoreT = estimateTransform(T, sidx, tidx);
-
- if (scoreT < score) {
- score = scoreT;
- bestT = T;
- best = idx;
- }
- }
-
- return icp(bestT, tr, best);
-
- // TODO Add these best points to actual clouds.
- //log_ = best;
- //tr = bestT;
- //uptodate_ = true;
- //return score;
-}
-
-Eigen::Matrix4d Correspondances::transform() {
- if (!uptodate_) estimateTransform(transform_);
- return (parent_) ? parent_->transform() * transform_ : transform_;
-}
diff --git a/applications/registration/src/correspondances.hpp b/applications/registration/src/correspondances.hpp
deleted file mode 100644
index 72f39cf86640834f4ee49855f19ed237b4883d1c..0000000000000000000000000000000000000000
--- a/applications/registration/src/correspondances.hpp
+++ /dev/null
@@ -1,110 +0,0 @@
-#ifndef _FTL_REG_CORRESPONDANCES_HPP_
-#define _FTL_REG_CORRESPONDANCES_HPP_
-
-#include <ftl/rgbd/source.hpp>
-#include <nlohmann/json.hpp>
-
-#include <pcl/common/transforms.h>
-
-#include <pcl/registration/transformation_estimation_svd.h>
-#include <pcl/registration/transformation_estimation_svd_scale.h>
-#include <pcl/registration/transformation_validation.h>
-#include <pcl/registration/transformation_validation_euclidean.h>
-
-#include <vector>
-#include <string>
-#include <map>
-#include <tuple>
-
-namespace ftl {
-namespace registration {
-
-/**
- * Manage the identified set of correspondances between two sources. There may
- * also be a parent correspondances object to support the chaining of
- * transforms.
- */
-class Correspondances {
- public:
- Correspondances(ftl::rgbd::Source *src, ftl::rgbd::Source *targ);
- Correspondances(Correspondances *parent, ftl::rgbd::Source *src);
-
- ftl::rgbd::Source *source() { return src_; }
- ftl::rgbd::Source *target() { return targ_; }
-
- void clear();
- void clearCorrespondances();
-
- bool capture(cv::Mat &rgb1, cv::Mat &rgb2);
-
- /**
- * Add a new correspondance point. The point will only be added if there is
- * a valid depth value at that location.
- *
- * @param tx X-Coordinate in target image
- * @param ty Y-Coordinate in target image
- * @param sx X-Coordinate in source image
- * @param sy Y-Coordinate in source image
- * @return False if point was invalid and not added.
- */
- bool add(int tx, int ty, int sx, int sy);
-
- void removeLast();
-
- const std::vector<std::tuple<int,int,int,int>> &screenPoints() const { return log_; }
- void getFeatures3D(std::vector<Eigen::Vector4d> &f);
- void getTransformedFeatures(std::vector<Eigen::Vector4d> &f);
- void getTransformedFeatures2D(std::vector<Eigen::Vector2i> &f);
-
- void setPoints(const std::vector<std::tuple<int,int,int,int>> &p) { log_ = p; }
-
- /**
- * Calculate a transform using current set of correspondances.
- *
- * @return Validation score of the transform.
- */
- double estimateTransform(Eigen::Matrix4d &);
- double estimateTransform(Eigen::Matrix4d &T, const std::vector<int> &src_feat, const std::vector<int> &targ_feat);
- double estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat, bool doicp=false);
-
- double findBestSubset(Eigen::Matrix4d &tr, int K, int N);
-
- double icp();
- double icp(const Eigen::Matrix4d &T_in, Eigen::Matrix4d &T_out, const std::vector<int> &idx);
-
- void convertToPCL(const std::vector<std::tuple<int,int,int,int>> &p, std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &p2);
-
- /**
- * Get the estimated transform. This includes any parent transforms to make
- * it relative to root camera.
- */
- Eigen::Matrix4d transform();
-
- void setTransform(Eigen::Matrix4d &t) { uptodate_ = true; transform_ = t; }
-
- void drawTarget(cv::Mat &);
- void drawSource(cv::Mat &);
-
- private:
- Correspondances *parent_;
- ftl::rgbd::Source *targ_;
- ftl::rgbd::Source *src_;
- pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud_;
- pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud_;
- Eigen::Matrix4d transform_;
- bool uptodate_;
- std::vector<std::tuple<int,int,int,int>> log_;
- cv::Mat src_index_;
- cv::Mat targ_index_;
- std::vector<int> targ_feat_;
- std::vector<int> src_feat_;
-};
-
-void build_correspondances(const std::vector<ftl::rgbd::Source*> &sources,
- std::map<std::string, Correspondances*> &cs, int origin,
- std::map<std::string, Eigen::Matrix4d> &old);
-
-}
-}
-
-#endif // _FTL_REG_CORRESPONDANCES_HPP_
diff --git a/applications/registration/src/main.cpp b/applications/registration/src/main.cpp
deleted file mode 100644
index bb566ed074c6af8b1a011102aebbc6ec06831e9b..0000000000000000000000000000000000000000
--- a/applications/registration/src/main.cpp
+++ /dev/null
@@ -1,21 +0,0 @@
-#include <loguru.hpp>
-#include <ftl/configuration.hpp>
-#include <string>
-
-#include "manual.hpp"
-#include "aruco.hpp"
-
-using std::string;
-
-int main(int argc, char **argv) {
- auto root = ftl::configure(argc, argv, "registration_default");
-
- string mode = root->value("mode", string("manual"));
- if (mode == "manual") {
- ftl::registration::manual(root);
- } else if (mode == "aruco") {
- ftl::registration::aruco(root);
- }
-
- return 0;
-}
diff --git a/applications/registration/src/manual.cpp b/applications/registration/src/manual.cpp
deleted file mode 100644
index 90f128855ea1cd9b3c87f750f0d72e11cf912890..0000000000000000000000000000000000000000
--- a/applications/registration/src/manual.cpp
+++ /dev/null
@@ -1,269 +0,0 @@
-#include "manual.hpp"
-#include "correspondances.hpp"
-#include "sfm.hpp"
-#include "common.hpp"
-
-#include <loguru.hpp>
-
-#include <ftl/net/universe.hpp>
-#include <ftl/rgbd/source.hpp>
-
-#include <opencv2/core.hpp>
-#include <opencv2/core/utility.hpp>
-#include <opencv2/imgproc.hpp>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/imgcodecs.hpp>
-#include <opencv2/videoio.hpp>
-#include <opencv2/highgui.hpp>
-
-#include <map>
-
-using std::string;
-using std::vector;
-using std::pair;
-using std::map;
-using std::tuple;
-
-using ftl::net::Universe;
-using ftl::rgbd::Source;
-using ftl::registration::Correspondances;
-
-using cv::Mat;
-using cv::Point;
-using cv::Scalar;
-
-using namespace ftl::registration;
-
-using MouseAction = std::function<void(int, int, int, int)>;
-
-static void setMouseAction(const std::string& winName, const MouseAction &action)
-{
- cv::setMouseCallback(winName,
- [] (int event, int x, int y, int flags, void* userdata) {
- (*(MouseAction*)userdata)(event, x, y, flags);
- }, (void*)&action);
-}
-
-static MouseAction tmouse;
-static MouseAction smouse;
-
-void ftl::registration::manual(ftl::Configurable *root) {
- using namespace cv;
-
- Universe *net = ftl::create<Universe>(root, "net");
-
- net->start();
- net->waitConnections();
-
- auto sources = ftl::createArray<Source>(root, "sources", net);
-
- int curtarget = 0;
- bool freeze = false;
-
- vector<Mat> rgb;
- vector<Mat> depth;
-
- if (sources.size() == 0) return;
-
- rgb.resize(sources.size());
- depth.resize(sources.size());
-
- cv::namedWindow("Target", cv::WINDOW_KEEPRATIO);
- cv::namedWindow("Source", cv::WINDOW_KEEPRATIO);
-
- map<string, Eigen::Matrix4d> oldTransforms;
- ftl::registration::loadTransformations(root->value("output", string("./test.json")), oldTransforms);
-
- //Correspondances c(sources[targsrc], sources[cursrc]);
- map<string, Correspondances*> corrs;
- ftl::registration::build_correspondances(sources, corrs, root->value("origin", 0), oldTransforms);
-
- int lastTX = 0;
- int lastTY = 0;
- int lastSX = 0;
- int lastSY = 0;
-
- tmouse = [&]( int event, int ux, int uy, int) {
- if (event == 1) { // click
- lastTX = ux;
- lastTY = uy;
- }
- };
- setMouseAction("Target", tmouse);
-
- smouse = [&]( int event, int ux, int uy, int) {
- if (event == 1) { // click
- lastSX = ux;
- lastSY = uy;
- }
- };
- setMouseAction("Source", smouse);
-
- bool depthtoggle = false;
- double lastScore = 0.0;
- bool insearch = false;
- int point_n = -1;
-
- Correspondances *current = corrs[sources[curtarget]->getURI()];
-
- while (ftl::running) {
- if (!freeze) {
- // Grab the latest frames from sources
- for (int i=0; i<sources.size(); i++) {
- if (sources[i]->isReady()) {
- sources[i]->grab();
- }
- }
- }
-
- // Get the raw rgb and depth frames from sources
- Mat ttrgb, trgb, tdepth, ttdepth;
- Mat tsrgb, srgb, sdepth, tsdepth;
-
- if (current == nullptr) {
- srgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
- trgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
-
- putText(srgb, string("No correspondance for ") + sources[curtarget]->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-
- if (!trgb.empty()) imshow("Target", trgb);
- if (!srgb.empty()) imshow("Source", srgb);
- } else if (current->source()->isReady() && current->target()->isReady()) {
- current->source()->getFrames(tsrgb, tsdepth);
- current->target()->getFrames(ttrgb, ttdepth);
-
- tsrgb.copyTo(srgb);
- ttrgb.copyTo(trgb);
- ttdepth.convertTo(tdepth, CV_8U, 255.0f / 10.0f);
- applyColorMap(tdepth, tdepth, cv::COLORMAP_JET);
- tsdepth.convertTo(sdepth, CV_8U, 255.0f / 10.0f);
- applyColorMap(sdepth, sdepth, cv::COLORMAP_JET);
-
- current->drawTarget(trgb);
- current->drawTarget(tdepth);
- current->drawSource(srgb);
- current->drawSource(sdepth);
-
- // Add most recent click position
- drawMarker(srgb, Point(lastSX, lastSY), Scalar(0,0,255), MARKER_TILTED_CROSS);
- drawMarker(trgb, Point(lastTX, lastTY), Scalar(0,0,255), MARKER_TILTED_CROSS);
- drawMarker(sdepth, Point(lastSX, lastSY), Scalar(0,0,255), MARKER_TILTED_CROSS);
- drawMarker(tdepth, Point(lastTX, lastTY), Scalar(0,0,255), MARKER_TILTED_CROSS);
-
- putText(srgb, string("Source: ") + current->source()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
- putText(trgb, string("Target: ") + current->target()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
- putText(srgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
- putText(trgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
- if (freeze) putText(srgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
- if (freeze) putText(trgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-
- if (!trgb.empty()) imshow("Target", (depthtoggle) ? tdepth : trgb);
- if (!srgb.empty()) imshow("Source", (depthtoggle) ? sdepth : srgb);
- }
-
- int key = cv::waitKey(20);
- if (key == 27) break;
- else if (key >= 48 && key <= 57) {
- curtarget = key-48;
- if (curtarget >= sources.size()) curtarget = 0;
- current = corrs[sources[curtarget]->getURI()];
- lastScore = 1000.0;
- } else if (key == 'd') {
- depthtoggle = !depthtoggle;
- } else if (key == 'c') {
- current->clear();
- lastScore = 1000.0;
- } else if (key == 'n') {
- point_n++;
- } else if (key == 'p') {
- LOG(INFO) << "Captured..";
- lastScore = 1000.0;
- Mat f1,f2;
- current->capture(f1,f2);
- }else if (key == 'a') {
- Eigen::Matrix4d t;
- if (current->add(lastTX,lastTY,lastSX,lastSY)) {
- lastTX = lastTY = lastSX = lastSY = 0;
- lastScore = current->estimateTransform(t);
- current->setTransform(t);
- }
- } else if (key == 'u') {
- current->removeLast();
- Eigen::Matrix4d t;
- lastScore = current->estimateTransform(t);
- current->setTransform(t);
- } else if (key == 'i') {
- current->icp();
- } else if (key == 's') {
- // Save
- map<string, Eigen::Matrix4d> transforms;
- //transforms[sources[targsrc]->getURI()] = Eigen::Matrix4f().setIdentity();
- //transforms[sources[cursrc]->getURI()] = c.transform();
-
- for (auto x : corrs) {
- if (x.second == nullptr) {
- transforms[x.first] = Eigen::Matrix4d().setIdentity();
- } else {
- transforms[x.first] = x.second->transform();
- }
- }
-
- saveTransformations(root->value("output", string("./test.json")), transforms);
- LOG(INFO) << "Saved!";
- } else if (key == 't') {
- current->source()->setPose(current->transform());
- } else if (key == 'g') {
- if (!insearch) {
- insearch = true;
-
- ftl::pool.push([&lastScore,&insearch,current](int id) {
- LOG(INFO) << "START";
- lastScore = 1000.0;
- do {
- Eigen::Matrix4d tr;
- float s = current->findBestSubset(tr, 20, 100); // (int)(current->screenPoints().size() * 0.4f)
- LOG(INFO) << "SCORE = " << s;
- if (s < lastScore) {
- lastScore = s;
- current->setTransform(tr);
- //current->source()->setPose(current->transform());
- }
- } while (ftl::running && insearch && lastScore > 0.000001);
- insearch = false;
- LOG(INFO) << "DONE: " << lastScore;
- });
- /*for (int i=0; i<feat.size(); i++) {
- auto [sx,sy,tx,ty] = feat[i];
- current->add(tx,ty,sx,sy);
- }
- lastsScore = current->estimateTransform();*/
- } else {
- insearch = false;
- }
- } else if (key == 'f') {
- if (!insearch) {
- Mat f1,f2;
- current->capture(f1,f2);
-
- vector<tuple<int,int,int,int>> feat;
- if (ftl::registration::featuresSIFT(f1, f2, feat, 10)) {
- for (int j=0; j<feat.size(); j++) {
- auto [sx,sy,tx,ty] = feat[j];
- current->add(tx, ty, sx, sy);
- }
- }
-
- LOG(INFO) << "Found " << current->screenPoints().size() << " features";
- } else {
- LOG(ERROR) << "Can't add features whilst searching...";
- }
- } else if (key == 32) freeze = !freeze;
- }
-
- // store transformations in map<string Matrix4f>
-
- // TODO: (later) extract features and correspondencies from complete cloud
- // and do ICP to find best possible transformation
-
- // saveTransformations(const string &path, map<string, Matrix4f> &data)
-}
diff --git a/applications/registration/src/manual.hpp b/applications/registration/src/manual.hpp
deleted file mode 100644
index 1b315cb038b22dc85adc1f0dc0b05ad4c772a920..0000000000000000000000000000000000000000
--- a/applications/registration/src/manual.hpp
+++ /dev/null
@@ -1,14 +0,0 @@
-#ifndef _FTL_REGISTRATION_MANUAL_HPP_
-#define _FTL_REGISTRATION_MANUAL_HPP_
-
-#include <ftl/configurable.hpp>
-
-namespace ftl {
-namespace registration {
-
-void manual(ftl::Configurable *root);
-
-}
-}
-
-#endif // _FTL_REGISTRATION_MANUAL_HPP_
diff --git a/applications/registration/src/sfm.cpp b/applications/registration/src/sfm.cpp
deleted file mode 100644
index 43711312dd31eedbce08aad47661d65254ee37d5..0000000000000000000000000000000000000000
--- a/applications/registration/src/sfm.cpp
+++ /dev/null
@@ -1,164 +0,0 @@
-#include "sfm.hpp"
-#include <opencv2/sfm.hpp>
-#include <opencv2/viz.hpp>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/core.hpp>
-#include <opencv2/features2d.hpp>
-#include <opencv2/xfeatures2d.hpp>
-
-using std::vector;
-using std::tuple;
-
-using namespace cv;
-using namespace cv::sfm;
-using namespace cv::xfeatures2d;
-using ftl::registration::CalibrationChessboard;
-
-CalibrationChessboard::CalibrationChessboard(ftl::Configurable *root) {
- pattern_size_ = Size(9, 6);
- image_size_ = Size(1280, 720);
- pattern_square_size_ = 36.0;//0.036;
- // CALIB_CB_NORMALIZE_IMAfE |Â CALIB_CB_EXHAUSTIVE |Â CALIB_CB_ACCURACY
- chessboard_flags_ = cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_ACCURACY | cv::CALIB_CB_EXHAUSTIVE;
-}
-
-void CalibrationChessboard::objectPoints(vector<Vec3f> &out) {
- out.reserve(pattern_size_.width * pattern_size_.height);
- for (int row = 0; row < pattern_size_.height; ++row) {
- for (int col = 0; col < pattern_size_.width; ++col) {
- out.push_back(Vec3f(col * pattern_square_size_, row * pattern_square_size_, 0));
- }}
-}
-
-bool CalibrationChessboard::findPoints(Mat &img, vector<Vec2f> &points) {
- return cv::findChessboardCornersSB(img, pattern_size_, points, chessboard_flags_);
-}
-
-void CalibrationChessboard::drawPoints(Mat &img, const vector<Vec2f> &points) {
- cv::drawChessboardCorners(img, pattern_size_, points, true);
-}
-
-static void get_tracks(const vector<vector<DMatch>> &matches, const vector<KeyPoint> &kp1,
- const vector<KeyPoint> &kp2, vector<vector<Vec2d>> &tracks, int k) {
-
- for (size_t i=0; i<matches.size(); i++) {
- // TODO Generalise to any number of keypoint frames
- if (k >= matches[i].size()) continue;
- Point2f point1 = kp1[matches[i][k].queryIdx].pt;
- Point2f point2 = kp2[matches[i][k].trainIdx].pt;
-
- vector<Vec2d> track;
- track.push_back(Vec2d(point1.x,point1.y));
- track.push_back(Vec2d(point2.x,point2.y));
- tracks.push_back(track);
- }
-}
-
-static void get_tracks(const vector<DMatch> &matches, const vector<KeyPoint> &kp1,
- const vector<KeyPoint> &kp2, vector<vector<Vec2d>> &tracks) {
-
- for (size_t i=0; i<matches.size(); i++) {
- // TODO Generalise to any number of keypoint frames
- //if (k >= matches[i].size()) continue;
- Point2f point1 = kp1[matches[i].queryIdx].pt;
- Point2f point2 = kp2[matches[i].trainIdx].pt;
-
- vector<Vec2d> track;
- track.push_back(Vec2d(point1.x,point1.y));
- track.push_back(Vec2d(point2.x,point2.y));
- tracks.push_back(track);
- }
-}
-
-static void convert_tracks(const vector<vector<Vec2d>> &tracks, vector<tuple<int,int,int,int>> &points2d) {
- //int n_frames = 2;
- int n_tracks = tracks.size();
-
- //for (int i = 0; i < n_frames; ++i)
- //{
- //Mat_<double> frame(2, n_tracks);
- for (int j = 0; j < n_tracks; ++j)
- {
- //frame(0,j) = tracks[j][i][0];
- //frame(1,j) = tracks[j][i][1];
- points2d.push_back(std::make_tuple(tracks[j][0][0], tracks[j][0][1], tracks[j][1][0], tracks[j][1][1]));
- }
- //}
-}
-
-bool ftl::registration::featuresChess(cv::Mat &frame1, cv::Mat &frame2, std::vector<std::tuple<int,int,int,int>> &points) {
- CalibrationChessboard cb(nullptr);
-
- vector<Vec2f> points1, points2;
- if (!cb.findPoints(frame1, points1)) {
- LOG(ERROR) << "Could not find chessboard (1)";
- return false;
- }
- if (!cb.findPoints(frame2, points2)) {
- LOG(ERROR) << "Could not find chessboard (2)";
- return false;
- }
-
- if (points1.size() != points2.size()) return false;
-
- for (size_t i=0; i<points1.size(); i++) {
- points.push_back(std::make_tuple(points1[i][0], points1[i][1], points2[i][0], points2[i][1]));
- }
- return true;
-}
-
-bool ftl::registration::featuresSIFT(cv::Mat &frame1, cv::Mat &frame2, std::vector<std::tuple<int,int,int,int>> &points, int K) {
- int minHessian = 400;
- Ptr<SIFT> detector = SIFT::create(10000,3,0.04,10);
- //detector->setHessianThreshold(minHessian);
-
- vector<vector<KeyPoint>> keypoints;
- vector<Mat> descriptors;
-
- {
- vector<KeyPoint> kps;
- Mat desc;
- detector->detectAndCompute(frame1, Mat(), kps, desc);
- keypoints.push_back(kps);
- descriptors.push_back(desc);
- }
-
- {
- vector<KeyPoint> kps;
- Mat desc;
- detector->detectAndCompute(frame2, Mat(), kps, desc);
- keypoints.push_back(kps);
- descriptors.push_back(desc);
- }
-
- // TODO This can be done on node machines...
-
- //-- Step 2: Matching descriptor vectors using FLANN matcher
- // Match between each sequential pair of images in the feed.
- FlannBasedMatcher matcher;
- std::vector<std::vector< DMatch >> matches;
- matcher.knnMatch( descriptors[0], descriptors[1], matches, K );
-
- const float ratio_thresh = 0.9f;
- std::vector<DMatch> good_matches;
- for (size_t i = 0; i < matches.size(); i++)
- {
- for (int k=0; k<K-1; k++) {
- if (matches[i][k].distance < ratio_thresh * matches[i][k+1].distance)
- {
- good_matches.push_back(matches[i][k]);
- } else break;
- }
- }
-
- // --- SFM ---
-
- //for (int i=0; i<K; i++) {
- vector<vector<Vec2d>> tracks;
- get_tracks(good_matches, keypoints[0], keypoints[1], tracks);
- convert_tracks(tracks, points);
- //}
-
- //cout << "Tracks: " << tracks.size() << endl;
- return true;
-}
diff --git a/applications/registration/src/sfm.hpp b/applications/registration/src/sfm.hpp
deleted file mode 100644
index 97e27da1031f039da8981f5171c73d747986b912..0000000000000000000000000000000000000000
--- a/applications/registration/src/sfm.hpp
+++ /dev/null
@@ -1,44 +0,0 @@
-#ifndef _FTL_REGISTRATION_SFM_HPP_
-#define _FTL_REGISTRATION_SFM_HPP_
-
-#include <ftl/configurable.hpp>
-#include <opencv2/opencv.hpp>
-#include <vector>
-#include <tuple>
-
-namespace ftl {
-namespace registration {
-
-/**
- * @brief Chessboard calibration pattern. Uses OpenCV's
- * findChessboardCornersSB function.
- * @todo Parameters hardcoded in constructor
- *
- * All parameters:
- * - pattern size (inner corners)
- * - square size, millimeters (TODO: meters)
- * - image size, pixels
- * - flags, see ChessboardCornersSB documentation
- */
-class CalibrationChessboard {
-public:
- CalibrationChessboard(ftl::Configurable *root);
- void objectPoints(std::vector<cv::Vec3f> &out);
- bool findPoints(cv::Mat &in, std::vector<cv::Vec2f> &out);
- void drawPoints(cv::Mat &img, const std::vector<cv::Vec2f> &points);
-
-private:
- int chessboard_flags_ = 0;
- float pattern_square_size_;
- cv::Size pattern_size_;
- cv::Size image_size_;
-};
-
-bool featuresSIFT(cv::Mat &frame1, cv::Mat &frame2, std::vector<std::tuple<int,int,int,int>> &points, int);
-
-bool featuresChess(cv::Mat &frame1, cv::Mat &frame2, std::vector<std::tuple<int,int,int,int>> &points);
-
-}
-}
-
-#endif // _FTL_REGISTRATION_SFM_HPP_
diff --git a/components/common/cpp/CMakeLists.txt b/components/common/cpp/CMakeLists.txt
index de0f7707c504447a16967625c2f01ab76e1218bb..e2f1e70fa304a62b1946972604ac86ffb6565d3d 100644
--- a/components/common/cpp/CMakeLists.txt
+++ b/components/common/cpp/CMakeLists.txt
@@ -4,7 +4,6 @@ set(COMMONSRC
src/configuration.cpp
src/configurable.cpp
src/loguru.cpp
- src/opencv_to_pcl.cpp
src/cuda_common.cpp
src/ctpl_stl.cpp
src/timer.cpp
@@ -17,11 +16,10 @@ add_library(ftlcommon ${COMMONSRC})
target_compile_options(ftlcommon PUBLIC $<$<COMPILE_LANGUAGE:CXX>:-fPIC>)
target_include_directories(ftlcommon PUBLIC
- ${PCL_INCLUDE_DIRS}
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
PRIVATE src)
-target_link_libraries(ftlcommon Threads::Threads ${OS_LIBS} ${OpenCV_LIBS} ${PCL_LIBRARIES} ${URIPARSER_LIBRARIES} ${CUDA_LIBRARIES})
+target_link_libraries(ftlcommon Threads::Threads ${OS_LIBS} ${OpenCV_LIBS} ${URIPARSER_LIBRARIES} ${CUDA_LIBRARIES})
add_subdirectory(test)
diff --git a/components/common/cpp/include/ftl/config.h.in b/components/common/cpp/include/ftl/config.h.in
index 540b332035a2ad5f671a51ed80f0de31d60f1ce7..52fa616b01676ad071723f2d5ea5b6ef0fdedbbe 100644
--- a/components/common/cpp/include/ftl/config.h.in
+++ b/components/common/cpp/include/ftl/config.h.in
@@ -17,7 +17,6 @@
#cmakedefine HAVE_CUDA
#cmakedefine HAVE_OPENCV
#cmakedefine HAVE_OPTFLOW
-#cmakedefine HAVE_PCL
#cmakedefine HAVE_RENDER
#cmakedefine HAVE_LIBSGM
#cmakedefine HAVE_REALSENSE
diff --git a/components/common/cpp/include/ftl/utility/opencv_to_pcl.hpp b/components/common/cpp/include/ftl/utility/opencv_to_pcl.hpp
deleted file mode 100644
index f273d5e23e460bc70b4fb35ecfd16ce97308ffef..0000000000000000000000000000000000000000
--- a/components/common/cpp/include/ftl/utility/opencv_to_pcl.hpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef _FTL_COMMON_OPENCV_TO_PCL_HPP_
-#define _FTL_COMMON_OPENCV_TO_PCL_HPP_
-
-#include <ftl/config.h>
-#include <opencv2/opencv.hpp>
-
-#if defined HAVE_PCL
-#include <pcl/common/common_headers.h>
-
-namespace ftl {
-namespace utility {
-
-/**
- * Convert an OpenCV point cloud matrix and RGB image to a PCL XYZRGB point cloud.
- */
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr matToPointXYZ(const cv::Mat &cvcloud, const cv::Mat &rgbimage);
-
-};
-};
-#endif // HAVE_PCL
-
-#endif // _FTL_COMMON_OPENCV_TO_PCL_HPP_
diff --git a/components/common/cpp/src/opencv_to_pcl.cpp b/components/common/cpp/src/opencv_to_pcl.cpp
deleted file mode 100644
index c56196b31b92ebf51bb6df54cd6029471dd9d5d7..0000000000000000000000000000000000000000
--- a/components/common/cpp/src/opencv_to_pcl.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-#include <ftl/utility/opencv_to_pcl.hpp>
-
-#if defined HAVE_PCL
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr ftl::utility::matToPointXYZ(const cv::Mat &cvcloud, const cv::Mat &rgbimage) {
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
- point_cloud_ptr->width = cvcloud.cols * cvcloud.rows;
- point_cloud_ptr->height = 1;
-
- for(int i=0;i<cvcloud.rows;i++) {
- for(int j=0;j<cvcloud.cols;j++) {
- //std::cout<<i<<endl;
-
- pcl::PointXYZRGB point;
- cv::Vec3f cvpoint = cvcloud.at<cv::Vec3f>(i,j);
- point.x = cvpoint[0];
- point.y = cvpoint[1];
- point.z = cvpoint[2];
-
- if (point.x == INFINITY || point.y == INFINITY || point.z == INFINITY || point.z < 600.0f) {
- point.x = 0; point.y = 0; point.z = 0;
- }
-
- cv::Point3_<uchar> prgb = rgbimage.at<cv::Point3_<uchar>>(i, j);
-
- // when color needs to be added:
- uint32_t rgb = (static_cast<uint32_t>(prgb.z) << 16 | static_cast<uint32_t>(prgb.y) << 8 | static_cast<uint32_t>(prgb.x));
- // cppcheck-suppress invalidPointerCast
- point.rgb = *reinterpret_cast<float*>(&rgb);
-
- point_cloud_ptr -> points.push_back(point);
- }
- }
-
- return point_cloud_ptr;
-}
-#endif // HAVE_PCL
\ No newline at end of file
diff --git a/components/operators/CMakeLists.txt b/components/operators/CMakeLists.txt
index 2a55cce0d60e763867c631a3e8b7099dbf3995f4..3bff44a4cb44ecf14b71a0f1e3b12af74ddd2ca3 100644
--- a/components/operators/CMakeLists.txt
+++ b/components/operators/CMakeLists.txt
@@ -39,11 +39,7 @@ endif()
add_library(ftloperators ${OPERSRC})
-# These cause errors in CI build and are being removed from PCL in newer versions
-# target_compile_options(ftlrender PUBLIC ${PCL_DEFINITIONS})
-
target_include_directories(ftloperators PUBLIC
- ${PCL_INCLUDE_DIRS}
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
PRIVATE src)
diff --git a/components/operators/src/disparity/optflow_smoothing.cpp b/components/operators/src/disparity/optflow_smoothing.cpp
index 5304da7dc7fc4d7c5e0e4531c90851c3f41a37a4..efd5d1cc18f41ddd9bff9ee219b4235d48968ef3 100644
--- a/components/operators/src/disparity/optflow_smoothing.cpp
+++ b/components/operators/src/disparity/optflow_smoothing.cpp
@@ -1,7 +1,6 @@
#include <loguru.hpp>
#include "ftl/operators/disparity.hpp"
-#include "ftl/offilter.hpp"
#include "disparity/cuda.hpp"
#ifdef HAVE_OPTFLOW
diff --git a/components/renderers/cpp/CMakeLists.txt b/components/renderers/cpp/CMakeLists.txt
index 14bacda712522b930e632f7a4c4e5e4790091037..112650feee5853b71c8c3cd7f1e265c3e2c0e564 100644
--- a/components/renderers/cpp/CMakeLists.txt
+++ b/components/renderers/cpp/CMakeLists.txt
@@ -10,11 +10,7 @@ add_library(ftlrender
src/tri_render.cpp
)
-# These cause errors in CI build and are being removed from PCL in newer versions
-# target_compile_options(ftlrender PUBLIC ${PCL_DEFINITIONS})
-
target_include_directories(ftlrender PUBLIC
- ${PCL_INCLUDE_DIRS}
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
PRIVATE src)
diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt
index 6027ab17a7d64110a72dd51e6cfde098c0b0abc7..b22623bcef38c04fd5b2d7535728bee1616c5fb6 100644
--- a/components/rgbd-sources/CMakeLists.txt
+++ b/components/rgbd-sources/CMakeLists.txt
@@ -5,15 +5,10 @@ set(RGBDSRC
src/frame.cpp
src/frameset.cpp
src/sources/stereovideo/stereovideo.cpp
-# src/sources/middlebury/middlebury_source.cpp
src/sources/net/net.cpp
src/streamer.cpp
src/colour.cpp
src/group.cpp
-# src/algorithms/rtcensus.cpp
-# src/algorithms/rtcensus_sgm.cpp
-# src/algorithms/opencv_sgbm.cpp
-# src/algorithms/opencv_bm.cpp
src/cb_segmentation.cpp
src/abr.cpp
src/sources/virtual/virtual.cpp
@@ -32,24 +27,6 @@ if (LibArchive_FOUND)
)
endif (LibArchive_FOUND)
-#if (LIBSGM_FOUND)
-# list(APPEND RGBDSRC "src/algorithms/fixstars_sgm.cpp")
-#endif (LIBSGM_FOUND)
-
-if (CUDA_FOUND)
- list(APPEND RGBDSRC
-# "src/algorithms/opencv_cuda_bm.cpp"
-# "src/algorithms/opencv_cuda_bp.cpp"
-# "src/algorithms/rtcensus.cu"
-# "src/algorithms/rtcensus_sgm.cu"
-# "src/algorithms/consistency.cu"
-# "src/algorithms/sparse_census.cu"
-# "src/algorithms/tex_filter.cu"
-# "src/algorithms/nick1.cu"
-# "src/algorithms/nick.cpp")
-)
-endif (CUDA_FOUND)
-
add_library(ftlrgbd ${RGBDSRC})
# target_compile_options(ftlrgbd PUBLIC $<$<COMPILE_LANGUAGE:CXX>:-fPIC>)
@@ -64,7 +41,6 @@ if (CUDA_FOUND)
set_property(TARGET ftlrgbd PROPERTY CUDA_SEPARABLE_COMPILATION OFF)
endif()
-#target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
target_link_libraries(ftlrgbd ftlcommon ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} Eigen3::Eigen ${REALSENSE_LIBRARY} ftlnet ${LibArchive_LIBRARIES} ftlcodecs ftloperators)
add_subdirectory(test)
diff --git a/components/rgbd-sources/include/ftl/offilter.hpp b/components/rgbd-sources/include/ftl/offilter.hpp
deleted file mode 100644
index 0e273d80d0382dd1a805bb50f658e8180ded567e..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/include/ftl/offilter.hpp
+++ /dev/null
@@ -1,32 +0,0 @@
-#pragma once
-
-#include <ftl/config.h>
-#include <ftl/rgbd/frame.hpp>
-
-#ifdef HAVE_OPTFLOW
-#include <ftl/cuda_util.hpp>
-#include <opencv2/core.hpp>
-#include <opencv2/core/cuda.hpp>
-#include <opencv2/cudaoptflow.hpp>
-
-namespace ftl {
-namespace rgbd {
-
-class OFDisparityFilter {
-public:
- OFDisparityFilter() : n_max_(0), threshold_(0.0) {}
- OFDisparityFilter(cv::Size size, int n_frames, float threshold);
- void filter(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream);
- void filter(cv::cuda::GpuMat &disp, cv::cuda::GpuMat &optflow, cv::cuda::Stream &stream);
-
-private:
- int n_max_;
- float threshold_;
-
- cv::cuda::GpuMat disp_old_;
-};
-
-}
-}
-
-#endif // HAVE_OPTFLOW
diff --git a/components/rgbd-sources/src/algorithms/consistency.cu b/components/rgbd-sources/src/algorithms/consistency.cu
deleted file mode 100644
index ca9a3df22af36ac3623ac9b9e316275e25946e7f..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/consistency.cu
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- * Algorithms for checking the consistency of two disparity maps.
- */
-
-#include <ftl/cuda_common.hpp>
-
-#define CONSISTENCY_THRESHOLD 1.0f
-
-/*
- * Check for consistency between the LR and RL disparity maps, only selecting
- * those that are similar. Otherwise it sets the disparity to NAN.
- */
-__global__ void consistency_kernel(cudaTextureObject_t d_sub_l,
- cudaTextureObject_t d_sub_r, float *disp, int w, int h, int pitch) {
-
- // TODO This doesn't work at either edge (+-max_disparities)
- for (STRIDE_Y(v,h)) {
- for (STRIDE_X(u,w)) {
- float a = (int)tex2D<float>(d_sub_l, u, v);
- if (u-a < 0) {
- disp[v*pitch+u] = NAN; // TODO Check
- continue;
- }
-
- auto b = tex2D<float>(d_sub_r, u-a, v);
-
- if (abs(a-b) <= CONSISTENCY_THRESHOLD) disp[v*pitch+u] = abs((a+b)/2);
- else disp[v*pitch+u] = NAN;
- }
- }
-
-}
-
-namespace ftl {
-namespace cuda {
- void consistency(const TextureObject<float> &dl, const TextureObject<float> &dr,
- TextureObject<float> &disp) {
- dim3 grid(1,1,1);
- dim3 threads(128, 1, 1);
- grid.x = cv::cuda::device::divUp(disp.width(), 128);
- grid.y = cv::cuda::device::divUp(disp.height(), 11);
- consistency_kernel<<<grid, threads>>>(
- dl.cudaTexture(),
- dr.cudaTexture(),
- disp.devicePtr(),
- disp.width(),
- disp.height(),
- disp.pitch() / sizeof(float));
- cudaSafeCall( cudaGetLastError() );
- }
-}
-}
-
diff --git a/components/rgbd-sources/src/algorithms/elas.cpp b/components/rgbd-sources/src/algorithms/elas.cpp
deleted file mode 100644
index 07d0de9780092908bb87bc2b643d037fc5fca169..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/elas.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-/* Copyright 2019 Nicolas Pope */
-
-#include <ftl/algorithms/elas.hpp>
-#include <loguru.hpp>
-
-#include <chrono>
-
-using ftl::algorithms::ELAS;
-using cv::Mat;
-
-static ftl::Disparity::Register elass("elas", ELAS::create);
-
-ELAS::ELAS(nlohmann::json &config) : Disparity(config) {
- // TODO(nick) See if these can improve the situation
- param_.postprocess_only_left = true;
- param_.disp_min = 0;
- param_.disp_max = config["maximum"];
- param_.add_corners = 0;
- param_.gamma = 3;
- param_.sradius = 2;
- param_.match_texture = 1;
- param_.ipol_gap_width = 3;
- param_.support_threshold = 0.85;
- elas_ = new Elas(param_);
-}
-
-void ELAS::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
- //Mat left_disp;
- //Mat right_disp;
-
- Mat lbw, rbw;
- cv::cvtColor(l, lbw, cv::COLOR_BGR2GRAY);
- cv::cvtColor(r, rbw, cv::COLOR_BGR2GRAY);
-
- disp = Mat(cv::Size(l.cols, l.rows), CV_32F);
- Mat dispr(cv::Size(l.cols, l.rows), CV_32F);
-
- const int32_t dims[3] = {l.cols,l.rows,static_cast<int32_t>(lbw.step)};
-
- if (disp.step/sizeof(float) != lbw.step) LOG(WARNING) << "Incorrect disparity step";
-
- auto start = std::chrono::high_resolution_clock::now();
- elas_->process(lbw.data, rbw.data, (float*)disp.data, (float*)dispr.data, dims);
- std::chrono::duration<double> elapsed =
- std::chrono::high_resolution_clock::now() - start;
- LOG(INFO) << "Elas in " << elapsed.count() << "s";
-
- //disp.convertTo(disp, CV_32F, 1.0f);
-}
-
diff --git a/components/rgbd-sources/src/algorithms/elas.hpp b/components/rgbd-sources/src/algorithms/elas.hpp
deleted file mode 100644
index 29b76cd9ef219013d8a47042a9e2a8529da08ba3..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/elas.hpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#ifndef _FTL_ALGORITHMS_ELAS_HPP_
-#define _FTL_ALGORITHMS_ELAS_HPP_
-
-#include <opencv2/core.hpp>
-#include <opencv2/opencv.hpp>
-#include <elas.h>
-#include <ftl/disparity.hpp>
-#include <ftl/configuration.hpp>
-
-namespace ftl {
-namespace algorithms {
-
-/**
- * LibELAS - Efficient Large-scale Stereo Matching
- * @see http://www.cvlibs.net/software/libelas/
- */
-class ELAS : public ftl::Disparity {
- public:
- explicit ELAS(nlohmann::json &config);
-
- void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
-
- /* Factory creator */
- static inline Disparity *create(ftl::Configurable *p, const std::string &name) {
- return ftl::create<ELAS>(p, name);
- }
-
- private:
- Elas::parameters param_;
- Elas *elas_;
-};
-};
-};
-
-#endif // _FTL_ALGORITHMS_ELAS_HPP_
-
diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
deleted file mode 100644
index bbcad8c63f923bd92cbda56cb009e88d2ec50eb0..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
+++ /dev/null
@@ -1,239 +0,0 @@
-/* Copyright 2019 Nicolas Pope */
-
-#include "fixstars_sgm.hpp"
-#include <loguru.hpp>
-#include <opencv2/cudastereo.hpp>
-
-using ftl::algorithms::FixstarsSGM;
-using cv::Mat;
-using cv::cuda::GpuMat;
-using ftl::codecs::Channel;
-using ftl::rgbd::Format;
-
-//static ftl::Disparity::Register fixstarssgm("libsgm", FixstarsSGM::create);
-
-FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
- ssgm_ = nullptr;
- const int width = size_.width;
- const int height = size_.height;
-
- CHECK((width >= 480) && (height >= 360));
-
- uniqueness_ = value("uniqueness", 0.95f);
- P1_ = value("P1", 10);
- P2_ = value("P2", 120);
-
- CHECK((uniqueness_ >= 0.0) && (uniqueness_ <= 1.0));
- CHECK(P1_ >= 0);
- CHECK(P2_ > P1_);
-
- updateBilateralFilter();
-
- on("use_filter", [this](const ftl::config::Event&) {
- updateBilateralFilter();
- });
-
- on("filter_radius", [this](const ftl::config::Event&) {
- updateBilateralFilter();
- });
-
- on("filter_iter", [this](const ftl::config::Event&) {
- updateBilateralFilter();
- });
-
-#ifdef HAVE_OPTFLOW
-/*
- updateOFDisparityFilter();
-
- on("use_off", [this](const ftl::config::Event&) {
- updateOFDisparityFilter();
- });
-
- on("use_off", [this](const ftl::config::Event&) {
- updateOFDisparityFilter();
- });
-*/
-#endif
-
- init(size_);
-
- on("uniqueness", [this](const ftl::config::Event&) {
- float uniqueness = value("uniqueness", uniqueness_);
- if ((uniqueness >= 0.0) && (uniqueness <= 1.0)) {
- uniqueness_ = uniqueness;
- updateParameters();
- }
- else {
- LOG(WARNING) << "invalid uniquness: " << uniqueness;
- }
- });
-
- on("P1", [this](const ftl::config::Event&) {
- int P1 = value("P1", P1_);
- if (P1 <= P2_) {
- P1_ = P1;
- updateParameters();
- }
- else {
- LOG(WARNING) << "invalid P1: " << P1 << ", (P1 <= P2), P2 is " << P2_;
- }
- });
-
- on("P2", [this](const ftl::config::Event&) {
- int P2 = value("P2", P2_);
- if (P2 >= P1_) {
- P2_ = P2;
- updateParameters();
- }
- else {
- LOG(WARNING) << "invalid P2: " << P2 << ", (P1 <= P2), P1 is " << P1_;
- }
- });
-}
-
-void FixstarsSGM::init(const cv::Size size) {
- if (ssgm_) { delete ssgm_; }
- lbw_ = GpuMat(size, CV_8UC1);
- rbw_ = GpuMat(size, CV_8UC1);
- dispt_ = GpuMat(size, CV_16SC1);
-
- ssgm_ = new sgm::StereoSGM(size.width, size.height, max_disp_, 8, 16,
- lbw_.step, dispt_.step / sizeof(short),
- sgm::EXECUTE_INOUT_CUDA2CUDA,
- sgm::StereoSGM::Parameters(P1_, P2_, uniqueness_, true)
- );
-}
-
-bool FixstarsSGM::updateParameters() {
- if (ssgm_ == nullptr) { return false; }
- return this->ssgm_->updateParameters(
- sgm::StereoSGM::Parameters(P1_, P2_, uniqueness_, true));
-}
-
-bool FixstarsSGM::updateBilateralFilter() {
- bool enable = value("use_filter", false);
- int radius = value("filter_radius", 25);
- int iter = value("filter_iter", 1);
-
- if (enable) {
- if (radius <= 0) {
- LOG(WARNING) << "filter_radius must be greater than 0";
- enable = false;
- }
- if (iter <= 0) {
- LOG(WARNING) << "filter_iter must be greater than 0";
- enable = false;
- }
- }
-
- if (enable) {
- filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, radius, iter);
- use_filter_ = true;
- }
- else {
- use_filter_ = false;
- }
-
- return use_filter_;
-}
-
-#ifdef HAVE_OPTFLOW
-/*
-bool FixstarsSGM::updateOFDisparityFilter() {
- bool enable = value("use_off", false);
- int off_size = value("off_size", 9);
- double off_threshold = value("off_threshold", 0.9);
-
- if (enable) {
- if (off_size <= 0) {
- LOG(WARNING) << "bad off_size: " << off_size;
- enable = false;
- }
-
- if (off_threshold <= 0.0) {
- LOG(WARNING) << "bad off_threshold: " << off_threshold;
- enable = false;
- }
- }
-
- if (enable) {
- LOG(INFO) << "Optical flow filter, size: " << off_size << ", threshold: " << off_threshold;
- off_ = ftl::rgbd::OFDisparityFilter(size_, off_size, off_threshold);
- use_off_ = true;
- }
- else {
- use_off_ = false;
- }
-
- return use_off_;
-}*/
-#endif
-
-void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream)
-{
- const auto &l = frame.get<GpuMat>(Channel::Left);
- const auto &r = frame.get<GpuMat>(Channel::Right);
- auto &disp = frame.create<GpuMat>(Channel::Disparity, Format<float>(l.size()));
-
- GpuMat l_scaled;
- if (l.size() != size_)
- {
- GpuMat _r;
- scaleInput(l, r, l_scaled, _r, stream);
- cv::cuda::cvtColor(l_scaled, lbw_, cv::COLOR_BGR2GRAY, 0, stream);
- cv::cuda::cvtColor(_r, rbw_, cv::COLOR_BGR2GRAY, 0, stream);
- }
- else
- {
- cv::cuda::cvtColor(l, lbw_, cv::COLOR_BGR2GRAY, 0, stream);
- cv::cuda::cvtColor(r, rbw_, cv::COLOR_BGR2GRAY, 0, stream);
- }
-
- stream.waitForCompletion();
- ssgm_->execute(lbw_.data, rbw_.data, dispt_.data);
- // GpuMat left_pixels(dispt_, cv::Rect(0, 0, max_disp_, dispt_.rows));
- // left_pixels.setTo(0);
- cv::cuda::threshold(dispt_, dispt_, 4096.0f, 0.0f, cv::THRESH_TOZERO_INV, stream);
-
- GpuMat dispt_scaled;
- if (l.size() != size_)
- {
- scaleDisparity(l.size(), dispt_, dispt_scaled, stream);
- }
- else
- {
- dispt_scaled = dispt_;
- }
-
- // TODO: filter could be applied after upscaling (to the upscaled disparity image)
- if (use_filter_)
- {
- filter_->apply(
- dispt_,
- l,
- dispt_,
- stream
- );
- }
-
- dispt_scaled.convertTo(disp, CV_32F, 1.0f / 16.0f, stream);
-
-#ifdef HAVE_OPTFLOW
-/*
- // TODO: Optical flow filter expects CV_32F
- if (use_off_) {
- frame.upload(Channel::Flow, stream);
- stream.waitForCompletion();
- off_.filter(disp, frame.get<GpuMat>(Channel::Flow), stream);
- }
-*/
-#endif
-}
-
-void FixstarsSGM::setMask(Mat &mask) {
- return; // TODO(Nick) Not needed, but also code below does not work with new GPU pipeline
- CHECK(mask.type() == CV_8UC1) << "mask type must be CV_8U";
- if (!ssgm_) { init(size_); }
- mask_l_ = GpuMat(mask);
- ssgm_->setMask((uint8_t*)mask.data, mask.cols);
-}
\ No newline at end of file
diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
deleted file mode 100644
index 2de1c41ef377570915397ea2758d265fa875b179..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#ifndef _FTL_ALGORITHMS_FIXSTARS_SGM_HPP_
-#define _FTL_ALGORITHMS_FIXSTARS_SGM_HPP_
-
-#include <ftl/cuda_util.hpp>
-#include <opencv2/core.hpp>
-#include <opencv2/opencv.hpp>
-#include <opencv2/cudastereo.hpp>
-
-#include "../disparity.hpp"
-#include <ftl/configuration.hpp>
-#include <ftl/config.h>
-
-#include <libsgm.h>
-#include "ftl/offilter.hpp"
-
-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::rgbd::detail::Disparity {
- public:
- explicit FixstarsSGM(nlohmann::json &config);
-
- void compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream) override;
- void setMask(cv::Mat &mask) override;
-
- /* Factory creator */
- static inline Disparity *create(ftl::Configurable *p, const std::string &name) {
- return ftl::create<FixstarsSGM>(p, name);
- }
-
- private:
- void init(const cv::Size size);
- bool updateParameters();
- bool updateBilateralFilter();
- #ifdef HAVE_OPTFLOW
- bool updateOFDisparityFilter();
- #endif
-
- float uniqueness_;
- int P1_;
- int P2_;
- bool use_filter_;
- bool use_off_;
- cv::Ptr<cv::cuda::DisparityBilateralFilter> filter_;
- sgm::StereoSGM *ssgm_;
- cv::cuda::GpuMat lbw_;
- cv::cuda::GpuMat rbw_;
- cv::cuda::GpuMat dispt_;
-
- #ifdef HAVE_OPTFLOW
- //ftl::rgbd::OFDisparityFilter off_;
- #endif
- };
- };
-};
-
-#endif // _FTL_ALGORITHMS_FIXSTARS_SGM_HPP_
-
diff --git a/components/rgbd-sources/src/algorithms/nick.cpp b/components/rgbd-sources/src/algorithms/nick.cpp
deleted file mode 100644
index 55ccd1db439e61bc13114239461da44d53ffc7f6..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/nick.cpp
+++ /dev/null
@@ -1,41 +0,0 @@
-#include "nick.hpp"
-#include <vector_types.h>
-
-using ftl::algorithms::NickCuda;
-using namespace cv;
-using namespace cv::cuda;
-
-static ftl::Disparity::Register nickcuda("nick", NickCuda::create);
-
-NickCuda::NickCuda(nlohmann::json &config) : Disparity(config) {
-
-}
-
-namespace ftl { namespace gpu {
-void nick1_call(const PtrStepSz<uchar4> &l, const PtrStepSz<uchar4> &r, const PtrStepSz<float> &disp, size_t num_disp);
-}}
-
-void NickCuda::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
- if (disp_.empty()) disp_ = cuda::GpuMat(l.size(), CV_32FC1);
- if (left_.empty()) left_ = cuda::GpuMat(l.size(), CV_8UC4);
- if (right_.empty()) right_ = cuda::GpuMat(l.size(), CV_8UC4);
-
- Mat lhsv, rhsv;
- cv::cvtColor(l, lhsv, COLOR_BGR2HSV);
- cv::cvtColor(r, rhsv, COLOR_BGR2HSV);
- int from_to[] = {0,0,1,1,2,2,-1,3};
- Mat hsval(lhsv.size(), CV_8UC4);
- Mat hsvar(rhsv.size(), CV_8UC4);
- mixChannels(&lhsv, 1, &hsval, 1, from_to, 4);
- mixChannels(&rhsv, 1, &hsvar, 1, from_to, 4);
-
- left_.upload(hsval);
- right_.upload(hsvar);
-
- ftl::gpu::nick1_call(left_, right_, disp_, 200);
-
- disp_.download(disp);
-}
-
-
-
diff --git a/components/rgbd-sources/src/algorithms/nick.hpp b/components/rgbd-sources/src/algorithms/nick.hpp
deleted file mode 100644
index 1b5c71ccf07a2757e321845cbd7d423e8e88ff75..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/nick.hpp
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef _FTL_ALGORITHMS_NICK_HPP_
-#define _FTL_ALGORITHMS_NICK_HPP_
-
-#include <opencv2/core.hpp>
-#include <opencv2/opencv.hpp>
-#include <opencv2/cudastereo.hpp>
-#include "../disparity.hpp"
-#include <ftl/configuration.hpp>
-
-namespace ftl {
-namespace algorithms {
-class NickCuda : public ftl::Disparity {
- public:
- NickCuda(nlohmann::json &config);
-
- void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
-
- static inline Disparity *create(ftl::Configurable *p, const std::string &name) {
- return ftl::create<NickCuda>(p, name);
- }
-
- private:
- cv::cuda::GpuMat disp_;
- //cv::cuda::GpuMat filtered_;
- cv::cuda::GpuMat left_;
- cv::cuda::GpuMat right_;
-};
-};
-};
-
-#endif // _FTL_ALGORITHMS_NICK_HPP_
-
diff --git a/components/rgbd-sources/src/algorithms/nick1.cu b/components/rgbd-sources/src/algorithms/nick1.cu
deleted file mode 100644
index ab6c0d4bbb60b51c16b95a4bd42b3e67ddedb46d..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/nick1.cu
+++ /dev/null
@@ -1,380 +0,0 @@
-/*
- * Author: Nicolas Pope
- * Based initially on rtcensus
- *
- */
-
-#include <ftl/cuda_common.hpp>
-#include "../cuda_algorithms.hpp"
-
-using namespace cv::cuda;
-using namespace cv;
-
-
-#define BLOCK_W 60
-#define RADIUS 7
-#define RADIUS2 2
-#define ROWSperTHREAD 1
-
-template <typename T>
-__host__ __device__
-inline T lerp(T v0, T v1, T t) {
- return fma(t, v1, fma(-t, v0, v0));
-}
-
-#define FILTER_WINDOW 21
-#define FILTER_WINDOW_R 10
-#define EDGE_SENSITIVITY 10.0f
-
-__device__ float calculate_edge_disp(cudaTextureObject_t t, cudaTextureObject_t d, cudaTextureObject_t pT, cudaTextureObject_t pD, uchar4 pixel, int u, int v) {
- float est = 0.0;
- int nn = 0;
- //float pest = 0.0;
- //int pnn = 0;
-
- //cudaTextureObject_t nTex = (pT) ? pT : t;
- //cudaTextureObject_t nDisp = (pD) ? pD : d;
-
- for (int m=-FILTER_WINDOW_R; m<=FILTER_WINDOW_R; m++) {
- for (int n=-FILTER_WINDOW_R; n<=FILTER_WINDOW_R; n++) {
- uchar4 neigh = tex2D<uchar4>(t, u+n, v+m);
- float ndisp = tex2D<float>(d,u+n,v+m);
-
- //uchar4 pneigh = tex2D<uchar4>(nTex, u+n, v+m);
- //float pndisp = tex2D<float>(nDisp,u+n,v+m);
-
- //if (isnan(tex2D<float>(nDisp,u+n,v+m))) continue;
- //if (m == 0 && n == 0) continue;
-
- if (!isnan(ndisp) && (abs(neigh.z-pixel.z) <= EDGE_SENSITIVITY)) { // && (isnan(disp) || abs(ndisp-disp) < FILTER_DISP_THRESH)) {
- est += ndisp;
- nn++;
- }
-
- //if (!isnan(pndisp) && (abs(pneigh.z-pixel.z) <= EDGE_SENSITIVITY)) { // && (isnan(disp) || abs(ndisp-disp) < FILTER_DISP_THRESH)) {
- // pest += pndisp;
- // pnn++;
- //}
- }
- }
-
- est = (nn > 0) ? est/nn : NAN;
- //pest = (pnn > 0) ? pest/pnn : NAN;
-
- return est;
-}
-
-__device__ float colour_error(uchar4 v1, uchar4 v2) {
- float dx = 0.05*abs(v1.x-v2.x);
- float dy = 0.1*abs(v1.y-v2.y);
- float dz = 0.85*abs(v1.z-v2.z);
- return dx + dz + dy;
-}
-
-// TODO Use HUE also and perhaps increase window?
-// Or use more complex notion of texture?
-
-/* Just crossed and currently on edge */
-__device__ bool is_edge_left(uchar4 *line, int x, int n) {
- if (x < 1 || x >= n-1) return false;
- return (colour_error(line[x-1],line[x]) > EDGE_SENSITIVITY && colour_error(line[x],line[x+1]) <= EDGE_SENSITIVITY);
-}
-
-/* Just crossed but not on edge now */
-__device__ bool is_edge_right(uchar4 *line, int x, int n) {
- if (x < 1 || x >= n-1) return false;
- return (colour_error(line[x-1],line[x]) <= EDGE_SENSITIVITY && colour_error(line[x],line[x+1]) > EDGE_SENSITIVITY);
-}
-
-/*__global__ void filter_kernel(cudaTextureObject_t t, cudaTextureObject_t d,
- cudaTextureObject_t prevD,
- cudaTextureObject_t prevT, PtrStepSz<float> f, int num_disp) {
-
-
- extern __shared__ uchar4 line[]; // One entire line of hsv image
-
- for (STRIDE_Y(v,f.rows)) {
- for (STRIDE_X(u,f.cols)) {
- line[u] = tex2D<uchar4>(t, u, v);
- }
- __syncthreads();
-
- for (STRIDE_X(u,f.cols)) {
- if (is_edge_right(line, u, f.cols)) {
- float edge_disp = calculate_edge_disp(t,d,prevT,prevD,line[u],u+2,v); // tex2D<float>(d, u, v);
- f(v,u) = edge_disp;
- continue;
-
- float est = 0.0f;
- int nn = 0;
-
- if (!isnan(edge_disp)) {
- est += edge_disp;
- nn++;
- }
- //f(v,u) = edge_disp;
-
- // TODO, find other edge first to get disparity
- // Use middle disparities to:
- // estimate curve or linear (choose equation)
- // or ignore as noise if impossible
-
- // TODO For edge disparity, use a window to:
- // a) find a missing disparity
- // b) make sure disparity has some consensus (above or below mostly)
-
- // TODO Use past values?
- // Another way to fill blanks and gain concensus
-
- // TODO Maintain a disparity stack to pop back to background?
- // Issue of background disparity not being detected.
- // Only if hsv also matches previous background
-
- // TODO Edge prediction (in vertical direction at least) could
- // help fill both edge and disparity gaps. Propagate disparity
- // along edges
-
- float last_disp = edge_disp;
-
- int i;
- for (i=1; u+i<f.cols; i++) {
- if (is_edge_right(line, u+i, f.cols)) {
- //float end_disp = calculate_edge_disp(t,d,prevT,prevD,line[u+i-1],u+i-3,v);
- //if (!isnan(end_disp)) last_disp = end_disp;
- break;
- }
-
- float di = tex2D<float>(d,u+i,v);
- if (!isnan(di)) {
- est += di;
- nn++;
- }
- //f(v,u+i) = edge_disp;
- }
-
- est = (nn > 0) ? est / nn : NAN;
- //for (int j=1; j<i; j++) {
- // f(v,u+j) = est; //lerp(edge_disp, last_disp, (float)j / (float)(i-1));
- //}
- } else f(v,u) = NAN;
- }
- }
-}*/
-
-
-__device__ float neighbour_factor(float a, cudaTextureObject_t p, int u, int v) {
- float f = 1.0f;
-
- for (int m=-1; m<=1; m++) {
- for (int n=-1; n<=1; n++) {
- float2 neighbour = tex2D<float2>(p, u+n, v+m);
- if (neighbour.x > 8.0f && abs(neighbour.y-a) < 1.0f) f += neighbour.x / 10.0f;
- }
- }
-
- return f;
-}
-
-/* Use Prewitt operator */
-__global__ void edge_invar1_kernel(cudaTextureObject_t t, cudaTextureObject_t p, ftl::cuda::TextureObject<float2> o) {
- for (STRIDE_Y(v,o.height())) {
- for (STRIDE_X(u,o.width())) {
- float gx = ((tex2D<uchar4>(t, u-1, v-1).z - tex2D<uchar4>(t, u+1, v-1).z) +
- (tex2D<uchar4>(t, u-1, v).z - tex2D<uchar4>(t, u+1, v).z) +
- (tex2D<uchar4>(t, u-1, v+1).z - tex2D<uchar4>(t, u+1, v+1).z)) / 3;
- float gy = ((tex2D<uchar4>(t, u-1, v-1).z - tex2D<uchar4>(t, u-1, v+1).z) +
- (tex2D<uchar4>(t, u, v-1).z - tex2D<uchar4>(t, u, v+1).z) +
- (tex2D<uchar4>(t, u+1, v-1).z - tex2D<uchar4>(t, u+1, v+1).z)) / 3;
-
- float g = sqrt(gx*gx+gy*gy);
- float a = atan2(gy,gx);
-
- if (g > 1.0f) {
- float2 n = tex2D<float2>(p, u, v);
- float avg = (n.x > g && abs(n.y-a) < 0.1f) ? n.x : g;
- o(u,v) = make_float2(avg,abs(a));
- } else {
- o(u,v) = make_float2(NAN,NAN);
- }
- }
- }
-}
-
-__device__ void edge_follow(float &sum, int &count, cudaTextureObject_t i1, int u, int v, int sign) {
- int u2 = u;
- int v2 = v;
- int n = 0;
- float sumchange = 0.0f;
- float2 pixel_i1 = tex2D<float2>(i1,u,v);
-
- for (int j=0; j<5; j++) {
- // Vertical edge = 0, so to follow it don't move in x
- int dx = ((pixel_i1.y >= 0.785 && pixel_i1.y <= 2.356) ) ? 0 : 1;
- int dy = (dx == 1) ? 0 : 1;
-
- // Check perpendicular to edge to find strongest gradient
- //if (tex2D<float2>(i1, u2+dy, v2+dx).x < pixel_i1.x && tex2D<float2>(i1, u2-dy, v2-dx).x < pixel_i1.x) {
- //o(u,v) = pixel_i1.y*81.0f;
- //} else {
- // break;
- //}
- //continue;
-
- float2 next_pix;
- next_pix.x = NAN;
- next_pix.y = NAN;
- float diff = 10000.0f;
- int nu, nv;
-
- for (int i=-5; i<=5; i++) {
- float2 pix = tex2D<float2>(i1,u2+dx*i+dy*sign, v2+dy*i+dx*sign);
- if (isnan(pix.x)) continue;
-
- float d = abs(pix.x-pixel_i1.x); //*abs(pix.y-pixel_i1.y);
- if (d < diff) {
- nu = u2+dx*i+dy*sign;
- nv = v2+dy*i+dx*sign;
- next_pix = pix;
- diff = d;
- }
- }
-
- if (!isnan(next_pix.x) && diff < 10.0f) {
- float change = abs(pixel_i1.y - next_pix.y);
-
- // Corner or edge change.
- //if (change > 0.785f) break;
- if (change > 2.0f) break;
-
- sumchange += (nu-u) / (nv-v);
-
- u2 = nu;
- v2 = nv;
- pixel_i1 = next_pix;
- n++;
- } else {
- //o(u,v) = NAN;
- break;
- }
- }
-
- //if (n == 0) sum = 0.0f;
- sum = sumchange;
- count = n;
-}
-
-__global__ void edge_invar2_kernel(cudaTextureObject_t i1, ftl::cuda::TextureObject<float> o) {
- for (STRIDE_Y(v,o.height())) {
- for (STRIDE_X(u,o.width())) {
- float2 pixel_i1 = tex2D<float2>(i1,u,v);
-
- if (isnan(pixel_i1.x) || pixel_i1.x < 10.0f) {
- o(u,v) = NAN;
- continue;
- }
-
- int dx = ((pixel_i1.y >= 0.785 && pixel_i1.y <= 2.356) ) ? 1 : 0;
- int dy = (dx == 1) ? 0 : 1;
-
- // Check perpendicular to edge to find strongest gradient
- if (tex2D<float2>(i1, u+dy, v+dx).x < pixel_i1.x && tex2D<float2>(i1, u-dy, v-dx).x < pixel_i1.x) {
- //o(u,v) = pixel_i1.y*81.0f;
- } else {
- o(u,v) = NAN;
- continue;
- }
-
- float sum_a, sum_b;
- int count_a, count_b;
- edge_follow(sum_a, count_a, i1, u, v, 1);
- edge_follow(sum_b, count_b, i1, u, v, -1);
-
-
- // Output curvature of edge
- if (count_a+count_b > 3) {
- float curvature = ((sum_a+sum_b) / (float)(count_a+count_b));
- o(u,v) = curvature * 150.0f + 50.0f;
- //o(u,v) = (count_a+count_b) * 3.0f;
- //o(u,v) = pixel_i1.y*81.0f;
- } else {
- o(u,v) = NAN;
- }
- //o(u,v) = (sumchange / (float)(j-1))*100.0f;
-
- // Search in two directions for next edge pixel
- // Calculate curvature by monitoring gradient angle change
- // Calculate length by stopping when change exceeds threshold
- }
- }
-}
-
-__global__ void disparity_kernel(cudaTextureObject_t l, cudaTextureObject_t r, ftl::cuda::TextureObject<float> o) {
- for (STRIDE_Y(v,o.height())) {
- for (STRIDE_X(u,o.width())) {
- float dl = tex2D<float>(l,u,v);
- float dr = tex2D<float>(r,u,v);
- if (isnan(dl)) o(u,v) = dr;
- else if (isnan(dr)) o(u,v) = dl;
- else o(u,v) = max(dl,dr);
-
- }
- }
-}
-
-ftl::cuda::TextureObject<float2> prevEdge1;
-ftl::cuda::TextureObject<float2> prevEdge2;
-ftl::cuda::TextureObject<float> prevDisp;
-ftl::cuda::TextureObject<uchar4> prevImage;
-
-namespace ftl {
-namespace gpu {
-
-void nick1_call(const PtrStepSz<uchar4> &l, const PtrStepSz<uchar4> &r, const PtrStepSz<float> &disp, size_t num_disp) {
- // Make all the required texture steps
- // TODO Could reduce re-allocations by caching these
- ftl::cuda::TextureObject<uchar4> texLeft(l);
- ftl::cuda::TextureObject<uchar4> texRight(r);
- ftl::cuda::TextureObject<float2> invl1(l.cols, l.rows);
- ftl::cuda::TextureObject<float2> invr1(r.cols, r.rows);
- ftl::cuda::TextureObject<float> invl2(l.cols, l.rows);
- ftl::cuda::TextureObject<float> invr2(r.cols, r.rows);
- ftl::cuda::TextureObject<float> output(disp);
-
- dim3 grid(1,1,1);
- dim3 threads(BLOCK_W, 1, 1);
- grid.x = cv::cuda::device::divUp(l.cols - 2 * RADIUS2, BLOCK_W);
- grid.y = cv::cuda::device::divUp(l.rows - 2 * RADIUS2, ROWSperTHREAD);
-
- edge_invar1_kernel<<<grid,threads>>>(texLeft.cudaTexture(), prevEdge1.cudaTexture(), invl1);
- cudaSafeCall( cudaGetLastError() );
-
- edge_invar1_kernel<<<grid,threads>>>(texRight.cudaTexture(), prevEdge2.cudaTexture(), invr1);
- cudaSafeCall( cudaGetLastError() );
-
- edge_invar2_kernel<<<grid,threads>>>(invl1.cudaTexture(), invl2);
- cudaSafeCall( cudaGetLastError() );
-
- edge_invar2_kernel<<<grid,threads>>>(invr1.cudaTexture(), invr2);
- cudaSafeCall( cudaGetLastError() );
-
- disparity_kernel<<<grid,threads>>>(invl2.cudaTexture(), invr2.cudaTexture(), output);
-
- prevEdge1.free();
- prevEdge1 = invl1;
- prevEdge2.free();
- prevEdge2 = invr1;
-
- //if (&stream == Stream::Null())
- cudaSafeCall( cudaDeviceSynchronize() );
-
- texLeft.free();
- texRight.free();
- //inv1.free();
- invl2.free();
- invr2.free();
- output.free();
-}
-
-}
-}
-
diff --git a/components/rgbd-sources/src/algorithms/opencv_bm.cpp b/components/rgbd-sources/src/algorithms/opencv_bm.cpp
deleted file mode 100644
index e4a840e46b9f9045e1e0ef4ce206780440f1a2e9..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/opencv_bm.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-#include "opencv_bm.hpp"
-
-using ftl::algorithms::OpenCVBM;
-using namespace cv::ximgproc;
-using namespace cv;
-
-static ftl::Disparity::Register opencvbm("bm", OpenCVBM::create);
-
-OpenCVBM::OpenCVBM(nlohmann::json &config) : Disparity(config) {
- int wsize = config.value("windows_size", 5);
- float sigma = config.value("sigma", 1.5);
- float lambda = config.value("lambda", 8000.0);
-
- left_matcher_ = StereoBM::create(max_disp_,wsize);
- wls_filter_ = createDisparityWLSFilter(left_matcher_);
- right_matcher_ = createRightMatcher(left_matcher_);
-
- wls_filter_->setLambda(lambda);
- wls_filter_->setSigmaColor(sigma);
-}
-
-void OpenCVBM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
- Mat left_disp;
- Mat right_disp;
- Mat lg, rg;
- cv::cvtColor(l, lg, cv::COLOR_BGR2GRAY);
- cv::cvtColor(r, rg, cv::COLOR_BGR2GRAY);
- left_matcher_-> compute(lg, rg,left_disp);
- right_matcher_->compute(rg, lg, right_disp);
- wls_filter_->filter(left_disp, l, disp, right_disp);
-}
-
-
-
diff --git a/components/rgbd-sources/src/algorithms/opencv_bm.hpp b/components/rgbd-sources/src/algorithms/opencv_bm.hpp
deleted file mode 100644
index 4a6b5af0c5442acbc45c1662a3f7a7270b84b714..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/opencv_bm.hpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#ifndef _FTL_ALGORITHMS_OPENCV_BM_HPP_
-#define _FTL_ALGORITHMS_OPENCV_BM_HPP_
-
-#include <opencv2/core.hpp>
-#include <opencv2/opencv.hpp>
-#include "opencv2/ximgproc.hpp"
-#include <opencv2/calib3d.hpp>
-#include "../disparity.hpp"
-#include <ftl/configuration.hpp>
-
-namespace ftl {
-namespace algorithms {
-
-/**
- * OpenCV Block Matching algorithm.
- */
-class OpenCVBM : public ftl::Disparity {
- public:
- explicit OpenCVBM(nlohmann::json &config);
-
- void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
-
- static inline Disparity *create(ftl::Configurable *p, const std::string &name) {
- return ftl::create<OpenCVBM>(p, name);
- }
-
- private:
- cv::Ptr<cv::StereoBM> left_matcher_;
- cv::Ptr<cv::StereoMatcher> right_matcher_;
- cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls_filter_;
-};
-};
-};
-
-#endif // _FTL_ALGORITHMS_OPENCV_SGBM_HPP_
-
diff --git a/components/rgbd-sources/src/algorithms/opencv_cuda_bm.cpp b/components/rgbd-sources/src/algorithms/opencv_cuda_bm.cpp
deleted file mode 100644
index 6261049f07d53bfbcd1152857ab77dc630d6f726..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/opencv_cuda_bm.cpp
+++ /dev/null
@@ -1,31 +0,0 @@
-#include "opencv_cuda_bm.hpp"
-
-using ftl::algorithms::OpenCVCudaBM;
-using namespace cv;
-
-static ftl::Disparity::Register opencvcudabm("cuda_bm", OpenCVCudaBM::create);
-
-OpenCVCudaBM::OpenCVCudaBM(nlohmann::json &config) : Disparity(config) {
- matcher_ = cuda::createStereoBM(max_disp_);
-
- // TODO Add filter
- filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_, 5, 5);
-}
-
-void OpenCVCudaBM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
- if (disp_.empty()) disp_ = cuda::GpuMat(l.size(), CV_8U);
- if (filtered_.empty()) filtered_ = cuda::GpuMat(l.size(), CV_8U);
- if (left_.empty()) left_ = cuda::GpuMat(l.size(), CV_8U);
- if (right_.empty()) right_ = cuda::GpuMat(l.size(), CV_8U);
-
- left_.upload(l);
- right_.upload(r);
-
- matcher_->compute(left_, right_, disp_);
- filter_->apply(disp_, left_, filtered_);
-
- filtered_.download(disp);
-}
-
-
-
diff --git a/components/rgbd-sources/src/algorithms/opencv_cuda_bm.hpp b/components/rgbd-sources/src/algorithms/opencv_cuda_bm.hpp
deleted file mode 100644
index 13c88ccd55380e4ef2f0fd74f3a1e668602d29af..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/opencv_cuda_bm.hpp
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#ifndef _FTL_ALGORITHMS_OPENCV_CUDA_BM_HPP_
-#define _FTL_ALGORITHMS_OPENCV_CUDA_BM_HPP_
-
-#include <opencv2/core.hpp>
-#include <opencv2/opencv.hpp>
-#include <opencv2/cudastereo.hpp>
-#include "../disparity.hpp"
-#include <ftl/configuration.hpp>
-
-namespace ftl {
-namespace algorithms {
-
-/**
- * OpenCV CUDA Block Matching algorithm.
- */
-class OpenCVCudaBM : public ftl::Disparity {
- public:
- explicit OpenCVCudaBM(nlohmann::json &config);
-
- void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
-
- static inline Disparity *create(ftl::Configurable *p, const std::string &name) {
- return ftl::create<OpenCVCudaBM>(p, name);
- }
-
- private:
- cv::Ptr<cv::cuda::StereoBM> matcher_;
- cv::Ptr<cv::cuda::DisparityBilateralFilter> filter_;
- cv::cuda::GpuMat disp_;
- cv::cuda::GpuMat filtered_;
- cv::cuda::GpuMat left_;
- cv::cuda::GpuMat right_;
-};
-};
-};
-
-#endif // _FTL_ALGORITHMS_OPENCV_CUDA_BM_HPP_
-
diff --git a/components/rgbd-sources/src/algorithms/opencv_cuda_bp.cpp b/components/rgbd-sources/src/algorithms/opencv_cuda_bp.cpp
deleted file mode 100644
index ac0eafc50010af9062243564873599062d722420..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/opencv_cuda_bp.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-#include "opencv_cuda_bp.hpp"
-
-using ftl::algorithms::OpenCVCudaBP;
-using namespace cv;
-
-static ftl::Disparity::Register opencvcudabp("cuda_bp", OpenCVCudaBP::create);
-
-OpenCVCudaBP::OpenCVCudaBP(nlohmann::json &config) : Disparity(config) {
- matcher_ = cuda::createStereoBeliefPropagation(max_disp_);
-
- // TODO Add filter
-}
-
-void OpenCVCudaBP::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
- if (disp_.empty()) disp_ = cuda::GpuMat(l.size(), CV_8U);
- if (left_.empty()) left_ = cuda::GpuMat(l.size(), CV_8U);
- if (right_.empty()) right_ = cuda::GpuMat(l.size(), CV_8U);
-
- left_.upload(l);
- right_.upload(r);
-
- matcher_->compute(left_, right_, disp_);
-
- disp_.download(disp);
-}
-
diff --git a/components/rgbd-sources/src/algorithms/opencv_cuda_bp.hpp b/components/rgbd-sources/src/algorithms/opencv_cuda_bp.hpp
deleted file mode 100644
index 3a498552d09ac828863d37c8dff1388e730aef1c..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/opencv_cuda_bp.hpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#ifndef _FTL_ALGORITHMS_OPENCV_CUDA_BP_HPP_
-#define _FTL_ALGORITHMS_OPENCV_CUDA_BP_HPP_
-
-#include <opencv2/core.hpp>
-#include <opencv2/opencv.hpp>
-#include <opencv2/cudastereo.hpp>
-#include "../disparity.hpp"
-#include <ftl/configuration.hpp>
-
-namespace ftl {
-namespace algorithms {
-
-/**
- * OpenCV CUDA Belief Propagation algorithm.
- */
-class OpenCVCudaBP : public ftl::Disparity {
- public:
- explicit OpenCVCudaBP(nlohmann::json &config);
-
- void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
-
- static inline Disparity *create(ftl::Configurable *p, const std::string &name) {
- return ftl::create<OpenCVCudaBP>(p, name);
- }
-
- private:
- cv::Ptr<cv::cuda::StereoBeliefPropagation> matcher_;
- cv::cuda::GpuMat disp_;
- cv::cuda::GpuMat left_;
- cv::cuda::GpuMat right_;
-};
-};
-};
-
-#endif // _FTL_ALGORITHMS_OPENCV_CUDA_BP_HPP_
-
diff --git a/components/rgbd-sources/src/algorithms/opencv_cuda_csbp.cpp b/components/rgbd-sources/src/algorithms/opencv_cuda_csbp.cpp
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/components/rgbd-sources/src/algorithms/opencv_cuda_csbp.hpp b/components/rgbd-sources/src/algorithms/opencv_cuda_csbp.hpp
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/components/rgbd-sources/src/algorithms/opencv_sgbm.cpp b/components/rgbd-sources/src/algorithms/opencv_sgbm.cpp
deleted file mode 100644
index 0d5f0f9e9034c7b456639751d80221a6b0184610..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/opencv_sgbm.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#include "opencv_sgbm.hpp"
-
-using ftl::algorithms::OpenCVSGBM;
-using cv::Mat;
-
-static ftl::Disparity::Register opencvsgbm("sgbm", OpenCVSGBM::create);
-
-OpenCVSGBM::OpenCVSGBM(nlohmann::json &config) : Disparity(config) {
- int wsize = config.value("windows_size", 5);
- float sigma = config.value("sigma", 1.5);
- float lambda = config.value("lambda", 8000.0);
-
- 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(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);
-}
-
-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, 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/components/rgbd-sources/src/algorithms/opencv_sgbm.hpp b/components/rgbd-sources/src/algorithms/opencv_sgbm.hpp
deleted file mode 100644
index 4e6437b0fa75093a4ef2a1d0f3fb1dc8130ae16d..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/opencv_sgbm.hpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#ifndef _FTL_ALGORITHMS_OPENCV_SGBM_HPP_
-#define _FTL_ALGORITHMS_OPENCV_SGBM_HPP_
-
-#include <opencv2/core.hpp>
-#include <opencv2/opencv.hpp>
-#include "opencv2/ximgproc.hpp"
-#include <opencv2/calib3d.hpp>
-#include "../disparity.hpp"
-#include <ftl/configuration.hpp>
-
-namespace ftl {
-namespace algorithms {
-
-/**
- * OpenCV Semi Global Matching algorithm.
- */
-class OpenCVSGBM : public ftl::Disparity {
- public:
- explicit OpenCVSGBM(nlohmann::json &config);
-
- void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
-
- static inline Disparity *create(ftl::Configurable *p, const std::string &name) {
- return ftl::create<OpenCVSGBM>(p, name);
- }
-
- private:
- cv::Ptr<cv::StereoSGBM> left_matcher_;
- cv::Ptr<cv::StereoMatcher> right_matcher_;
- cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls_filter_;
-};
-};
-};
-
-#endif // _FTL_ALGORITHMS_OPENCV_SGBM_HPP_
-
diff --git a/components/rgbd-sources/src/algorithms/rtcensus.cpp b/components/rgbd-sources/src/algorithms/rtcensus.cpp
deleted file mode 100644
index b288045e0f24eb6a2b5b3ef4729c5853b5e17ae5..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/rtcensus.cpp
+++ /dev/null
@@ -1,294 +0,0 @@
-/* Created by Nicolas Pope and Sebastian Hahta
- *
- * Copyright 2019 Nicolas Pope
- *
- * Implementation of algorithm presented in article(s):
- *
- * [1] Humenberger, Engelke, Kubinger: A fast stereo matching algorithm suitable
- * for embedded real-time systems
- * [2] Humenberger, Zinner, Kubinger: Performance Evaluation of Census-Based
- * Stereo Matching Algorithm on Embedded and Multi-Core Hardware
- * [3] Humenberger, Engelke, Kubinger: A Census-Based Stereo Vision Algorithm Using Modified Semi-Global Matching
- * and Plane Fitting to Improve Matching Quality.
- *
- * Equation numbering uses [1] unless otherwise stated
- */
-
-#include <loguru.hpp>
-#include <cmath>
-
-#include <vector>
-#include <tuple>
-#include <bitset>
-#include <chrono>
-
-#include "rtcensus.hpp"
-
-using ftl::algorithms::RTCensus;
-using std::vector;
-using cv::Mat;
-using cv::Point;
-using cv::Size;
-using std::tuple;
-using std::get;
-using std::make_tuple;
-using std::bitset;
-
-/* (8) and (16) */
-#define XHI(P1, P2) ((P1 <= P2) ? 0 : 1)
-
-/* (14) and (15), based on (9) */
-static vector<uint64_t> sparse_census_16x16(const Mat &arr) {
- vector<uint64_t> result;
- result.resize(arr.cols*arr.rows, 0);
-
- /* Loops adapted to avoid edge out-of-bounds checks */
- for (int v=7; v < arr.rows-7; v++) {
- for (int u=7; u < arr.cols-7; u++) {
- uint64_t r = 0;
-
- /* 16x16 sparse kernel to 8x8 mask (64 bits) */
- for (int n=-7; n <= 7; n+=2) {
- auto u_ = u + n;
- for (int m=-7; m <= 7; m+=2) {
- auto v_ = v + m;
- r <<= 1;
- r |= XHI(arr.at<uint8_t>(v, u), arr.at<uint8_t>(v_, u_));
- }
- }
-
- result[u+v*arr.cols] = r;
- }
- }
-
- return result;
-}
-
-/*
- * (19) note: M and N not the same as in (17), see also (8) in [2].
- * DSI: Disparity Space Image. LTR/RTL matching can be with sign +1/-1
- */
-static void dsi_ca(vector<uint16_t> &result, const vector<uint64_t> &census_R,
- const vector<uint64_t> &census_L, size_t w, size_t h, size_t d_start,
- size_t d_stop, int sign = 1) {
- // TODO(nick) Add asserts
- assert(census_R.size() == w*h);
- assert(census_L.size() == w*h);
- assert(d_stop-d_start > 0);
-
- auto ds = d_stop - d_start; // Number of disparities to check
- result.resize(census_R.size()*ds, 0);
-
- // Change bounds depending upon disparity direction
- const size_t eu = (sign > 0) ? w-2-ds : w-2;
-
- // Adapt bounds to avoid out-of-bounds checks
- for (size_t v=2; v < h-2; v++) {
- for (size_t u=(sign > 0) ? 2 : ds+2; u < eu; u++) {
- const size_t ix = v*w*ds+u*ds;
-
- // 5x5 window size
- for (int n=-2; n <= 2; n++) {
- const auto u_ = u + n;
- for (int m=-2; m <= 2; m++) {
- const auto v_ = (v + m)*w;
- auto r = census_R[u_+v_];
-
- 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
- }
- }
- }
- }
- }
-}
-
-/*
- * Find the minimum value in a sub array.
- * TODO This can be removed entirely (see CUDA version)
- */
-static size_t arrmin(vector<uint16_t> &a, size_t ix, size_t len) {
- uint32_t m = UINT32_MAX;
- size_t mi = 0;
- for (size_t i=ix; i < ix+len; i++) {
- if (a[i] < m) {
- m = a[i];
- mi = i;
- }
- }
- return mi-ix;
-}
-
-/*
- * WTA + subpixel disparity (parabilic fitting) (20)
- * TODO remove need to pass tuples (see CUDA version)
- */
-static inline double fit_parabola(tuple<size_t, uint16_t> p,
- tuple<size_t, uint16_t> pl, tuple<size_t, uint16_t> pr) {
- double a = get<1>(pr) - get<1>(pl);
- double b = 2 * (2 * get<1>(p) - get<1>(pl) - get<1>(pr));
- return static_cast<double>(get<0>(p)) + (a / b);
-}
-
-static cv::Mat d_sub(vector<uint16_t> &dsi, size_t w, size_t h, size_t ds) {
- Mat result = Mat::zeros(Size(w, h), CV_64FC1);
-
- assert(dsi.size() == w*h*ds);
-
- for (size_t v=0; v < h; v++) {
- const size_t vwds = v * w * ds;
- for (size_t u=0; u < w; u++) {
- const size_t uds = u*ds;
- auto d_min = arrmin(dsi, vwds+uds, ds);
- double d_sub;
-
- if (d_min == 0 || d_min == ds-1) {
- d_sub = d_min;
- } else {
- // TODO(nick) Remove use of tuples
- auto p = make_tuple(d_min, dsi[d_min+vwds+uds]);
- auto pl = make_tuple(d_min-1, dsi[d_min-1+vwds+uds]);
- auto pr = make_tuple(d_min+1, dsi[d_min+1+vwds+uds]);
-
- d_sub = fit_parabola(p, pl, pr);
- }
-
- result.at<double>(v, u) = d_sub;
- }
- }
-
- // TODO(nick) Parameter pass not return
- return result;
-}
-
-/*
- * consistency between LR and RL disparity (23) and (24)
- */
-static cv::Mat consistency(cv::Mat &d_sub_r, cv::Mat &d_sub_l) {
- size_t w = d_sub_r.cols;
- size_t h = d_sub_r.rows;
- Mat result = Mat::zeros(Size(w, h), CV_32FC1);
-
- for (size_t v=0; v < h; v++) {
- for (size_t u=0; u < w; u++) {
- auto a = static_cast<int>(d_sub_l.at<double>(v, u));
- if (u-a < 0) continue;
-
- auto b = d_sub_r.at<double>(v, u-a);
-
- if (std::abs(a-b) <= 1.0) {
- result.at<float>(v, u) = std::abs((a+b)/2);
- } else {
- result.at<float>(v, u) = 0.0f;
- }
- }
- }
-
- return result;
-}
-
-RTCensus::RTCensus(nlohmann::json &config)
- : Disparity(config),
- gamma_(0.0f),
- tau_(0.0f),
- use_cuda_(config.value("use_cuda", true)),
- alternate_(false) {}
-
-/*
- * Choose the implementation and perform disparity calculation.
- */
-void RTCensus::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
- #if defined HAVE_CUDA
- if (use_cuda_) {
- computeCUDA(l, r, disp);
- } else {
- computeCPU(l, r, disp);
- }
- #else // !HAVE_CUDA
- computeCPU(l, r, disp);
- #endif
-}
-
-void RTCensus::computeCPU(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
- size_t d_min = min_disp_;
- size_t d_max = max_disp_;
-
- Mat lbw, rbw;
- cv::cvtColor(l, lbw, cv::COLOR_BGR2GRAY);
- cv::cvtColor(r, rbw, cv::COLOR_BGR2GRAY);
-
- auto start = std::chrono::high_resolution_clock::now();
- auto census_R = sparse_census_16x16(rbw);
- auto census_L = sparse_census_16x16(lbw);
- std::chrono::duration<double> elapsed =
- std::chrono::high_resolution_clock::now() - start;
- LOG(INFO) << "Census in " << elapsed.count() << "s";
-
- start = std::chrono::high_resolution_clock::now();
- vector<uint16_t> dsi_ca_R, dsi_ca_L;
- dsi_ca(dsi_ca_R, census_R, census_L, l.cols, l.rows, d_min, d_max);
- dsi_ca(dsi_ca_L, census_L, census_R, l.cols, l.rows, d_min, d_max, -1);
- elapsed = std::chrono::high_resolution_clock::now() - start;
- LOG(INFO) << "DSI in " << elapsed.count() << "s";
-
- auto disp_R = d_sub(dsi_ca_R, l.cols, l.rows, d_max-d_min);
- auto disp_L = d_sub(dsi_ca_L, l.cols, l.rows, d_max-d_min);
- LOG(INFO) << "Disp done";
-
- disp = consistency(disp_R, disp_L);
-
- // TODO(nick) confidence and texture filtering
-}
-
-#if defined HAVE_CUDA
-
-using cv::cuda::PtrStepSz;
-using cv::cuda::GpuMat;
-
-#include <vector_types.h>
-
-namespace ftl { namespace gpu {
-void rtcensus_call(const PtrStepSz<uchar4> &l, const PtrStepSz<uchar4> &r,
- const PtrStepSz<float> &disp, size_t num_disp, const int &s = 0);
-}}
-
-void RTCensus::computeCUDA(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
- // Initialise gpu memory here because we need image size
- if (disp_.empty()) disp_ = GpuMat(l.size(), CV_32FC1);
- if (left_.empty()) left_ = GpuMat(l.size(), CV_8UC4);
- if (left2_.empty()) left2_ = GpuMat(l.size(), CV_8UC4);
- if (right_.empty()) right_ = GpuMat(l.size(), CV_8UC4);
-
- Mat lhsv, rhsv;
- cv::cvtColor(l, lhsv, cv::COLOR_BGR2HSV);
- cv::cvtColor(r, rhsv, cv::COLOR_BGR2HSV);
- int from_to[] = {0, 0, 1, 1, 2, 2, -1, 3};
- Mat hsval(lhsv.size(), CV_8UC4);
- Mat hsvar(rhsv.size(), CV_8UC4);
- mixChannels(&lhsv, 1, &hsval, 1, from_to, 4);
- mixChannels(&rhsv, 1, &hsvar, 1, from_to, 4);
-
- // Send images to GPU
- if (alternate_) {
- left_.upload(hsval);
- } else {
- left2_.upload(hsval);
- }
- right_.upload(hsvar);
-
- auto start = std::chrono::high_resolution_clock::now();
- ftl::gpu::rtcensus_call((alternate_)?left_:left2_, right_, disp_, max_disp_);
- std::chrono::duration<double> elapsed =
- std::chrono::high_resolution_clock::now() - start;
- LOG(INFO) << "CUDA census in " << elapsed.count() << "s";
-
- alternate_ = !alternate_;
-
- // Read disparity from GPU
- disp_.download(disp);
-}
-
-#endif
-
diff --git a/components/rgbd-sources/src/algorithms/rtcensus.cu b/components/rgbd-sources/src/algorithms/rtcensus.cu
deleted file mode 100644
index 6ff8e331e6d7feba47522175c9243a7ea60e8aff..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/rtcensus.cu
+++ /dev/null
@@ -1,205 +0,0 @@
-/*
- * Author: Nicolas Pope and Sebastian Hahta (2019)
- * Implementation of algorithm presented in article(s):
- *
- * [1] Humenberger, Engelke, Kubinger: A fast stereo matching algorithm suitable
- * for embedded real-time systems
- * [2] Humenberger, Zinner, Kubinger: Performance Evaluation of Census-Based
- * Stereo Matching Algorithm on Embedded and Multi-Core Hardware
- * [3] Humenberger, Engelke, Kubinger: A Census-Based Stereo Vision Algorithm Using Modified Semi-Global Matching
- * and Plane Fitting to Improve Matching Quality.
- *
- * Equation numbering uses [1] unless otherwise stated
- *
- */
-
-#include <ftl/cuda_common.hpp>
-#include "../cuda_algorithms.hpp"
-
-using namespace cv::cuda;
-using namespace cv;
-
-
-#define BLOCK_W 60
-#define RADIUS 7
-#define RADIUS2 2
-#define ROWSperTHREAD 1
-
-namespace ftl {
-namespace gpu {
-
-// --- SUPPORT -----------------------------------------------------------------
-
-
-/*
- * Parabolic interpolation between matched disparities either side.
- * Results in subpixel disparity. (20).
- */
-__device__ float fit_parabola(size_t pi, uint16_t p, uint16_t pl, uint16_t pr) {
- float a = pr - pl;
- float b = 2 * (2 * p - pl - pr);
- return static_cast<float>(pi) + (a / b);
-}
-
-// --- KERNELS -----------------------------------------------------------------
-
-/* Convert vector uint2 (32bit x2) into a single uint64_t */
-__forceinline__ __device__ uint64_t uint2asull (uint2 a) {
- uint64_t res;
- asm ("mov.b64 %0, {%1,%2};" : "=l"(res) : "r"(a.x), "r"(a.y));
- return res;
-}
-
-/*
- * Generate left and right disparity images from census data. (18)(19)(25)
- */
-__global__ void disp_kernel(float *disp_l, float *disp_r,
- int pitchL, int pitchR,
- size_t width, size_t height,
- cudaTextureObject_t censusL, cudaTextureObject_t censusR,
- size_t ds) {
- //extern __shared__ uint64_t cache[];
-
- const int gamma = 35;
-
- int u = (blockIdx.x * BLOCK_W) + threadIdx.x + RADIUS2;
- int v_start = (blockIdx.y * ROWSperTHREAD) + RADIUS2;
- int v_end = v_start + ROWSperTHREAD;
- int maxdisp = ds;
-
- // Local cache
- uint64_t l_cache_l1[5][5];
- uint64_t l_cache_l2[5][5];
-
- if (v_end >= height) v_end = height;
- if (u+maxdisp >= width) maxdisp = width-u;
-
- for (int v=v_start; v<v_end; v++) {
- for (int m=-2; m<=2; m++) {
- for (int n=-2; n<=2; n++) {
- l_cache_l2[m+2][n+2] = uint2asull(tex2D<uint2>(censusL,u+n,v+m));
- l_cache_l1[m+2][n+2] = uint2asull(tex2D<uint2>(censusR,u+n,v+m));
- }
- }
-
- uint16_t last_ham1 = 65535;
- uint16_t last_ham2 = 65535;
- uint16_t min_disp1 = 65535;
- uint16_t min_disp2 = 65535;
- uint16_t min_disp1b = 65535;
- uint16_t min_disp2b = 65535;
- uint16_t min_before1 = 0;
- uint16_t min_before2 = 0;
- uint16_t min_after1 = 0;
- uint16_t min_after2 = 0;
- int dix1 = 0;
- int dix2 = 0;
-
- // (19)
- for (int d=0; d<maxdisp; d++) {
- uint16_t hamming1 = 0;
- uint16_t hamming2 = 0;
-
- //if (u+2+ds >= width) break;
-
- for (int m=-2; m<=2; m++) {
- const auto v_ = (v + m);
- for (int n=-2; n<=2; n++) {
- const auto u_ = u + n;
-
- // (18)
- auto l1 = l_cache_l1[m+2][n+2];
- auto l2 = l_cache_l2[m+2][n+2];
- auto r1 = uint2asull(tex2D<uint2>(censusL, u_+d, v_));
- auto r2 = uint2asull(tex2D<uint2>(censusR, u_-d, v_));
- hamming1 += __popcll(r1^l1);
- hamming2 += __popcll(r2^l2);
- }
- }
-
- // Find the two minimum costs
- if (hamming1 < min_disp1) {
- min_before1 = last_ham1;
- min_disp1 = hamming1;
- dix1 = d;
- } else if (hamming1 < min_disp1b) {
- min_disp1b = hamming1;
- }
- if (dix1 == d) min_after1 = hamming1;
- last_ham1 = hamming1;
-
- if (hamming2 < min_disp2) {
- min_before2 = last_ham2;
- min_disp2 = hamming2;
- dix2 = d;
- } else if (hamming2 < min_disp2b) {
- min_disp2b = hamming2;
- }
- if (dix2 == d) min_after2 = hamming2;
- last_ham2 = hamming2;
-
- }
-
- //float d1 = (dix1 == 0 || dix1 == ds-1) ? (float)dix1 : fit_parabola(dix1, min_disp1, min_before1, min_after1);
- //float d2 = (dix2 == 0 || dix2 == ds-1) ? (float)dix2 : fit_parabola(dix2, min_disp2, min_before2, min_after2);
-
- // TODO Allow for discontinuities with threshold
- // Subpixel disparity (20)
- float d1 = fit_parabola(dix1, min_disp1, min_before1, min_after1);
- float d2 = fit_parabola(dix2, min_disp2, min_before2, min_after2);
-
- // Confidence filter based on (25)
- disp_l[v*pitchL+u] = ((min_disp2b - min_disp2) >= gamma) ? d2 : NAN;
- disp_r[v*pitchR+u] = ((min_disp1b - min_disp1) >= gamma) ? d1 : NAN;
- }
-}
-
-void rtcensus_call(const PtrStepSz<uchar4> &l, const PtrStepSz<uchar4> &r, const PtrStepSz<float> &disp, size_t num_disp, const int &stream) {
- // Make all the required texture steps
- // TODO Could reduce re-allocations by caching these
- ftl::cuda::TextureObject<uchar4> texLeft(l);
- ftl::cuda::TextureObject<uchar4> texRight(r);
- ftl::cuda::TextureObject<uint2> censusTexLeft(l.cols, l.rows);
- ftl::cuda::TextureObject<uint2> censusTexRight(r.cols, r.rows);
- ftl::cuda::TextureObject<float> dispTexLeft(l.cols, l.rows);
- ftl::cuda::TextureObject<float> dispTexRight(r.cols, r.rows);
- ftl::cuda::TextureObject<float> dispTex(r.cols, r.rows);
- ftl::cuda::TextureObject<float> output(disp);
-
- // Calculate the census for left and right (14)(15)(16)
- ftl::cuda::sparse_census(texLeft, texRight, censusTexLeft, censusTexRight);
-
- dim3 grid(1,1,1);
- dim3 threads(BLOCK_W, 1, 1);
- grid.x = cv::cuda::device::divUp(l.cols - 2 * RADIUS2, BLOCK_W);
- grid.y = cv::cuda::device::divUp(l.rows - 2 * RADIUS2, ROWSperTHREAD);
-
- // Calculate L and R disparities (18)(19)(20)(21)(22)(25)
- disp_kernel<<<grid, threads>>>(
- dispTexLeft.devicePtr(), dispTexRight.devicePtr(),
- dispTexLeft.pitch()/sizeof(float), dispTexRight.pitch()/sizeof(float),
- l.cols, l.rows,
- censusTexLeft.cudaTexture(), censusTexRight.cudaTexture(),
- num_disp);
- cudaSafeCall( cudaGetLastError() );
-
- // Check consistency between L and R disparities. (23)(24)
- consistency(dispTexLeft, dispTexRight, dispTex);
-
- // TM in (7) of paper [3]. Eq (26) in [1] is wrong.
- texture_filter(texLeft, dispTex, output, num_disp, 10.0);
-
- cudaSafeCall( cudaDeviceSynchronize() );
-
- texLeft.free();
- texRight.free();
- censusTexLeft.free();
- censusTexRight.free();
- dispTexLeft.free();
- dispTexRight.free();
- dispTex.free();
- output.free();
-}
-
-};
-};
diff --git a/components/rgbd-sources/src/algorithms/rtcensus.hpp b/components/rgbd-sources/src/algorithms/rtcensus.hpp
deleted file mode 100644
index 785af6261535b6e3891a02cc7034b4d22e4d8e25..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/rtcensus.hpp
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#ifndef _FTL_ALGORITHMS_RTCENSUS_HPP_
-#define _FTL_ALGORITHMS_RTCENSUS_HPP_
-
-#include <opencv2/core.hpp>
-#include <opencv2/opencv.hpp>
-#include "../disparity.hpp"
-#include <nlohmann/json.hpp>
-
-#include <ftl/config.h>
-#include <ftl/configuration.hpp>
-
-#if defined HAVE_CUDA
-#include <opencv2/core/cuda.hpp>
-#endif
-
-namespace ftl {
-namespace algorithms {
-
-/**
- * Real-time Sparse Census disparity algorithm.
- */
-class RTCensus : public ftl::rgbd::detail::Disparity {
- public:
- 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(ftl::Configurable *p, const std::string &name) {
- return ftl::create<RTCensus>(p, name);
- }
-
- private:
- float gamma_;
- float tau_;
- bool use_cuda_;
- bool alternate_;
-
- #if defined HAVE_CUDA
- cv::cuda::GpuMat disp_;
- cv::cuda::GpuMat filtered_;
- cv::cuda::GpuMat left_;
- cv::cuda::GpuMat left2_;
- cv::cuda::GpuMat right_;
- #endif
-
- private:
- void computeCPU(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
-
- #if defined HAVE_CUDA
- void computeCUDA(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
- #endif
-};
-};
-};
-
-#endif // _FTL_ALGORITHMS_RTCENSUS_HPP_
-
diff --git a/components/rgbd-sources/src/algorithms/rtcensus_sgm.cpp b/components/rgbd-sources/src/algorithms/rtcensus_sgm.cpp
deleted file mode 100644
index e84819e603e1bee4acd4e87f4914292e7269ade6..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/rtcensus_sgm.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-/* Created by Nicolas Pope and Sebastian Hahta
- *
- * Implementation of algorithm presented in article(s):
- *
- * [1] Humenberger, Engelke, Kubinger: A fast stereo matching algorithm suitable
- * for embedded real-time systems
- * [2] Humenberger, Zinner, Kubinger: Performance Evaluation of Census-Based
- * Stereo Matching Algorithm on Embedded and Multi-Core Hardware
- * [3] Humenberger, Engelke, Kubinger: A Census-Based Stereo Vision Algorithm Using Modified Semi-Global Matching
- * and Plane Fitting to Improve Matching Quality.
- *
- * Equation numbering uses [1] unless otherwise stated
- */
-
-
-#include "rtcensus_sgm.hpp"
-#include <vector>
-#include <tuple>
-#include <bitset>
-#include <cmath>
-#include <loguru.hpp>
-
-using ftl::algorithms::RTCensusSGM;
-using std::vector;
-using cv::Mat;
-using cv::Point;
-using cv::Size;
-using std::tuple;
-using std::get;
-using std::make_tuple;
-using std::bitset;
-
-//static ftl::Disparity::Register rtcensus("rtcensus_sgm", RTCensusSGM::create);
-
-RTCensusSGM::RTCensusSGM(nlohmann::json &config)
- : Disparity(config),
- gamma_(0.0f),
- tau_(0.0f),
- use_cuda_(config.value("use_cuda",true)),
- alternate_(false) {}
-
-/*
- * Choose the implementation and perform disparity calculation.
- */
-void RTCensusSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
- #if defined HAVE_CUDA
- if (use_cuda_) {
- computeCUDA(l,r,disp);
- } else {
- //computeCPU(l,r,disp);
- LOG(ERROR) << "RTCensus SGM requires CUDA";
- }
- #else // !HAVE_CUDA
- //computeCPU(l,r,disp);
- LOG(ERROR) << "RTCensus SGM requires CUDA";
- #endif
-}
-
-#if defined HAVE_CUDA
-
-using namespace cv::cuda;
-using namespace cv;
-
-#include <vector_types.h>
-
-namespace ftl { namespace gpu {
-void rtcensus_sgm_call(const PtrStepSz<uchar4> &l, const PtrStepSz<uchar4> &r, const PtrStepSz<float> &disp, size_t num_disp, const int &s=0);
-}}
-
-void RTCensusSGM::computeCUDA(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
- // Initialise gpu memory here because we need image size
- if (disp_.empty()) disp_ = cuda::GpuMat(l.size(), CV_32FC1);
- if (left_.empty()) left_ = cuda::GpuMat(l.size(), CV_8UC4);
- if (left2_.empty()) left2_ = cuda::GpuMat(l.size(), CV_8UC4);
- if (right_.empty()) right_ = cuda::GpuMat(l.size(), CV_8UC4);
-
- Mat lhsv, rhsv;
- cv::cvtColor(l, lhsv, COLOR_BGR2HSV);
- cv::cvtColor(r, rhsv, COLOR_BGR2HSV);
- int from_to[] = {0,0,1,1,2,2,-1,3};
- Mat hsval(lhsv.size(), CV_8UC4);
- Mat hsvar(rhsv.size(), CV_8UC4);
- mixChannels(&lhsv, 1, &hsval, 1, from_to, 4);
- mixChannels(&rhsv, 1, &hsvar, 1, from_to, 4);
-
- // Send images to GPU
- if (alternate_) left_.upload(hsval);
- else left2_.upload(hsval);
- right_.upload(hsvar);
-
- auto start = std::chrono::high_resolution_clock::now();
- ftl::gpu::rtcensus_sgm_call((alternate_)?left_:left2_, right_, disp_, max_disp_);
- std::chrono::duration<double> elapsed = std::chrono::high_resolution_clock::now() - start;
- LOG(INFO) << "CUDA rtcensus_sgm in " << elapsed.count() << "s";
-
- alternate_ = !alternate_;
-
- // Read disparity from GPU
- disp_.download(disp);
-}
-
-#endif
-
diff --git a/components/rgbd-sources/src/algorithms/rtcensus_sgm.cu b/components/rgbd-sources/src/algorithms/rtcensus_sgm.cu
deleted file mode 100644
index d59a77bc0b8fe3f0538f8974350144bf1c3b386e..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/rtcensus_sgm.cu
+++ /dev/null
@@ -1,205 +0,0 @@
-/*
- * Author: Nicolas Pope and Sebastian Hahta (2019)
- * Implementation of algorithm presented in article(s):
- *
- * [1] Humenberger, Engelke, Kubinger: A fast stereo matching algorithm suitable
- * for embedded real-time systems
- * [2] Humenberger, Zinner, Kubinger: Performance Evaluation of Census-Based
- * Stereo Matching Algorithm on Embedded and Multi-Core Hardware
- * [3] Humenberger, Engelke, Kubinger: A Census-Based Stereo Vision Algorithm Using Modified Semi-Global Matching
- * and Plane Fitting to Improve Matching Quality.
- *
- * Equation numbering uses [1] unless otherwise stated
- *
- */
-
-#include <ftl/cuda_common.hpp>
-#include "../cuda_algorithms.hpp"
-
-using namespace cv::cuda;
-using namespace cv;
-
-
-#define BLOCK_W 60
-#define RADIUS 7
-#define RADIUS2 2
-#define ROWSperTHREAD 1
-
-namespace ftl {
-namespace gpu {
-
-// --- SUPPORT -----------------------------------------------------------------
-
-
-/*
- * Parabolic interpolation between matched disparities either side.
- * Results in subpixel disparity. (20).
- */
-__device__ static float fit_parabola(size_t pi, uint16_t p, uint16_t pl, uint16_t pr) {
- float a = pr - pl;
- float b = 2 * (2 * p - pl - pr);
- return static_cast<float>(pi) + (a / b);
-}
-
-// --- KERNELS -----------------------------------------------------------------
-
-/* Convert vector uint2 (32bit x2) into a single uint64_t */
-__forceinline__ __device__ static uint64_t uint2asull (uint2 a) {
- uint64_t res;
- asm ("mov.b64 %0, {%1,%2};" : "=l"(res) : "r"(a.x), "r"(a.y));
- return res;
-}
-
-/*
- * Generate left and right disparity images from census data. (18)(19)(25)
- */
-__global__ static void disp_kernel(float *disp_l, float *disp_r,
- int pitchL, int pitchR,
- size_t width, size_t height,
- cudaTextureObject_t censusL, cudaTextureObject_t censusR,
- size_t ds) {
- //extern __shared__ uint64_t cache[];
-
- const int gamma = 35;
-
- int u = (blockIdx.x * BLOCK_W) + threadIdx.x + RADIUS2;
- int v_start = (blockIdx.y * ROWSperTHREAD) + RADIUS2;
- int v_end = v_start + ROWSperTHREAD;
- int maxdisp = ds;
-
- // Local cache
- uint64_t l_cache_l1[5][5];
- uint64_t l_cache_l2[5][5];
-
- if (v_end >= height) v_end = height;
- if (u+maxdisp >= width) maxdisp = width-u;
-
- for (int v=v_start; v<v_end; v++) {
- for (int m=-2; m<=2; m++) {
- for (int n=-2; n<=2; n++) {
- l_cache_l2[m+2][n+2] = uint2asull(tex2D<uint2>(censusL,u+n,v+m));
- l_cache_l1[m+2][n+2] = uint2asull(tex2D<uint2>(censusR,u+n,v+m));
- }
- }
-
- uint16_t last_ham1 = 65535;
- uint16_t last_ham2 = 65535;
- uint16_t min_disp1 = 65535;
- uint16_t min_disp2 = 65535;
- uint16_t min_disp1b = 65535;
- uint16_t min_disp2b = 65535;
- uint16_t min_before1 = 0;
- uint16_t min_before2 = 0;
- uint16_t min_after1 = 0;
- uint16_t min_after2 = 0;
- int dix1 = 0;
- int dix2 = 0;
-
- // (19)
- for (int d=0; d<maxdisp; d++) {
- uint16_t hamming1 = 0;
- uint16_t hamming2 = 0;
-
- //if (u+2+ds >= width) break;
-
- for (int m=-2; m<=2; m++) {
- const auto v_ = (v + m);
- for (int n=-2; n<=2; n++) {
- const auto u_ = u + n;
-
- // (18)
- auto l1 = l_cache_l1[m+2][n+2];
- auto l2 = l_cache_l2[m+2][n+2];
- auto r1 = uint2asull(tex2D<uint2>(censusL, u_+d, v_));
- auto r2 = uint2asull(tex2D<uint2>(censusR, u_-d, v_));
- hamming1 += __popcll(r1^l1);
- hamming2 += __popcll(r2^l2);
- }
- }
-
- // Find the two minimum costs
- if (hamming1 < min_disp1) {
- min_before1 = last_ham1;
- min_disp1 = hamming1;
- dix1 = d;
- } else if (hamming1 < min_disp1b) {
- min_disp1b = hamming1;
- }
- if (dix1 == d) min_after1 = hamming1;
- last_ham1 = hamming1;
-
- if (hamming2 < min_disp2) {
- min_before2 = last_ham2;
- min_disp2 = hamming2;
- dix2 = d;
- } else if (hamming2 < min_disp2b) {
- min_disp2b = hamming2;
- }
- if (dix2 == d) min_after2 = hamming2;
- last_ham2 = hamming2;
-
- }
-
- //float d1 = (dix1 == 0 || dix1 == ds-1) ? (float)dix1 : fit_parabola(dix1, min_disp1, min_before1, min_after1);
- //float d2 = (dix2 == 0 || dix2 == ds-1) ? (float)dix2 : fit_parabola(dix2, min_disp2, min_before2, min_after2);
-
- // TODO Allow for discontinuities with threshold
- // Subpixel disparity (20)
- float d1 = fit_parabola(dix1, min_disp1, min_before1, min_after1);
- float d2 = fit_parabola(dix2, min_disp2, min_before2, min_after2);
-
- // Confidence filter based on (25)
- disp_l[v*pitchL+u] = ((min_disp2b - min_disp2) >= gamma) ? d2 : NAN;
- disp_r[v*pitchR+u] = ((min_disp1b - min_disp1) >= gamma) ? d1 : NAN;
- }
-}
-
-void rtcensus_sgm_call(const PtrStepSz<uchar4> &l, const PtrStepSz<uchar4> &r, const PtrStepSz<float> &disp, size_t num_disp, const int &stream) {
- // Make all the required texture steps
- // TODO Could reduce re-allocations by caching these
- ftl::cuda::TextureObject<uchar4> texLeft(l);
- ftl::cuda::TextureObject<uchar4> texRight(r);
- ftl::cuda::TextureObject<uint2> censusTexLeft(l.cols, l.rows);
- ftl::cuda::TextureObject<uint2> censusTexRight(r.cols, r.rows);
- ftl::cuda::TextureObject<float> dispTexLeft(l.cols, l.rows);
- ftl::cuda::TextureObject<float> dispTexRight(r.cols, r.rows);
- ftl::cuda::TextureObject<float> dispTex(r.cols, r.rows);
- ftl::cuda::TextureObject<float> output(disp);
-
- // Calculate the census for left and right (14)(15)(16)
- ftl::cuda::sparse_census(texLeft, texRight, censusTexLeft, censusTexRight);
-
- dim3 grid(1,1,1);
- dim3 threads(BLOCK_W, 1, 1);
- grid.x = cv::cuda::device::divUp(l.cols - 2 * RADIUS2, BLOCK_W);
- grid.y = cv::cuda::device::divUp(l.rows - 2 * RADIUS2, ROWSperTHREAD);
-
- // Calculate L and R disparities (18)(19)(20)(21)(22)(25)
- disp_kernel<<<grid, threads>>>(
- dispTexLeft.devicePtr(), dispTexRight.devicePtr(),
- dispTexLeft.pitch()/sizeof(float), dispTexRight.pitch()/sizeof(float),
- l.cols, l.rows,
- censusTexLeft.cudaTexture(), censusTexRight.cudaTexture(),
- num_disp);
- cudaSafeCall( cudaGetLastError() );
-
- // Check consistency between L and R disparities. (23)(24)
- consistency(dispTexLeft, dispTexRight, dispTex);
-
- // TM in (7) of paper [3]. Eq (26) in [1] is wrong.
- texture_filter(texLeft, dispTex, output, num_disp, 10.0);
-
- cudaSafeCall( cudaDeviceSynchronize() );
-
- texLeft.free();
- texRight.free();
- censusTexLeft.free();
- censusTexRight.free();
- dispTexLeft.free();
- dispTexRight.free();
- dispTex.free();
- output.free();
-}
-
-};
-};
diff --git a/components/rgbd-sources/src/algorithms/rtcensus_sgm.hpp b/components/rgbd-sources/src/algorithms/rtcensus_sgm.hpp
deleted file mode 100644
index ef4dd7fd9cdf84bc2cde0eca7b3d556af5d9e64e..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/rtcensus_sgm.hpp
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#ifndef _FTL_ALGORITHMS_RTCENSUSSGM_HPP_
-#define _FTL_ALGORITHMS_RTCENSUSSGM_HPP_
-
-#include <opencv2/core.hpp>
-#include <opencv2/opencv.hpp>
-#include "../disparity.hpp"
-#include <nlohmann/json.hpp>
-#include <ftl/configuration.hpp>
-
-#if defined HAVE_CUDA
-#include <opencv2/core/cuda.hpp>
-#endif
-
-namespace ftl {
-namespace algorithms {
-
-/**
- * WORK IN PROGRESS
- */
-class RTCensusSGM : public ftl::rgbd::detail::Disparity {
- public:
- 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(ftl::Configurable *p, const std::string &name) {
- return ftl::create<RTCensusSGM>(p, name);
- }
-
- private:
- float gamma_;
- float tau_;
- bool use_cuda_;
- bool alternate_;
-
- #if defined HAVE_CUDA
- cv::cuda::GpuMat disp_;
- cv::cuda::GpuMat filtered_;
- cv::cuda::GpuMat left_;
- cv::cuda::GpuMat left2_;
- cv::cuda::GpuMat right_;
- #endif
-
- private:
- #if defined HAVE_CUDA
- void computeCUDA(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
- #endif
-};
-};
-};
-
-#endif // _FTL_ALGORITHMS_RTCENSUSSGM_HPP_
-
diff --git a/components/rgbd-sources/src/algorithms/sparse_census.cu b/components/rgbd-sources/src/algorithms/sparse_census.cu
deleted file mode 100644
index c912354085a3a73078c3ad54666342305a59be72..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/sparse_census.cu
+++ /dev/null
@@ -1,81 +0,0 @@
-/*
- * Author: Nicolas Pope and Sebastian Hahta (2019)
- * Implementation of algorithm presented in article(s):
- *
- * [1] Humenberger, Engelke, Kubinger: A fast stereo matching algorithm suitable
- * for embedded real-time systems
- * [2] Humenberger, Zinner, Kubinger: Performance Evaluation of Census-Based
- * Stereo Matching Algorithm on Embedded and Multi-Core Hardware
- *
- * Equation numbering uses [1] unless otherwise stated
- *
- */
-
-#include <ftl/cuda_common.hpp>
-
-#define XHI(P1,P2) ((P1 <= P2) ? 0 : 1)
-
-/*
- * Sparse 16x16 census (so 8x8) creating a 64bit mask
- * (14) & (15), based upon (9)
- */
-__device__ uint64_t _sparse_census(cudaTextureObject_t tex, int u, int v) {
- uint64_t r = 0;
-
- unsigned char t = tex2D<uchar4>(tex, u,v).z;
-
- for (int m=-7; m<=7; m+=2) {
- //auto start_ix = (v + m)*w + u;
- for (int n=-7; n<=7; n+=2) {
- r <<= 1;
- r |= XHI(t, tex2D<uchar4>(tex, u+n, v+m).z);
- }
- }
-
- return r;
-}
-
-/*
- * Calculate census mask for left and right images together.
- */
-__global__ void census_kernel(cudaTextureObject_t l, cudaTextureObject_t r,
- int w, int h, uint64_t *censusL, uint64_t *censusR,
- size_t pL, size_t pR) {
-
- //if (v_end+RADIUS >= h) v_end = h-RADIUS;
- //if (u+RADIUS >= w) return;
-
- for (STRIDE_Y(v,h)) {
- for (STRIDE_X(u,w)) {
- //int ix = (u + v*pL);
- uint64_t cenL = _sparse_census(l, u, v);
- uint64_t cenR = _sparse_census(r, u, v);
-
- censusL[(u + v*pL)] = cenL;
- censusR[(u + v*pR)] = cenR;
- }
- }
-}
-
-namespace ftl {
-namespace cuda {
- void sparse_census(const TextureObject<uchar4> &l, const TextureObject<uchar4> &r,
- TextureObject<uint2> &cl, TextureObject<uint2> &cr) {
- dim3 grid(1,1,1);
- dim3 threads(128, 1, 1);
- grid.x = cv::cuda::device::divUp(l.width(), 128);
- grid.y = cv::cuda::device::divUp(l.height(), 11);
-
-
- census_kernel<<<grid, threads>>>(
- l.cudaTexture(), r.cudaTexture(),
- l.width(), l.height(),
- (uint64_t*)cl.devicePtr(), (uint64_t*)cr.devicePtr(),
- cl.pitch()/sizeof(uint64_t),
- cr.pitch()/sizeof(uint64_t));
- cudaSafeCall( cudaGetLastError() );
- }
-}
-}
-
-
diff --git a/components/rgbd-sources/src/algorithms/tex_filter.cu b/components/rgbd-sources/src/algorithms/tex_filter.cu
deleted file mode 100644
index 12c9157267b9f433752462c70f8a6b7f071c628c..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/algorithms/tex_filter.cu
+++ /dev/null
@@ -1,101 +0,0 @@
-#include <ftl/cuda_common.hpp>
-
-#define FILTER_WINDOW 11.0
-#define FILTER_WINDOW_R 5
-
-// --- Filter by texture complexity --------------------------------------------
-
-__global__ void texture_filter_kernel(cudaTextureObject_t t, cudaTextureObject_t d,
- ftl::cuda::TextureObject<float> f, int num_disp, double thresh) { // Thresh = -5000000
-
- for (STRIDE_Y(v,f.height())) {
- for (STRIDE_X(u,f.width())) {
- float disp = tex2D<float>(d,u,v);
- double neigh_sq = 0.0;
- double neigh_sum = 0.0;
-
- for (int m=-FILTER_WINDOW_R; m<=FILTER_WINDOW_R; m++) {
- for (int n=-FILTER_WINDOW_R; n<=FILTER_WINDOW_R; n++) {
- uchar4 neigh = tex2D<uchar4>(t, u+n, v+m);
- neigh_sq += (double)(neigh.z*neigh.z);
- neigh_sum += (double)neigh.z;
- }
- }
-
- // Texture map filtering
- double tm = (neigh_sq / (FILTER_WINDOW*FILTER_WINDOW)) -
- ((neigh_sum / (FILTER_WINDOW*FILTER_WINDOW)) * (neigh_sum / (FILTER_WINDOW*FILTER_WINDOW)));
-
- if (tm >= thresh) {
- f(u,v) = disp;
- } else {
- f(u,v) = NAN; //(tm <= 200.0) ? 0 : 200.0f;;
- }
-
- //f(u,v) = tm;
- }
- }
-}
-
-namespace ftl {
-namespace cuda {
- void texture_filter(const TextureObject<uchar4> &t, const TextureObject<float> &d,
- TextureObject<float> &f, int num_disp, double thresh) {
- dim3 grid(1,1,1);
- dim3 threads(128, 1, 1);
- grid.x = cv::cuda::device::divUp(d.width(), 128);
- grid.y = cv::cuda::device::divUp(d.height(), 1);
- texture_filter_kernel<<<grid, threads>>>(
- t.cudaTexture(),
- d.cudaTexture(),
- f,
- num_disp,
- thresh);
- cudaSafeCall( cudaGetLastError() );
- }
-}
-}
-
-// --- Generate a texture map --------------------------------------------------
-
-__global__ void texture_map_kernel(cudaTextureObject_t t,
- ftl::cuda::TextureObject<float> f) {
-
- for (STRIDE_Y(v,f.height())) {
- for (STRIDE_X(u,f.width())) {
- double neigh_sq = 0.0;
- double neigh_sum = 0.0;
-
- for (int m=-FILTER_WINDOW_R; m<=FILTER_WINDOW_R; m++) {
- for (int n=-FILTER_WINDOW_R; n<=FILTER_WINDOW_R; n++) {
- uchar4 neigh = tex2D<uchar4>(t, u+n, v+m);
- neigh_sq += (double)(neigh.z*neigh.z);
- neigh_sum += (double)neigh.z;
- }
- }
-
- // Texture map filtering
- double tm = (neigh_sq / (FILTER_WINDOW*FILTER_WINDOW)) -
- ((neigh_sum / (FILTER_WINDOW*FILTER_WINDOW)) * (neigh_sum / (FILTER_WINDOW*FILTER_WINDOW)));
-
- f(u,v) = tm;
- }
- }
-}
-
-namespace ftl {
-namespace cuda {
- void texture_map(const TextureObject<uchar4> &t,
- TextureObject<float> &f) {
- dim3 grid(1,1,1);
- dim3 threads(128, 1, 1);
- grid.x = cv::cuda::device::divUp(f.width(), 128);
- grid.y = cv::cuda::device::divUp(f.height(), 1);
- texture_map_kernel<<<grid, threads>>>(
- t.cudaTexture(),
- f);
- cudaSafeCall( cudaGetLastError() );
- }
-}
-}
-
diff --git a/reconstruct/src/voxel_hash.cu b/reconstruct/src/voxel_hash.cu
deleted file mode 100644
index 6dfe838a71bb81e1407bc02ba48e06db0b39e70e..0000000000000000000000000000000000000000
--- a/reconstruct/src/voxel_hash.cu
+++ /dev/null
@@ -1,127 +0,0 @@
-#include <ftl/voxel_hash.hpp>
-
-using ftl::voxhash::VoxelHash;
-using ftl::voxhash::hash;
-using ftl::voxhash::HashBucket;
-using ftl::voxhash::HashEntry;
-
-__device__ HashEntry *ftl::voxhash::cuda::lookupBlock(HashBucket *table, int3 vox) {
- uint32_t hashcode = hash<HASH_SIZE>(vox.x / 8, vox.y / 8, vox.z / 8);
- HashBucket *bucket = table[hashcode];
-
- while (bucket) {
- for (int i=0; i<ENTRIES_PER_BUCKET; i++) {
- HashEntry *entry = &bucket->entries[i];
- if (entry->position[0] == vox.x && entry->position[1] == vox.y && entry->position[2] == vox.z) {
- return entry;
- }
- }
- if (bucket->entries[ENTRIES_PER_BUCKET-1].offset) {
- bucket += bucket->entries[ENTRIES_PER_BUCKET-1].offset;
- } else {
- break;
- }
- }
-
- return nullptr;
-}
-
-__device__ HashEntry *ftl::voxhash::cuda::insertBlock(HashBucket *table, int3 vox) {
- uint32_t hashcode = hash<HASH_SIZE>(vox.x / 8, vox.y / 8, vox.z / 8);
- HashBucket *bucket = table[hashcode];
-
- while (bucket) {
- for (int i=0; i<ENTRIES_PER_BUCKET; i++) {
- HashEntry *entry = &bucket->entries[i];
- if (entry->position[0] == vox.x && entry->position[1] == vox.y && entry->position[2] == vox.z) {
- return entry;
- } else if (entry->pointer == 0) {
- // Allocate block.
- entry->position[0] = vox.x;
- entry->position[1] = vox.y;
- entry->position[2] = vox.z;
- }
- }
- if (bucket->entries[ENTRIES_PER_BUCKET-1].offset) {
- bucket += bucket->entries[ENTRIES_PER_BUCKET-1].offset;
- } else {
- // Find a new bucket to append to this list
- }
- }
-
- return nullptr;
-}
-
-__device__ Voxel *ftl::voxhash::cuda::insert(HashBucket *table, VoxelBlocks *blocks, int3 vox) {
- HashEntry *entry = insertBlock(table, vox);
- VoxelBlock *block = blocks[entry->pointer];
- return &block->voxels[vox.x % 8][vox.y % 8][vox.z % 8];
-}
-
-__device__ Voxel *ftl::voxhash::cuda::lookup(HashBucket *table, int3 vox) {
- HashEntry *entry = lookupBlock(table, vox);
- if (entry == nullptr) return nullptr;
- VoxelBlock *block = blocks[entry->pointer];
- return &block->voxels[vox.x % 8][vox.y % 8][vox.z % 8];
-}
-
-VoxelHash::VoxelHash(float resolution) : resolution_(resolution) {
- // Allocate CPU memory
- table_cpu_.resize(HASH_SIZE);
-
- // Allocate and init GPU memory
- table_gpu_ = table_cpu_;
-}
-
-VoxelHash::~VoxelHash() {
-
-}
-
-void VoxelHash::nextFrame() {
- // Clear the hash for now...
-}
-
-__global__ static void merge_view_kernel(PtrStepSz<cv::Point3f> cloud, PtrStepSz<uchar3> rgb, PtrStepSz<double> pose, ftl::voxhash::cuda::VoxelHash vhash) {
- for (STRIDE_Y(v,cloud.rows) {
- for (STRIDE_X(u,cloud.cols) {
- cv::Point3f p = cloud.at<cv::Point3f>(v,u);
- // TODO Lock hash block on insert.
- Voxel *voxel = ftl::voxhash::cuda::insert(vhash.table, make_int3(p.x,p.y,p.z));
- uchar3 colour = rgb.at<uchar3>(v,u);
- voxel->colorRGB[0] = colour[0];
- voxel->colorRGB[1] = colour[1];
- voxel->colorRGB[2] = colour[2];
- voxel->weight = 1;
- }
- }
-}
-
-static void merge_view_call(const PtrStepSz<cv::Point3f> &cloud, const PtrStepSz<uchar3> &rgb, const PtrStepSz<double> &pose, ftl::voxhash::cuda::VoxelHash vhash) {
- dim3 grid(1,1,1);
- dim3 threads(128, 1, 1);
- grid.x = cv::cuda::device::divUp(l.cols, 128);
- grid.y = cv::cuda::device::divUp(l.rows, 21);
-
- merge_view_kernel<<<grid,threads>>>(cloud, rgb, pose, vhash);
- cudaSafeCall( cudaGetLastError() );
-}
-
-void VoxelHash::mergeView(const cv::Mat &cloud, const cv::Mat &rgb, const cv::Mat &pose) {
- // Upload GPU Mat
- gpu_cloud_.upload(cloud);
- gpu_rgb_.upload(rgb);
- gpu_pose_.upload(pose);
-
- // Call kernel to insert each point in hash table
- merge_view_call(gpu_cloud_, gpu_rgb_, gpu_pose_, getCUDAStructure());
-}
-
-// Called per voxel
-__global__ static void render_kernel(PtrStepSz<uchar3> out, PtrStepSz<double> cam, ftl::voxhash::cuda::VoxelHash vhash) {
-
-}
-
-void VoxelHash::render(cv::Mat &output, const cv::Mat &cam) {
- // Call kernel to process every voxel and add to image
- // Download gpu mat into output.
-}