diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 28e09eeb330a07a43e139e475f0350fcc6cfac9f..512a4d19087bd66c5c895f3f4c5595190122132c 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -25,7 +25,7 @@ linux:
script:
- mkdir build
- cd build
- - cmake ..
+ - cmake .. -DWITH_OPTFLOW=TRUE -DBUILD_CALIBRATION=TRUE
- make
- ctest --output-on-failure
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 95530e23a154175eac8a303897f73a91fe6432f5..b0aecb25a357a5c978db35a80cb20487adb5d08d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -40,6 +40,19 @@ if (LibArchive_FOUND)
set(HAVE_LIBARCHIVE true)
endif()
+## OpenVR API path
+find_library(OPENVR_LIBRARIES
+ NAMES
+ openvr_api
+)
+set(OPENVR_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../headers)
+
+if (OPENVR_LIBRARIES)
+ message(STATUS "Found OpenVR: ${OPENVR_LIBRARIES}")
+ set(HAVE_OPENVR true)
+endif()
+
+
if (WITH_FIXSTARS)
find_package( LibSGM )
if (LibSGM_FOUND)
@@ -175,7 +188,7 @@ check_include_file_cxx("opencv2/cudastereo.hpp" HAVE_OPENCVCUDA)
find_program(CPPCHECK_FOUND cppcheck)
if (CPPCHECK_FOUND)
message(STATUS "Found cppcheck: will perform source checks")
- set(CMAKE_CXX_CPPCHECK "cppcheck" "--enable=warning,performance,portability,style" "--inline-suppr" "--std=c++11" "--suppress=*:*catch.hpp" "--suppress=*:*elas*" "--suppress=*:*nanogui*" "--suppress=*:*json.hpp" "--quiet")
+ set(CMAKE_CXX_CPPCHECK "cppcheck" "-D__align__(A)" "-DCUDARTAPI" "--enable=warning,performance,style" "--inline-suppr" "--std=c++14" "--suppress=*:*catch.hpp" "--suppress=*:*elas*" "--suppress=*:*nanogui*" "--suppress=*:*json.hpp" "--quiet")
endif()
# include_directories(${PROJECT_SOURCE_DIR}/common/cpp/include)
diff --git a/applications/calibration-multi/src/main.cpp b/applications/calibration-multi/src/main.cpp
index f9c258d7665933b510e6dc5af08e6c499e16ec91..ea770f69b03981164c2b6c97f95aa8841f1d8ae7 100644
--- a/applications/calibration-multi/src/main.cpp
+++ b/applications/calibration-multi/src/main.cpp
@@ -37,6 +37,7 @@ using cv::Vec4d;
using ftl::net::Universe;
using ftl::rgbd::Source;
+using ftl::rgbd::Channel;
Mat getCameraMatrix(const ftl::rgbd::Camera ¶meters) {
Mat m = (cv::Mat_<double>(3,3) << parameters.fx, 0.0, -parameters.cx, 0.0, parameters.fy, -parameters.cy, 0.0, 0.0, 1.0);
@@ -97,7 +98,7 @@ bool loadRegistration(const string &ifile, map<string, Eigen::Matrix4d> &data) {
//
-bool saveIntrinsics(const string &ofile, const vector<Mat> &M) {
+bool saveIntrinsics(const string &ofile, const vector<Mat> &M, const Size &size) {
vector<Mat> D;
{
cv::FileStorage fs(ofile, cv::FileStorage::READ);
@@ -107,6 +108,7 @@ bool saveIntrinsics(const string &ofile, const vector<Mat> &M) {
{
cv::FileStorage fs(ofile, cv::FileStorage::WRITE);
if (fs.isOpened()) {
+ fs << "resolution" << size;
fs << "K" << M << "D" << D;
fs.release();
return true;
@@ -195,6 +197,7 @@ struct CalibrationParams {
bool optimize_intrinsic = false;
int reference_camera = -1;
double alpha = 0.0;
+ Size size;
};
void calibrate( MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
@@ -232,8 +235,8 @@ void calibrate( MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
Mat R_c1c2, T_c1c2;
calculateTransform(R[c], t[c], R[c + 1], t[c + 1], R_c1c2, T_c1c2);
- cv::stereoRectify(K1, D1, K2, D2, Size(1280, 720), R_c1c2, T_c1c2, R1, R2, P1, P2, Q, 0, params.alpha);
-
+ cv::stereoRectify(K1, D1, K2, D2, params.size, R_c1c2, T_c1c2, R1, R2, P1, P2, Q, 0, params.alpha);
+
Mat _t = Mat(Size(1, 3), CV_64FC1, Scalar(0.0));
Rt_out[c] = getMat4x4(R[c], t[c]) * getMat4x4(R1, _t).inv();
Rt_out[c + 1] = getMat4x4(R[c + 1], t[c + 1]) * getMat4x4(R2, _t).inv();
@@ -243,15 +246,20 @@ void calibrate( MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
size_t pos1 = uri_cameras[c/2].find("node");
size_t pos2 = uri_cameras[c/2].find("#", pos1);
node_name = uri_cameras[c/2].substr(pos1, pos2 - pos1);
- //LOG(INFO) << c << ":" << calib.getCameraMatNormalized(c, 1280, 720);
- //LOG(INFO) << c + 1 << ":" << calib.getCameraMatNormalized(c + 1, 1280, 720);
+
if (params.save_extrinsic) {
+ // TODO: only R and T required, rectification performed on vision node,
+ // consider saving global extrinsic calibration?
saveExtrinsics(params.output_path + node_name + "-extrinsic.yml", R_c1c2, T_c1c2, R1, R2, P1, P2, Q);
LOG(INFO) << "Saved: " << params.output_path + node_name + "-extrinsic.yml";
}
if (params.save_intrinsic) {
- saveIntrinsics(params.output_path + node_name + "-intrinsic.yml",
- {calib.getCameraMatNormalized(c, 1280, 720), calib.getCameraMatNormalized(c + 1, 1280, 720)}
+ saveIntrinsics(
+ params.output_path + node_name + "-intrinsic.yml",
+ {calib.getCameraMat(c),
+ calib.getCameraMat(c + 1)},
+ params.size
+
);
LOG(INFO) << "Saved: " << params.output_path + node_name + "-intrinsic.yml";
}
@@ -259,9 +267,9 @@ void calibrate( MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
// for visualization
Size new_size;
- cv::stereoRectify(K1, D1, K2, D2, Size(1280, 720), R_c1c2, T_c1c2, R1, R2, P1, P2, Q, 0, 1.0, new_size, &roi[c], &roi[c + 1]);
- cv::initUndistortRectifyMap(K1, D1, R1, P1, Size(1280, 720), CV_16SC2, map1[c], map2[c]);
- cv::initUndistortRectifyMap(K2, D2, R2, P2, Size(1280, 720), CV_16SC2, map1[c + 1], map2[c + 1]);
+ cv::stereoRectify(K1, D1, K2, D2, params.size, R_c1c2, T_c1c2, R1, R2, P1, P2, Q, 0, 1.0, new_size, &roi[c], &roi[c + 1]);
+ cv::initUndistortRectifyMap(K1, D1, R1, P1, params.size, CV_16SC2, map1[c], map2[c]);
+ cv::initUndistortRectifyMap(K2, D2, R2, P2, params.size, CV_16SC2, map1[c + 1], map2[c + 1]);
}
{
@@ -297,6 +305,7 @@ void calibrateFromPath( const string &path,
vector<string> uri_cameras;
cv::FileStorage fs(path + filename, cv::FileStorage::READ);
fs["uri"] >> uri_cameras;
+ fs["resolution"] >> params.size;
//params.idx_cameras = {2, 3};//{0, 1, 4, 5, 6, 7, 8, 9, 10, 11};
params.idx_cameras.resize(uri_cameras.size() * 2);
@@ -361,11 +370,11 @@ void runCameraCalibration( ftl::Configurable* root,
const size_t n_sources = sources.size();
const size_t n_cameras = n_sources * 2;
size_t reference_camera = 0;
- Size resolution;
+
{
- auto params = sources[0]->parameters();
- resolution = Size(params.width, params.height);
- LOG(INFO) << "Camera resolution: " << resolution;
+ auto camera = sources[0]->parameters();
+ params.size = Size(camera.width, camera.height);
+ LOG(INFO) << "Camera resolution: " << params.size;
}
params.idx_cameras.resize(n_cameras);
@@ -373,29 +382,29 @@ void runCameraCalibration( ftl::Configurable* root,
// TODO: parameter for calibration target type
auto calib = MultiCameraCalibrationNew( n_cameras, reference_camera,
- resolution, CalibrationTarget(0.250)
+ params.size, CalibrationTarget(0.250)
);
for (size_t i = 0; i < n_sources; i++) {
- auto params_r = sources[i]->parameters(ftl::rgbd::kChanRight);
- auto params_l = sources[i]->parameters(ftl::rgbd::kChanLeft);
+ auto camera_r = sources[i]->parameters(Channel::Right);
+ auto camera_l = sources[i]->parameters(Channel::Left);
- CHECK(resolution == Size(params_r.width, params_r.height));
- CHECK(resolution == Size(params_l.width, params_l.height));
+ CHECK(params.size == Size(camera_r.width, camera_r.height));
+ CHECK(params.size == Size(camera_l.width, camera_l.height));
Mat K;
- K = getCameraMatrix(params_r);
+ K = getCameraMatrix(camera_r);
LOG(INFO) << "K[" << 2 * i + 1 << "] = \n" << K;
calib.setCameraParameters(2 * i + 1, K);
- K = getCameraMatrix(params_l);
+ K = getCameraMatrix(camera_l);
LOG(INFO) << "K[" << 2 * i << "] = \n" << K;
calib.setCameraParameters(2 * i, K);
}
ftl::rgbd::Group group;
for (Source* src : sources) {
- src->setChannel(ftl::rgbd::kChanRight);
+ src->setChannel(Channel::Right);
group.addSource(src);
}
@@ -408,14 +417,14 @@ void runCameraCalibration( ftl::Configurable* root,
group.sync([&mutex, &new_frames, &rgb_new](ftl::rgbd::FrameSet &frames) {
mutex.lock();
bool good = true;
- for (size_t i = 0; i < frames.channel1.size(); i ++) {
- if (frames.channel1[i].empty()) good = false;
- if (frames.channel1[i].empty()) good = false;
- if (frames.channel1[i].channels() != 3) good = false; // ASSERT
- if (frames.channel2[i].channels() != 3) good = false;
+ for (size_t i = 0; i < frames.sources.size(); i ++) {
+ if (frames.frames[i].get<cv::Mat>(Channel::Left).empty()) good = false;
+ if (frames.frames[i].get<cv::Mat>(Channel::Right).empty()) good = false;
+ if (frames.frames[i].get<cv::Mat>(Channel::Left).channels() != 3) good = false; // ASSERT
+ if (frames.frames[i].get<cv::Mat>(Channel::Right).channels() != 3) good = false;
if (!good) break;
- cv::swap(frames.channel1[i], rgb_new[2 * i]);
- cv::swap(frames.channel2[i], rgb_new[2 * i + 1]);
+ cv::swap(frames.frames[i].get<cv::Mat>(Channel::Left), rgb_new[2 * i]);
+ cv::swap(frames.frames[i].get<cv::Mat>(Channel::Right), rgb_new[2 * i + 1]);
}
new_frames = good;
@@ -550,7 +559,8 @@ int main(int argc, char **argv) {
params.optimize_intrinsic = optimize_intrinsic;
params.output_path = output_directory;
params.registration_file = registration_file;
-
+ params.reference_camera = ref_camera;
+
LOG(INFO) << "\n"
<< "\nIMPORTANT: Remeber to set \"use_intrinsics\" to false for nodes!"
<< "\n"
diff --git a/applications/calibration-multi/src/multicalibrate.cpp b/applications/calibration-multi/src/multicalibrate.cpp
index 2292d178d70f0f7c68b79cbbf1cb9172cdd173be..9d85d1af7ae938981bb445a852fd85a913c6e26e 100644
--- a/applications/calibration-multi/src/multicalibrate.cpp
+++ b/applications/calibration-multi/src/multicalibrate.cpp
@@ -651,6 +651,7 @@ double MultiCameraCalibrationNew::calibrateAll(int reference_camera) {
if (calibratePair(c1, c2, R, t) > 16.0) {
LOG(ERROR) << "Pairwise calibration failed, skipping cameras "
<< c1 << " and " << c2;
+ visibility_graph_.deleteEdge(c1, c2);
continue;
}
diff --git a/applications/calibration-multi/src/visibility.cpp b/applications/calibration-multi/src/visibility.cpp
index bd001f60371d02aa7e5b76a8f060950f05b42db3..a4c16165b47decadfa9ca18ccf641a32b1cb16c1 100644
--- a/applications/calibration-multi/src/visibility.cpp
+++ b/applications/calibration-multi/src/visibility.cpp
@@ -49,6 +49,12 @@ int Visibility::getOptimalCamera() {
return best_i;
}
+void Visibility::deleteEdge(int camera1, int camera2)
+{
+ visibility_.at<int>(camera1, camera2) = 0;
+ visibility_.at<int>(camera2, camera1) = 0;
+}
+
int Visibility::getMinVisibility() {
int min_i;
int min_count = INT_MAX;
diff --git a/applications/calibration-multi/src/visibility.hpp b/applications/calibration-multi/src/visibility.hpp
index dcf7e71ddb29304c5a0825e310e566ef3622cd55..3521435dd62e2cd6eeea417f926b02f8049eb560 100644
--- a/applications/calibration-multi/src/visibility.hpp
+++ b/applications/calibration-multi/src/visibility.hpp
@@ -27,6 +27,7 @@ public:
vector<vector<pair<int, int>>> findShortestPaths(int reference);
vector<int> getClosestCameras(int c);
+ void deleteEdge(int camera1, int camera2);
int getOptimalCamera();
int getMinVisibility();
int getViewsCount(int camera);
diff --git a/applications/calibration/src/common.cpp b/applications/calibration/src/common.cpp
index 0098bba58c5e6cbcbc0b75185f4286894447f16c..8a678e4b9ba808dccea146c2ce4c8c1c6942a7f0 100644
--- a/applications/calibration/src/common.cpp
+++ b/applications/calibration/src/common.cpp
@@ -62,20 +62,24 @@ bool saveExtrinsics(const string &ofile, Mat &R, Mat &T, Mat &R1, Mat &R2, Mat &
return false;
}
-bool saveIntrinsics(const string &ofile, const vector<Mat> &M, const vector<Mat>& D) {
+bool saveIntrinsics(const string &ofile, const vector<Mat> &M, const vector<Mat>& D, const Size &size)
+{
cv::FileStorage fs(ofile, cv::FileStorage::WRITE);
- if (fs.isOpened()) {
+ if (fs.isOpened())
+ {
+ fs << "resolution" << size;
fs << "K" << M << "D" << D;
fs.release();
return true;
}
- else {
+ else
+ {
LOG(ERROR) << "Error: can not save the intrinsic parameters to '" << ofile << "'";
}
return false;
}
-bool loadIntrinsics(const string &ifile, vector<Mat> &K1, vector<Mat> &D1) {
+bool loadIntrinsics(const string &ifile, vector<Mat> &K1, vector<Mat> &D1, Size &size) {
using namespace cv;
FileStorage fs;
@@ -89,9 +93,10 @@ bool loadIntrinsics(const string &ifile, vector<Mat> &K1, vector<Mat> &D1) {
LOG(INFO) << "Intrinsics from: " << ifile;
+ fs["resolution"] >> size;
fs["K"] >> K1;
fs["D"] >> D1;
-
+
return true;
}
@@ -211,6 +216,23 @@ bool CalibrationChessboard::findPoints(Mat &img, vector<Vec2f> &points) {
return cv::findChessboardCornersSB(img, pattern_size_, points, chessboard_flags_);
}
+
+void CalibrationChessboard::drawCorners(Mat &img, const vector<Vec2f> &points) {
+ using cv::Point2i;
+ vector<Point2i> corners(4);
+ corners[1] = Point2i(points[0]);
+ corners[0] = Point2i(points[pattern_size_.width - 1]);
+ corners[2] = Point2i(points[pattern_size_.width * (pattern_size_.height - 1)]);
+ corners[3] = Point2i(points.back());
+
+ cv::Scalar color = cv::Scalar(200, 200, 200);
+
+ for (int i = 0; i <= 4; i++)
+ {
+ cv::line(img, corners[i % 4], corners[(i + 1) % 4], color, 2);
+ }
+}
+
void CalibrationChessboard::drawPoints(Mat &img, const vector<Vec2f> &points) {
cv::drawChessboardCorners(img, pattern_size_, points, true);
}
diff --git a/applications/calibration/src/common.hpp b/applications/calibration/src/common.hpp
index 274698b32b51ae9b741b94e25b492ab059637e37..c84f25d249b49eee094e3e898090ffb9ff129f03 100644
--- a/applications/calibration/src/common.hpp
+++ b/applications/calibration/src/common.hpp
@@ -15,8 +15,8 @@ int getOptionInt(const std::map<std::string, std::string> &options, const std::s
double getOptionDouble(const std::map<std::string, std::string> &options, const std::string &opt, double default_value);
std::string getOptionString(const std::map<std::string, std::string> &options, const std::string &opt, std::string default_value);
-bool loadIntrinsics(const std::string &ifile, std::vector<cv::Mat> &K, std::vector<cv::Mat> &D);
-bool saveIntrinsics(const std::string &ofile, const std::vector<cv::Mat> &K, const std::vector<cv::Mat> &D);
+bool loadIntrinsics(const std::string &ifile, std::vector<cv::Mat> &K, std::vector<cv::Mat> &D, cv::Size &size);
+bool saveIntrinsics(const std::string &ofile, const std::vector<cv::Mat> &K, const std::vector<cv::Mat> &D, const cv::Size &size);
// TODO loadExtrinsics()
bool saveExtrinsics(const std::string &ofile, cv::Mat &R, cv::Mat &T, cv::Mat &R1, cv::Mat &R2, cv::Mat &P1, cv::Mat &P2, cv::Mat &Q);
@@ -90,10 +90,11 @@ public:
*/
class CalibrationChessboard : Calibration {
public:
- CalibrationChessboard(const std::map<std::string, std::string> &opt);
+ explicit CalibrationChessboard(const std::map<std::string, std::string> &opt);
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);
+ void drawCorners(cv::Mat &img, const std::vector<cv::Vec2f> &points);
private:
int chessboard_flags_ = 0;
diff --git a/applications/calibration/src/lens.cpp b/applications/calibration/src/lens.cpp
index f793dbfc6907b79ac65dfcbcf4a921884e6ca5cc..b69b26cc6e4cfec9dab04d337d75b21721d2fbae 100644
--- a/applications/calibration/src/lens.cpp
+++ b/applications/calibration/src/lens.cpp
@@ -14,6 +14,8 @@
#include <opencv2/highgui.hpp>
#include <vector>
+#include <atomic>
+#include <thread>
using std::map;
using std::string;
@@ -30,8 +32,8 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
LOG(INFO) << "Begin intrinsic calibration";
// TODO PARAMETERS TO CONFIG FILE
- const Size image_size = Size( getOptionInt(opt, "width", 1280),
- getOptionInt(opt, "height", 720));
+ const Size image_size = Size( getOptionInt(opt, "width", 1920),
+ getOptionInt(opt, "height", 1080));
const int n_cameras = getOptionInt(opt, "n_cameras", 2);
const int iter = getOptionInt(opt, "iter", 40);
const int delay = getOptionInt(opt, "delay", 1000);
@@ -39,18 +41,21 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
const double aperture_height = getOptionDouble(opt, "aperture_height", 4.6);
const string filename_intrinsics = getOptionString(opt, "profile", FTL_LOCAL_CONFIG_ROOT "/intrinsics.yml");
CalibrationChessboard calib(opt);
- const bool use_guess = getOptionInt(opt, "use_guess", 1);
+ bool use_guess = getOptionInt(opt, "use_guess", 1);
+ //bool use_guess_distortion = getOptionInt(opt, "use_guess_distortion", 0);
LOG(INFO) << "Intrinsic calibration parameters";
- LOG(INFO) << " profile: " << filename_intrinsics;
- LOG(INFO) << " n_cameras: " << n_cameras;
- LOG(INFO) << " width: " << image_size.width;
- LOG(INFO) << " height: " << image_size.height;
- LOG(INFO) << " iter: " << iter;
- LOG(INFO) << " delay: " << delay;
- LOG(INFO) << " aperture_width: " << aperture_width;
- LOG(INFO) << " aperture_height: " << aperture_height;
- LOG(INFO) << " use_guess: " << use_guess;
+ LOG(INFO) << " profile: " << filename_intrinsics;
+ LOG(INFO) << " n_cameras: " << n_cameras;
+ LOG(INFO) << " width: " << image_size.width;
+ LOG(INFO) << " height: " << image_size.height;
+ LOG(INFO) << " iter: " << iter;
+ LOG(INFO) << " delay: " << delay;
+ LOG(INFO) << " aperture_width: " << aperture_width;
+ LOG(INFO) << " aperture_height: " << aperture_height;
+ LOG(INFO) << " use_guess: " << use_guess;
+ //LOG(INFO) << " use_guess_distortion: " << use_guess_distortion;
+
LOG(INFO) << "-----------------------------------";
int calibrate_flags = cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_ASPECT_RATIO;
@@ -61,18 +66,39 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
vector<Mat> camera_matrix(n_cameras), dist_coeffs(n_cameras);
- if (use_guess) {
+ for (Mat &d : dist_coeffs)
+ {
+ d = Mat(Size(5, 1), CV_64FC1, cv::Scalar(0.0));
+ }
+
+ if (use_guess)
+ {
camera_matrix.clear();
vector<Mat> tmp;
- //dist_coeffs.clear();
- loadIntrinsics(filename_intrinsics, camera_matrix, tmp);
+ Size tmp_size;
+
+ loadIntrinsics(filename_intrinsics, camera_matrix, tmp, tmp_size);
CHECK(camera_matrix.size() == n_cameras); // (camera_matrix.size() == dist_coeffs.size())
+ if ((tmp_size != image_size) && (!tmp_size.empty()))
+ {
+ Mat scale = Mat::eye(Size(3, 3), CV_64FC1);
+ scale.at<double>(0, 0) = ((double) image_size.width) / ((double) tmp_size.width);
+ scale.at<double>(1, 1) = ((double) image_size.height) / ((double) tmp_size.height);
+ for (Mat &K : camera_matrix) { K = scale * K; }
+ }
+
+ if (tmp_size.empty())
+ {
+ use_guess = false;
+ LOG(FATAL) << "No valid calibration found.";
+ }
}
vector<cv::VideoCapture> cameras;
cameras.reserve(n_cameras);
for (int c = 0; c < n_cameras; c++) { cameras.emplace_back(c); }
- for (auto &camera : cameras) {
+ for (auto &camera : cameras)
+ {
if (!camera.isOpened()) {
LOG(ERROR) << "Could not open camera device";
return;
@@ -83,41 +109,102 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
vector<vector<vector<Vec2f>>> image_points(n_cameras);
vector<vector<vector<Vec3f>>> object_points(n_cameras);
+
vector<Mat> img(n_cameras);
+ vector<Mat> img_display(n_cameras);
vector<int> count(n_cameras, 0);
+ Mat display(Size(image_size.width * n_cameras, image_size.height), CV_8UC3);
- while (iter > *std::min_element(count.begin(), count.end())) {
+ for (int c = 0; c < n_cameras; c++)
+ {
+ img_display[c] = Mat(display, cv::Rect(c * image_size.width, 0, image_size.width, image_size.height));
+ }
- for (auto &camera : cameras) { camera.grab(); }
+ std::mutex m;
+ std::atomic<bool> ready = false;
+ auto capture = std::thread([n_cameras, delay, &m, &ready, &count, &calib, &img, &image_points, &object_points]()
+ {
+ vector<Mat> tmp(n_cameras);
+ while(true)
+ {
+ if (!ready)
+ {
+ std::this_thread::sleep_for(std::chrono::milliseconds(delay));
+ continue;
+ }
- for (int c = 0; c < n_cameras; c++) {
- vector<Vec2f> points;
- cameras[c].retrieve(img[c]);
-
- if (calib.findPoints(img[c], points)) {
- calib.drawPoints(img[c], points);
- count[c]++;
+ m.lock();
+ ready = false;
+ for (int c = 0; c < n_cameras; c++)
+ {
+ img[c].copyTo(tmp[c]);
+ }
+ m.unlock();
+
+ for (int c = 0; c < n_cameras; c++)
+ {
+ vector<Vec2f> points;
+ if (calib.findPoints(tmp[c], points))
+ {
+ count[c]++;
+ }
+ else { continue; }
+
+ vector<Vec3f> points_ref;
+ calib.objectPoints(points_ref);
+ Mat camera_matrix, dist_coeffs;
+ image_points[c].push_back(points);
+ object_points[c].push_back(points_ref);
}
- else { continue; }
-
- vector<Vec3f> points_ref;
- calib.objectPoints(points_ref);
- Mat camera_matrix, dist_coeffs;
- vector<Mat> rvecs, tvecs;
-
- image_points[c].push_back(points);
- object_points[c].push_back(points_ref);
+ std::this_thread::sleep_for(std::chrono::milliseconds(delay));
+ }
+ });
+
+ while (iter > *std::min_element(count.begin(), count.end()))
+ {
+ if (m.try_lock())
+ {
+ for (auto &camera : cameras) { camera.grab(); }
+
+ for (int c = 0; c < n_cameras; c++)
+ {
+ cameras[c].retrieve(img[c]);
+ }
+
+ ready = true;
+ m.unlock();
}
- for (int c = 0; c < n_cameras; c++) {
- cv::imshow("Camera " + std::to_string(c), img[c]);
+ for (int c = 0; c < n_cameras; c++)
+ {
+ img[c].copyTo(img_display[c]);
+ m.lock();
+
+ if (image_points[c].size() > 0)
+ {
+
+ for (auto &points : image_points[c])
+ {
+ calib.drawCorners(img_display[c], points);
+ }
+
+ calib.drawPoints(img_display[c], image_points[c].back());
+ }
+
+ m.unlock();
}
- cv::waitKey(delay);
+ cv::namedWindow("Cameras", cv::WINDOW_KEEPRATIO | cv::WINDOW_NORMAL);
+ cv::imshow("Cameras", display);
+
+ cv::waitKey(10);
}
- for (int c = 0; c < n_cameras; c++) {
+ cv::destroyAllWindows();
+
+ for (int c = 0; c < n_cameras; c++)
+ {
LOG(INFO) << "Calculating intrinsic paramters for camera " << std::to_string(c);
vector<Mat> rvecs, tvecs;
@@ -148,7 +235,7 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
LOG(INFO) << "";
}
- saveIntrinsics(filename_intrinsics, camera_matrix, dist_coeffs);
+ saveIntrinsics(filename_intrinsics, camera_matrix, dist_coeffs, image_size);
LOG(INFO) << "intrinsic paramaters saved to: " << filename_intrinsics;
vector<Mat> map1(n_cameras), map2(n_cameras);
diff --git a/applications/calibration/src/stereo.cpp b/applications/calibration/src/stereo.cpp
index 0d2e1636996438ec9d241f227ff1aab003406f2a..964cf815300971cf31130e78fae94e1c21a172ad 100644
--- a/applications/calibration/src/stereo.cpp
+++ b/applications/calibration/src/stereo.cpp
@@ -93,11 +93,17 @@ void ftl::calibration::stereo(map<string, string> &opt) {
vector<Mat> dist_coeffs(2);
vector<Mat> camera_matrices(2);
-
- if (!loadIntrinsics(filename_intrinsics, camera_matrices, dist_coeffs)) {
+ Size intrinsic_resolution;
+ if (!loadIntrinsics(filename_intrinsics, camera_matrices, dist_coeffs, intrinsic_resolution))
+ {
LOG(FATAL) << "Failed to load intrinsic camera parameters from file.";
}
+ if (intrinsic_resolution != image_size)
+ {
+ LOG(FATAL) << "Intrinsic resolution is not same as input resolution (TODO)";
+ }
+
Mat R, T, E, F, per_view_errors;
// capture calibration patterns
diff --git a/applications/groupview/src/main.cpp b/applications/groupview/src/main.cpp
index 3a821a01323fbe51dbc8b6fdef98597f0bd20853..6b32221ef1a13ad05f9e5bb915c1186eca0aa52c 100644
--- a/applications/groupview/src/main.cpp
+++ b/applications/groupview/src/main.cpp
@@ -16,6 +16,7 @@ using std::string;
using std::vector;
using cv::Size;
using cv::Mat;
+using ftl::rgbd::Channel;
// TODO: remove code duplication (function from reconstruction)
static void from_json(nlohmann::json &json, map<string, Matrix4d> &transformations) {
@@ -79,7 +80,7 @@ void modeLeftRight(ftl::Configurable *root) {
ftl::rgbd::Group group;
for (auto* src : sources) {
- src->setChannel(ftl::rgbd::kChanRight);
+ src->setChannel(Channel::Right);
group.addSource(src);
}
@@ -90,15 +91,17 @@ void modeLeftRight(ftl::Configurable *root) {
group.sync([&mutex, &new_frames, &rgb_new](ftl::rgbd::FrameSet &frames) {
mutex.lock();
bool good = true;
- for (size_t i = 0; i < frames.channel1.size(); i ++) {
- if (frames.channel1[i].empty()) good = false;
- if (frames.channel1[i].empty()) good = false;
- if (frames.channel1[i].channels() != 3) good = false; // ASSERT
- if (frames.channel2[i].channels() != 3) good = false;
+ for (size_t i = 0; i < frames.frames.size(); i ++) {
+ auto &chan1 = frames.frames[i].get<cv::Mat>(Channel::Colour);
+ auto &chan2 = frames.frames[i].get<cv::Mat>(frames.sources[i]->getChannel());
+ if (chan1.empty()) good = false;
+ if (chan2.empty()) good = false;
+ if (chan1.channels() != 3) good = false; // ASSERT
+ if (chan2.channels() != 3) good = false;
if (!good) break;
- frames.channel1[i].copyTo(rgb_new[2 * i]);
- frames.channel2[i].copyTo(rgb_new[2 * i + 1]);
+ chan1.copyTo(rgb_new[2 * i]);
+ chan2.copyTo(rgb_new[2 * i + 1]);
}
new_frames = good;
@@ -107,11 +110,12 @@ void modeLeftRight(ftl::Configurable *root) {
});
int idx = 0;
- int key;
Mat show;
while (ftl::running) {
+ int key;
+
while (!new_frames) {
for (auto src : sources) { src->grab(30); }
key = cv::waitKey(10);
@@ -164,7 +168,7 @@ void modeFrame(ftl::Configurable *root, int frames=1) {
} else {
s->setPose(T->second);
}
- s->setChannel(ftl::rgbd::kChanDepth);
+ s->setChannel(Channel::Depth);
group.addSource(s);
}
@@ -180,14 +184,19 @@ void modeFrame(ftl::Configurable *root, int frames=1) {
//LOG(INFO) << "Complete set: " << fs.timestamp;
if (!ftl::running) { return false; }
+ std::vector<cv::Mat> frames;
for (size_t i=0; i<fs.sources.size(); ++i) {
- if (fs.channel1[i].empty() || fs.channel2[i].empty()) return true;
+ auto &chan1 = fs.frames[i].get<cv::Mat>(Channel::Colour);
+ auto &chan2 = fs.frames[i].get<cv::Mat>(fs.sources[i]->getChannel());
+ if (chan1.empty() || chan2.empty()) return true;
+
+ frames.push_back(chan1);
}
cv::Mat show;
- stack(fs.channel1, show);
+ stack(frames, show);
cv::resize(show, show, cv::Size(1280,720));
cv::namedWindow("Cameras", cv::WINDOW_KEEPRATIO | cv::WINDOW_NORMAL);
@@ -206,9 +215,12 @@ void modeFrame(ftl::Configurable *root, int frames=1) {
auto writer = ftl::rgbd::SnapshotWriter(std::string(timestamp) + ".tar.gz");
for (size_t i=0; i<fs.sources.size(); ++i) {
+ auto &chan1 = fs.frames[i].get<cv::Mat>(Channel::Colour);
+ auto &chan2 = fs.frames[i].get<cv::Mat>(fs.sources[i]->getChannel());
+
writer.addSource(fs.sources[i]->getURI(), fs.sources[i]->parameters(), fs.sources[i]->getPose());
- LOG(INFO) << "SAVE: " << fs.channel1[i].cols << ", " << fs.channel2[i].type();
- writer.addRGBD(i, fs.channel1[i], fs.channel2[i]);
+ //LOG(INFO) << "SAVE: " << fs.channel1[i].cols << ", " << fs.channel2[i].type();
+ writer.addRGBD(i, chan1, chan2);
}
}
#endif // HAVE_LIBARCHIVE
@@ -248,7 +260,7 @@ void modeVideo(ftl::Configurable *root) {
auto sources = ftl::createArray<ftl::rgbd::Source>(root, "sources", net);
const string path = root->value<string>("save_to", "./");
- for (auto* src : sources) { src->setChannel(ftl::rgbd::kChanDepth); }
+ for (auto* src : sources) { src->setChannel(Channel::Depth); }
cv::Mat show;
vector<cv::Mat> rgb(sources.size());
diff --git a/applications/gui/CMakeLists.txt b/applications/gui/CMakeLists.txt
index baffb9bdd645c85cbfc593d81e14687965b9bf4c..fbed0680dde997c0f0a76519ac272fb3e5c1c64f 100644
--- a/applications/gui/CMakeLists.txt
+++ b/applications/gui/CMakeLists.txt
@@ -27,6 +27,6 @@ target_include_directories(ftl-gui PUBLIC
#endif()
#target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
-target_link_libraries(ftl-gui ftlcommon ftlctrl ftlrgbd Threads::Threads ${OpenCV_LIBS} glog::glog ftlnet ftlrender nanogui GL)
+target_link_libraries(ftl-gui ftlcommon ftlctrl ftlrgbd Threads::Threads ${OpenCV_LIBS} ${OPENVR_LIBRARIES} glog::glog ftlnet nanogui GL)
diff --git a/applications/gui/src/camera.cpp b/applications/gui/src/camera.cpp
index f44521b3a81d06a4087a56a8eacf6d2bf4d4700b..32d78a4a0ff8bbccfed60b13293b1277ab599792 100644
--- a/applications/gui/src/camera.cpp
+++ b/applications/gui/src/camera.cpp
@@ -7,6 +7,8 @@
using ftl::rgbd::isValidDepth;
using ftl::gui::GLTexture;
using ftl::gui::PoseWindow;
+using ftl::rgbd::Channel;
+using ftl::rgbd::Channels;
// TODO(Nick) MOVE
class StatisticsImage {
@@ -16,7 +18,7 @@ private:
float n_; // total number of samples
public:
- StatisticsImage(cv::Size size);
+ explicit StatisticsImage(cv::Size size);
StatisticsImage(cv::Size size, float max_f);
/* @brief reset all statistics to 0
@@ -133,10 +135,10 @@ ftl::gui::Camera::Camera(ftl::gui::Screen *screen, ftl::rgbd::Source *src) : scr
ftime_ = (float)glfwGetTime();
pause_ = false;
- channel_ = ftl::rgbd::kChanLeft;
+ channel_ = Channel::Left;
- channels_.push_back(ftl::rgbd::kChanLeft);
- channels_.push_back(ftl::rgbd::kChanDepth);
+ channels_ += Channel::Left;
+ channels_ += Channel::Depth;
// Create pose window...
posewin_ = new PoseWindow(screen, src_->getURI());
@@ -149,6 +151,8 @@ ftl::gui::Camera::Camera(ftl::gui::Screen *screen, ftl::rgbd::Source *src) : scr
depth_.create(depth.size(), depth.type());
cv::swap(rgb_,rgb);
cv::swap(depth_, depth);
+ cv::flip(rgb_,rgb_,0);
+ cv::flip(depth_,depth_,0);
});
}
@@ -227,30 +231,41 @@ void ftl::gui::Camera::showSettings() {
}
-void ftl::gui::Camera::setChannel(ftl::rgbd::channel_t c) {
+void ftl::gui::Camera::setChannel(Channel c) {
channel_ = c;
switch (c) {
- case ftl::rgbd::kChanEnergy:
- case ftl::rgbd::kChanFlow:
- case ftl::rgbd::kChanConfidence:
- case ftl::rgbd::kChanNormals:
- case ftl::rgbd::kChanRight:
+ case Channel::Energy:
+ case Channel::Flow:
+ case Channel::Confidence:
+ case Channel::Normals:
+ case Channel::Right:
src_->setChannel(c);
break;
- case ftl::rgbd::kChanDeviation:
+ case Channel::Deviation:
if (stats_) { stats_->reset(); }
- src_->setChannel(ftl::rgbd::kChanDepth);
+ src_->setChannel(Channel::Depth);
break;
- case ftl::rgbd::kChanDepth:
+ case Channel::Depth:
src_->setChannel(c);
break;
- default: src_->setChannel(ftl::rgbd::kChanNone);
+ default: src_->setChannel(Channel::None);
}
}
+static Eigen::Matrix4d ConvertSteamVRMatrixToMatrix4( const vr::HmdMatrix34_t &matPose )
+{
+ Eigen::Matrix4d matrixObj;
+ matrixObj <<
+ matPose.m[0][0], matPose.m[1][0], matPose.m[2][0], 0.0,
+ matPose.m[0][1], matPose.m[1][1], matPose.m[2][1], 0.0,
+ matPose.m[0][2], matPose.m[1][2], matPose.m[2][2], 0.0,
+ matPose.m[0][3], matPose.m[1][3], matPose.m[2][3], 1.0f;
+ return matrixObj;
+}
+
static void visualizeDepthMap( const cv::Mat &depth, cv::Mat &out,
const float max_depth)
{
@@ -305,22 +320,50 @@ const GLTexture &ftl::gui::Camera::captureFrame() {
if (src_ && src_->isReady()) {
UNIQUE_LOCK(mutex_, lk);
- // Lerp the Eye
- eye_[0] += (neye_[0] - eye_[0]) * lerpSpeed_ * delta_;
- eye_[1] += (neye_[1] - eye_[1]) * lerpSpeed_ * delta_;
- eye_[2] += (neye_[2] - eye_[2]) * lerpSpeed_ * delta_;
+ if (screen_->hasVR()) {
+ #ifdef HAVE_OPENVR
+ src_->setChannel(Channel::Right);
+
+ vr::VRCompositor()->WaitGetPoses(rTrackedDevicePose_, vr::k_unMaxTrackedDeviceCount, NULL, 0 );
+
+ if ( rTrackedDevicePose_[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid )
+ {
+ auto pose = ConvertSteamVRMatrixToMatrix4( rTrackedDevicePose_[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking );
+ pose.inverse();
- Eigen::Translation3d trans(eye_);
- Eigen::Affine3d t(trans);
- Eigen::Matrix4d viewPose = t.matrix() * rotmat_;
+ // Lerp the Eye
+ eye_[0] += (neye_[0] - eye_[0]) * lerpSpeed_ * delta_;
+ eye_[1] += (neye_[1] - eye_[1]) * lerpSpeed_ * delta_;
+ eye_[2] += (neye_[2] - eye_[2]) * lerpSpeed_ * delta_;
+
+ Eigen::Translation3d trans(eye_);
+ Eigen::Affine3d t(trans);
+ Eigen::Matrix4d viewPose = t.matrix() * pose;
+
+ if (src_->hasCapabilities(ftl::rgbd::kCapMovable)) src_->setPose(viewPose);
+ } else {
+ LOG(ERROR) << "No VR Pose";
+ }
+ #endif
+ } else {
+ // Lerp the Eye
+ eye_[0] += (neye_[0] - eye_[0]) * lerpSpeed_ * delta_;
+ eye_[1] += (neye_[1] - eye_[1]) * lerpSpeed_ * delta_;
+ eye_[2] += (neye_[2] - eye_[2]) * lerpSpeed_ * delta_;
+
+ Eigen::Translation3d trans(eye_);
+ Eigen::Affine3d t(trans);
+ Eigen::Matrix4d viewPose = t.matrix() * rotmat_;
+
+ if (src_->hasCapabilities(ftl::rgbd::kCapMovable)) src_->setPose(viewPose);
+ }
- if (src_->hasCapabilities(ftl::rgbd::kCapMovable)) src_->setPose(viewPose);
src_->grab();
//src_->getFrames(rgb, depth);
// When switching from right to depth, client may still receive
// right images from previous batch (depth.channels() == 1 check)
- if (channel_ == ftl::rgbd::kChanDeviation &&
+ if (channel_ == Channel::Deviation &&
depth_.rows > 0 && depth_.channels() == 1)
{
if (!stats_) {
@@ -333,19 +376,19 @@ const GLTexture &ftl::gui::Camera::captureFrame() {
cv::Mat tmp;
switch(channel_) {
- case ftl::rgbd::kChanEnergy:
+ case Channel::Energy:
if (depth_.rows == 0) { break; }
visualizeEnergy(depth_, tmp, 10.0);
texture_.update(tmp);
break;
- case ftl::rgbd::kChanDepth:
+ case Channel::Depth:
if (depth_.rows == 0) { break; }
visualizeDepthMap(depth_, tmp, 7.0);
if (screen_->root()->value("showEdgesInDepth", false)) drawEdges(rgb_, tmp);
texture_.update(tmp);
break;
- case ftl::rgbd::kChanDeviation:
+ case Channel::Deviation:
if (depth_.rows == 0) { break; }
//imageSize = Vector2f(depth.cols, depth.rows);
stats_->getStdDev(tmp);
@@ -354,10 +397,10 @@ const GLTexture &ftl::gui::Camera::captureFrame() {
texture_.update(tmp);
break;
- case ftl::rgbd::kChanFlow:
- case ftl::rgbd::kChanConfidence:
- case ftl::rgbd::kChanNormals:
- case ftl::rgbd::kChanRight:
+ case Channel::Flow:
+ case Channel::Confidence:
+ case Channel::Normals:
+ case Channel::Right:
if (depth_.rows == 0 || depth_.type() != CV_8UC3) { break; }
texture_.update(depth_);
break;
@@ -366,6 +409,13 @@ const GLTexture &ftl::gui::Camera::captureFrame() {
if (rgb_.rows == 0) { break; }
//imageSize = Vector2f(rgb.cols,rgb.rows);
texture_.update(rgb_);
+
+ #ifdef HAVE_OPENVR
+ if (screen_->hasVR() && depth_.channels() >= 3) {
+ LOG(INFO) << "DRAW RIGHT";
+ textureRight_.update(depth_);
+ }
+ #endif
}
}
diff --git a/applications/gui/src/camera.hpp b/applications/gui/src/camera.hpp
index f412d9c43d1809374566b1ba24c427ecda792695..55042fe0d7b3baf5f24a26e390b1348084bcf320 100644
--- a/applications/gui/src/camera.hpp
+++ b/applications/gui/src/camera.hpp
@@ -6,6 +6,10 @@
#include <string>
+#ifdef HAVE_OPENVR
+#include <openvr/openvr.h>
+#endif
+
class StatisticsImage;
namespace ftl {
@@ -19,6 +23,8 @@ class Camera {
Camera(ftl::gui::Screen *screen, ftl::rgbd::Source *src);
~Camera();
+ Camera(const Camera &)=delete;
+
ftl::rgbd::Source *source();
int width() { return (src_) ? src_->parameters().width : 0; }
@@ -32,13 +38,15 @@ class Camera {
void showPoseWindow();
void showSettings();
- void setChannel(ftl::rgbd::channel_t c);
+ void setChannel(ftl::rgbd::Channel c);
void togglePause();
void isPaused();
- const std::vector<ftl::rgbd::channel_t> &availableChannels();
+ const ftl::rgbd::Channels &availableChannels();
const GLTexture &captureFrame();
+ const GLTexture &getLeft() const { return texture_; }
+ const GLTexture &getRight() const { return textureRight_; }
bool thumbnail(cv::Mat &thumb);
@@ -51,6 +59,7 @@ class Camera {
ftl::rgbd::Source *src_;
GLTexture thumb_;
GLTexture texture_;
+ GLTexture textureRight_;
ftl::gui::PoseWindow *posewin_;
nlohmann::json meta_;
Eigen::Vector4d neye_;
@@ -62,11 +71,15 @@ class Camera {
float lerpSpeed_;
bool sdepth_;
bool pause_;
- ftl::rgbd::channel_t channel_;
- std::vector<ftl::rgbd::channel_t> channels_;
+ ftl::rgbd::Channel channel_;
+ ftl::rgbd::Channels channels_;
cv::Mat rgb_;
cv::Mat depth_;
MUTEX mutex_;
+
+ #ifdef HAVE_OPENVR
+ vr::TrackedDevicePose_t rTrackedDevicePose_[ vr::k_unMaxTrackedDeviceCount ];
+ #endif
};
}
diff --git a/applications/gui/src/media_panel.cpp b/applications/gui/src/media_panel.cpp
index 2ee06a69d09690e497bb8f88283a947b466aae46..cb44400bb1c850669332376e0134f138b9c460f3 100644
--- a/applications/gui/src/media_panel.cpp
+++ b/applications/gui/src/media_panel.cpp
@@ -12,6 +12,7 @@
#endif
using ftl::gui::MediaPanel;
+using ftl::rgbd::Channel;
MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""), screen_(screen) {
using namespace nanogui;
@@ -115,7 +116,7 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
button->setCallback([this]() {
ftl::gui::Camera *cam = screen_->activeCamera();
if (cam) {
- cam->setChannel(ftl::rgbd::kChanLeft);
+ cam->setChannel(Channel::Left);
}
});
@@ -124,7 +125,7 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
right_button_->setCallback([this]() {
ftl::gui::Camera *cam = screen_->activeCamera();
if (cam) {
- cam->setChannel(ftl::rgbd::kChanRight);
+ cam->setChannel(Channel::Right);
}
});
@@ -133,7 +134,7 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
depth_button_->setCallback([this]() {
ftl::gui::Camera *cam = screen_->activeCamera();
if (cam) {
- cam->setChannel(ftl::rgbd::kChanDepth);
+ cam->setChannel(Channel::Depth);
}
});
@@ -150,7 +151,7 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
button->setCallback([this]() {
ftl::gui::Camera *cam = screen_->activeCamera();
if (cam) {
- cam->setChannel(ftl::rgbd::kChanDeviation);
+ cam->setChannel(Channel::Deviation);
}
});
@@ -159,7 +160,7 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
button->setCallback([this]() {
ftl::gui::Camera *cam = screen_->activeCamera();
if (cam) {
- cam->setChannel(ftl::rgbd::kChanNormals);
+ cam->setChannel(Channel::Normals);
}
});
@@ -168,7 +169,7 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
button->setCallback([this]() {
ftl::gui::Camera *cam = screen_->activeCamera();
if (cam) {
- cam->setChannel(ftl::rgbd::kChanFlow);
+ cam->setChannel(Channel::Flow);
}
});
@@ -177,7 +178,7 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
button->setCallback([this]() {
ftl::gui::Camera *cam = screen_->activeCamera();
if (cam) {
- cam->setChannel(ftl::rgbd::kChanConfidence);
+ cam->setChannel(Channel::Confidence);
}
});
@@ -186,7 +187,7 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
button->setCallback([this]() {
ftl::gui::Camera *cam = screen_->activeCamera();
if (cam) {
- cam->setChannel(ftl::rgbd::kChanEnergy);
+ cam->setChannel(Channel::Energy);
}
});
diff --git a/applications/gui/src/media_panel.hpp b/applications/gui/src/media_panel.hpp
index d7f9aa9938ee51418629ee42781e738b535c38d0..9e9154d860483a6bf6c88b8da856a4a89862de2f 100644
--- a/applications/gui/src/media_panel.hpp
+++ b/applications/gui/src/media_panel.hpp
@@ -15,7 +15,7 @@ class Screen;
class MediaPanel : public nanogui::Window {
public:
- MediaPanel(ftl::gui::Screen *);
+ explicit MediaPanel(ftl::gui::Screen *);
~MediaPanel();
void cameraChanged();
diff --git a/applications/gui/src/screen.cpp b/applications/gui/src/screen.cpp
index 56624ce61d9524c43e6dffe00948e807528ae595..03d1847112fa86468d9661d5a9966b71a3d46b09 100644
--- a/applications/gui/src/screen.cpp
+++ b/applications/gui/src/screen.cpp
@@ -37,7 +37,7 @@ namespace {
uv = vertex;
vec2 scaledVertex = (vertex * scaleFactor) + position;
gl_Position = vec4(2.0*scaledVertex.x - 1.0,
- 1.0 - 2.0*scaledVertex.y,
+ 2.0*scaledVertex.y - 1.0,
0.0, 1.0);
})";
@@ -51,15 +51,15 @@ namespace {
})";
}
-ftl::gui::Screen::Screen(ftl::Configurable *proot, ftl::net::Universe *pnet, ftl::ctrl::Master *controller) : nanogui::Screen(Eigen::Vector2i(1024, 768), "FT-Lab Remote Presence") {
+ftl::gui::Screen::Screen(ftl::Configurable *proot, ftl::net::Universe *pnet, ftl::ctrl::Master *controller) :
+ nanogui::Screen(Eigen::Vector2i(1024, 768), "FT-Lab Remote Presence"),
+ status_("FT-Lab Remote Presence System") {
using namespace nanogui;
net_ = pnet;
ctrl_ = controller;
root_ = proot;
camera_ = nullptr;
- status_ = "FT-Lab Remote Presence System";
-
setSize(Vector2i(1280,720));
toolbuttheme = new Theme(*theme());
@@ -244,10 +244,31 @@ ftl::gui::Screen::Screen(ftl::Configurable *proot, ftl::net::Universe *pnet, ftl
setVisible(true);
performLayout();
+
+
+ #ifdef HAVE_OPENVR
+ if (vr::VR_IsHmdPresent()) {
+ // Loading the SteamVR Runtime
+ vr::EVRInitError eError = vr::VRInitError_None;
+ HMD_ = vr::VR_Init( &eError, vr::VRApplication_Scene );
+
+ if ( eError != vr::VRInitError_None )
+ {
+ HMD_ = nullptr;
+ LOG(ERROR) << "Unable to init VR runtime: " << vr::VR_GetVRInitErrorAsEnglishDescription( eError );
+ }
+ } else {
+ HMD_ = nullptr;
+ }
+ #endif
}
ftl::gui::Screen::~Screen() {
mShader.free();
+
+ #ifdef HAVE_OPENVR
+ vr::VR_Shutdown();
+ #endif
}
void ftl::gui::Screen::setActiveCamera(ftl::gui::Camera *cam) {
@@ -337,6 +358,18 @@ void ftl::gui::Screen::draw(NVGcontext *ctx) {
imageSize = {camera_->width(), camera_->height()};
mImageID = camera_->captureFrame().texture();
+ leftEye_ = mImageID;
+ rightEye_ = camera_->getRight().texture();
+
+ #ifdef HAVE_OPENVR
+ if (hasVR() && imageSize[0] > 0 && camera_->getLeft().isValid() && camera_->getRight().isValid()) {
+ vr::Texture_t leftEyeTexture = {(void*)(uintptr_t)leftEye_, vr::TextureType_OpenGL, vr::ColorSpace_Gamma };
+ vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTexture );
+ glBindTexture(GL_TEXTURE_2D, rightEye_);
+ vr::Texture_t rightEyeTexture = {(void*)(uintptr_t)rightEye_, vr::TextureType_OpenGL, vr::ColorSpace_Gamma };
+ vr::VRCompositor()->Submit(vr::Eye_Right, &rightEyeTexture );
+ }
+ #endif
if (mImageID < std::numeric_limits<unsigned int>::max() && imageSize[0] > 0) {
auto mScale = (screenSize.cwiseQuotient(imageSize).minCoeff());
diff --git a/applications/gui/src/screen.hpp b/applications/gui/src/screen.hpp
index 8bfce830a034e024ec42037dbc43539a62d50101..d51cec2bf2c34afc7c589b7e6114f503379afe30 100644
--- a/applications/gui/src/screen.hpp
+++ b/applications/gui/src/screen.hpp
@@ -11,6 +11,10 @@
#include "src_window.hpp"
#include "gltexture.hpp"
+#ifdef HAVE_OPENVR
+#include <openvr/openvr.h>
+#endif
+
class StatisticsImageNSamples;
namespace ftl {
@@ -39,6 +43,12 @@ class Screen : public nanogui::Screen {
void setActiveCamera(ftl::gui::Camera*);
ftl::gui::Camera *activeCamera() { return camera_; }
+ #ifdef HAVE_OPENVR
+ bool hasVR() const { return HMD_ != nullptr; }
+ #else
+ bool hasVR() const { return false; }
+ #endif
+
nanogui::Theme *windowtheme;
nanogui::Theme *specialtheme;
nanogui::Theme *mediatheme;
@@ -68,6 +78,13 @@ class Screen : public nanogui::Screen {
ftl::Configurable *root_;
std::string status_;
ftl::gui::Camera *camera_;
+
+ GLuint leftEye_;
+ GLuint rightEye_;
+
+ #ifdef HAVE_OPENVR
+ vr::IVRSystem *HMD_;
+ #endif
};
}
diff --git a/applications/gui/src/src_window.hpp b/applications/gui/src/src_window.hpp
index 7f28279c7fa3cd83bdb62a074e55d4d549799f73..b2fe8a9e0957f3345a719a0c37bac46bf473089c 100644
--- a/applications/gui/src/src_window.hpp
+++ b/applications/gui/src/src_window.hpp
@@ -22,7 +22,7 @@ class Camera;
class SourceWindow : public nanogui::Window {
public:
- SourceWindow(ftl::gui::Screen *screen);
+ explicit SourceWindow(ftl::gui::Screen *screen);
~SourceWindow();
const std::vector<ftl::gui::Camera*> &getCameras();
diff --git a/applications/reconstruct/CMakeLists.txt b/applications/reconstruct/CMakeLists.txt
index 1e55c671c87487b995b0dd772495ff8a612a8c78..dcee7afa0147378ffaabd91263e200f8fe284c98 100644
--- a/applications/reconstruct/CMakeLists.txt
+++ b/applications/reconstruct/CMakeLists.txt
@@ -4,24 +4,19 @@
set(REPSRC
src/main.cpp
- src/voxel_scene.cpp
- src/scene_rep_hash_sdf.cu
- src/compactors.cu
- src/garbage.cu
- src/integrators.cu
+ #src/voxel_scene.cpp
#src/ray_cast_sdf.cu
- src/voxel_render.cu
src/camera_util.cu
- src/voxel_hash.cu
- src/voxel_hash.cpp
#src/ray_cast_sdf.cpp
src/registration.cpp
#src/virtual_source.cpp
- src/splat_render.cpp
- src/dibr.cu
- src/mls.cu
- src/depth_camera.cu
- src/depth_camera.cpp
+ #src/splat_render.cpp
+ #src/dibr.cu
+ #src/mls.cu
+ #src/depth_camera.cu
+ #src/depth_camera.cpp
+ src/ilw.cpp
+ src/ilw.cu
)
add_executable(ftl-reconstruct ${REPSRC})
diff --git a/applications/reconstruct/include/ftl/depth_camera.hpp b/applications/reconstruct/include/ftl/depth_camera.hpp
index cff8ff71f8872a965b2f05071e96f26a4b0a604a..39e037abd90f64e6730577578e09a1f69998b43c 100644
--- a/applications/reconstruct/include/ftl/depth_camera.hpp
+++ b/applications/reconstruct/include/ftl/depth_camera.hpp
@@ -4,8 +4,8 @@
//#include <cutil_inline.h>
//#include <cutil_math.h>
-#include <vector_types.h>
-#include <cuda_runtime.h>
+//#include <vector_types.h>
+//#include <cuda_runtime.h>
#include <ftl/cuda_matrix_util.hpp>
#include <ftl/cuda_common.hpp>
diff --git a/applications/reconstruct/include/ftl/depth_camera_params.hpp b/applications/reconstruct/include/ftl/depth_camera_params.hpp
index 4864fccbdc9fa52647687e885f955a12132b0c04..c1c3b8e615577e2d4006ad15cfa975a27c5797fe 100644
--- a/applications/reconstruct/include/ftl/depth_camera_params.hpp
+++ b/applications/reconstruct/include/ftl/depth_camera_params.hpp
@@ -4,12 +4,13 @@
//#include <cutil_inline.h>
//#include <cutil_math.h>
-#include <vector_types.h>
-#include <cuda_runtime.h>
+//#include <cuda_runtime.h>
#include <ftl/cuda_matrix_util.hpp>
#include <ftl/rgbd/camera.hpp>
+//#include <vector_types.h>
+
struct __align__(16) DepthCameraParams {
float fx;
float fy;
diff --git a/applications/reconstruct/include/ftl/voxel_hash.hpp b/applications/reconstruct/include/ftl/voxel_hash.hpp
deleted file mode 100644
index 98c2eca90530c309b5d2e46848493634aaaa053c..0000000000000000000000000000000000000000
--- a/applications/reconstruct/include/ftl/voxel_hash.hpp
+++ /dev/null
@@ -1,428 +0,0 @@
-// From: https://github.com/niessner/VoxelHashing/blob/master/DepthSensingCUDA/Source/VoxelUtilHashSDF.h
-
-#pragma once
-
-#ifndef sint
-typedef signed int sint;
-#endif
-
-#ifndef uint
-typedef unsigned int uint;
-#endif
-
-#ifndef slong
-typedef signed long slong;
-#endif
-
-#ifndef ulong
-typedef unsigned long ulong;
-#endif
-
-#ifndef uchar
-typedef unsigned char uchar;
-#endif
-
-#ifndef schar
-typedef signed char schar;
-#endif
-
-
-
-
-#include <ftl/cuda_util.hpp>
-
-#include <ftl/cuda_matrix_util.hpp>
-#include <ftl/voxel_hash_params.hpp>
-
-#include <ftl/depth_camera.hpp>
-
-#define SDF_BLOCK_SIZE 8
-#define SDF_BLOCK_SIZE_OLAP 8
-
-#ifndef MINF
-#define MINF __int_as_float(0xff800000)
-#endif
-
-#ifndef PINF
-#define PINF __int_as_float(0x7f800000)
-#endif
-
-extern __constant__ ftl::voxhash::HashParams c_hashParams;
-extern "C" void updateConstantHashParams(const ftl::voxhash::HashParams& hashParams);
-
-namespace ftl {
-namespace voxhash {
-
-//status flags for hash entries
-static const int LOCK_ENTRY = -1;
-static const int FREE_ENTRY = -2147483648;
-static const int NO_OFFSET = 0;
-
-static const uint kFlagSurface = 0x00000001;
-
-struct __align__(16) HashEntryHead {
- union {
- short4 posXYZ; // hash position (lower left corner of SDFBlock))
- uint64_t pos;
- };
- int offset; // offset for collisions
- uint flags;
-};
-
-struct __align__(16) HashEntry
-{
- HashEntryHead head;
- uint voxels[16]; // 512 bits, 1 bit per voxel
- //uint validity[16]; // Is the voxel valid, 512 bit
-
- /*__device__ void operator=(const struct HashEntry& e) {
- ((long long*)this)[0] = ((const long long*)&e)[0];
- ((long long*)this)[1] = ((const long long*)&e)[1];
- //((int*)this)[4] = ((const int*)&e)[4];
- ((long long*)this)[2] = ((const long long*)&e)[2];
- ((long long*)this)[2] = ((const long long*)&e)[3];
- ((long long*)this)[2] = ((const long long*)&e)[4];
- ((long long*)this)[2] = ((const long long*)&e)[5];
- ((long long*)this)[2] = ((const long long*)&e)[6];
- ((long long*)this)[2] = ((const long long*)&e)[7];
- ((long long*)this)[2] = ((const long long*)&e)[8];
- ((long long*)this)[2] = ((const long long*)&e)[9];
- ((long long*)this)[2] = ((const long long*)&e)[10];
- }*/
-};
-
-struct __align__(8) Voxel {
- float sdf; //signed distance function
- uchar3 color; //color
- uchar weight; //accumulated sdf weight
-
- __device__ void operator=(const struct Voxel& v) {
- ((long long*)this)[0] = ((const long long*)&v)[0];
- }
-
-};
-
-/**
- * Voxel Hash Table structure and operations. Works on both CPU and GPU with
- * host <-> device transfer included.
- */
-struct HashData {
-
- ///////////////
- // Host part //
- ///////////////
-
- __device__ __host__
- HashData() {
- //d_heap = NULL;
- //d_heapCounter = NULL;
- d_hash = NULL;
- d_hashDecision = NULL;
- d_hashDecisionPrefix = NULL;
- d_hashCompactified = NULL;
- d_hashCompactifiedCounter = NULL;
- //d_SDFBlocks = NULL;
- d_hashBucketMutex = NULL;
- m_bIsOnGPU = false;
- }
-
- /**
- * Create all the data structures, either on GPU or system memory.
- */
- __host__ void allocate(const HashParams& params, bool dataOnGPU = true);
-
- __host__ void updateParams(const HashParams& params);
-
- __host__ void free();
-
- /**
- * Download entire hash table from GPU to CPU memory.
- */
- __host__ HashData download() const;
-
- /**
- * Upload entire hash table from CPU to GPU memory.
- */
- __host__ HashData upload() const;
-
- __host__ size_t getAllocatedBlocks() const;
-
- __host__ size_t getFreeBlocks() const;
-
- __host__ size_t getCollisionCount() const;
-
-
-
- /////////////////
- // Device part //
- /////////////////
-//#define __CUDACC__
-#ifdef __CUDACC__
-
- __device__
- const HashParams& params() const {
- return c_hashParams;
- }
-
- //! see teschner et al. (but with correct prime values)
- __device__
- uint computeHashPos(const int3& virtualVoxelPos) const {
- const int p0 = 73856093;
- const int p1 = 19349669;
- const int p2 = 83492791;
-
- int res = ((virtualVoxelPos.x * p0) ^ (virtualVoxelPos.y * p1) ^ (virtualVoxelPos.z * p2)) % params().m_hashNumBuckets;
- if (res < 0) res += params().m_hashNumBuckets;
- return (uint)res;
- }
-
- //merges two voxels (v0 the currently stored voxel, v1 is the input voxel)
- __device__
- void combineVoxel(const Voxel &v0, const Voxel& v1, Voxel &out) const {
-
- //v.color = (10*v0.weight * v0.color + v1.weight * v1.color)/(10*v0.weight + v1.weight); //give the currently observed color more weight
- //v.color = (v0.weight * v0.color + v1.weight * v1.color)/(v0.weight + v1.weight);
- //out.color = 0.5f * (v0.color + v1.color); //exponential running average
-
-
- float3 c0 = make_float3(v0.color.x, v0.color.y, v0.color.z);
- float3 c1 = make_float3(v1.color.x, v1.color.y, v1.color.z);
-
- //float3 res = (c0.x+c0.y+c0.z == 0) ? c1 : 0.5f*c0 + 0.5f*c1;
- //float3 res = (c0+c1)/2;
- float3 res = (c0 * (float)v0.weight + c1 * (float)v1.weight) / ((float)v0.weight + (float)v1.weight);
- //float3 res = c1;
-
- out.color.x = (uchar)(res.x+0.5f); out.color.y = (uchar)(res.y+0.5f); out.color.z = (uchar)(res.z+0.5f);
-
- // Nick: reduces colour flicker but not ideal..
- //out.color = v1.color;
-
- // Option 3 (Nick): Use colour with minimum SDF since it should be closest to surface.
- // Results in stable but pixelated output
- //out.color = (v0.weight > 0 && (fabs(v0.sdf) < fabs(v1.sdf))) ? v0.color : v1.color;
-
- // Option 4 (Nick): Merge colours based upon relative closeness
- /*float3 c0 = make_float3(v0.color.x, v0.color.y, v0.color.z);
- float3 c1 = make_float3(v1.color.x, v1.color.y, v1.color.z);
- float factor = fabs(v0.sdf - v1.sdf) / 0.05f / 2.0f;
- if (factor > 0.5f) factor = 0.5f;
- float factor0 = (fabs(v0.sdf) < fabs(v1.sdf)) ? 1.0f - factor : factor;
- float factor1 = 1.0f - factor0;
- out.color.x = (v0.weight > 0) ? (uchar)(c0.x * factor0 + c1.x * factor1) : c1.x;
- out.color.y = (v0.weight > 0) ? (uchar)(c0.y * factor0 + c1.y * factor1) : c1.y;
- out.color.z = (v0.weight > 0) ? (uchar)(c0.z * factor0 + c1.z * factor1) : c1.z;*/
-
- out.sdf = (v0.sdf * (float)v0.weight + v1.sdf * (float)v1.weight) / ((float)v0.weight + (float)v1.weight);
- out.weight = min(params().m_integrationWeightMax, (unsigned int)v0.weight + (unsigned int)v1.weight);
- }
-
-
- //! returns the truncation of the SDF for a given distance value
- __device__
- float getTruncation(float z) const {
- return params().m_truncation + params().m_truncScale * z;
- }
-
-
- __device__
- float3 worldToVirtualVoxelPosFloat(const float3& pos) const {
- return pos / params().m_virtualVoxelSize;
- }
-
- __device__
- int3 worldToVirtualVoxelPos(const float3& pos) const {
- //const float3 p = pos*g_VirtualVoxelResolutionScalar;
- const float3 p = pos / params().m_virtualVoxelSize;
- return make_int3(p+make_float3(sign(p))*0.5f);
- }
-
- __device__
- int3 virtualVoxelPosToSDFBlock(int3 virtualVoxelPos) const {
- if (virtualVoxelPos.x < 0) virtualVoxelPos.x -= SDF_BLOCK_SIZE_OLAP-1;
- if (virtualVoxelPos.y < 0) virtualVoxelPos.y -= SDF_BLOCK_SIZE_OLAP-1;
- if (virtualVoxelPos.z < 0) virtualVoxelPos.z -= SDF_BLOCK_SIZE_OLAP-1;
-
- return make_int3(
- virtualVoxelPos.x/SDF_BLOCK_SIZE_OLAP,
- virtualVoxelPos.y/SDF_BLOCK_SIZE_OLAP,
- virtualVoxelPos.z/SDF_BLOCK_SIZE_OLAP);
- }
-
- // Computes virtual voxel position of corner sample position
- __device__
- int3 SDFBlockToVirtualVoxelPos(const int3& sdfBlock) const {
- return sdfBlock*SDF_BLOCK_SIZE_OLAP;
- }
-
- __device__
- float3 virtualVoxelPosToWorld(const int3& pos) const {
- return make_float3(pos)*params().m_virtualVoxelSize;
- }
-
- __device__
- float3 SDFBlockToWorld(const int3& sdfBlock) const {
- return virtualVoxelPosToWorld(SDFBlockToVirtualVoxelPos(sdfBlock));
- }
-
- __device__
- int3 worldToSDFBlock(const float3& worldPos) const {
- return virtualVoxelPosToSDFBlock(worldToVirtualVoxelPos(worldPos));
- }
-
- __device__
- bool isInBoundingBox(const HashParams &hashParams, const int3& sdfBlock) {
- // NOTE (Nick): Changed, just assume all voxels are potentially in frustrum
- //float3 posWorld = virtualVoxelPosToWorld(SDFBlockToVirtualVoxelPos(sdfBlock)) + hashParams.m_virtualVoxelSize * 0.5f * (SDF_BLOCK_SIZE - 1.0f);
- //return camera.isInCameraFrustumApprox(hashParams.m_rigidTransformInverse, posWorld);
- return !(hashParams.m_flags & ftl::voxhash::kFlagClipping) || sdfBlock.x > hashParams.m_minBounds.x && sdfBlock.x < hashParams.m_maxBounds.x &&
- sdfBlock.y > hashParams.m_minBounds.y && sdfBlock.y < hashParams.m_maxBounds.y &&
- sdfBlock.z > hashParams.m_minBounds.z && sdfBlock.z < hashParams.m_maxBounds.z;
- }
-
- //! computes the (local) virtual voxel pos of an index; idx in [0;511]
- __device__
- uint3 delinearizeVoxelIndex(uint idx) const {
- uint x = idx % SDF_BLOCK_SIZE;
- uint y = (idx % (SDF_BLOCK_SIZE * SDF_BLOCK_SIZE)) / SDF_BLOCK_SIZE;
- uint z = idx / (SDF_BLOCK_SIZE * SDF_BLOCK_SIZE);
- return make_uint3(x,y,z);
- }
-
- //! computes the linearized index of a local virtual voxel pos; pos in [0;7]^3
- __device__
- uint linearizeVoxelPos(const int3& virtualVoxelPos) const {
- return
- virtualVoxelPos.z * SDF_BLOCK_SIZE * SDF_BLOCK_SIZE +
- virtualVoxelPos.y * SDF_BLOCK_SIZE +
- virtualVoxelPos.x;
- }
-
- __device__
- int virtualVoxelPosToLocalSDFBlockIndex(const int3& virtualVoxelPos) const {
- int3 localVoxelPos = make_int3(
- virtualVoxelPos.x % SDF_BLOCK_SIZE,
- virtualVoxelPos.y % SDF_BLOCK_SIZE,
- virtualVoxelPos.z % SDF_BLOCK_SIZE);
-
- if (localVoxelPos.x < 0) localVoxelPos.x += SDF_BLOCK_SIZE;
- if (localVoxelPos.y < 0) localVoxelPos.y += SDF_BLOCK_SIZE;
- if (localVoxelPos.z < 0) localVoxelPos.z += SDF_BLOCK_SIZE;
-
- return linearizeVoxelPos(localVoxelPos);
- }
-
- __device__
- int worldToLocalSDFBlockIndex(const float3& world) const {
- int3 virtualVoxelPos = worldToVirtualVoxelPos(world);
- return virtualVoxelPosToLocalSDFBlockIndex(virtualVoxelPos);
- }
-
-
- //! returns the hash entry for a given worldPos; if there was no hash entry the returned entry will have a ptr with FREE_ENTRY set
- __device__
- int getHashEntry(const float3& worldPos) const {
- //int3 blockID = worldToSDFVirtualVoxelPos(worldPos)/SDF_BLOCK_SIZE; //position of sdf block
- int3 blockID = worldToSDFBlock(worldPos);
- return getHashEntryForSDFBlockPos(blockID);
- }
-
-
- __device__
- void deleteHashEntry(uint id) {
- deleteHashEntry(d_hash[id]);
- }
-
- __device__
- void deleteHashEntry(HashEntry& hashEntry) {
- hashEntry.head.pos = 0;
- hashEntry.head.offset = FREE_ENTRY;
- for (int i=0; i<16; ++i) hashEntry.voxels[i] = 0;
- }
-
- __device__
- bool voxelExists(const float3& worldPos) const {
- int hashEntry = getHashEntry(worldPos);
- return (hashEntry != -1);
- }
-
- __device__
- void deleteVoxel(Voxel& v) const {
- v.color = make_uchar3(0,0,0);
- v.weight = 0;
- v.sdf = 0.0f;
- }
-
-
- __device__
- bool getVoxel(const float3& worldPos) const {
- int hashEntry = getHashEntry(worldPos);
- if (hashEntry == -1) {
- return false;
- } else {
- int3 virtualVoxelPos = worldToVirtualVoxelPos(worldPos);
- int ix = virtualVoxelPosToLocalSDFBlockIndex(virtualVoxelPos);
- return d_hash[hashEntry].voxels[ix/32] & (0x1 << (ix % 32));
- }
- }
-
- __device__
- bool getVoxel(const int3& virtualVoxelPos) const {
- int hashEntry = getHashEntryForSDFBlockPos(virtualVoxelPosToSDFBlock(virtualVoxelPos));
- if (hashEntry == -1) {
- return false;
- } else {
- int ix = virtualVoxelPosToLocalSDFBlockIndex(virtualVoxelPos);
- return d_hash[hashEntry].voxels[ix >> 5] & (0x1 << (ix & 0x1F));
- }
- }
-
- /*__device__
- void setVoxel(const int3& virtualVoxelPos, bool voxelInput) const {
- int hashEntry = getHashEntryForSDFBlockPos(virtualVoxelPosToSDFBlock(virtualVoxelPos));
- if (hashEntry == -1) {
- d_SDFBlocks[hashEntry.ptr + virtualVoxelPosToLocalSDFBlockIndex(virtualVoxelPos)] = voxelInput;
- int ix = virtualVoxelPosToLocalSDFBlockIndex(virtualVoxelPos);
- d_hash[hashEntry].voxels[ix >> 5] |= (0x1 << (ix & 0x1F));
- }
- }*/
-
- //! returns the hash entry for a given sdf block id; if there was no hash entry the returned entry will have a ptr with FREE_ENTRY set
- __device__
- int getHashEntryForSDFBlockPos(const int3& sdfBlock) const;
-
- //for histogram (no collision traversal)
- __device__
- unsigned int getNumHashEntriesPerBucket(unsigned int bucketID);
-
- //for histogram (collisions traversal only)
- __device__
- unsigned int getNumHashLinkedList(unsigned int bucketID);
-
-
- //pos in SDF block coordinates
- __device__
- void allocBlock(const int3& pos);
-
- //!inserts a hash entry without allocating any memory: used by streaming: TODO MATTHIAS check the atomics in this function
- __device__
- bool insertHashEntry(HashEntry entry);
-
- //! deletes a hash entry position for a given sdfBlock index (returns true uppon successful deletion; otherwise returns false)
- __device__
- bool deleteHashEntryElement(const int3& sdfBlock);
-
-#endif //CUDACC
-
- int* d_hashDecision; //
- int* d_hashDecisionPrefix; //
- HashEntry* d_hash; //hash that stores pointers to sdf blocks
- HashEntry** d_hashCompactified; //same as before except that only valid pointers are there
- int* d_hashCompactifiedCounter; //atomic counter to add compactified entries atomically
- int* d_hashBucketMutex; //binary flag per hash bucket; used for allocation to atomically lock a bucket
-
- bool m_bIsOnGPU; //the class be be used on both cpu and gpu
-};
-
-} // namespace voxhash
-} // namespace ftl
diff --git a/applications/reconstruct/include/ftl/voxel_hash_params.hpp b/applications/reconstruct/include/ftl/voxel_hash_params.hpp
index 5e13ec21c259eec02d9a2f49b25ad3a98c06d08c..ac5fcaa726febaff0b56426b6d481d48bae3e421 100644
--- a/applications/reconstruct/include/ftl/voxel_hash_params.hpp
+++ b/applications/reconstruct/include/ftl/voxel_hash_params.hpp
@@ -4,8 +4,8 @@
//#include <cutil_inline.h>
//#include <cutil_math.h>
-#include <vector_types.h>
-#include <cuda_runtime.h>
+//#include <vector_types.h>
+//#include <cuda_runtime.h>
#include <ftl/cuda_matrix_util.hpp>
diff --git a/applications/reconstruct/include/ftl/voxel_scene.hpp b/applications/reconstruct/include/ftl/voxel_scene.hpp
index b1ee6f3bc1388a0c57398fc63971b7ccb163235a..4ea2d5eb3d98b0a3baac018f2c9ace9b5ad7a6f0 100644
--- a/applications/reconstruct/include/ftl/voxel_scene.hpp
+++ b/applications/reconstruct/include/ftl/voxel_scene.hpp
@@ -2,7 +2,7 @@
#pragma once
-#include <cuda_runtime.h>
+//#include <cuda_runtime.h>
#include <ftl/cuda_common.hpp>
#include <ftl/rgbd/source.hpp>
diff --git a/applications/reconstruct/src/compactors.cu b/applications/reconstruct/src/compactors.cu
deleted file mode 100644
index b7cdd5028f0f5ec78de47d8bf6f9099c5448a494..0000000000000000000000000000000000000000
--- a/applications/reconstruct/src/compactors.cu
+++ /dev/null
@@ -1,236 +0,0 @@
-#include "compactors.hpp"
-
-using ftl::voxhash::HashData;
-using ftl::voxhash::HashParams;
-using ftl::voxhash::Voxel;
-using ftl::voxhash::HashEntry;
-using ftl::voxhash::FREE_ENTRY;
-
-#define COMPACTIFY_HASH_THREADS_PER_BLOCK 256
-//#define COMPACTIFY_HASH_SIMPLE
-
-
-/*__global__ void fillDecisionArrayKernel(HashData hashData, DepthCameraData depthCameraData)
-{
- const HashParams& hashParams = c_hashParams;
- const unsigned int idx = blockIdx.x*blockDim.x + threadIdx.x;
-
- if (idx < hashParams.m_hashNumBuckets * HASH_BUCKET_SIZE) {
- hashData.d_hashDecision[idx] = 0;
- if (hashData.d_hash[idx].ptr != FREE_ENTRY) {
- if (hashData.isSDFBlockInCameraFrustumApprox(hashData.d_hash[idx].pos)) {
- hashData.d_hashDecision[idx] = 1; //yes
- }
- }
- }
-}*/
-
-/*extern "C" void fillDecisionArrayCUDA(HashData& hashData, const HashParams& hashParams, const DepthCameraData& depthCameraData)
-{
- const dim3 gridSize((HASH_BUCKET_SIZE * hashParams.m_hashNumBuckets + (T_PER_BLOCK*T_PER_BLOCK) - 1)/(T_PER_BLOCK*T_PER_BLOCK), 1);
- const dim3 blockSize((T_PER_BLOCK*T_PER_BLOCK), 1);
-
- fillDecisionArrayKernel<<<gridSize, blockSize>>>(hashData, depthCameraData);
-
-#ifdef _DEBUG
- cudaSafeCall(cudaDeviceSynchronize());
- //cutilCheckMsg(__FUNCTION__);
-#endif
-
-}*/
-
-/*__global__ void compactifyHashKernel(HashData hashData)
-{
- const HashParams& hashParams = c_hashParams;
- const unsigned int idx = blockIdx.x*blockDim.x + threadIdx.x;
- if (idx < hashParams.m_hashNumBuckets * HASH_BUCKET_SIZE) {
- if (hashData.d_hashDecision[idx] == 1) {
- hashData.d_hashCompactified[hashData.d_hashDecisionPrefix[idx]-1] = hashData.d_hash[idx];
- }
- }
-}*/
-
-/*extern "C" void compactifyHashCUDA(HashData& hashData, const HashParams& hashParams)
-{
- const dim3 gridSize((HASH_BUCKET_SIZE * hashParams.m_hashNumBuckets + (T_PER_BLOCK*T_PER_BLOCK) - 1)/(T_PER_BLOCK*T_PER_BLOCK), 1);
- const dim3 blockSize((T_PER_BLOCK*T_PER_BLOCK), 1);
-
- compactifyHashKernel<<<gridSize, blockSize>>>(hashData);
-
-#ifdef _DEBUG
- cudaSafeCall(cudaDeviceSynchronize());
- //cutilCheckMsg(__FUNCTION__);
-#endif
-}*/
-
-/*__global__ void compactifyVisibleKernel(HashData hashData, HashParams hashParams, DepthCameraParams camera)
-{
- //const HashParams& hashParams = c_hashParams;
- const unsigned int idx = blockIdx.x*blockDim.x + threadIdx.x;
-#ifdef COMPACTIFY_HASH_SIMPLE
- if (idx < hashParams.m_hashNumBuckets) {
- if (hashData.d_hash[idx].ptr != FREE_ENTRY) {
- if (hashData.isSDFBlockInCameraFrustumApprox(hashParams, camera, hashData.d_hash[idx].pos))
- {
- int addr = atomicAdd(hashData.d_hashCompactifiedCounter, 1);
- hashData.d_hashCompactified[addr] = hashData.d_hash[idx];
- }
- }
- }
-#else
- __shared__ int localCounter;
- if (threadIdx.x == 0) localCounter = 0;
- __syncthreads();
-
- int addrLocal = -1;
- if (idx < hashParams.m_hashNumBuckets) {
- if (hashData.d_hash[idx].ptr != FREE_ENTRY) {
- if (hashData.isSDFBlockInCameraFrustumApprox(hashParams, camera, hashData.d_hash[idx].pos))
- {
- addrLocal = atomicAdd(&localCounter, 1);
- }
- }
- }
-
- __syncthreads();
-
- __shared__ int addrGlobal;
- if (threadIdx.x == 0 && localCounter > 0) {
- addrGlobal = atomicAdd(hashData.d_hashCompactifiedCounter, localCounter);
- }
- __syncthreads();
-
- if (addrLocal != -1) {
- const unsigned int addr = addrGlobal + addrLocal;
- hashData.d_hashCompactified[addr] = hashData.d_hash[idx];
- }
-#endif
-}
-
-void ftl::cuda::compactifyVisible(HashData& hashData, const HashParams& hashParams, const DepthCameraParams &camera, cudaStream_t stream) {
- const unsigned int threadsPerBlock = COMPACTIFY_HASH_THREADS_PER_BLOCK;
- const dim3 gridSize((hashParams.m_hashNumBuckets + threadsPerBlock - 1) / threadsPerBlock, 1);
- const dim3 blockSize(threadsPerBlock, 1);
-
- cudaSafeCall(cudaMemsetAsync(hashData.d_hashCompactifiedCounter, 0, sizeof(int),stream));
- compactifyVisibleKernel << <gridSize, blockSize, 0, stream >> >(hashData, hashParams, camera);
- //unsigned int res = 0;
- //cudaSafeCall(cudaMemcpyAsync(&res, hashData.d_hashCompactifiedCounter, sizeof(unsigned int), cudaMemcpyDeviceToHost, stream));
-
-#ifdef _DEBUG
- cudaSafeCall(cudaDeviceSynchronize());
- //cutilCheckMsg(__FUNCTION__);
-#endif
- //return res;
-}*/
-
-__global__ void compactifyAllocatedKernel(HashData hashData)
-{
- const HashParams& hashParams = c_hashParams;
- const unsigned int idx = blockIdx.x*blockDim.x + threadIdx.x;
-#ifdef COMPACTIFY_HASH_SIMPLE
- if (idx < hashParams.m_hashNumBuckets) {
- if (hashData.d_hash[idx].head.offset != FREE_ENTRY) {
- int addr = atomicAdd(hashData.d_hashCompactifiedCounter, 1);
- hashData.d_hashCompactified[addr] = &hashData.d_hash[idx];
- }
- }
-#else
- __shared__ int localCounter;
- if (threadIdx.x == 0) localCounter = 0;
- __syncthreads();
-
- int addrLocal = -1;
- if (idx < hashParams.m_hashNumBuckets) {
- if (hashData.d_hash[idx].head.offset != FREE_ENTRY) {
- addrLocal = atomicAdd(&localCounter, 1);
- }
- }
-
- __syncthreads();
-
- __shared__ int addrGlobal;
- if (threadIdx.x == 0 && localCounter > 0) {
- addrGlobal = atomicAdd(hashData.d_hashCompactifiedCounter, localCounter);
- }
- __syncthreads();
-
- if (addrLocal != -1) {
- const unsigned int addr = addrGlobal + addrLocal;
- hashData.d_hashCompactified[addr] = &hashData.d_hash[idx];
- }
-#endif
-}
-
-void ftl::cuda::compactifyAllocated(HashData& hashData, const HashParams& hashParams, cudaStream_t stream) {
- const unsigned int threadsPerBlock = COMPACTIFY_HASH_THREADS_PER_BLOCK;
- const dim3 gridSize((hashParams.m_hashNumBuckets + threadsPerBlock - 1) / threadsPerBlock, 1);
- const dim3 blockSize(threadsPerBlock, 1);
-
- cudaSafeCall(cudaMemsetAsync(hashData.d_hashCompactifiedCounter, 0, sizeof(int), stream));
- compactifyAllocatedKernel << <gridSize, blockSize, 0, stream >> >(hashData);
- //unsigned int res = 0;
- //cudaSafeCall(cudaMemcpyAsync(&res, hashData.d_hashCompactifiedCounter, sizeof(unsigned int), cudaMemcpyDeviceToHost, stream));
-
-#ifdef _DEBUG
- cudaSafeCall(cudaDeviceSynchronize());
- //cutilCheckMsg(__FUNCTION__);
-#endif
- //return res;
-}
-
-
-__global__ void compactifyOccupiedKernel(HashData hashData)
-{
- const HashParams& hashParams = c_hashParams;
- const unsigned int idx = blockIdx.x*blockDim.x + threadIdx.x;
-#ifdef COMPACTIFY_HASH_SIMPLE
- if (idx < hashParams.m_hashNumBuckets) {
- if (hashData.d_hash[idx].head.offset != FREE_ENTRY && hashData.d_hash[idx].head.flags & ftl::voxhash::kFlagSurface) {
- int addr = atomicAdd(hashData.d_hashCompactifiedCounter, 1);
- hashData.d_hashCompactified[addr] = &hashData.d_hash[idx];
- }
- }
-#else
- __shared__ int localCounter;
- if (threadIdx.x == 0) localCounter = 0;
- __syncthreads();
-
- int addrLocal = -1;
- if (idx < hashParams.m_hashNumBuckets) {
- if (hashData.d_hash[idx].head.offset != FREE_ENTRY && (hashData.d_hash[idx].head.flags & ftl::voxhash::kFlagSurface)) { // TODO:(Nick) Check voxels for all 0 or all 1
- addrLocal = atomicAdd(&localCounter, 1);
- }
- }
-
- __syncthreads();
-
- __shared__ int addrGlobal;
- if (threadIdx.x == 0 && localCounter > 0) {
- addrGlobal = atomicAdd(hashData.d_hashCompactifiedCounter, localCounter);
- }
- __syncthreads();
-
- if (addrLocal != -1) {
- const unsigned int addr = addrGlobal + addrLocal;
- hashData.d_hashCompactified[addr] = &hashData.d_hash[idx];
- }
-#endif
-}
-
-void ftl::cuda::compactifyOccupied(HashData& hashData, const HashParams& hashParams, cudaStream_t stream) {
- const unsigned int threadsPerBlock = COMPACTIFY_HASH_THREADS_PER_BLOCK;
- const dim3 gridSize((hashParams.m_hashNumBuckets + threadsPerBlock - 1) / threadsPerBlock, 1);
- const dim3 blockSize(threadsPerBlock, 1);
-
- cudaSafeCall(cudaMemsetAsync(hashData.d_hashCompactifiedCounter, 0, sizeof(int), stream));
- compactifyAllocatedKernel << <gridSize, blockSize, 0, stream >> >(hashData);
- //unsigned int res = 0;
- //cudaSafeCall(cudaMemcpyAsync(&res, hashData.d_hashCompactifiedCounter, sizeof(unsigned int), cudaMemcpyDeviceToHost, stream));
-
-#ifdef _DEBUG
- cudaSafeCall(cudaDeviceSynchronize());
- //cutilCheckMsg(__FUNCTION__);
-#endif
- //return res;
-}
diff --git a/applications/reconstruct/src/compactors.hpp b/applications/reconstruct/src/compactors.hpp
deleted file mode 100644
index 6c61985eea8448a078b8abe3e821992519c9425f..0000000000000000000000000000000000000000
--- a/applications/reconstruct/src/compactors.hpp
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef _FTL_RECONSTRUCT_COMPACTORS_HPP_
-#define _FTL_RECONSTRUCT_COMPACTORS_HPP_
-
-#include <ftl/voxel_hash.hpp>
-
-namespace ftl {
-namespace cuda {
-
-// Compact visible
-//void compactifyVisible(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const DepthCameraParams &camera, cudaStream_t);
-
-// Compact allocated
-void compactifyAllocated(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, cudaStream_t);
-
-// Compact visible surfaces
-void compactifyOccupied(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, cudaStream_t stream);
-
-}
-}
-
-#endif // _FTL_RECONSTRUCT_COMPACTORS_HPP_
diff --git a/applications/reconstruct/src/garbage.cu b/applications/reconstruct/src/garbage.cu
deleted file mode 100644
index b685e9e6b7d94434ff425eff268699a715261522..0000000000000000000000000000000000000000
--- a/applications/reconstruct/src/garbage.cu
+++ /dev/null
@@ -1,135 +0,0 @@
-#include <ftl/voxel_hash.hpp>
-#include "garbage.hpp"
-
-using namespace ftl::voxhash;
-
-#define T_PER_BLOCK 8
-#define NUM_CUDA_BLOCKS 10000
-
-/*__global__ void starveVoxelsKernel(HashData hashData) {
- int ptr;
-
- // Stride over all allocated blocks
- for (int bi=blockIdx.x; bi<*hashData.d_hashCompactifiedCounter; bi+=NUM_CUDA_BLOCKS) {
-
- ptr = hashData.d_hashCompactified[bi].ptr;
- int weight = hashData.d_SDFBlocks[ptr + threadIdx.x].weight;
- weight = max(0, weight-2);
- hashData.d_SDFBlocks[ptr + threadIdx.x].weight = weight; //CHECK Remove to totally clear previous frame (Nick)
-
- }
-}
-
-void ftl::cuda::starveVoxels(HashData& hashData, const HashParams& hashParams, cudaStream_t stream) {
- const unsigned int threadsPerBlock = SDF_BLOCK_SIZE*SDF_BLOCK_SIZE*SDF_BLOCK_SIZE;
- const dim3 gridSize(NUM_CUDA_BLOCKS, 1);
- const dim3 blockSize(threadsPerBlock, 1);
-
- //if (hashParams.m_numOccupiedBlocks > 0) {
- starveVoxelsKernel << <gridSize, blockSize, 0, stream >> >(hashData);
- //}
-#ifdef _DEBUG
- cudaSafeCall(cudaDeviceSynchronize());
- //cutilCheckMsg(__FUNCTION__);
-#endif
-}*/
-
-#define ENTRIES_PER_BLOCK 4
-
-__global__ void clearVoxelsKernel(HashData hashData) {
- const int lane = threadIdx.x % 16;
- const int halfWarp = threadIdx.x / 16;
-
- // Stride over all allocated blocks
- for (int bi=blockIdx.x+halfWarp; bi<*hashData.d_hashCompactifiedCounter; bi+=NUM_CUDA_BLOCKS*ENTRIES_PER_BLOCK) {
-
- HashEntry *entry = hashData.d_hashCompactified[bi];
- //hashData.d_SDFBlocks[entry.ptr + threadIdx.x].weight = 0;
- entry->voxels[lane] = 0;
-
- }
-}
-
-void ftl::cuda::clearVoxels(HashData& hashData, const HashParams& hashParams) {
- const unsigned int threadsPerBlock = 16 * ENTRIES_PER_BLOCK;
- const dim3 gridSize(NUM_CUDA_BLOCKS, 1);
- const dim3 blockSize(threadsPerBlock, 1);
-
- clearVoxelsKernel << <gridSize, blockSize >> >(hashData);
-}
-
-
-__global__ void garbageCollectIdentifyKernel(HashData hashData) {
- const int lane = threadIdx.x % 16;
- const int halfWarp = threadIdx.x / 16;
-
- // Stride over all allocated blocks
- for (int bi=blockIdx.x+halfWarp; bi<*hashData.d_hashCompactifiedCounter; bi+=NUM_CUDA_BLOCKS * ENTRIES_PER_BLOCK) {
-
- const HashEntry *entry = hashData.d_hashCompactified[bi];
-
- const uint v = entry->voxels[lane];
- const uint mask = (halfWarp & 0x1) ? 0xFFFF0000 : 0x0000FFFF;
- uint ballot_result = __ballot_sync(mask, v == 0 || v == 0xFFFFFFFF);
-
- if (lane == 0) hashData.d_hashDecision[bi] = (ballot_result == mask) ? 1 : 0;
-
- }
-}
-
-void ftl::cuda::garbageCollectIdentify(HashData& hashData, const HashParams& hashParams, cudaStream_t stream) {
-
- const unsigned int threadsPerBlock = SDF_BLOCK_SIZE * SDF_BLOCK_SIZE * SDF_BLOCK_SIZE / 2;
- const dim3 gridSize(NUM_CUDA_BLOCKS, 1);
- const dim3 blockSize(threadsPerBlock, 1);
-
- //if (hashParams.m_numOccupiedBlocks > 0) {
- garbageCollectIdentifyKernel << <gridSize, blockSize, 0, stream >> >(hashData);
- //}
-#ifdef _DEBUG
- cudaSafeCall(cudaDeviceSynchronize());
- //cutilCheckMsg(__FUNCTION__);
-#endif
-}
-
-
-__global__ void garbageCollectFreeKernel(HashData hashData) {
-
- // Stride over all allocated blocks
- for (int bi=blockIdx.x*blockDim.x + threadIdx.x; bi<*hashData.d_hashCompactifiedCounter; bi+=NUM_CUDA_BLOCKS*blockDim.x) {
-
- HashEntry *entry = hashData.d_hashCompactified[bi];
-
- if ((entry->head.flags & ftl::voxhash::kFlagSurface) == 0) { //decision to delete the hash entry
-
-
- //if (entry->head.offset == FREE_ENTRY) return; //should never happen since we did compactify before
-
- int3 posI3 = make_int3(entry->head.posXYZ.x, entry->head.posXYZ.y, entry->head.posXYZ.z);
-
- if (hashData.deleteHashEntryElement(posI3)) { //delete hash entry from hash (and performs heap append)
- //#pragma unroll
- //for (uint i = 0; i < 16; i++) { //clear sdf block: CHECK TODO another kernel?
- // entry->voxels[i] = 0;
- //}
- }
- }
-
- }
-}
-
-
-void ftl::cuda::garbageCollectFree(HashData& hashData, const HashParams& hashParams, cudaStream_t stream) {
-
- const unsigned int threadsPerBlock = T_PER_BLOCK*T_PER_BLOCK;
- const dim3 gridSize(NUM_CUDA_BLOCKS, 1); // (hashParams.m_numOccupiedBlocks + threadsPerBlock - 1) / threadsPerBlock
- const dim3 blockSize(threadsPerBlock, 1);
-
- //if (hashParams.m_numOccupiedBlocks > 0) {
- garbageCollectFreeKernel << <gridSize, blockSize, 0, stream >> >(hashData);
- //}
-#ifdef _DEBUG
- cudaSafeCall(cudaDeviceSynchronize());
- //cutilCheckMsg(__FUNCTION__);
-#endif
-}
diff --git a/applications/reconstruct/src/garbage.hpp b/applications/reconstruct/src/garbage.hpp
deleted file mode 100644
index 5d1d7574d252b40da18008da39f1bf89a7d667fb..0000000000000000000000000000000000000000
--- a/applications/reconstruct/src/garbage.hpp
+++ /dev/null
@@ -1,15 +0,0 @@
-#ifndef _FTL_RECONSTRUCTION_GARBAGE_HPP_
-#define _FTL_RECONSTRUCTION_GARBAGE_HPP_
-
-namespace ftl {
-namespace cuda {
-
-void clearVoxels(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams);
-void starveVoxels(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, cudaStream_t stream);
-void garbageCollectIdentify(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, cudaStream_t stream);
-void garbageCollectFree(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, cudaStream_t stream);
-
-}
-}
-
-#endif // _FTL_RECONSTRUCTION_GARBAGE_HPP_
diff --git a/applications/reconstruct/src/ilw.cpp b/applications/reconstruct/src/ilw.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..86a4cca5e4f82047ed6591a137b694788da879eb
--- /dev/null
+++ b/applications/reconstruct/src/ilw.cpp
@@ -0,0 +1,120 @@
+#include "ilw.hpp"
+#include <ftl/utility/matrix_conversion.hpp>
+#include <ftl/rgbd/source.hpp>
+#include <ftl/cuda/points.hpp>
+#include <loguru.hpp>
+
+#include "ilw_cuda.hpp"
+
+using ftl::ILW;
+using ftl::detail::ILWData;
+using ftl::rgbd::Channel;
+using ftl::rgbd::Channels;
+using ftl::rgbd::Format;
+using cv::cuda::GpuMat;
+
+ILW::ILW(nlohmann::json &config) : ftl::Configurable(config) {
+
+}
+
+ILW::~ILW() {
+
+}
+
+bool ILW::process(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
+ _phase0(fs, stream);
+
+ //for (int i=0; i<2; ++i) {
+ _phase1(fs, stream);
+ //for (int j=0; j<3; ++j) {
+ // _phase2(fs);
+ //}
+
+ // TODO: Break if no time left
+ //}
+
+ return true;
+}
+
+bool ILW::_phase0(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
+ // Make points channel...
+ for (size_t i=0; i<fs.frames.size(); ++i) {
+ auto &f = fs.frames[i];
+ auto *s = fs.sources[i];
+
+ if (f.empty(Channel::Depth + Channel::Colour)) {
+ LOG(ERROR) << "Missing required channel";
+ continue;
+ }
+
+ auto &t = f.createTexture<float4>(Channel::Points, Format<float4>(f.get<GpuMat>(Channel::Colour).size()));
+ auto pose = MatrixConversion::toCUDA(s->getPose().cast<float>()); //.inverse());
+ ftl::cuda::point_cloud(t, f.createTexture<float>(Channel::Depth), s->parameters(), pose, stream);
+
+ // TODO: Create energy vector texture and clear it
+ // Create energy and clear it
+
+ // Convert colour from BGR to BGRA if needed
+ if (f.get<GpuMat>(Channel::Colour).type() == CV_8UC3) {
+ // Convert to 4 channel colour
+ auto &col = f.get<GpuMat>(Channel::Colour);
+ GpuMat tmp(col.size(), CV_8UC4);
+ cv::cuda::swap(col, tmp);
+ cv::cuda::cvtColor(tmp,col, cv::COLOR_BGR2BGRA);
+ }
+
+ f.createTexture<float4>(Channel::EnergyVector, Format<float4>(f.get<GpuMat>(Channel::Colour).size()));
+ f.createTexture<float>(Channel::Energy, Format<float>(f.get<GpuMat>(Channel::Colour).size()));
+ f.createTexture<uchar4>(Channel::Colour);
+ }
+
+ return true;
+}
+
+bool ILW::_phase1(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
+ // Run correspondence kernel to create an energy vector
+
+ // For each camera combination
+ for (size_t i=0; i<fs.frames.size(); ++i) {
+ for (size_t j=0; j<fs.frames.size(); ++j) {
+ if (i == j) continue;
+
+ LOG(INFO) << "Running phase1";
+
+ auto &f1 = fs.frames[i];
+ auto &f2 = fs.frames[j];
+ //auto s1 = fs.frames[i];
+ auto s2 = fs.sources[j];
+
+ auto pose = MatrixConversion::toCUDA(s2->getPose().cast<float>().inverse());
+
+ try {
+ //Calculate energy vector to best correspondence
+ ftl::cuda::correspondence_energy_vector(
+ f1.getTexture<float4>(Channel::Points),
+ f2.getTexture<float4>(Channel::Points),
+ f1.getTexture<uchar4>(Channel::Colour),
+ f2.getTexture<uchar4>(Channel::Colour),
+ // TODO: Add normals and other things...
+ f1.getTexture<float4>(Channel::EnergyVector),
+ f1.getTexture<float>(Channel::Energy),
+ pose,
+ s2->parameters(),
+ stream
+ );
+ } catch (ftl::exception &e) {
+ LOG(ERROR) << "Exception in correspondence: " << e.what();
+ }
+
+ LOG(INFO) << "Correspondences done... " << i;
+ }
+ }
+
+ return true;
+}
+
+bool ILW::_phase2(ftl::rgbd::FrameSet &fs) {
+ // Run energies and motion kernel
+
+ return true;
+}
diff --git a/applications/reconstruct/src/ilw.cu b/applications/reconstruct/src/ilw.cu
new file mode 100644
index 0000000000000000000000000000000000000000..90133a3a57800ee87a91fd50902deea5f701258a
--- /dev/null
+++ b/applications/reconstruct/src/ilw.cu
@@ -0,0 +1,86 @@
+#include "ilw_cuda.hpp"
+
+using ftl::cuda::TextureObject;
+using ftl::rgbd::Camera;
+
+#define WARP_SIZE 32
+#define T_PER_BLOCK 8
+#define FULL_MASK 0xffffffff
+
+__device__ inline float warpMax(float e) {
+ for (int i = WARP_SIZE/2; i > 0; i /= 2) {
+ const float other = __shfl_xor_sync(FULL_MASK, e, i, WARP_SIZE);
+ e = max(e, other);
+ }
+ return e;
+}
+
+__global__ void correspondence_energy_vector_kernel(
+ TextureObject<float4> p1,
+ TextureObject<float4> p2,
+ TextureObject<uchar4> c1,
+ TextureObject<uchar4> c2,
+ TextureObject<float4> vout,
+ TextureObject<float> eout,
+ float4x4 pose2, // Inverse
+ Camera cam2) {
+
+ // Each warp picks point in p1
+ const int tid = (threadIdx.x + threadIdx.y * blockDim.x);
+ const int x = (blockIdx.x*blockDim.x + threadIdx.x) / WARP_SIZE;
+ const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+ const float3 world1 = make_float3(p1.tex2D(x, y));
+ const float3 camPos2 = pose2 * world1;
+ const uint2 screen2 = cam2.camToScreen<uint2>(camPos2);
+
+ const int upsample = 8;
+
+ // Project to p2 using cam2
+ // Each thread takes a possible correspondence and calculates a weighting
+ const int lane = tid % WARP_SIZE;
+ for (int i=lane; i<upsample*upsample; i+=WARP_SIZE) {
+ const float u = (i % upsample) - (upsample / 2);
+ const float v = (i / upsample) - (upsample / 2);
+
+ const float3 world2 = make_float3(p2.tex2D(screen2.x+u, screen2.y+v));
+
+ // Determine degree of correspondence
+ const float confidence = 1.0f / length(world1 - world2);
+
+ printf("conf %f\n", confidence);
+ const float maxconf = warpMax(confidence);
+
+ // This thread has best confidence value
+ if (maxconf == confidence) {
+ vout(x,y) = vout.tex2D(x, y) + make_float4(
+ (world1.x - world2.x) * maxconf,
+ (world1.y - world2.y) * maxconf,
+ (world1.z - world2.z) * maxconf,
+ maxconf);
+ eout(x,y) = eout.tex2D(x,y) + length(world1 - world2)*maxconf;
+ }
+ }
+}
+
+void ftl::cuda::correspondence_energy_vector(
+ TextureObject<float4> &p1,
+ TextureObject<float4> &p2,
+ TextureObject<uchar4> &c1,
+ TextureObject<uchar4> &c2,
+ TextureObject<float4> &vout,
+ TextureObject<float> &eout,
+ float4x4 &pose2,
+ const Camera &cam2,
+ cudaStream_t stream) {
+
+ const dim3 gridSize((p1.width() + 2 - 1)/2, (p1.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
+ const dim3 blockSize(2*WARP_SIZE, T_PER_BLOCK);
+
+ printf("COR SIZE %d,%d\n", p1.width(), p1.height());
+
+ correspondence_energy_vector_kernel<<<gridSize, blockSize, 0, stream>>>(
+ p1, p2, c1, c2, vout, eout, pose2, cam2
+ );
+ cudaSafeCall( cudaGetLastError() );
+}
diff --git a/applications/reconstruct/src/ilw.hpp b/applications/reconstruct/src/ilw.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0be45d015e976b540263a2c16cc5605376092a43
--- /dev/null
+++ b/applications/reconstruct/src/ilw.hpp
@@ -0,0 +1,66 @@
+#ifndef _FTL_RECONSTRUCT_ILW_HPP_
+#define _FTL_RECONSTRUCT_ILW_HPP_
+
+#include <ftl/cuda_common.hpp>
+#include <ftl/rgbd/frameset.hpp>
+#include <ftl/configurable.hpp>
+#include <vector>
+
+namespace ftl {
+
+namespace detail {
+struct ILWData{
+ // x,y,z + confidence
+ ftl::cuda::TextureObject<float4> correspondence;
+
+ ftl::cuda::TextureObject<float4> points;
+
+ // Residual potential energy
+ ftl::cuda::TextureObject<float> residual;
+
+ // Flow magnitude
+ ftl::cuda::TextureObject<float> flow;
+};
+}
+
+/**
+ * For a set of sources, perform Iterative Lattice Warping to correct the
+ * location of points between the cameras. The algorithm finds possible
+ * correspondences and warps the original pixel lattice of points in each
+ * camera towards the correspondences, iterating the process as many times as
+ * possible. The result is that both local and global adjustment is made to the
+ * point clouds to improve micro alignment that may have been incorrect due to
+ * either inaccurate camera pose estimation or noise/errors in the depth maps.
+ */
+class ILW : public ftl::Configurable {
+ public:
+ explicit ILW(nlohmann::json &config);
+ ~ILW();
+
+ /**
+ * Take a frameset and perform the iterative lattice warping.
+ */
+ bool process(ftl::rgbd::FrameSet &fs, cudaStream_t stream=0);
+
+ private:
+ /*
+ * Initialise data.
+ */
+ bool _phase0(ftl::rgbd::FrameSet &fs, cudaStream_t stream);
+
+ /*
+ * Find possible correspondences and a confidence value.
+ */
+ bool _phase1(ftl::rgbd::FrameSet &fs, cudaStream_t stream);
+
+ /*
+ * Calculate energies and move the points.
+ */
+ bool _phase2(ftl::rgbd::FrameSet &fs);
+
+ std::vector<detail::ILWData> data_;
+};
+
+}
+
+#endif // _FTL_RECONSTRUCT_ILW_HPP_
diff --git a/applications/reconstruct/src/ilw_cuda.hpp b/applications/reconstruct/src/ilw_cuda.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a01af75149409fe033ba39ffb0170489ee926be9
--- /dev/null
+++ b/applications/reconstruct/src/ilw_cuda.hpp
@@ -0,0 +1,26 @@
+#ifndef _FTL_ILW_CUDA_HPP_
+#define _FTL_ILW_CUDA_HPP_
+
+#include <ftl/cuda_common.hpp>
+#include <ftl/rgbd/camera.hpp>
+#include <ftl/cuda_matrix_util.hpp>
+
+namespace ftl {
+namespace cuda {
+
+void correspondence_energy_vector(
+ ftl::cuda::TextureObject<float4> &p1,
+ ftl::cuda::TextureObject<float4> &p2,
+ ftl::cuda::TextureObject<uchar4> &c1,
+ ftl::cuda::TextureObject<uchar4> &c2,
+ ftl::cuda::TextureObject<float4> &vout,
+ ftl::cuda::TextureObject<float> &eout,
+ float4x4 &pose2,
+ const ftl::rgbd::Camera &cam2,
+ cudaStream_t stream
+);
+
+}
+}
+
+#endif // _FTL_ILW_CUDA_HPP_
diff --git a/applications/reconstruct/src/integrators.cu b/applications/reconstruct/src/integrators.cu
deleted file mode 100644
index d23fada9982aed2ff49039aa91bfa8760f91fcd1..0000000000000000000000000000000000000000
--- a/applications/reconstruct/src/integrators.cu
+++ /dev/null
@@ -1,342 +0,0 @@
-#include "integrators.hpp"
-//#include <ftl/ray_cast_params.hpp>
-#include <vector_types.h>
-#include <cuda_runtime.h>
-#include <ftl/cuda_matrix_util.hpp>
-#include <ftl/cuda_util.hpp>
-#include <ftl/cuda_common.hpp>
-
-#define T_PER_BLOCK 8
-#define NUM_CUDA_BLOCKS 10000
-#define WARP_SIZE 32
-
-using ftl::voxhash::HashData;
-using ftl::voxhash::HashParams;
-using ftl::voxhash::Voxel;
-using ftl::voxhash::HashEntry;
-using ftl::voxhash::HashEntryHead;
-using ftl::voxhash::FREE_ENTRY;
-
-extern __constant__ ftl::voxhash::DepthCameraCUDA c_cameras[MAX_CAMERAS];
-extern __constant__ HashParams c_hashParams;
-
-__device__ float4 make_float4(uchar4 c) {
- return make_float4(static_cast<float>(c.x), static_cast<float>(c.y), static_cast<float>(c.z), static_cast<float>(c.w));
-}
-
-__device__ float colourDistance(const uchar4 &c1, const uchar3 &c2) {
- float x = c1.x-c2.x;
- float y = c1.y-c2.y;
- float z = c1.z-c2.z;
- return x*x + y*y + z*z;
-}
-
-/*
- * Kim, K., Chalidabhongse, T. H., Harwood, D., & Davis, L. (2005).
- * Real-time foreground-background segmentation using codebook model.
- * Real-Time Imaging. https://doi.org/10.1016/j.rti.2004.12.004
- */
-__device__ bool colordiff(const uchar4 &pa, const uchar3 &pb, float epsilon) {
- float x_2 = pb.x * pb.x + pb.y * pb.y + pb.z * pb.z;
- float v_2 = pa.x * pa.x + pa.y * pa.y + pa.z * pa.z;
- float xv_2 = powf(float(pb.x * pa.x + pb.y * pa.y + pb.z * pa.z), 2.0f);
- float p_2 = xv_2 / v_2;
- return sqrt(x_2 - p_2) < epsilon;
-}
-
-/*
- * Guennebaud, G.; Gross, M. Algebraic point set surfaces. ACMTransactions on Graphics Vol. 26, No. 3, Article No. 23, 2007.
- * Used in: FusionMLS: Highly dynamic 3D reconstruction with consumer-grade RGB-D cameras
- * r = distance between points
- * h = smoothing parameter in meters (default 4cm)
- */
-__device__ float spatialWeighting(float r) {
- const float h = c_hashParams.m_spatialSmoothing;
- if (r >= h) return 0.0f;
- float rh = r / h;
- rh = 1.0f - rh*rh;
- return rh*rh*rh*rh;
-}
-
-__device__ float spatialWeighting(float r, float h) {
- //const float h = c_hashParams.m_spatialSmoothing;
- if (r >= h) return 0.0f;
- float rh = r / h;
- rh = 1.0f - rh*rh;
- return rh*rh*rh*rh;
-}
-
-
-__global__ void integrateDepthMapsKernel(HashData hashData, HashParams hashParams, int numcams) {
- __shared__ uint all_warp_ballot;
- __shared__ uint voxels[16];
-
- const uint i = threadIdx.x; //inside of an SDF block
- const int3 po = make_int3(hashData.delinearizeVoxelIndex(i));
-
- // Stride over all allocated blocks
- for (int bi=blockIdx.x; bi<*hashData.d_hashCompactifiedCounter; bi+=NUM_CUDA_BLOCKS) {
-
- //TODO check if we should load this in shared memory
- //HashEntryHead entry = hashData.d_hashCompactified[bi]->head;
-
- int3 pi_base = hashData.SDFBlockToVirtualVoxelPos(make_int3(hashData.d_hashCompactified[bi]->head.posXYZ));
-
- //uint idx = entry.offset + i;
- int3 pi = pi_base + po;
- float3 pfb = hashData.virtualVoxelPosToWorld(pi);
- int count = 0;
- //float camdepths[MAX_CAMERAS];
-
- Voxel oldVoxel; // = hashData.d_SDFBlocks[idx];
- hashData.deleteVoxel(oldVoxel);
-
- for (uint cam=0; cam<numcams; ++cam) {
- const ftl::voxhash::DepthCameraCUDA &camera = c_cameras[cam];
-
- float3 pf = camera.poseInverse * pfb;
- uint2 screenPos = make_uint2(camera.params.cameraToKinectScreenInt(pf));
-
- // For this voxel in hash, get its screen position and check it is on screen
- if (screenPos.x < camera.params.m_imageWidth && screenPos.y < camera.params.m_imageHeight) { //on screen
-
- //float depth = g_InputDepth[screenPos];
- float depth = tex2D<float>(camera.depth, screenPos.x, screenPos.y);
- //if (depth > 20.0f) return;
-
- //uchar4 color = make_uchar4(0, 0, 0, 0);
- //if (cameraData.d_colorData) {
- //color = (cam == 0) ? make_uchar4(255,0,0,255) : make_uchar4(0,0,255,255);
- //color = tex2D<uchar4>(camera.colour, screenPos.x, screenPos.y);
- //color = bilinearFilterColor(cameraData.cameraToKinectScreenFloat(pf));
- //}
-
- //printf("screen pos %d\n", color.x);
- //return;
-
- // TODO:(Nick) Accumulate weighted positions
- // TODO:(Nick) Accumulate weighted normals
- // TODO:(Nick) Accumulate weights
-
- // Depth is within accepted max distance from camera
- if (depth > 0.01f && depth < hashParams.m_maxIntegrationDistance) { // valid depth and color (Nick: removed colour check)
- //camdepths[count] = depth;
- ++count;
-
- // Calculate SDF of this voxel wrt the depth map value
- float sdf = depth - pf.z;
- float truncation = hashData.getTruncation(depth);
- float depthZeroOne = camera.params.cameraToKinectProjZ(depth);
-
- // Is this voxel close enough to cam for depth map value
- // CHECK Nick: If is too close then free space violation so remove?
- if (sdf > -truncation) // && depthZeroOne >= 0.0f && depthZeroOne <= 1.0f) //check if in truncation range should already be made in depth map computation
- {
- float weightUpdate = max(hashParams.m_integrationWeightSample * 1.5f * (1.0f-depthZeroOne), 1.0f);
-
- Voxel curr; //construct current voxel
- curr.sdf = sdf;
- curr.weight = weightUpdate;
- //curr.color = make_uchar3(color.x, color.y, color.z);
-
-
- //if (entry.flags != cameraParams.flags & 0xFF) {
- // entry.flags = cameraParams.flags & 0xFF;
- //hashData.d_SDFBlocks[idx].color = make_uchar3(0,0,0);
- //}
-
- Voxel newVoxel;
- //if (color.x == MINF) hashData.combineVoxelDepthOnly(hashData.d_SDFBlocks[idx], curr, newVoxel);
- //else hashData.combineVoxel(hashData.d_SDFBlocks[idx], curr, newVoxel);
- hashData.combineVoxel(oldVoxel, curr, newVoxel);
-
- oldVoxel = newVoxel;
-
- //Voxel prev = getVoxel(g_SDFBlocksSDFUAV, g_SDFBlocksRGBWUAV, idx);
- //Voxel newVoxel = combineVoxel(curr, prev);
- //setVoxel(g_SDFBlocksSDFUAV, g_SDFBlocksRGBWUAV, idx, newVoxel);
- }
- } else {
- // Depth is invalid so what to do here?
- // TODO(Nick) Use past voxel if available (set weight from 0 to 1)
-
- // Naive: need to know if this is a foreground voxel
- //bool coldist = colordiff(color, hashData.d_SDFBlocks[idx].color, 5.0f);
- //if (!coldist) ++count;
-
- }
- }
- }
-
- // Calculate voxel sign values across a warp
- int warpNum = i / WARP_SIZE;
- //uint ballot_result = __ballot_sync(0xFFFFFFFF, (oldVoxel.sdf >= 0.0f) ? 0 : 1);
- uint ballot_result = __ballot_sync(0xFFFFFFFF, (fabs(oldVoxel.sdf) <= hashParams.m_virtualVoxelSize && oldVoxel.weight > 0) ? 1 : 0);
-
- // Aggregate each warp result into voxel mask
- if (i % WARP_SIZE == 0) {
- voxels[warpNum] = ballot_result;
- }
-
- __syncthreads();
-
- // Work out if block is occupied or not and save voxel masks
- // TODO:(Nick) Is it faster to do this in a separate garbage kernel?
- if (i < 16) {
- const uint v = voxels[i];
- hashData.d_hashCompactified[bi]->voxels[i] = v;
- const uint mask = 0x0000FFFF;
- uint b1 = __ballot_sync(mask, v == 0xFFFFFFFF);
- uint b2 = __ballot_sync(mask, v == 0);
- if (i == 0) {
- if (b1 != mask && b2 != mask) hashData.d_hashCompactified[bi]->head.flags |= ftl::voxhash::kFlagSurface;
- else hashData.d_hashCompactified[bi]->head.flags &= ~ftl::voxhash::kFlagSurface;
- }
- }
-
- }
-}
-
-#define WINDOW_RADIUS 1
-#define PATCH_SIZE 32
-
-__global__ void integrateMLSKernel(HashData hashData, HashParams hashParams, int numcams) {
- __shared__ uint voxels[16];
-
- const uint i = threadIdx.x; //inside of an SDF block
- const int3 po = make_int3(hashData.delinearizeVoxelIndex(i));
- const int warpNum = i / WARP_SIZE;
- const int lane = i % WARP_SIZE;
-
- // Stride over all allocated blocks
- for (int bi=blockIdx.x; bi<*hashData.d_hashCompactifiedCounter; bi+=NUM_CUDA_BLOCKS) {
-
- //TODO check if we should load this in shared memory
- //HashEntryHead entry = hashData.d_hashCompactified[bi]->head;
-
- const int3 pi_base = hashData.SDFBlockToVirtualVoxelPos(make_int3(hashData.d_hashCompactified[bi]->head.posXYZ));
-
- //uint idx = entry.offset + i;
- const int3 pi = pi_base + po;
- const float3 pfb = hashData.virtualVoxelPosToWorld(pi);
- //int count = 0;
- //float camdepths[MAX_CAMERAS];
-
- //Voxel oldVoxel; // = hashData.d_SDFBlocks[idx];
- //hashData.deleteVoxel(oldVoxel);
-
- //float3 awpos = make_float3(0.0f);
- //float3 awnorm = make_float3(0.0f);
- //float aweights = 0.0f;
- float sdf = 0.0f;
- float weights = 0.0f;
-
- // Preload depth values
- // 1. Find min and max screen positions
- // 2. Subtract/Add WINDOW_RADIUS to min/max
- // ... check that the buffer is not too small to cover this
- // ... if buffer not big enough then don't buffer at all.
- // 3. Populate shared mem depth map buffer using all threads
- // 4. Adjust window lookups to use shared mem buffer
-
- //uint cam=0;
- for (uint cam=0; cam<numcams; ++cam) {
- const ftl::voxhash::DepthCameraCUDA &camera = c_cameras[cam];
- const uint height = camera.params.m_imageHeight;
- const uint width = camera.params.m_imageWidth;
-
- const float3 pf = camera.poseInverse * pfb;
- const uint2 screenPos = make_uint2(camera.params.cameraToKinectScreenInt(pf));
-
- //float3 wpos = make_float3(0.0f);
- float3 wnorm = make_float3(0.0f);
-
-
- #pragma unroll
- for (int v=-WINDOW_RADIUS; v<=WINDOW_RADIUS; ++v) {
- for (int u=-WINDOW_RADIUS; u<=WINDOW_RADIUS; ++u) {
- if (screenPos.x+u < width && screenPos.y+v < height) { //on screen
- float4 depth = tex2D<float4>(camera.points, screenPos.x+u, screenPos.y+v);
- if (depth.z == MINF) continue;
-
- //float4 normal = tex2D<float4>(camera.normal, screenPos.x+u, screenPos.y+v);
- const float3 camPos = camera.poseInverse * make_float3(depth); //camera.pose * camera.params.kinectDepthToSkeleton(screenPos.x+u, screenPos.y+v, depth);
- const float weight = spatialWeighting(length(pf - camPos));
-
- //wpos += weight*worldPos;
- sdf += weight*(camPos.z - pf.z);
- //sdf += camPos.z - pf.z;
- //wnorm += weight*make_float3(normal);
- //weights += 1.0f;
- weights += weight;
- }
- }
- }
-
- //awpos += wpos;
- //aweights += weights;
- }
-
- //awpos /= aweights;
- //wnorm /= weights;
-
- sdf /= weights;
-
- //float sdf = (aweights == 0.0f) ? MINF : length(pfb - awpos);
- //float sdf = wnorm.x * (pfb.x - wpos.x) + wnorm.y * (pfb.y - wpos.y) + wnorm.z * (pfb.z - wpos.z);
-
- //printf("WEIGHTS: %f\n", weights);
-
- //if (weights < 0.00001f) sdf = 0.0f;
-
- // Calculate voxel sign values across a warp
- int warpNum = i / WARP_SIZE;
-
- //uint solid_ballot = __ballot_sync(0xFFFFFFFF, (fabs(sdf) < hashParams.m_virtualVoxelSize && aweights >= 0.5f) ? 1 : 0);
- //uint solid_ballot = __ballot_sync(0xFFFFFFFF, (fabs(sdf) <= hashParams.m_virtualVoxelSize) ? 1 : 0);
- //uint solid_ballot = __ballot_sync(0xFFFFFFFF, (aweights >= 0.0f) ? 1 : 0);
- uint solid_ballot = __ballot_sync(0xFFFFFFFF, (sdf < 0.0f ) ? 1 : 0);
-
- // Aggregate each warp result into voxel mask
- if (i % WARP_SIZE == 0) {
- voxels[warpNum] = solid_ballot;
- //valid[warpNum] = valid_ballot;
- }
-
- __syncthreads();
-
- // Work out if block is occupied or not and save voxel masks
- // TODO:(Nick) Is it faster to do this in a separate garbage kernel?
- if (i < 16) {
- const uint v = voxels[i];
- hashData.d_hashCompactified[bi]->voxels[i] = v;
- //hashData.d_hashCompactified[bi]->validity[i] = valid[i];
- const uint mask = 0x0000FFFF;
- uint b1 = __ballot_sync(mask, v == 0xFFFFFFFF);
- uint b2 = __ballot_sync(mask, v == 0);
- if (i == 0) {
- if (b1 != mask && b2 != mask) hashData.d_hashCompactified[bi]->head.flags |= ftl::voxhash::kFlagSurface;
- else hashData.d_hashCompactified[bi]->head.flags &= ~ftl::voxhash::kFlagSurface;
- }
- }
-
- }
-}
-
-
-
-void ftl::cuda::integrateDepthMaps(HashData& hashData, const HashParams& hashParams, int numcams, cudaStream_t stream) {
-const unsigned int threadsPerBlock = SDF_BLOCK_SIZE*SDF_BLOCK_SIZE*SDF_BLOCK_SIZE;
-const dim3 gridSize(NUM_CUDA_BLOCKS, 1);
-const dim3 blockSize(threadsPerBlock, 1);
-
-//if (hashParams.m_numOccupiedBlocks > 0) { //this guard is important if there is no depth in the current frame (i.e., no blocks were allocated)
- integrateMLSKernel << <gridSize, blockSize, 0, stream >> >(hashData, hashParams, numcams);
-//}
-
-//cudaSafeCall( cudaGetLastError() );
-#ifdef _DEBUG
-cudaSafeCall(cudaDeviceSynchronize());
-//cutilCheckMsg(__FUNCTION__);
-#endif
-}
diff --git a/applications/reconstruct/src/integrators.hpp b/applications/reconstruct/src/integrators.hpp
deleted file mode 100644
index 789551dd1fa7347bf02c518c8c5a73f6ae4269b4..0000000000000000000000000000000000000000
--- a/applications/reconstruct/src/integrators.hpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef _FTL_RECONSTRUCTION_INTEGRATORS_HPP_
-#define _FTL_RECONSTRUCTION_INTEGRATORS_HPP_
-
-#include <ftl/voxel_hash.hpp>
-#include <ftl/depth_camera.hpp>
-
-namespace ftl {
-namespace cuda {
-
-/*void integrateDepthMap(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams,
- const DepthCameraData& depthCameraData, const DepthCameraParams& depthCameraParams, cudaStream_t stream);
-
-void integrateRegistration(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams,
- const DepthCameraData& depthCameraData, const DepthCameraParams& depthCameraParams, cudaStream_t stream);
-*/
-
-void integrateDepthMaps(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, int numcams, cudaStream_t stream);
-
-}
-}
-
-#endif // _FTL_RECONSTRUCTION_INTEGRATORS_HPP_
diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index 01745d2dd2e6dcaf902fa4260f781a3b7fb3dea0..6c1d8a8813dd5f5a39a1457003df31bc388ab642 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -9,14 +9,15 @@
#include <ftl/config.h>
#include <ftl/configuration.hpp>
#include <ftl/depth_camera.hpp>
-#include <ftl/voxel_scene.hpp>
#include <ftl/rgbd.hpp>
-#include <ftl/virtual_source.hpp>
+#include <ftl/rgbd/virtual.hpp>
#include <ftl/rgbd/streamer.hpp>
#include <ftl/slave.hpp>
#include <ftl/rgbd/group.hpp>
+#include <ftl/threads.hpp>
-#include "splat_render.hpp"
+#include "ilw.hpp"
+#include <ftl/render/splat_render.hpp>
#include <string>
#include <vector>
@@ -37,6 +38,7 @@ using std::string;
using std::vector;
using ftl::rgbd::Source;
using ftl::config::json_t;
+using ftl::rgbd::Channel;
using json = nlohmann::json;
using std::this_thread::sleep_for;
@@ -91,102 +93,78 @@ static void run(ftl::Configurable *root) {
}
}
- ftl::voxhash::SceneRep *scene = ftl::create<ftl::voxhash::SceneRep>(root, "voxelhash");
+ ftl::rgbd::FrameSet scene_A; // Output of align process
+ ftl::rgbd::FrameSet scene_B; // Input of render process
+
+ //ftl::voxhash::SceneRep *scene = ftl::create<ftl::voxhash::SceneRep>(root, "voxelhash");
ftl::rgbd::Streamer *stream = ftl::create<ftl::rgbd::Streamer>(root, "stream", net);
- ftl::rgbd::Source *virt = ftl::create<ftl::rgbd::Source>(root, "virtual", net);
- ftl::render::Splatter *splat = new ftl::render::Splatter(scene);
+ ftl::rgbd::VirtualSource *virt = ftl::create<ftl::rgbd::VirtualSource>(root, "virtual");
+ ftl::render::Splatter *splat = ftl::create<ftl::render::Splatter>(root, "renderer", &scene_B);
ftl::rgbd::Group group;
+ ftl::ILW *align = ftl::create<ftl::ILW>(root, "merge");
- //auto virtimpl = new ftl::rgbd::VirtualSource(virt);
- //virt->customImplementation(virtimpl);
- //virtimpl->setScene(scene);
+ // Generate virtual camera render when requested by streamer
+ virt->onRender([splat,virt,&scene_B](ftl::rgbd::Frame &out) {
+ virt->setTimestamp(scene_B.timestamp);
+ splat->render(virt, out);
+ });
stream->add(virt);
for (size_t i=0; i<sources.size(); i++) {
Source *in = sources[i];
- in->setChannel(ftl::rgbd::kChanDepth);
- //stream->add(in);
- scene->addSource(in);
+ in->setChannel(Channel::Depth);
group.addSource(in);
}
+ stream->setLatency(4); // FIXME: This depends on source!?
stream->run();
bool busy = false;
+ group.setLatency(4);
group.setName("ReconGroup");
- group.sync([scene,splat,virt,&busy,&slave](ftl::rgbd::FrameSet &fs) -> bool {
- cudaSetDevice(scene->getCUDADevice());
+ group.sync([splat,virt,&busy,&slave,&scene_A,&scene_B,&align](ftl::rgbd::FrameSet &fs) -> bool {
+ //cudaSetDevice(scene->getCUDADevice());
+
+ if (slave.isPaused()) return true;
if (busy) {
LOG(INFO) << "Group frameset dropped: " << fs.timestamp;
return true;
}
busy = true;
- scene->nextFrame();
- // Send all frames to GPU, block until done?
- // TODO: Allow non-block and keep frameset locked until later
- if (!slave.isPaused()) scene->upload(fs);
+ // Swap the entire frameset to allow rapid return
+ fs.swapTo(scene_A);
- int64_t ts = fs.timestamp;
-
- ftl::pool.push([scene,splat,virt,&busy,ts,&slave](int id) {
- cudaSetDevice(scene->getCUDADevice());
+ ftl::pool.push([&scene_B,&scene_A,&busy,&slave,&align](int id) {
+ //cudaSetDevice(scene->getCUDADevice());
// TODO: Release frameset here...
- cudaSafeCall(cudaStreamSynchronize(scene->getIntegrationStream()));
+ //cudaSafeCall(cudaStreamSynchronize(scene->getIntegrationStream()));
- if (!slave.isPaused()) {
- scene->integrate();
- scene->garbage();
- }
+ UNIQUE_LOCK(scene_A.mtx, lk);
- // Don't render here... but update timestamp.
- splat->render(ts, virt, scene->getIntegrationStream());
+ // Send all frames to GPU, block until done?
+ scene_A.upload(Channel::Colour + Channel::Depth); // TODO: (Nick) Add scene stream.
+ //align->process(scene_A);
+
+ // TODO: To use second GPU, could do a download, swap, device change,
+ // then upload to other device. Or some direct device-2-device copy.
+ scene_A.swapTo(scene_B);
+ LOG(INFO) << "Align complete... " << scene_A.timestamp;
busy = false;
});
return true;
});
-
- /*int active = sources.size();
- while (ftl::running) {
- if (active == 0) {
- LOG(INFO) << "Waiting for sources...";
- sleep_for(milliseconds(1000));
- }
-
- active = 0;
-
- if (!slave.isPaused()) {
- // Mark voxels as cleared
- scene->nextFrame();
-
- // Grab, upload frames and allocate voxel blocks
- active = scene->upload();
-
- // Make sure previous virtual camera frame has finished rendering
- //stream->wait();
- cudaSafeCall(cudaStreamSynchronize(scene->getIntegrationStream()));
-
-
- // Merge new frames into the voxel structure
- scene->integrate();
-
- //LOG(INFO) << "Allocated: " << scene->getOccupiedCount();
-
- // Remove any redundant voxels
- scene->garbage();
-
- } else {
- active = 1;
- }
-
- splat->render(virt, scene->getIntegrationStream());
-
- // Start virtual camera rendering and previous frame compression
- stream->poll();
- }*/
+ ftl::timer::stop();
+ slave.stop();
+ net->shutdown();
+ delete align;
+ delete splat;
+ delete stream;
+ delete virt;
+ delete net;
}
int main(int argc, char **argv) {
diff --git a/applications/reconstruct/src/ray_cast_sdf.cu b/applications/reconstruct/src/ray_cast_sdf.cu
index a43b608429b1fad1ac160b188c9be6b084274154..10fd3e0b7b84ca694432abc7dc68fc513ad483eb 100644
--- a/applications/reconstruct/src/ray_cast_sdf.cu
+++ b/applications/reconstruct/src/ray_cast_sdf.cu
@@ -1,5 +1,5 @@
-#include <cuda_runtime.h>
+//#include <cuda_runtime.h>
#include <ftl/cuda_matrix_util.hpp>
diff --git a/applications/reconstruct/src/scene_rep_hash_sdf.cu b/applications/reconstruct/src/scene_rep_hash_sdf.cu
index 247247b6cc5279186f9f9bdc81bbe627ab0621ea..4750d3e7ed4f7aeb68875e0b5050d76efed5b715 100644
--- a/applications/reconstruct/src/scene_rep_hash_sdf.cu
+++ b/applications/reconstruct/src/scene_rep_hash_sdf.cu
@@ -2,8 +2,8 @@
//#include <cutil_inline.h>
//#include <cutil_math.h>
-#include <vector_types.h>
-#include <cuda_runtime.h>
+//#include <vector_types.h>
+//#include <cuda_runtime.h>
#include <ftl/cuda_matrix_util.hpp>
diff --git a/applications/reconstruct/src/splat_render.cpp b/applications/reconstruct/src/splat_render.cpp
deleted file mode 100644
index cc52bb7bf33d8688a9fc1da9f3b0d07474aff811..0000000000000000000000000000000000000000
--- a/applications/reconstruct/src/splat_render.cpp
+++ /dev/null
@@ -1,137 +0,0 @@
-#include "splat_render.hpp"
-#include "splat_render_cuda.hpp"
-#include "compactors.hpp"
-#include "depth_camera_cuda.hpp"
-
-using ftl::render::Splatter;
-
-Splatter::Splatter(ftl::voxhash::SceneRep *scene) : scene_(scene) {
-
-}
-
-Splatter::~Splatter() {
-
-}
-
-void Splatter::render(int64_t ts, ftl::rgbd::Source *src, cudaStream_t stream) {
- if (!src->isReady()) return;
-
- const auto &camera = src->parameters();
-
- cudaSafeCall(cudaSetDevice(scene_->getCUDADevice()));
-
- // Create buffers if they don't exists
- if ((unsigned int)depth1_.width() != camera.width || (unsigned int)depth1_.height() != camera.height) {
- depth1_ = ftl::cuda::TextureObject<int>(camera.width, camera.height);
- }
- if ((unsigned int)depth3_.width() != camera.width || (unsigned int)depth3_.height() != camera.height) {
- depth3_ = ftl::cuda::TextureObject<int>(camera.width, camera.height);
- }
- if ((unsigned int)colour1_.width() != camera.width || (unsigned int)colour1_.height() != camera.height) {
- colour1_ = ftl::cuda::TextureObject<uchar4>(camera.width, camera.height);
- }
- if ((unsigned int)colour_tmp_.width() != camera.width || (unsigned int)colour_tmp_.height() != camera.height) {
- colour_tmp_ = ftl::cuda::TextureObject<float4>(camera.width, camera.height);
- }
- if ((unsigned int)normal1_.width() != camera.width || (unsigned int)normal1_.height() != camera.height) {
- normal1_ = ftl::cuda::TextureObject<float4>(camera.width, camera.height);
- }
- if ((unsigned int)depth2_.width() != camera.width || (unsigned int)depth2_.height() != camera.height) {
- depth2_ = ftl::cuda::TextureObject<float>(camera.width, camera.height);
- }
- if ((unsigned int)colour2_.width() != camera.width || (unsigned int)colour2_.height() != camera.height) {
- colour2_ = ftl::cuda::TextureObject<uchar4>(camera.width, camera.height);
- }
-
- // Parameters object to pass to CUDA describing the camera
- SplatParams params;
- params.m_flags = 0;
- if (src->value("splatting", true) == false) params.m_flags |= ftl::render::kNoSplatting;
- if (src->value("upsampling", true) == false) params.m_flags |= ftl::render::kNoUpsampling;
- if (src->value("texturing", true) == false) params.m_flags |= ftl::render::kNoTexturing;
-
- params.m_viewMatrix = MatrixConversion::toCUDA(src->getPose().cast<float>().inverse());
- params.m_viewMatrixInverse = MatrixConversion::toCUDA(src->getPose().cast<float>());
- params.voxelSize = scene_->getHashParams().m_virtualVoxelSize;
- params.camera.flags = 0;
- params.camera.fx = camera.fx;
- params.camera.fy = camera.fy;
- params.camera.mx = -camera.cx;
- params.camera.my = -camera.cy;
- params.camera.m_imageWidth = camera.width;
- params.camera.m_imageHeight = camera.height;
- params.camera.m_sensorDepthWorldMax = camera.maxDepth;
- params.camera.m_sensorDepthWorldMin = camera.minDepth;
-
- //ftl::cuda::compactifyAllocated(scene_->getHashData(), scene_->getHashParams(), stream);
- //LOG(INFO) << "Occupied: " << scene_->getOccupiedCount();
-
- if (scene_->value("voxels", false)) {
- // TODO:(Nick) Stereo for voxel version
- ftl::cuda::isosurface_point_image(scene_->getHashData(), depth1_, params, stream);
- //ftl::cuda::splat_points(depth1_, depth2_, params, stream);
- //ftl::cuda::dibr(depth2_, colour1_, scene_->cameraCount(), params, stream);
- src->writeFrames(ts, colour1_, depth2_, stream);
- } else {
- ftl::cuda::clear_depth(depth1_, stream);
- ftl::cuda::clear_depth(depth3_, stream);
- ftl::cuda::clear_depth(depth2_, stream);
- ftl::cuda::clear_colour(colour2_, stream);
- ftl::cuda::dibr(depth1_, colour1_, normal1_, depth2_, colour_tmp_, depth3_, scene_->cameraCount(), params, stream);
-
- // Step 1: Put all points into virtual view to gather them
- //ftl::cuda::dibr_raw(depth1_, scene_->cameraCount(), params, stream);
-
- // Step 2: For each point, use a warp to do MLS and up sample
- //ftl::cuda::mls_render_depth(depth1_, depth3_, params, scene_->cameraCount(), stream);
-
- if (src->getChannel() == ftl::rgbd::kChanDepth) {
- //ftl::cuda::int_to_float(depth1_, depth2_, 1.0f / 1000.0f, stream);
- if (src->value("splatting", false)) {
- //ftl::cuda::splat_points(depth1_, colour1_, normal1_, depth2_, colour2_, params, stream);
- ftl::cuda::int_to_float(depth1_, depth2_, 1.0f / 1000.0f, stream);
- src->writeFrames(ts, colour1_, depth2_, stream);
- } else {
- ftl::cuda::int_to_float(depth1_, depth2_, 1.0f / 1000.0f, stream);
- src->writeFrames(ts, colour1_, depth2_, stream);
- }
- } else if (src->getChannel() == ftl::rgbd::kChanEnergy) {
- //ftl::cuda::int_to_float(depth1_, depth2_, 1.0f / 1000.0f, stream);
- //if (src->value("splatting", false)) {
- //ftl::cuda::splat_points(depth1_, colour1_, normal1_, depth2_, colour2_, params, stream);
- //ftl::cuda::int_to_float(depth1_, depth2_, 1.0f / 1000.0f, stream);
- src->writeFrames(ts, colour1_, depth2_, stream);
- //} else {
- //ftl::cuda::int_to_float(depth1_, depth2_, 1.0f / 1000.0f, stream);
- // src->writeFrames(colour1_, depth2_, stream);
- //}
- } else if (src->getChannel() == ftl::rgbd::kChanRight) {
- // Adjust pose to right eye position
- Eigen::Affine3f transform(Eigen::Translation3f(camera.baseline,0.0f,0.0f));
- Eigen::Matrix4f matrix = src->getPose().cast<float>() * transform.matrix();
- params.m_viewMatrix = MatrixConversion::toCUDA(matrix.inverse());
- params.m_viewMatrixInverse = MatrixConversion::toCUDA(matrix);
-
- ftl::cuda::clear_depth(depth1_, stream);
- ftl::cuda::dibr(depth1_, colour1_, normal1_, depth2_, colour_tmp_, depth3_, scene_->cameraCount(), params, stream);
- src->writeFrames(ts, colour1_, colour2_, stream);
- } else {
- if (src->value("splatting", false)) {
- //ftl::cuda::splat_points(depth1_, colour1_, normal1_, depth2_, colour2_, params, stream);
- src->writeFrames(ts, colour1_, depth2_, stream);
- } else {
- ftl::cuda::int_to_float(depth1_, depth2_, 1.0f / 1000.0f, stream);
- src->writeFrames(ts, colour1_, depth2_, stream);
- }
- }
- }
-
- //ftl::cuda::median_filter(depth1_, depth2_, stream);
- //ftl::cuda::splat_points(depth1_, depth2_, params, stream);
-
- // TODO: Second pass
-}
-
-void Splatter::setOutputDevice(int device) {
- device_ = device;
-}
diff --git a/applications/reconstruct/src/voxel_hash.cpp b/applications/reconstruct/src/voxel_hash.cpp
deleted file mode 100644
index 6f929c746d66cae5c382bf15b2d25e26f6b46702..0000000000000000000000000000000000000000
--- a/applications/reconstruct/src/voxel_hash.cpp
+++ /dev/null
@@ -1,95 +0,0 @@
-#include <ftl/voxel_hash.hpp>
-#include <loguru.hpp>
-
-using ftl::voxhash::HashData;
-using ftl::voxhash::HashParams;
-
-void HashData::allocate(const HashParams& params, bool dataOnGPU) {
- m_bIsOnGPU = dataOnGPU;
- if (m_bIsOnGPU) {
- cudaSafeCall(cudaMalloc(&d_hash, sizeof(HashEntry)* params.m_hashNumBuckets));
- cudaSafeCall(cudaMalloc(&d_hashDecision, sizeof(int)* params.m_hashNumBuckets));
- cudaSafeCall(cudaMalloc(&d_hashDecisionPrefix, sizeof(int)* params.m_hashNumBuckets));
- cudaSafeCall(cudaMalloc(&d_hashCompactified, sizeof(HashEntry*)* params.m_hashNumBuckets));
- cudaSafeCall(cudaMalloc(&d_hashCompactifiedCounter, sizeof(int)));
- cudaSafeCall(cudaMalloc(&d_hashBucketMutex, sizeof(int)* params.m_hashNumBuckets));
- } else {
- d_hash = new HashEntry[params.m_hashNumBuckets];
- d_hashDecision = new int[params.m_hashNumBuckets];
- d_hashDecisionPrefix = new int[params.m_hashNumBuckets];
- d_hashCompactified = new HashEntry*[params.m_hashNumBuckets];
- d_hashCompactifiedCounter = new int[1];
- d_hashBucketMutex = new int[params.m_hashNumBuckets];
- }
-
- updateParams(params);
-}
-
-void HashData::updateParams(const HashParams& params) {
- if (m_bIsOnGPU) {
- updateConstantHashParams(params);
- }
-}
-
-void HashData::free() {
- if (m_bIsOnGPU) {
- cudaSafeCall(cudaFree(d_hash));
- cudaSafeCall(cudaFree(d_hashDecision));
- cudaSafeCall(cudaFree(d_hashDecisionPrefix));
- cudaSafeCall(cudaFree(d_hashCompactified));
- cudaSafeCall(cudaFree(d_hashCompactifiedCounter));
- cudaSafeCall(cudaFree(d_hashBucketMutex));
- } else {
- if (d_hash) delete[] d_hash;
- if (d_hashDecision) delete[] d_hashDecision;
- if (d_hashDecisionPrefix) delete[] d_hashDecisionPrefix;
- if (d_hashCompactified) delete[] d_hashCompactified;
- if (d_hashCompactifiedCounter) delete[] d_hashCompactifiedCounter;
- if (d_hashBucketMutex) delete[] d_hashBucketMutex;
- }
-
- d_hash = NULL;
- d_hashDecision = NULL;
- d_hashDecisionPrefix = NULL;
- d_hashCompactified = NULL;
- d_hashCompactifiedCounter = NULL;
- d_hashBucketMutex = NULL;
-}
-
-HashData HashData::download() const {
- if (!m_bIsOnGPU) return *this;
- HashParams params;
-
- HashData hashData;
- hashData.allocate(params, false); //allocate the data on the CPU
- cudaSafeCall(cudaMemcpy(hashData.d_hash, d_hash, sizeof(HashEntry)* params.m_hashNumBuckets, cudaMemcpyDeviceToHost));
- cudaSafeCall(cudaMemcpy(hashData.d_hashDecision, d_hashDecision, sizeof(int)*params.m_hashNumBuckets, cudaMemcpyDeviceToHost));
- cudaSafeCall(cudaMemcpy(hashData.d_hashDecisionPrefix, d_hashDecisionPrefix, sizeof(int)*params.m_hashNumBuckets, cudaMemcpyDeviceToHost));
- cudaSafeCall(cudaMemcpy(hashData.d_hashCompactified, d_hashCompactified, sizeof(HashEntry*)* params.m_hashNumBuckets, cudaMemcpyDeviceToHost));
- cudaSafeCall(cudaMemcpy(hashData.d_hashCompactifiedCounter, d_hashCompactifiedCounter, sizeof(unsigned int), cudaMemcpyDeviceToHost));
- cudaSafeCall(cudaMemcpy(hashData.d_hashBucketMutex, d_hashBucketMutex, sizeof(int)* params.m_hashNumBuckets, cudaMemcpyDeviceToHost));
-
- return hashData;
-}
-
-HashData HashData::upload() const {
- if (m_bIsOnGPU) return *this;
- HashParams params;
-
- HashData hashData;
- hashData.allocate(params, false); //allocate the data on the CPU
- cudaSafeCall(cudaMemcpy(hashData.d_hash, d_hash, sizeof(HashEntry)* params.m_hashNumBuckets, cudaMemcpyHostToDevice));
- cudaSafeCall(cudaMemcpy(hashData.d_hashDecision, d_hashDecision, sizeof(int)*params.m_hashNumBuckets, cudaMemcpyHostToDevice));
- cudaSafeCall(cudaMemcpy(hashData.d_hashDecisionPrefix, d_hashDecisionPrefix, sizeof(int)*params.m_hashNumBuckets, cudaMemcpyHostToDevice));
- cudaSafeCall(cudaMemcpy(hashData.d_hashCompactified, d_hashCompactified, sizeof(HashEntry)* params.m_hashNumBuckets, cudaMemcpyHostToDevice));
- cudaSafeCall(cudaMemcpy(hashData.d_hashCompactifiedCounter, d_hashCompactifiedCounter, sizeof(unsigned int), cudaMemcpyHostToDevice));
- cudaSafeCall(cudaMemcpy(hashData.d_hashBucketMutex, d_hashBucketMutex, sizeof(int)* params.m_hashNumBuckets, cudaMemcpyHostToDevice));
-
- return hashData;
-}
-
-/*size_t HashData::getAllocatedBlocks() const {
- unsigned int count;
- cudaSafeCall(cudaMemcpy(d_heapCounter, &count, sizeof(unsigned int), cudaMemcpyDeviceToHost));
- return count;
-}*/
diff --git a/applications/reconstruct/src/voxel_hash.cu b/applications/reconstruct/src/voxel_hash.cu
deleted file mode 100644
index c2d07c391a6e48d2b45cc23dbf32b00878ffd5c9..0000000000000000000000000000000000000000
--- a/applications/reconstruct/src/voxel_hash.cu
+++ /dev/null
@@ -1,257 +0,0 @@
-#include <ftl/voxel_hash.hpp>
-
-using namespace ftl::voxhash;
-
-#define COLLISION_LIST_SIZE 6
-
-__device__ inline uint64_t compactPosition(const int3 &pos) {
- union __align__(8) {
- short4 posXYZ;
- uint64_t pos64;
- };
- posXYZ.x = pos.x; posXYZ.y = pos.y; posXYZ.z = pos.z; posXYZ.w = 0;
- return pos64;
-}
-
-//! returns the hash entry for a given sdf block id; if there was no hash entry the returned entry will have a ptr with FREE_ENTRY set
-__device__
-int HashData::getHashEntryForSDFBlockPos(const int3& sdfBlock) const {
- uint h = computeHashPos(sdfBlock); //hash
- uint64_t pos = compactPosition(sdfBlock);
-
- HashEntryHead curr;
-
- int i = h;
- unsigned int maxIter = 0;
-
- #pragma unroll 2
- while (maxIter < COLLISION_LIST_SIZE) {
- curr = d_hash[i].head;
-
- if (curr.pos == pos && curr.offset != FREE_ENTRY) return i;
- if (curr.offset == 0 || curr.offset == FREE_ENTRY) break;
-
- i += curr.offset; //go to next element in the list
- i %= (params().m_hashNumBuckets); //check for overflow
- ++maxIter;
- }
-
- // Could not find
- return -1;
-}
-
-//for histogram (collisions traversal only)
-__device__
-unsigned int HashData::getNumHashLinkedList(unsigned int bucketID) {
- unsigned int listLen = 0;
-
- unsigned int i = bucketID; //start with the last entry of the current bucket
- HashEntryHead curr; curr.offset = 0;
-
- unsigned int maxIter = 0;
-
- #pragma unroll 2
- while (maxIter < COLLISION_LIST_SIZE) {
- curr = d_hash[i].head;
-
- if (curr.offset == 0 || curr.offset == FREE_ENTRY) break;
-
- i += curr.offset; //go to next element in the list
- i %= (params().m_hashNumBuckets); //check for overflow
- ++listLen;
- ++maxIter;
- }
-
- return listLen;
-}
-
-//pos in SDF block coordinates
-__device__
-void HashData::allocBlock(const int3& pos) {
- uint h = computeHashPos(pos); //hash bucket
- uint i = h;
- HashEntryHead curr; //curr.offset = 0;
- const uint64_t pos64 = compactPosition(pos);
-
- unsigned int maxIter = 0;
- #pragma unroll 2
- while (maxIter < COLLISION_LIST_SIZE) {
- //offset = curr.offset;
- curr = d_hash[i].head; //TODO MATTHIAS do by reference
- if (curr.pos == pos64 && curr.offset != FREE_ENTRY) return;
- if (curr.offset == 0 || curr.offset == FREE_ENTRY) break;
-
- i += curr.offset; //go to next element in the list
- i %= (params().m_hashNumBuckets); //check for overflow
- ++maxIter;
- }
-
- // Limit reached...
- //if (maxIter == COLLISION_LIST_SIZE) return;
-
- int j = i;
- while (maxIter < COLLISION_LIST_SIZE) {
- //offset = curr.offset;
-
- if (curr.offset == FREE_ENTRY) {
- int prevValue = atomicExch(&d_hashBucketMutex[i], LOCK_ENTRY);
- if (prevValue != LOCK_ENTRY) {
- if (i == j) {
- HashEntryHead& entry = d_hash[j].head;
- entry.pos = pos64;
- entry.offset = 0;
- entry.flags = 0;
- } else {
- //InterlockedExchange(g_HashBucketMutex[h], LOCK_ENTRY, prevValue); //lock the hash bucket where we have found a free entry
- prevValue = atomicExch(&d_hashBucketMutex[j], LOCK_ENTRY);
- if (prevValue != LOCK_ENTRY) { //only proceed if the bucket has been locked
- HashEntryHead& entry = d_hash[j].head;
- entry.pos = pos64;
- entry.offset = 0;
- entry.flags = 0; // Flag block as valid in this frame (Nick)
- //entry.ptr = consumeHeap() * SDF_BLOCK_SIZE*SDF_BLOCK_SIZE*SDF_BLOCK_SIZE; //memory alloc
- d_hash[i].head.offset = j-i;
- //setHashEntry(g_Hash, idxLastEntryInBucket, lastEntryInBucket);
- }
- }
- }
- return; //bucket was already locked
- }
-
- ++j;
- j %= (params().m_hashNumBuckets); //check for overflow
- curr = d_hash[j].head; //TODO MATTHIAS do by reference
- ++maxIter;
- }
-}
-
-
-//!inserts a hash entry without allocating any memory: used by streaming: TODO MATTHIAS check the atomics in this function
-/*__device__
-bool HashData::insertHashEntry(HashEntry entry)
-{
- uint h = computeHashPos(entry.pos);
- uint hp = h * HASH_BUCKET_SIZE;
-
- for (uint j = 0; j < HASH_BUCKET_SIZE; j++) {
- uint i = j + hp;
- //const HashEntry& curr = d_hash[i];
- int prevWeight = 0;
- //InterlockedCompareExchange(hash[3*i+2], FREE_ENTRY, LOCK_ENTRY, prevWeight);
- prevWeight = atomicCAS(&d_hash[i].ptr, FREE_ENTRY, LOCK_ENTRY);
- if (prevWeight == FREE_ENTRY) {
- d_hash[i] = entry;
- //setHashEntry(hash, i, entry);
- return true;
- }
- }
-
-#ifdef HANDLE_COLLISIONS
- //updated variables as after the loop
- const uint idxLastEntryInBucket = (h+1)*HASH_BUCKET_SIZE - 1; //get last index of bucket
-
- uint i = idxLastEntryInBucket; //start with the last entry of the current bucket
- HashEntry curr;
-
- unsigned int maxIter = 0;
- //[allow_uav_condition]
- uint g_MaxLoopIterCount = params().m_hashMaxCollisionLinkedListSize;
- #pragma unroll 1
- while (maxIter < g_MaxLoopIterCount) { //traverse list until end // why find the end? we you are inserting at the start !!!
- //curr = getHashEntry(hash, i);
- curr = d_hash[i]; //TODO MATTHIAS do by reference
- if (curr.offset == 0) break; //we have found the end of the list
- i = idxLastEntryInBucket + curr.offset; //go to next element in the list
- i %= (HASH_BUCKET_SIZE * params().m_hashNumBuckets); //check for overflow
-
- maxIter++;
- }
-
- maxIter = 0;
- int offset = 0;
- #pragma unroll 1
- while (maxIter < g_MaxLoopIterCount) { //linear search for free entry
- offset++;
- uint i = (idxLastEntryInBucket + offset) % (HASH_BUCKET_SIZE * params().m_hashNumBuckets); //go to next hash element
- if ((offset % HASH_BUCKET_SIZE) == 0) continue; //cannot insert into a last bucket element (would conflict with other linked lists)
-
- int prevWeight = 0;
- //InterlockedCompareExchange(hash[3*i+2], FREE_ENTRY, LOCK_ENTRY, prevWeight); //check for a free entry
- uint* d_hashUI = (uint*)d_hash;
- prevWeight = prevWeight = atomicCAS(&d_hashUI[3*idxLastEntryInBucket+1], (uint)FREE_ENTRY, (uint)LOCK_ENTRY);
- if (prevWeight == FREE_ENTRY) { //if free entry found set prev->next = curr & curr->next = prev->next
- //[allow_uav_condition]
- //while(hash[3*idxLastEntryInBucket+2] == LOCK_ENTRY); // expects setHashEntry to set the ptr last, required because pos.z is packed into the same value -> prev->next = curr -> might corrput pos.z
-
- HashEntry lastEntryInBucket = d_hash[idxLastEntryInBucket]; //get prev (= lastEntry in Bucket)
-
- int newOffsetPrev = (offset << 16) | (lastEntryInBucket.pos.z & 0x0000ffff); //prev->next = curr (maintain old z-pos)
- int oldOffsetPrev = 0;
- //InterlockedExchange(hash[3*idxLastEntryInBucket+1], newOffsetPrev, oldOffsetPrev); //set prev offset atomically
- uint* d_hashUI = (uint*)d_hash;
- oldOffsetPrev = prevWeight = atomicExch(&d_hashUI[3*idxLastEntryInBucket+1], newOffsetPrev);
- entry.offset = oldOffsetPrev >> 16; //remove prev z-pos from old offset
-
- //setHashEntry(hash, i, entry); //sets the current hashEntry with: curr->next = prev->next
- d_hash[i] = entry;
- return true;
- }
-
- maxIter++;
- }
-#endif
-
- return false;
-}*/
-
-
-
-//! deletes a hash entry position for a given sdfBlock index (returns true uppon successful deletion; otherwise returns false)
-__device__
-bool HashData::deleteHashEntryElement(const int3& sdfBlock) {
- uint h = computeHashPos(sdfBlock); //hash bucket
- const uint64_t pos = compactPosition(sdfBlock);
-
- int i = h;
- int prev = -1;
- HashEntryHead curr;
- unsigned int maxIter = 0;
-
- #pragma unroll 2
- while (maxIter < COLLISION_LIST_SIZE) {
- curr = d_hash[i].head;
-
- //found that dude that we need/want to delete
- if (curr.pos == pos && curr.offset != FREE_ENTRY) {
- //int prevValue = 0;
- //InterlockedExchange(bucketMutex[h], LOCK_ENTRY, prevValue); //lock the hash bucket
- int prevValue = atomicExch(&d_hashBucketMutex[i], LOCK_ENTRY);
- if (prevValue == LOCK_ENTRY) return false;
- if (prevValue != LOCK_ENTRY) {
- prevValue = (prev >= 0) ? atomicExch(&d_hashBucketMutex[prev], LOCK_ENTRY) : 0;
- if (prevValue == LOCK_ENTRY) return false;
- if (prevValue != LOCK_ENTRY) {
- //const uint linBlockSize = SDF_BLOCK_SIZE * SDF_BLOCK_SIZE * SDF_BLOCK_SIZE;
- //appendHeap(curr.ptr / linBlockSize);
- deleteHashEntry(i);
-
- if (prev >= 0) {
- d_hash[prev].head.offset = curr.offset;
- }
- return true;
- }
- }
- }
-
- if (curr.offset == 0 || curr.offset == FREE_ENTRY) { //we have found the end of the list
- return false; //should actually never happen because we need to find that guy before
- }
- prev = i;
- i += curr.offset; //go to next element in the list
- i %= (params().m_hashNumBuckets); //check for overflow
-
- ++maxIter;
- }
-
- return false;
-}
\ No newline at end of file
diff --git a/applications/reconstruct/src/voxel_render.cu b/applications/reconstruct/src/voxel_render.cu
deleted file mode 100644
index 46eb6f57c031b0541add1dcd982a9c6d1e690140..0000000000000000000000000000000000000000
--- a/applications/reconstruct/src/voxel_render.cu
+++ /dev/null
@@ -1,261 +0,0 @@
-#include "splat_render_cuda.hpp"
-#include <cuda_runtime.h>
-
-#include <ftl/cuda_matrix_util.hpp>
-
-#include "splat_params.hpp"
-
-#define T_PER_BLOCK 8
-#define NUM_GROUPS_X 1024
-
-#define NUM_CUDA_BLOCKS 10000
-
-using ftl::cuda::TextureObject;
-using ftl::render::SplatParams;
-
-__global__ void clearDepthKernel(ftl::voxhash::HashData hashData, TextureObject<int> depth) {
- const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
- const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
-
- if (x < depth.width() && y < depth.height()) {
- depth(x,y) = 0x7f800000; //PINF;
- //colour(x,y) = make_uchar4(76,76,82,0);
- }
-}
-
-#define SDF_BLOCK_SIZE_PAD 8
-#define SDF_BLOCK_BUFFER 512 // > 8x8x8
-#define SDF_DX 1
-#define SDF_DY SDF_BLOCK_SIZE_PAD
-#define SDF_DZ (SDF_BLOCK_SIZE_PAD*SDF_BLOCK_SIZE_PAD)
-
-#define LOCKED 0x7FFFFFFF
-
-//! computes the (local) virtual voxel pos of an index; idx in [0;511]
-__device__
-int3 pdelinVoxelIndex(uint idx) {
- int x = idx % SDF_BLOCK_SIZE_PAD;
- int y = (idx % (SDF_BLOCK_SIZE_PAD * SDF_BLOCK_SIZE_PAD)) / SDF_BLOCK_SIZE_PAD;
- int z = idx / (SDF_BLOCK_SIZE_PAD * SDF_BLOCK_SIZE_PAD);
- return make_int3(x,y,z);
-}
-
-//! computes the linearized index of a local virtual voxel pos; pos in [0;7]^3
-__device__
-uint plinVoxelPos(const int3& virtualVoxelPos) {
- return
- virtualVoxelPos.z * SDF_BLOCK_SIZE_PAD * SDF_BLOCK_SIZE_PAD +
- virtualVoxelPos.y * SDF_BLOCK_SIZE_PAD +
- virtualVoxelPos.x;
-}
-
-//! computes the linearized index of a local virtual voxel pos; pos in [0;7]^3
-__device__
-uint plinVoxelPos(int x, int y, int z) {
- return
- z * SDF_BLOCK_SIZE_PAD * SDF_BLOCK_SIZE_PAD +
- y * SDF_BLOCK_SIZE_PAD + x;
-}
-
-__device__
-void deleteVoxel(ftl::voxhash::Voxel& v) {
- v.color = make_uchar3(0,0,0);
- v.weight = 0;
- v.sdf = PINF;
-}
-
-__device__ inline int3 blockDelinear(const int3 &base, uint i) {
- return make_int3(base.x + (i & 0x1), base.y + (i & 0x2), base.z + (i & 0x4));
-}
-
-__device__ inline uint blockLinear(int x, int y, int z) {
- return x + (y << 1) + (z << 2);
-}
-
-__device__ inline bool getVoxel(uint *voxels, int ix) {
- return voxels[ix/32] & (0x1 << (ix % 32));
-}
-
-__global__ void occupied_image_kernel(ftl::voxhash::HashData hashData, TextureObject<int> depth, SplatParams params) {
- __shared__ uint voxels[16];
- __shared__ ftl::voxhash::HashEntryHead block;
-
- // Stride over all allocated blocks
- for (int bi=blockIdx.x; bi<*hashData.d_hashCompactifiedCounter; bi+=NUM_CUDA_BLOCKS) {
- __syncthreads();
-
- const uint i = threadIdx.x; //inside of an SDF block
-
- if (i == 0) block = hashData.d_hashCompactified[bi]->head;
- if (i < 16) {
- voxels[i] = hashData.d_hashCompactified[bi]->voxels[i];
- //valid[i] = hashData.d_hashCompactified[bi]->validity[i];
- }
-
- // Make sure all hash entries are cached
- __syncthreads();
-
- const int3 pi_base = hashData.SDFBlockToVirtualVoxelPos(make_int3(block.posXYZ));
- const int3 vp = make_int3(hashData.delinearizeVoxelIndex(i));
- const int3 pi = pi_base + vp;
- const float3 worldPos = hashData.virtualVoxelPosToWorld(pi);
-
- const bool v = getVoxel(voxels, i);
-
- uchar4 color = make_uchar4(255,0,0,255);
- bool is_surface = v; //((params.m_flags & ftl::render::kShowBlockBorders) && edgeX + edgeY + edgeZ >= 2);
-
-
- // Only for surface voxels, work out screen coordinates
- if (!is_surface) continue;
-
- // TODO: For each original camera, render a new depth map
-
- const float3 camPos = params.m_viewMatrix * worldPos;
- const float2 screenPosf = params.camera.cameraToKinectScreenFloat(camPos);
- const uint2 screenPos = make_uint2(make_int2(screenPosf)); // + make_float2(0.5f, 0.5f)
-
- //printf("Worldpos: %f,%f,%f\n", camPos.x, camPos.y, camPos.z);
-
- if (camPos.z < params.camera.m_sensorDepthWorldMin) continue;
-
- const unsigned int x = screenPos.x;
- const unsigned int y = screenPos.y;
- const int idepth = static_cast<int>(camPos.z * 1000.0f);
-
- // See: Gunther et al. 2013. A GPGPU-based Pipeline for Accelerated Rendering of Point Clouds
- if (x < depth.width() && y < depth.height()) {
- atomicMin(&depth(x,y), idepth);
- }
-
- } // Stride
-}
-
-__global__ void isosurface_image_kernel(ftl::voxhash::HashData hashData, TextureObject<int> depth, SplatParams params) {
- // TODO:(Nick) Reduce bank conflicts by aligning these
- __shared__ uint voxels[16];
- //__shared__ uint valid[16];
- __shared__ ftl::voxhash::HashEntryHead block;
-
- // Stride over all allocated blocks
- for (int bi=blockIdx.x; bi<*hashData.d_hashCompactifiedCounter; bi+=NUM_CUDA_BLOCKS) {
- __syncthreads();
-
- const uint i = threadIdx.x; //inside of an SDF block
-
- if (i == 0) block = hashData.d_hashCompactified[bi]->head;
- if (i < 16) {
- voxels[i] = hashData.d_hashCompactified[bi]->voxels[i];
- //valid[i] = hashData.d_hashCompactified[bi]->validity[i];
- }
-
- // Make sure all hash entries are cached
- __syncthreads();
-
- const int3 pi_base = hashData.SDFBlockToVirtualVoxelPos(make_int3(block.posXYZ));
- const int3 vp = make_int3(hashData.delinearizeVoxelIndex(i));
- const int3 pi = pi_base + vp;
- //const uint j = plinVoxelPos(vp); // Padded linear index
- const float3 worldPos = hashData.virtualVoxelPosToWorld(pi);
-
- // Load distances and colours into shared memory + padding
- //const ftl::voxhash::Voxel &v = hashData.d_SDFBlocks[block.ptr + i];
- //voxels[j] = v;
- const bool v = getVoxel(voxels, i);
-
- //__syncthreads();
-
- //if (voxels[j].weight == 0) continue;
- if (vp.x == 7 || vp.y == 7 || vp.z == 7) continue;
-
-
- int edgeX = (vp.x == 0 ) ? 1 : 0;
- int edgeY = (vp.y == 0 ) ? 1 : 0;
- int edgeZ = (vp.z == 0 ) ? 1 : 0;
-
- uchar4 color = make_uchar4(255,0,0,255);
- bool is_surface = v; //((params.m_flags & ftl::render::kShowBlockBorders) && edgeX + edgeY + edgeZ >= 2);
- //if (is_surface) color = make_uchar4(255,(vp.x == 0 && vp.y == 0 && vp.z == 0) ? 255 : 0,0,255);
-
- if (v) continue; // !getVoxel(valid, i)
-
- //if (vp.z == 7) voxels[j].color = make_uchar3(0,255,(voxels[j].sdf < 0.0f) ? 255 : 0);
-
- // Identify surfaces through sign change. Since we only check in one direction
- // it is fine to check for any sign change?
-
-
-#pragma unroll
- for (int u=0; u<=1; u++) {
- for (int v=0; v<=1; v++) {
- for (int w=0; w<=1; w++) {
- const int3 uvi = make_int3(vp.x+u,vp.y+v,vp.z+w);
-
- // Skip these cases since we didn't load voxels properly
- //if (uvi.x == 8 || uvi.z == 8 || uvi.y == 8) continue;
-
- const bool vox = getVoxel(voxels, hashData.linearizeVoxelPos(uvi));
- if (vox) { //getVoxel(valid, hashData.linearizeVoxelPos(uvi))) {
- is_surface = true;
- // Should break but is slower?
- }
- }
- }
- }
-
- // Only for surface voxels, work out screen coordinates
- if (!is_surface) continue;
-
- // TODO: For each original camera, render a new depth map
-
- const float3 camPos = params.m_viewMatrix * worldPos;
- const float2 screenPosf = params.camera.cameraToKinectScreenFloat(camPos);
- const uint2 screenPos = make_uint2(make_int2(screenPosf)); // + make_float2(0.5f, 0.5f)
-
- //printf("Worldpos: %f,%f,%f\n", camPos.x, camPos.y, camPos.z);
-
- if (camPos.z < params.camera.m_sensorDepthWorldMin) continue;
-
- // For this voxel in hash, get its screen position and check it is on screen
- // Convert depth map to int by x1000 and use atomicMin
- //const int pixsize = static_cast<int>((c_hashParams.m_virtualVoxelSize*params.camera.fx/(camPos.z*0.8f)))+1; // Magic number increase voxel to ensure coverage
-
- const unsigned int x = screenPos.x;
- const unsigned int y = screenPos.y;
- const int idepth = static_cast<int>(camPos.z * 1000.0f);
-
- // See: Gunther et al. 2013. A GPGPU-based Pipeline for Accelerated Rendering of Point Clouds
- if (x < depth.width() && y < depth.height()) {
- atomicMin(&depth(x,y), idepth);
- }
-
- } // Stride
-}
-
-void ftl::cuda::isosurface_point_image(const ftl::voxhash::HashData& hashData,
- const TextureObject<int> &depth,
- const SplatParams ¶ms, cudaStream_t stream) {
-
- const dim3 clear_gridSize((depth.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (depth.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
- const dim3 clear_blockSize(T_PER_BLOCK, T_PER_BLOCK);
-
- clearDepthKernel<<<clear_gridSize, clear_blockSize, 0, stream>>>(hashData, depth);
-
-#ifdef _DEBUG
- cudaSafeCall(cudaDeviceSynchronize());
-#endif
-
- const unsigned int threadsPerBlock = SDF_BLOCK_SIZE*SDF_BLOCK_SIZE*SDF_BLOCK_SIZE;
- const dim3 gridSize(NUM_CUDA_BLOCKS, 1);
- const dim3 blockSize(threadsPerBlock, 1);
-
- occupied_image_kernel<<<gridSize, blockSize, 0, stream>>>(hashData, depth, params);
-
- cudaSafeCall( cudaGetLastError() );
-
-#ifdef _DEBUG
- cudaSafeCall(cudaDeviceSynchronize());
-#endif
-}
-
-
diff --git a/applications/reconstruct/src/voxel_scene.cpp b/applications/reconstruct/src/voxel_scene.cpp
index cba6e61e9e9134845e59d71dd6e69d4ad7ae8beb..09067b1f2334e0190e27022b64d374e31532f8c2 100644
--- a/applications/reconstruct/src/voxel_scene.cpp
+++ b/applications/reconstruct/src/voxel_scene.cpp
@@ -1,7 +1,4 @@
#include <ftl/voxel_scene.hpp>
-#include "compactors.hpp"
-#include "garbage.hpp"
-#include "integrators.hpp"
#include "depth_camera_cuda.hpp"
#include <opencv2/core/cuda_stream_accessor.hpp>
@@ -10,15 +7,16 @@
using namespace ftl::voxhash;
using ftl::rgbd::Source;
+using ftl::rgbd::Channel;
using ftl::Configurable;
using cv::Mat;
using std::vector;
#define SAFE_DELETE_ARRAY(a) { delete [] (a); (a) = NULL; }
-extern "C" void resetCUDA(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams);
-extern "C" void resetHashBucketMutexCUDA(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, cudaStream_t);
-extern "C" void allocCUDA(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, int camid, const DepthCameraParams &depthCameraParams, cudaStream_t);
+//extern "C" void resetCUDA(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams);
+//extern "C" void resetHashBucketMutexCUDA(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, cudaStream_t);
+//extern "C" void allocCUDA(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, int camid, const DepthCameraParams &depthCameraParams, cudaStream_t);
//extern "C" void fillDecisionArrayCUDA(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const DepthCameraData& depthCameraData);
//extern "C" void compactifyHashCUDA(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams);
//extern "C" unsigned int compactifyHashAllInOneCUDA(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams);
@@ -98,7 +96,7 @@ void SceneRep::addSource(ftl::rgbd::Source *src) {
auto &cam = cameras_.emplace_back();
cam.source = src;
cam.params.m_imageWidth = 0;
- src->setChannel(ftl::rgbd::kChanDepth);
+ src->setChannel(Channel::Depth);
}
extern "C" void updateCUDACameraConstant(ftl::voxhash::DepthCameraCUDA *data, int count);
@@ -184,7 +182,7 @@ int SceneRep::upload() {
//if (i > 0) cudaSafeCall(cudaStreamSynchronize(cv::cuda::StreamAccessor::getStream(cameras_[i-1].stream)));
//allocate all hash blocks which are corresponding to depth map entries
- if (value("voxels", false)) _alloc(i, cv::cuda::StreamAccessor::getStream(cam.stream));
+ //if (value("voxels", false)) _alloc(i, cv::cuda::StreamAccessor::getStream(cam.stream));
// Calculate normals
}
@@ -233,6 +231,10 @@ int SceneRep::upload(ftl::rgbd::FrameSet &fs) {
for (size_t i=0; i<cameras_.size(); ++i) {
auto &cam = cameras_[i];
+ auto &chan1 = fs.frames[i].get<cv::Mat>(Channel::Colour);
+ auto &chan2 = fs.frames[i].get<cv::Mat>(fs.sources[i]->getChannel());
+
+ auto test = fs.frames[i].createTexture<uchar4>(Channel::Flow, ftl::rgbd::Format<uchar4>(100,100));
// Get the RGB-Depth frame from input
Source *input = cam.source;
@@ -247,12 +249,12 @@ int SceneRep::upload(ftl::rgbd::FrameSet &fs) {
// Must be in RGBA for GPU
Mat rgbt, rgba;
- cv::cvtColor(fs.channel1[i],rgbt, cv::COLOR_BGR2Lab);
+ cv::cvtColor(chan1,rgbt, cv::COLOR_BGR2Lab);
cv::cvtColor(rgbt,rgba, cv::COLOR_BGR2BGRA);
// Send to GPU and merge view into scene
//cam.gpu.updateParams(cam.params);
- cam.gpu.updateData(fs.channel2[i], rgba, cam.stream);
+ cam.gpu.updateData(chan2, rgba, cam.stream);
//setLastRigidTransform(input->getPose().cast<float>());
@@ -262,7 +264,7 @@ int SceneRep::upload(ftl::rgbd::FrameSet &fs) {
//if (i > 0) cudaSafeCall(cudaStreamSynchronize(cv::cuda::StreamAccessor::getStream(cameras_[i-1].stream)));
//allocate all hash blocks which are corresponding to depth map entries
- if (value("voxels", false)) _alloc(i, cv::cuda::StreamAccessor::getStream(cam.stream));
+ //if (value("voxels", false)) _alloc(i, cv::cuda::StreamAccessor::getStream(cam.stream));
// Calculate normals
}
@@ -297,7 +299,7 @@ void SceneRep::integrate() {
void SceneRep::garbage() {
//_compactifyAllocated();
- if (value("voxels", false)) _garbageCollect();
+ //if (value("voxels", false)) _garbageCollect();
//cudaSafeCall(cudaStreamSynchronize(integ_stream_));
}
@@ -417,19 +419,19 @@ void SceneRep::_alloc(int camid, cudaStream_t stream) {
}
else {*/
//this version is faster, but it doesn't guarantee that all blocks are allocated (staggers alloc to the next frame)
- resetHashBucketMutexCUDA(m_hashData, m_hashParams, stream);
- allocCUDA(m_hashData, m_hashParams, camid, cameras_[camid].params, stream);
+ //resetHashBucketMutexCUDA(m_hashData, m_hashParams, stream);
+ //allocCUDA(m_hashData, m_hashParams, camid, cameras_[camid].params, stream);
//}
}
void SceneRep::_compactifyVisible(const DepthCameraParams &camera) { //const DepthCameraData& depthCameraData) {
- ftl::cuda::compactifyOccupied(m_hashData, m_hashParams, integ_stream_); //this version uses atomics over prefix sums, which has a much better performance
+ //ftl::cuda::compactifyOccupied(m_hashData, m_hashParams, integ_stream_); //this version uses atomics over prefix sums, which has a much better performance
//m_hashData.updateParams(m_hashParams); //make sure numOccupiedBlocks is updated on the GPU
}
void SceneRep::_compactifyAllocated() {
- ftl::cuda::compactifyAllocated(m_hashData, m_hashParams, integ_stream_); //this version uses atomics over prefix sums, which has a much better performance
+ //ftl::cuda::compactifyAllocated(m_hashData, m_hashParams, integ_stream_); //this version uses atomics over prefix sums, which has a much better performance
//std::cout << "Occ blocks = " << m_hashParams.m_numOccupiedBlocks << std::endl;
//m_hashData.updateParams(m_hashParams); //make sure numOccupiedBlocks is updated on the GPU
}
@@ -439,7 +441,7 @@ void SceneRep::_compactifyAllocated() {
else ftl::cuda::integrateRegistration(m_hashData, m_hashParams, depthCameraData, depthCameraParams, integ_stream_);
}*/
-extern "C" void bilateralFilterFloatMap(float* d_output, float* d_input, float sigmaD, float sigmaR, unsigned int width, unsigned int height);
+//extern "C" void bilateralFilterFloatMap(float* d_output, float* d_input, float sigmaD, float sigmaR, unsigned int width, unsigned int height);
void SceneRep::_integrateDepthMaps() {
//cudaSafeCall(cudaDeviceSynchronize());
@@ -454,11 +456,11 @@ void SceneRep::_integrateDepthMaps() {
//ftl::cuda::hole_fill(*(cameras_[i].gpu.depth2_tex_), *(cameras_[i].gpu.depth_tex_), cameras_[i].params, integ_stream_);
//bilateralFilterFloatMap(cameras_[i].gpu.depth_tex_->devicePtr(), cameras_[i].gpu.depth3_tex_->devicePtr(), 3, 7, cameras_[i].gpu.depth_tex_->width(), cameras_[i].gpu.depth_tex_->height());
}
- if (value("voxels", false)) ftl::cuda::integrateDepthMaps(m_hashData, m_hashParams, cameras_.size(), integ_stream_);
+ //if (value("voxels", false)) ftl::cuda::integrateDepthMaps(m_hashData, m_hashParams, cameras_.size(), integ_stream_);
}
void SceneRep::_garbageCollect() {
//ftl::cuda::garbageCollectIdentify(m_hashData, m_hashParams, integ_stream_);
- resetHashBucketMutexCUDA(m_hashData, m_hashParams, integ_stream_); //needed if linked lists are enabled -> for memeory deletion
- ftl::cuda::garbageCollectFree(m_hashData, m_hashParams, integ_stream_);
+ //resetHashBucketMutexCUDA(m_hashData, m_hashParams, integ_stream_); //needed if linked lists are enabled -> for memeory deletion
+ //ftl::cuda::garbageCollectFree(m_hashData, m_hashParams, integ_stream_);
}
diff --git a/applications/vision/CMakeLists.txt b/applications/vision/CMakeLists.txt
index 684b195b5a2a8605e9bc3e04e629d21a35dcd545..1753488f3407b74518efad555ecdd87be54ba1e9 100644
--- a/applications/vision/CMakeLists.txt
+++ b/applications/vision/CMakeLists.txt
@@ -21,6 +21,6 @@ set_property(TARGET ftl-vision PROPERTY CUDA_SEPARABLE_COMPILATION OFF)
endif()
#target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
-target_link_libraries(ftl-vision ftlrgbd ftlcommon ftlctrl ftlrender ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} ftlnet)
+target_link_libraries(ftl-vision ftlrgbd ftlcommon ftlctrl ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} ftlnet)
diff --git a/applications/vision/src/main.cpp b/applications/vision/src/main.cpp
index 1ce6cb162c322d82a51988fe63a9256505283b12..ba07e90424db91c5cc36e18d0aa40a68109bddbd 100644
--- a/applications/vision/src/main.cpp
+++ b/applications/vision/src/main.cpp
@@ -19,7 +19,7 @@
#include <opencv2/opencv.hpp>
#include <ftl/rgbd.hpp>
#include <ftl/middlebury.hpp>
-#include <ftl/display.hpp>
+//#include <ftl/display.hpp>
#include <ftl/rgbd/streamer.hpp>
#include <ftl/net/universe.hpp>
#include <ftl/slave.hpp>
@@ -36,7 +36,7 @@
using ftl::rgbd::Source;
using ftl::rgbd::Camera;
-using ftl::Display;
+//using ftl::Display;
using ftl::rgbd::Streamer;
using ftl::net::Universe;
using std::string;
@@ -87,7 +87,7 @@ static void run(ftl::Configurable *root) {
if (file != "") source->set("uri", file);
- Display *display = ftl::create<Display>(root, "display", "local");
+ //Display *display = ftl::create<Display>(root, "display", "local");
Streamer *stream = ftl::create<Streamer>(root, "stream", net);
stream->add(source);
@@ -95,7 +95,7 @@ static void run(ftl::Configurable *root) {
net->start();
LOG(INFO) << "Running...";
- if (display->hasDisplays()) {
+ /*if (display->hasDisplays()) {
stream->run();
while (ftl::running && display->active()) {
cv::Mat rgb, depth;
@@ -103,9 +103,9 @@ static void run(ftl::Configurable *root) {
if (!rgb.empty()) display->render(rgb, depth, source->parameters());
display->wait(10);
}
- } else {
+ } else {*/
stream->run(true);
- }
+ //}
LOG(INFO) << "Stopping...";
slave.stop();
@@ -115,7 +115,7 @@ static void run(ftl::Configurable *root) {
ftl::pool.stop();
delete stream;
- delete display;
+ //delete display;
//delete source; // TODO(Nick) Add ftl::destroy
delete net;
}
diff --git a/components/codecs/include/ftl/codecs/encoder.hpp b/components/codecs/include/ftl/codecs/encoder.hpp
index 4adeb4b0d625661306321073fad9f84383ed2a42..560810b84060b3b062b426614da40836634fdee5 100644
--- a/components/codecs/include/ftl/codecs/encoder.hpp
+++ b/components/codecs/include/ftl/codecs/encoder.hpp
@@ -1,6 +1,7 @@
#ifndef _FTL_CODECS_ENCODER_HPP_
#define _FTL_CODECS_ENCODER_HPP_
+#include <ftl/cuda_util.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core/cuda.hpp>
diff --git a/components/codecs/src/nvpipe_decoder.cpp b/components/codecs/src/nvpipe_decoder.cpp
index cbeced55e0bd07c1347523c5c682767a769055c0..0dda5884cc7bf156e7cb134795e6f0ff2c180130 100644
--- a/components/codecs/src/nvpipe_decoder.cpp
+++ b/components/codecs/src/nvpipe_decoder.cpp
@@ -2,7 +2,8 @@
#include <loguru.hpp>
-#include <cuda_runtime.h>
+#include <ftl/cuda_util.hpp>
+//#include <cuda_runtime.h>
using ftl::codecs::NvPipeDecoder;
diff --git a/components/codecs/src/nvpipe_encoder.cpp b/components/codecs/src/nvpipe_encoder.cpp
index 1060f0f2a581966cb7fe1b537999d3ab914f506b..42374bedecf95a749e842a3a2ed22b5226d6d390 100644
--- a/components/codecs/src/nvpipe_encoder.cpp
+++ b/components/codecs/src/nvpipe_encoder.cpp
@@ -2,7 +2,7 @@
#include <loguru.hpp>
#include <ftl/timer.hpp>
#include <ftl/codecs/bitrates.hpp>
-#include <cuda_runtime.h>
+#include <ftl/cuda_util.hpp>
using ftl::codecs::NvPipeEncoder;
using ftl::codecs::bitrate_t;
@@ -30,8 +30,6 @@ void NvPipeEncoder::reset() {
/* Check preset resolution is not better than actual resolution. */
definition_t NvPipeEncoder::_verifiedDefinition(definition_t def, const cv::Mat &in) {
- bool is_float = in.type() == CV_32F;
-
int height = ftl::codecs::getHeight(def);
// FIXME: Make sure this can't go forever
@@ -93,7 +91,7 @@ bool NvPipeEncoder::encode(const cv::Mat &in, definition_t odefinition, bitrate_
bool NvPipeEncoder::_encoderMatch(const cv::Mat &in, definition_t def) {
return ((in.type() == CV_32F && is_float_channel_) ||
- (in.type() == CV_8UC3 && !is_float_channel_)) && current_definition_ == def;
+ ((in.type() == CV_8UC3 || in.type() == CV_8UC4) && !is_float_channel_)) && current_definition_ == def;
}
bool NvPipeEncoder::_createEncoder(const cv::Mat &in, definition_t def, bitrate_t rate) {
diff --git a/components/codecs/src/opencv_encoder.cpp b/components/codecs/src/opencv_encoder.cpp
index 5dafabf29a64c14969504960da8af2bc2e27fd2a..028395b9e32865adb7a907d148308c9085588fa3 100644
--- a/components/codecs/src/opencv_encoder.cpp
+++ b/components/codecs/src/opencv_encoder.cpp
@@ -37,9 +37,8 @@ bool OpenCVEncoder::encode(const cv::Mat &in, definition_t definition, bitrate_t
tmp.convertTo(tmp, CV_16UC1, 1000);
}
- // TODO: Choose these base upon resolution
- chunk_count_ = 16;
- chunk_dim_ = 4;
+ chunk_dim_ = (definition == definition_t::LD360) ? 1 : 4;
+ chunk_count_ = chunk_dim_ * chunk_dim_;
jobs_ = chunk_count_;
for (int i=0; i<chunk_count_; ++i) {
diff --git a/components/common/cpp/CMakeLists.txt b/components/common/cpp/CMakeLists.txt
index 8759e81039afede246d4f7e783531da98a1bc473..de0f7707c504447a16967625c2f01ab76e1218bb 100644
--- a/components/common/cpp/CMakeLists.txt
+++ b/components/common/cpp/CMakeLists.txt
@@ -21,7 +21,7 @@ target_include_directories(ftlcommon PUBLIC
$<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})
+target_link_libraries(ftlcommon Threads::Threads ${OS_LIBS} ${OpenCV_LIBS} ${PCL_LIBRARIES} ${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 121abb0605ae912de254f91a4a5be665e7867fff..540b332035a2ad5f671a51ed80f0de31d60f1ce7 100644
--- a/components/common/cpp/include/ftl/config.h.in
+++ b/components/common/cpp/include/ftl/config.h.in
@@ -23,6 +23,7 @@
#cmakedefine HAVE_REALSENSE
#cmakedefine HAVE_NANOGUI
#cmakedefine HAVE_LIBARCHIVE
+#cmakedefine HAVE_OPENVR
#cmakedefine HAVE_NVPIPE
extern const char *FTL_BRANCH;
diff --git a/components/common/cpp/include/ftl/cuda_common.hpp b/components/common/cpp/include/ftl/cuda_common.hpp
index 3f004d0c8ffefaf692b84106dea3810e223d70f3..70a6a4ad6d4dc0def715979f9eaf348e621d103e 100644
--- a/components/common/cpp/include/ftl/cuda_common.hpp
+++ b/components/common/cpp/include/ftl/cuda_common.hpp
@@ -2,12 +2,19 @@
#define _FTL_CUDA_COMMON_HPP_
#include <ftl/config.h>
+#include <ftl/traits.hpp>
#if defined HAVE_CUDA
+#include <ftl/cuda_util.hpp>
#include <opencv2/core/cuda.hpp>
#include <opencv2/core/cuda/common.hpp>
+#ifndef __CUDACC__
+#include <loguru.hpp>
+#include <exception>
+#endif
+
/* Grid stride loop macros */
#define STRIDE_Y(I,N) int I = blockIdx.y * blockDim.y + threadIdx.y; I < N; I += blockDim.y * gridDim.y
#define STRIDE_X(I,N) int I = blockIdx.x * blockDim.x + threadIdx.x; I < N; I += blockDim.x * gridDim.x
@@ -15,69 +22,151 @@
namespace ftl {
namespace cuda {
-/*template <typename T>
-class HisteresisTexture {
+bool initialise();
+
+bool hasCompute(int major, int minor);
+
+int deviceCount();
+
+/**
+ * Represent a CUDA texture object. Instances of this class can be used on both
+ * host and device. A texture object base cannot be constructed directly, it
+ * must be constructed via a template TextureObject class.
+ */
+class TextureObjectBase {
public:
- HisteresisTexture();
- ~HisteresisTexture();
+ __host__ __device__ TextureObjectBase()
+ : texobj_(0), pitch_(0), pitch2_(0), width_(0), height_(0),
+ ptr_(nullptr), needsfree_(false), needsdestroy_(false),
+ cvType_(-1) {};
+ ~TextureObjectBase();
+
+ // Remove ability to copy object directly, instead must use
+ // templated derivative TextureObject.
+ TextureObjectBase(const TextureObjectBase &)=delete;
+ TextureObjectBase &operator=(const TextureObjectBase &)=delete;
+
+ TextureObjectBase(TextureObjectBase &&);
+ TextureObjectBase &operator=(TextureObjectBase &&);
+
+ inline size_t pitch() const { return pitch_; }
+ inline size_t pixelPitch() const { return pitch2_; }
+ inline uchar *devicePtr() const { return ptr_; };
+ __host__ __device__ inline uchar *devicePtr(int v) const { return &ptr_[v*pitch_]; }
+ __host__ __device__ inline int width() const { return width_; }
+ __host__ __device__ inline int height() const { return height_; }
+ __host__ __device__ inline cudaTextureObject_t cudaTexture() const { return texobj_; }
+
+ void upload(const cv::Mat &, cudaStream_t stream=0);
+ void download(cv::Mat &, cudaStream_t stream=0) const;
- HisteresisTexture<T> &operator=(TextureObject<T> &t);
-};*/
+ __host__ void free();
+ inline int cvType() const { return cvType_; }
+
+ protected:
+ cudaTextureObject_t texobj_;
+ size_t pitch_;
+ size_t pitch2_; // in T units
+ int width_;
+ int height_;
+ uchar *ptr_; // Device memory pointer
+ bool needsfree_; // We manage memory, so free it
+ bool needsdestroy_; // The texture object needs to be destroyed
+ int cvType_; // Used to validate casting
+};
+
+/**
+ * Create and manage CUDA texture objects with a particular pixel data type.
+ * Note: it is not possible to create texture objects for certain types,
+ * specificially for 3 channel types.
+ */
template <typename T>
-class TextureObject {
+class TextureObject : public TextureObjectBase {
public:
- __host__ __device__ TextureObject()
- : texobj_(0), pitch_(0), pitch2_(0), width_(0), height_(0), ptr_(nullptr), needsfree_(false) {};
- TextureObject(const cv::cuda::PtrStepSz<T> &d);
+ typedef T type;
+
+ static_assert((16u % sizeof(T)) == 0, "Channel format must be aligned with 16 bytes");
+
+ __host__ __device__ TextureObject() : TextureObjectBase() {};
+ explicit TextureObject(const cv::cuda::GpuMat &d);
+ explicit TextureObject(const cv::cuda::PtrStepSz<T> &d);
TextureObject(T *ptr, int pitch, int width, int height);
TextureObject(size_t width, size_t height);
TextureObject(const TextureObject<T> &t);
__host__ __device__ TextureObject(TextureObject<T> &&);
~TextureObject();
- __host__ TextureObject<T> &operator=(const TextureObject<T> &);
+ TextureObject<T> &operator=(const TextureObject<T> &);
__host__ __device__ TextureObject<T> &operator=(TextureObject<T> &&);
-
- size_t pitch() const { return pitch_; }
- size_t pixelPitch() const { return pitch2_; }
- T *devicePtr() const { return ptr_; };
- __host__ __device__ T *devicePtr(int v) const { return &ptr_[v*pitch2_]; }
- __host__ __device__ int width() const { return width_; }
- __host__ __device__ int height() const { return height_; }
- __host__ __device__ cudaTextureObject_t cudaTexture() const { return texobj_; }
+
+ operator cv::cuda::GpuMat();
+
+ __host__ __device__ T *devicePtr() const { return (T*)(ptr_); };
+ __host__ __device__ T *devicePtr(int v) const { return &(T*)(ptr_)[v*pitch2_]; }
#ifdef __CUDACC__
__device__ inline T tex2D(int u, int v) const { return ::tex2D<T>(texobj_, u, v); }
__device__ inline T tex2D(float u, float v) const { return ::tex2D<T>(texobj_, u, v); }
#endif
- __host__ __device__ inline const T &operator()(int u, int v) const { return ptr_[u+v*pitch2_]; }
- __host__ __device__ inline T &operator()(int u, int v) { return ptr_[u+v*pitch2_]; }
+ __host__ __device__ inline const T &operator()(int u, int v) const { return reinterpret_cast<T*>(ptr_)[u+v*pitch2_]; }
+ __host__ __device__ inline T &operator()(int u, int v) { return reinterpret_cast<T*>(ptr_)[u+v*pitch2_]; }
- void upload(const cv::Mat &, cudaStream_t stream=0);
- void download(cv::Mat &, cudaStream_t stream=0) const;
-
- __host__ void free() {
- if (needsfree_) {
- if (texobj_ != 0) cudaSafeCall( cudaDestroyTextureObject (texobj_) );
- if (ptr_) cudaFree(ptr_);
- ptr_ = nullptr;
- texobj_ = 0;
- }
- }
-
- private:
- cudaTextureObject_t texobj_;
- size_t pitch_;
- size_t pitch2_; // in T units
- int width_;
- int height_;
- T *ptr_;
- bool needsfree_;
- //bool needsdestroy_;
+ /**
+ * Cast a base texture object to this type of texture object. If the
+ * underlying pixel types do not match then a bad_cast exception is thrown.
+ */
+ static TextureObject<T> &cast(TextureObjectBase &);
};
+#ifndef __CUDACC__
+template <typename T>
+TextureObject<T> &TextureObject<T>::cast(TextureObjectBase &b) {
+ if (b.cvType() != ftl::traits::OpenCVType<T>::value) {
+ LOG(ERROR) << "Bad cast of texture object";
+ throw std::bad_cast();
+ }
+ return reinterpret_cast<TextureObject<T>&>(b);
+}
+
+/**
+ * Create a 2D array texture from an OpenCV GpuMat object.
+ */
+template <typename T>
+TextureObject<T>::TextureObject(const cv::cuda::GpuMat &d) {
+ // GpuMat must have correct data type
+ CHECK(d.type() == ftl::traits::OpenCVType<T>::value);
+
+ cudaResourceDesc resDesc;
+ memset(&resDesc, 0, sizeof(resDesc));
+ resDesc.resType = cudaResourceTypePitch2D;
+ resDesc.res.pitch2D.devPtr = d.data;
+ resDesc.res.pitch2D.pitchInBytes = d.step;
+ resDesc.res.pitch2D.desc = cudaCreateChannelDesc<T>();
+ resDesc.res.pitch2D.width = d.cols;
+ resDesc.res.pitch2D.height = d.rows;
+
+ cudaTextureDesc texDesc;
+ // cppcheck-suppress memsetClassFloat
+ memset(&texDesc, 0, sizeof(texDesc));
+ texDesc.readMode = cudaReadModeElementType;
+
+ cudaTextureObject_t tex = 0;
+ cudaSafeCall(cudaCreateTextureObject(&tex, &resDesc, &texDesc, NULL));
+ texobj_ = tex;
+ pitch_ = d.step;
+ pitch2_ = pitch_ / sizeof(T);
+ ptr_ = d.data;
+ width_ = d.cols;
+ height_ = d.rows;
+ needsfree_ = false;
+ cvType_ = ftl::traits::OpenCVType<T>::value;
+ //needsdestroy_ = true;
+}
+
+#endif // __CUDACC__
+
/**
* Create a 2D array texture from an OpenCV GpuMat object.
*/
@@ -93,6 +182,7 @@ TextureObject<T>::TextureObject(const cv::cuda::PtrStepSz<T> &d) {
resDesc.res.pitch2D.height = d.rows;
cudaTextureDesc texDesc;
+ // cppcheck-suppress memsetClassFloat
memset(&texDesc, 0, sizeof(texDesc));
texDesc.readMode = cudaReadModeElementType;
@@ -105,6 +195,7 @@ TextureObject<T>::TextureObject(const cv::cuda::PtrStepSz<T> &d) {
width_ = d.cols;
height_ = d.rows;
needsfree_ = false;
+ cvType_ = ftl::traits::OpenCVType<T>::value;
//needsdestroy_ = true;
}
@@ -124,6 +215,7 @@ TextureObject<T>::TextureObject(T *ptr, int pitch, int width, int height) {
resDesc.res.pitch2D.height = height;
cudaTextureDesc texDesc;
+ // cppcheck-suppress memsetClassFloat
memset(&texDesc, 0, sizeof(texDesc));
texDesc.readMode = cudaReadModeElementType;
@@ -136,6 +228,7 @@ TextureObject<T>::TextureObject(T *ptr, int pitch, int width, int height) {
width_ = width;
height_ = height;
needsfree_ = false;
+ cvType_ = ftl::traits::OpenCVType<T>::value;
//needsdestroy_ = true;
}
@@ -156,6 +249,7 @@ TextureObject<T>::TextureObject(size_t width, size_t height) {
resDesc.res.pitch2D.height = height;
cudaTextureDesc texDesc;
+ // cppcheck-suppress memsetClassFloat
memset(&texDesc, 0, sizeof(texDesc));
texDesc.readMode = cudaReadModeElementType;
cudaCreateTextureObject(&tex, &resDesc, &texDesc, NULL);
@@ -166,6 +260,7 @@ TextureObject<T>::TextureObject(size_t width, size_t height) {
height_ = (int)height;
needsfree_ = true;
pitch2_ = pitch_ / sizeof(T);
+ cvType_ = ftl::traits::OpenCVType<T>::value;
//needsdestroy_ = true;
}
@@ -177,6 +272,7 @@ TextureObject<T>::TextureObject(const TextureObject<T> &p) {
height_ = p.height_;
pitch_ = p.pitch_;
pitch2_ = pitch_ / sizeof(T);
+ cvType_ = ftl::traits::OpenCVType<T>::value;
needsfree_ = false;
}
@@ -192,6 +288,7 @@ TextureObject<T>::TextureObject(TextureObject<T> &&p) {
p.texobj_ = 0;
p.needsfree_ = false;
p.ptr_ = nullptr;
+ cvType_ = ftl::traits::OpenCVType<T>::value;
}
template <typename T>
@@ -202,6 +299,7 @@ TextureObject<T> &TextureObject<T>::operator=(const TextureObject<T> &p) {
height_ = p.height_;
pitch_ = p.pitch_;
pitch2_ = pitch_ / sizeof(T);
+ cvType_ = ftl::traits::OpenCVType<T>::value;
needsfree_ = false;
return *this;
}
@@ -218,6 +316,7 @@ TextureObject<T> &TextureObject<T>::operator=(TextureObject<T> &&p) {
p.texobj_ = 0;
p.needsfree_ = false;
p.ptr_ = nullptr;
+ cvType_ = ftl::traits::OpenCVType<T>::value;
return *this;
}
@@ -228,7 +327,7 @@ TextureObject<T>::~TextureObject() {
free();
}
-template <>
+/*template <>
void TextureObject<uchar4>::upload(const cv::Mat &m, cudaStream_t stream);
template <>
@@ -257,7 +356,7 @@ template <>
void TextureObject<float4>::download(cv::Mat &m, cudaStream_t stream) const;
template <>
-void TextureObject<uchar>::download(cv::Mat &m, cudaStream_t stream) const;
+void TextureObject<uchar>::download(cv::Mat &m, cudaStream_t stream) const;*/
}
}
diff --git a/applications/reconstruct/include/ftl/cuda_matrix_util.hpp b/components/common/cpp/include/ftl/cuda_matrix_util.hpp
similarity index 99%
rename from applications/reconstruct/include/ftl/cuda_matrix_util.hpp
rename to components/common/cpp/include/ftl/cuda_matrix_util.hpp
index e8d803e6fdf7b7c62d52d456ddce288bf554b233..4dd1db7fa8a58c84a40f9d93abd8cc0a492b2792 100644
--- a/applications/reconstruct/include/ftl/cuda_matrix_util.hpp
+++ b/components/common/cpp/include/ftl/cuda_matrix_util.hpp
@@ -1606,6 +1606,7 @@ inline __device__ __host__ matNxM<2, 2> matNxM<2, 2>::getInverse() const
// To Matrix from floatNxN
template<>
+// cppcheck-suppress syntaxError
template<>
inline __device__ __host__ matNxM<1, 1>::matNxM(const float& other)
{
diff --git a/applications/reconstruct/include/ftl/cuda_operators.hpp b/components/common/cpp/include/ftl/cuda_operators.hpp
similarity index 99%
rename from applications/reconstruct/include/ftl/cuda_operators.hpp
rename to components/common/cpp/include/ftl/cuda_operators.hpp
index ae85cfbd785fb72f79225ae28278bf4862b680db..5fc84fbcb158bc599b8bca55e38035757a857648 100644
--- a/applications/reconstruct/include/ftl/cuda_operators.hpp
+++ b/components/common/cpp/include/ftl/cuda_operators.hpp
@@ -19,7 +19,8 @@
#ifndef _FTL_CUDA_OPERATORS_HPP_
#define _FTL_CUDA_OPERATORS_HPP_
-#include <cuda_runtime.h>
+//#include <cuda_runtime.h>
+#include <ftl/cuda_util.hpp>
////////////////////////////////////////////////////////////////////////////////
diff --git a/applications/reconstruct/include/ftl/cuda_util.hpp b/components/common/cpp/include/ftl/cuda_util.hpp
similarity index 86%
rename from applications/reconstruct/include/ftl/cuda_util.hpp
rename to components/common/cpp/include/ftl/cuda_util.hpp
index bf018f07919fe52c71a1104a6bf029ce52f17b8e..e55c1430c1c013780e4b5d9fc0381492da281e49 100644
--- a/applications/reconstruct/include/ftl/cuda_util.hpp
+++ b/components/common/cpp/include/ftl/cuda_util.hpp
@@ -6,7 +6,12 @@
#undef max
#undef min
+#ifdef CPPCHECK
+#define __align__(A)
+#endif
+
#include <cuda_runtime.h>
+#include <vector_types.h>
#include <ftl/cuda_operators.hpp>
// Enable run time assertion checking in kernel code
diff --git a/components/common/cpp/include/ftl/exception.hpp b/components/common/cpp/include/ftl/exception.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6719158bba5f5f53abfcdeb0fb39a5b5dda0d5f2
--- /dev/null
+++ b/components/common/cpp/include/ftl/exception.hpp
@@ -0,0 +1,19 @@
+#ifndef _FTL_EXCEPTION_HPP_
+#define _FTL_EXCEPTION_HPP_
+
+namespace ftl {
+class exception : public std::exception
+{
+ public:
+ explicit exception(const char *msg) : msg_(msg) {};
+
+ const char * what () const throw () {
+ return msg_;
+ }
+
+ private:
+ const char *msg_;
+};
+}
+
+#endif // _FTL_EXCEPTION_HPP_
diff --git a/components/common/cpp/include/ftl/timer.hpp b/components/common/cpp/include/ftl/timer.hpp
index dad98a704dd127c23b961bd7429da14253399db3..97776c2c3ee9bcbb62b8d81909d87a7dc43ecd28 100644
--- a/components/common/cpp/include/ftl/timer.hpp
+++ b/components/common/cpp/include/ftl/timer.hpp
@@ -29,7 +29,9 @@ enum timerlevel_t {
* a destructor, for example.
*/
struct TimerHandle {
- const int id = -1;
+ TimerHandle() : id_(-1) {}
+ explicit TimerHandle(int i) : id_(i) {}
+ TimerHandle(const TimerHandle &t) : id_(t.id()) {}
/**
* Cancel the timer job. If currently executing it will block and wait for
@@ -52,7 +54,12 @@ struct TimerHandle {
/**
* Allow copy assignment.
*/
- TimerHandle &operator=(const TimerHandle &h) { const_cast<int&>(id) = h.id; return *this; }
+ TimerHandle &operator=(const TimerHandle &h) { id_ = h.id(); return *this; }
+
+ inline int id() const { return id_; }
+
+ private:
+ int id_;
};
int64_t get_time();
diff --git a/components/common/cpp/include/ftl/traits.hpp b/components/common/cpp/include/ftl/traits.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6ac54e8e66f6a12aa13e39e1228f5cf17ca69312
--- /dev/null
+++ b/components/common/cpp/include/ftl/traits.hpp
@@ -0,0 +1,44 @@
+#ifndef _FTL_TRAITS_HPP_
+#define _FTL_TRAITS_HPP_
+
+#include <opencv2/core.hpp>
+#include <ftl/cuda_util.hpp>
+
+namespace ftl {
+namespace traits {
+
+template <typename T>
+struct AlwaysFalse : std::false_type {};
+
+template <typename T> struct OpenCVType {
+ static_assert(AlwaysFalse<T>::value, "Not a valid format type");
+};
+template <> struct OpenCVType<uchar> { static const int value = CV_8UC1; };
+template <> struct OpenCVType<uchar2> { static const int value = CV_8UC2; };
+template <> struct OpenCVType<uchar3> { static const int value = CV_8UC3; };
+template <> struct OpenCVType<uchar4> { static const int value = CV_8UC4; };
+template <> struct OpenCVType<char> { static const int value = CV_8SC1; };
+template <> struct OpenCVType<char2> { static const int value = CV_8SC2; };
+template <> struct OpenCVType<char3> { static const int value = CV_8SC3; };
+template <> struct OpenCVType<char4> { static const int value = CV_8SC4; };
+template <> struct OpenCVType<ushort> { static const int value = CV_16UC1; };
+template <> struct OpenCVType<ushort2> { static const int value = CV_16UC2; };
+template <> struct OpenCVType<ushort3> { static const int value = CV_16UC3; };
+template <> struct OpenCVType<ushort4> { static const int value = CV_16UC4; };
+template <> struct OpenCVType<short> { static const int value = CV_16SC1; };
+template <> struct OpenCVType<short2> { static const int value = CV_16SC2; };
+template <> struct OpenCVType<short3> { static const int value = CV_16SC3; };
+template <> struct OpenCVType<short4> { static const int value = CV_16SC4; };
+template <> struct OpenCVType<int> { static const int value = CV_32SC1; };
+template <> struct OpenCVType<int2> { static const int value = CV_32SC2; };
+template <> struct OpenCVType<int3> { static const int value = CV_32SC3; };
+template <> struct OpenCVType<int4> { static const int value = CV_32SC4; };
+template <> struct OpenCVType<float> { static const int value = CV_32FC1; };
+template <> struct OpenCVType<float2> { static const int value = CV_32FC2; };
+template <> struct OpenCVType<float3> { static const int value = CV_32FC3; };
+template <> struct OpenCVType<float4> { static const int value = CV_32FC4; };
+
+}
+}
+
+#endif // _FTL_TRAITS_HPP_
diff --git a/components/common/cpp/src/configuration.cpp b/components/common/cpp/src/configuration.cpp
index ef33ab93a1c93dc5c74ef6787cb47bceab49f71e..8a3849e8ee8799119f3d15700dc277e1dece7654 100644
--- a/components/common/cpp/src/configuration.cpp
+++ b/components/common/cpp/src/configuration.cpp
@@ -20,6 +20,7 @@
#include <ftl/uri.hpp>
#include <ftl/threads.hpp>
#include <ftl/timer.hpp>
+#include <ftl/cuda_common.hpp>
#include <fstream>
#include <string>
@@ -457,7 +458,7 @@ Configurable *ftl::config::configure(ftl::config::json_t &cfg) {
loguru::g_preamble_uptime = false;
loguru::g_preamble_thread = false;
int argc = 1;
- const char *argv[] = {"d",0};
+ const char *argv[]{"d",0};
loguru::init(argc, const_cast<char**>(argv), "--verbosity");
config_index.clear();
@@ -519,6 +520,9 @@ Configurable *ftl::config::configure(int argc, char **argv, const std::string &r
// Some global settings
ftl::timer::setInterval(1000 / rootcfg->value("fps",20));
+ // Check CUDA
+ ftl::cuda::initialise();
+
int pool_size = rootcfg->value("thread_pool_factor", 2.0f)*std::thread::hardware_concurrency();
if (pool_size != ftl::pool.size()) ftl::pool.resize(pool_size);
diff --git a/components/common/cpp/src/cuda_common.cpp b/components/common/cpp/src/cuda_common.cpp
index 571d6816b63413163471ef9006ae1d28031511fd..b29c1df08ba14d61a17d8b7afce290b353c25ce6 100644
--- a/components/common/cpp/src/cuda_common.cpp
+++ b/components/common/cpp/src/cuda_common.cpp
@@ -1,8 +1,106 @@
#include <ftl/cuda_common.hpp>
-using ftl::cuda::TextureObject;
+using ftl::cuda::TextureObjectBase;
-template <>
+static int dev_count = 0;
+static std::vector<cudaDeviceProp> properties;
+
+bool ftl::cuda::initialise() {
+ // Do an initial CUDA check
+ cudaSafeCall(cudaGetDeviceCount(&dev_count));
+ CHECK_GE(dev_count, 1) << "No CUDA devices found";
+
+ LOG(INFO) << "CUDA Devices (" << dev_count << "):";
+
+ properties.resize(dev_count);
+ for (int i=0; i<dev_count; i++) {
+ cudaSafeCall(cudaGetDeviceProperties(&properties[i], i));
+ LOG(INFO) << " - " << properties[i].name;
+ }
+
+ return true;
+}
+
+bool ftl::cuda::hasCompute(int major, int minor) {
+ int dev = -1;
+ cudaSafeCall(cudaGetDevice(&dev));
+
+ if (dev > 0) {
+ return properties[dev].major > major ||
+ (properties[dev].major == major && properties[dev].minor >= minor);
+ }
+ return false;
+}
+
+int ftl::cuda::deviceCount() {
+ return dev_count;
+}
+
+TextureObjectBase::~TextureObjectBase() {
+ free();
+}
+
+TextureObjectBase::TextureObjectBase(TextureObjectBase &&o) {
+ needsfree_ = o.needsfree_;
+ needsdestroy_ = o.needsdestroy_;
+ ptr_ = o.ptr_;
+ texobj_ = o.texobj_;
+ cvType_ = o.cvType_;
+ width_ = o.width_;
+ height_ = o.height_;
+ pitch_ = o.pitch_;
+ pitch2_ = o.pitch2_;
+
+ o.ptr_ = nullptr;
+ o.needsfree_ = false;
+ o.texobj_ = 0;
+ o.needsdestroy_ = false;
+}
+
+TextureObjectBase &TextureObjectBase::operator=(TextureObjectBase &&o) {
+ free();
+
+ needsfree_ = o.needsfree_;
+ needsdestroy_ = o.needsdestroy_;
+ ptr_ = o.ptr_;
+ texobj_ = o.texobj_;
+ cvType_ = o.cvType_;
+ width_ = o.width_;
+ height_ = o.height_;
+ pitch_ = o.pitch_;
+ pitch2_ = o.pitch2_;
+
+ o.ptr_ = nullptr;
+ o.needsfree_ = false;
+ o.texobj_ = 0;
+ o.needsdestroy_ = false;
+ return *this;
+}
+
+void TextureObjectBase::free() {
+ if (needsfree_) {
+ if (texobj_ != 0) cudaSafeCall( cudaDestroyTextureObject (texobj_) );
+ if (ptr_) cudaFree(ptr_);
+ ptr_ = nullptr;
+ texobj_ = 0;
+ cvType_ = -1;
+ } else if (needsdestroy_) {
+ if (texobj_ != 0) cudaSafeCall( cudaDestroyTextureObject (texobj_) );
+ texobj_ = 0;
+ cvType_ = -1;
+ }
+}
+
+void TextureObjectBase::upload(const cv::Mat &m, cudaStream_t stream) {
+ cudaSafeCall(cudaMemcpy2DAsync(devicePtr(), pitch(), m.data, m.step, m.cols * m.elemSize(), m.rows, cudaMemcpyHostToDevice, stream));
+}
+
+void TextureObjectBase::download(cv::Mat &m, cudaStream_t stream) const {
+ m.create(height(), width(), cvType_);
+ cudaSafeCall(cudaMemcpy2DAsync(m.data, m.step, devicePtr(), pitch(), m.cols * m.elemSize(), m.rows, cudaMemcpyDeviceToHost, stream));
+}
+
+/*template <>
void TextureObject<uchar4>::upload(const cv::Mat &m, cudaStream_t stream) {
cudaSafeCall(cudaMemcpy2DAsync(devicePtr(), pitch(), m.data, m.step, m.cols * sizeof(uchar4), m.rows, cudaMemcpyHostToDevice, stream));
}
@@ -56,4 +154,4 @@ template <>
void TextureObject<uchar>::download(cv::Mat &m, cudaStream_t stream) const {
m.create(height(), width(), CV_8UC1);
cudaSafeCall(cudaMemcpy2DAsync(m.data, m.step, devicePtr(), pitch(), m.cols * sizeof(uchar), m.rows, cudaMemcpyDeviceToHost, stream));
-}
+}*/
diff --git a/components/common/cpp/src/timer.cpp b/components/common/cpp/src/timer.cpp
index d6d8441c65e401a021ceb68dc3fbf0eea0cfc4df..328006ab82041d9613ab9ef7847d74727e3aa7f2 100644
--- a/components/common/cpp/src/timer.cpp
+++ b/components/common/cpp/src/timer.cpp
@@ -30,9 +30,10 @@ struct TimerJob {
int id;
function<bool(int64_t)> job;
volatile bool active;
- bool paused;
- int multiplier;
- int countdown;
+ // TODO: (Nick) Implement richer forms of timer
+ //bool paused;
+ //int multiplier;
+ //int countdown;
std::string name;
};
@@ -105,12 +106,12 @@ void ftl::timer::setClockAdjustment(int64_t ms) {
}
const TimerHandle ftl::timer::add(timerlevel_t l, const std::function<bool(int64_t ts)> &f) {
- if (l < 0 || l >= kTimerMAXLEVEL) return {-1};
+ if (l < 0 || l >= kTimerMAXLEVEL) return {};
UNIQUE_LOCK(mtx, lk);
int newid = last_id++;
- jobs[l].push_back({newid, f, false, false, 0, 0, "NoName"});
- return {newid};
+ jobs[l].push_back({newid, f, false, "NoName"});
+ return TimerHandle(newid);
}
static void removeJob(int id) {
@@ -217,7 +218,7 @@ void ftl::timer::reset() {
// ===== TimerHandle ===========================================================
void ftl::timer::TimerHandle::cancel() const {
- removeJob(id);
+ removeJob(id());
}
void ftl::timer::TimerHandle::pause() const {
diff --git a/components/common/cpp/src/uri.cpp b/components/common/cpp/src/uri.cpp
index 0db7a4af505f28bd162d81eed763241ed6f5ff9d..90fb33522c1c2d701fac47b0a6d43a5a6afee3a7 100644
--- a/components/common/cpp/src/uri.cpp
+++ b/components/common/cpp/src/uri.cpp
@@ -41,8 +41,9 @@ void URI::_parse(uri_t puri) {
// NOTE: Non-standard additions to allow for Unix style relative file names.
if (suri[0] == '.') {
char cwdbuf[1024];
- getcwd(cwdbuf, 1024);
- suri = string("file://") + string(cwdbuf) + suri.substr(1);
+ if (getcwd(cwdbuf, 1024)) {
+ suri = string("file://") + string(cwdbuf) + suri.substr(1);
+ }
} else if (suri[0] == '~') {
#ifdef WIN32
suri = string("file://") + string(std::getenv("HOMEDRIVE")) + string(std::getenv("HOMEPATH")) + suri.substr(1);
diff --git a/components/common/cpp/test/CMakeLists.txt b/components/common/cpp/test/CMakeLists.txt
index 2ef8e4338ae4e5bb3b39f4eaf8c88ec343fb1b95..f9c1773b92718fc79eb1e26c1d63c7668f12fa53 100644
--- a/components/common/cpp/test/CMakeLists.txt
+++ b/components/common/cpp/test/CMakeLists.txt
@@ -7,12 +7,13 @@ add_executable(configurable_unit
../src/configuration.cpp
../src/loguru.cpp
../src/ctpl_stl.cpp
+ ../src/cuda_common.cpp
./configurable_unit.cpp
)
target_include_directories(configurable_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../include")
target_link_libraries(configurable_unit
${URIPARSER_LIBRARIES}
- Threads::Threads ${OS_LIBS})
+ Threads::Threads ${OS_LIBS} ${OpenCV_LIBS} ${CUDA_LIBRARIES})
### URI ########################################################################
add_executable(uri_unit
diff --git a/components/common/cpp/test/timer_unit.cpp b/components/common/cpp/test/timer_unit.cpp
index c1d6776299969bb55fe3d9b9c9a91bd3d35430d6..6cdea157e9228b920ce2220c0122c8dcad9cf76e 100644
--- a/components/common/cpp/test/timer_unit.cpp
+++ b/components/common/cpp/test/timer_unit.cpp
@@ -22,7 +22,7 @@ TEST_CASE( "Timer::add() High Precision Accuracy" ) {
return true;
});
- REQUIRE( (rc.id >= 0) );
+ REQUIRE( (rc.id() >= 0) );
ftl::timer::start(true);
REQUIRE( didrun == true );
@@ -40,7 +40,7 @@ TEST_CASE( "Timer::add() High Precision Accuracy" ) {
return true;
});
- REQUIRE( (rc.id >= 0) );
+ REQUIRE( (rc.id() >= 0) );
ftl::timer::start(true);
REQUIRE( didrun == true );
@@ -57,7 +57,7 @@ TEST_CASE( "Timer::add() High Precision Accuracy" ) {
return true;
});
- REQUIRE( (rc.id >= 0) );
+ REQUIRE( (rc.id() >= 0) );
ftl::timer::add(ftl::timer::kTimerHighPrecision, [&didrun](int64_t ts) {
didrun[1] = true;
@@ -90,7 +90,7 @@ TEST_CASE( "Timer::add() Idle10 job" ) {
return true;
});
- REQUIRE( (rc.id >= 0) );
+ REQUIRE( (rc.id() >= 0) );
ftl::timer::start(true);
REQUIRE( didrun == true );
@@ -108,7 +108,7 @@ TEST_CASE( "Timer::add() Idle10 job" ) {
return true;
});
- REQUIRE( (rc.id >= 0) );
+ REQUIRE( (rc.id() >= 0) );
ftl::timer::start(true);
REQUIRE( didrun == true );
@@ -125,7 +125,7 @@ TEST_CASE( "Timer::add() Idle10 job" ) {
return false;
});
- REQUIRE( (rc.id >= 0) );
+ REQUIRE( (rc.id() >= 0) );
ftl::timer::start(true);
REQUIRE( didrun == true );
@@ -145,7 +145,7 @@ TEST_CASE( "Timer::add() Main job" ) {
return true;
});
- REQUIRE( (rc.id >= 0) );
+ REQUIRE( (rc.id() >= 0) );
ftl::timer::start(true);
REQUIRE( didrun == true );
@@ -163,7 +163,7 @@ TEST_CASE( "Timer::add() Main job" ) {
return true;
});
- REQUIRE( (rc.id >= 0) );
+ REQUIRE( (rc.id() >= 0) );
ftl::timer::start(true);
REQUIRE( didrun == true );
@@ -182,7 +182,7 @@ TEST_CASE( "Timer::add() Main job" ) {
return true;
});
- REQUIRE( (rc.id >= 0) );
+ REQUIRE( (rc.id() >= 0) );
ftl::timer::add(ftl::timer::kTimerMain, [&job2](int64_t ts) {
job2++;
@@ -204,7 +204,7 @@ TEST_CASE( "Timer::add() Main job" ) {
return false;
});
- REQUIRE( (rc.id >= 0) );
+ REQUIRE( (rc.id() >= 0) );
ftl::timer::start(true);
REQUIRE( didrun == true );
@@ -224,7 +224,7 @@ TEST_CASE( "TimerHandle::cancel()" ) {
});
// Fake Handle
- ftl::timer::TimerHandle h = {44};
+ ftl::timer::TimerHandle h(44);
h.cancel();
ftl::timer::start(true);
REQUIRE( didjob );
diff --git a/components/net/cpp/include/ftl/net/peer.hpp b/components/net/cpp/include/ftl/net/peer.hpp
index 2c3e1fd636edd16772b3bf44ecb0e15fefa3b16b..976155ac94e323b112306f4fa675512ff3d23aca 100644
--- a/components/net/cpp/include/ftl/net/peer.hpp
+++ b/components/net/cpp/include/ftl/net/peer.hpp
@@ -6,6 +6,7 @@
#endif
#include <ftl/net/common.hpp>
+#include <ftl/exception.hpp>
//#define GLOG_NO_ABBREVIATED_SEVERITIES
#include <loguru.hpp>
@@ -343,7 +344,8 @@ R Peer::call(const std::string &name, ARGS... args) {
if (!hasreturned) {
cancelCall(id);
- throw 1;
+ LOG(ERROR) << "RPC Timeout: " << name;
+ throw ftl::exception("RPC failed with timeout");
}
return result;
diff --git a/components/net/cpp/include/ftl/net/universe.hpp b/components/net/cpp/include/ftl/net/universe.hpp
index b4419b1f7fc713259b0d9f1a14f00ff12332dd6b..29680c601c19ba37ff20771659ef379ce3511631 100644
--- a/components/net/cpp/include/ftl/net/universe.hpp
+++ b/components/net/cpp/include/ftl/net/universe.hpp
@@ -378,7 +378,7 @@ R Universe::call(const ftl::UUID &pid, const std::string &name, ARGS... args) {
if (p == nullptr || !p->isConnected()) {
if (p == nullptr) DLOG(WARNING) << "Attempting to call an unknown peer : " << pid.to_string();
else DLOG(WARNING) << "Attempting to call an disconnected peer : " << pid.to_string();
- throw -1;
+ throw ftl::exception("Calling disconnected peer");
}
return p->call<R>(name, args...);
}
diff --git a/components/net/cpp/src/dispatcher.cpp b/components/net/cpp/src/dispatcher.cpp
index 3231b8ddc4604c7929a04c5c33a36385e1bd94ea..7a5df5091235dc081c0c9e51c199dcca5e7f0f31 100644
--- a/components/net/cpp/src/dispatcher.cpp
+++ b/components/net/cpp/src/dispatcher.cpp
@@ -2,6 +2,7 @@
#include <loguru.hpp>
#include <ftl/net/dispatcher.hpp>
#include <ftl/net/peer.hpp>
+#include <ftl/exception.hpp>
#include <iostream>
using ftl::net::Peer;
@@ -88,13 +89,6 @@ void ftl::net::Dispatcher::dispatch_call(Peer &s, const msgpack::object &msg) {
std::stringstream buf;
msgpack::pack(buf, res_obj);
s.send("__return__", buf.str());*/
- } catch (int e) {
- //throw;
- LOG(ERROR) << "Exception when attempting to call RPC (" << e << ")";
- /*response_t res_obj = std::make_tuple(1,id,msgpack::object(e),msgpack::object());
- std::stringstream buf;
- msgpack::pack(buf, res_obj);
- s.send("__return__", buf.str());*/
}
} else {
LOG(WARNING) << "No binding found for " << name;
@@ -150,7 +144,7 @@ void ftl::net::Dispatcher::enforce_arg_count(std::string const &func, std::size_
std::size_t expected) {
if (found != expected) {
LOG(FATAL) << "RPC argument missmatch for '" << func << "' - " << found << " != " << expected;
- throw -1;
+ throw ftl::exception("RPC argument missmatch");
}
}
@@ -158,7 +152,7 @@ void ftl::net::Dispatcher::enforce_unique_name(std::string const &func) {
auto pos = funcs_.find(func);
if (pos != end(funcs_)) {
LOG(FATAL) << "RPC non unique binding for '" << func << "'";
- throw -1;
+ throw ftl::exception("RPC binding not unique");
}
}
diff --git a/components/net/cpp/src/peer.cpp b/components/net/cpp/src/peer.cpp
index 25059b5473c154d31ea6b31950a3e5e5eff02de8..0335ca67f3a284294b4973c83aea62100d56a5c2 100644
--- a/components/net/cpp/src/peer.cpp
+++ b/components/net/cpp/src/peer.cpp
@@ -424,50 +424,44 @@ void Peer::data() {
//LOG(INFO) << "Pool size: " << ftl::pool.q_size();
int rc=0;
- int c=0;
- //do {
- recv_buf_.reserve_buffer(kMaxMessage);
+ recv_buf_.reserve_buffer(kMaxMessage);
- if (recv_buf_.buffer_capacity() < (kMaxMessage / 10)) {
- LOG(WARNING) << "Net buffer at capacity";
- return;
- }
-
- int cap = recv_buf_.buffer_capacity();
- auto buf = recv_buf_.buffer();
- lk.unlock();
+ if (recv_buf_.buffer_capacity() < (kMaxMessage / 10)) {
+ LOG(WARNING) << "Net buffer at capacity";
+ return;
+ }
- /*#ifndef WIN32
- int n;
- unsigned int m = sizeof(n);
- getsockopt(sock_,SOL_SOCKET,SO_RCVBUF,(void *)&n, &m);
+ int cap = recv_buf_.buffer_capacity();
+ auto buf = recv_buf_.buffer();
+ lk.unlock();
- int pending;
- ioctl(sock_, SIOCINQ, &pending);
- if (pending > 100000) LOG(INFO) << "Buffer usage: " << float(pending) / float(n);
- #endif*/
- rc = ftl::net::internal::recv(sock_, buf, cap, 0);
+ /*#ifndef WIN32
+ int n;
+ unsigned int m = sizeof(n);
+ getsockopt(sock_,SOL_SOCKET,SO_RCVBUF,(void *)&n, &m);
- if (rc >= cap-1) {
- LOG(WARNING) << "More than buffers worth of data received";
- }
- if (cap < (kMaxMessage / 10)) LOG(WARNING) << "NO BUFFER";
-
- if (rc == 0) {
- close();
- return;
- } else if (rc < 0 && c == 0) {
- socketError();
- return;
- }
+ int pending;
+ ioctl(sock_, SIOCINQ, &pending);
+ if (pending > 100000) LOG(INFO) << "Buffer usage: " << float(pending) / float(n);
+ #endif*/
+ rc = ftl::net::internal::recv(sock_, buf, cap, 0);
- //if (rc == -1) break;
- ++c;
-
- lk.lock();
- recv_buf_.buffer_consumed(rc);
- //} while (rc > 0);
+ if (rc >= cap-1) {
+ LOG(WARNING) << "More than buffers worth of data received";
+ }
+ if (cap < (kMaxMessage / 10)) LOG(WARNING) << "NO BUFFER";
+
+ if (rc == 0) {
+ close();
+ return;
+ } else if (rc < 0) {
+ socketError();
+ return;
+ }
+
+ lk.lock();
+ recv_buf_.buffer_consumed(rc);
//auto end = std::chrono::high_resolution_clock::now();
//int64_t endts = std::chrono::time_point_cast<std::chrono::milliseconds>(end).time_since_epoch().count();
diff --git a/components/renderers/cpp/CMakeLists.txt b/components/renderers/cpp/CMakeLists.txt
index 33f910ca0342096bb551b430374776a86de89b2f..b575721587262e2e468d6cb48cf8c44c6771e6fc 100644
--- a/components/renderers/cpp/CMakeLists.txt
+++ b/components/renderers/cpp/CMakeLists.txt
@@ -1,6 +1,7 @@
add_library(ftlrender
- src/display.cpp
- src/rgbd_display.cpp
+ src/splat_render.cpp
+ src/splatter.cu
+ src/points.cu
)
# These cause errors in CI build and are being removed from PCL in newer versions
@@ -11,6 +12,6 @@ target_include_directories(ftlrender PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
PRIVATE src)
-target_link_libraries(ftlrender ftlrgbd ftlcommon ftlnet Eigen3::Eigen Threads::Threads glog::glog ${OpenCV_LIBS} ${PCL_LIBRARIES})
+target_link_libraries(ftlrender ftlrgbd ftlcommon Eigen3::Eigen Threads::Threads ${OpenCV_LIBS})
#ADD_SUBDIRECTORY(test)
diff --git a/applications/reconstruct/src/splat_render_cuda.hpp b/components/renderers/cpp/include/ftl/cuda/intersections.hpp
similarity index 56%
rename from applications/reconstruct/src/splat_render_cuda.hpp
rename to components/renderers/cpp/include/ftl/cuda/intersections.hpp
index e60fc8c27c0d39ef8798803a526891c5da2fca62..9cfdbc2544d9c1bd32c9f5e12a0a161f45c50d54 100644
--- a/applications/reconstruct/src/splat_render_cuda.hpp
+++ b/components/renderers/cpp/include/ftl/cuda/intersections.hpp
@@ -1,11 +1,9 @@
-#ifndef _FTL_RECONSTRUCTION_SPLAT_CUDA_HPP_
-#define _FTL_RECONSTRUCTION_SPLAT_CUDA_HPP_
+#ifndef _FTL_CUDA_INTERSECTIONS_HPP_
+#define _FTL_CUDA_INTERSECTIONS_HPP_
-#include <ftl/depth_camera.hpp>
-#include <ftl/voxel_hash.hpp>
-//#include <ftl/ray_cast_util.hpp>
-
-#include "splat_params.hpp"
+#ifndef PINF
+#define PINF __int_as_float(0x7f800000)
+#endif
namespace ftl {
namespace cuda {
@@ -84,45 +82,7 @@ __device__ inline float intersectDistance(const float3 &n, const float3 &p0, con
return PINF;
}
-/**
- * NOTE: Not strictly isosurface currently since it includes the internals
- * of objects up to at most truncation depth.
- */
-void isosurface_point_image(const ftl::voxhash::HashData& hashData,
- const ftl::cuda::TextureObject<int> &depth,
- const ftl::render::SplatParams ¶ms, cudaStream_t stream);
-
-//void isosurface_point_image_stereo(const ftl::voxhash::HashData& hashData,
-// const ftl::voxhash::HashParams& hashParams,
-// const RayCastData &rayCastData, const RayCastParams ¶ms,
-// cudaStream_t stream);
-
-// TODO: isosurface_point_cloud
-
-void splat_points(const ftl::cuda::TextureObject<int> &depth_in,
- const ftl::cuda::TextureObject<uchar4> &colour_in,
- const ftl::cuda::TextureObject<float4> &normal_in,
- const ftl::cuda::TextureObject<float> &depth_out,
- const ftl::cuda::TextureObject<uchar4> &colour_out, const ftl::render::SplatParams ¶ms, cudaStream_t stream);
-
-void dibr(const ftl::cuda::TextureObject<int> &depth_out,
- const ftl::cuda::TextureObject<uchar4> &colour_out,
- const ftl::cuda::TextureObject<float4> &normal_out,
- const ftl::cuda::TextureObject<float> &confidence_out,
- const ftl::cuda::TextureObject<float4> &tmp_colour,
- const ftl::cuda::TextureObject<int> &tmp_depth, int numcams,
- const ftl::render::SplatParams ¶ms, cudaStream_t stream);
-
-/**
- * Directly render input depth maps to virtual view with clipping.
- */
-void dibr_raw(const ftl::cuda::TextureObject<int> &depth_out, int numcams,
- const ftl::render::SplatParams ¶ms, cudaStream_t stream);
-
-void dibr(const ftl::cuda::TextureObject<float> &depth_out,
- const ftl::cuda::TextureObject<uchar4> &colour_out, int numcams, const ftl::render::SplatParams ¶ms, cudaStream_t stream);
-
}
}
-#endif // _FTL_RECONSTRUCTION_SPLAT_CUDA_HPP_
+#endif // _FTL_CUDA_INTERSECTIONS_HPP_
diff --git a/components/renderers/cpp/include/ftl/cuda/points.hpp b/components/renderers/cpp/include/ftl/cuda/points.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..deffe32777789e2b58a96aef2106975ad37e0cdd
--- /dev/null
+++ b/components/renderers/cpp/include/ftl/cuda/points.hpp
@@ -0,0 +1,16 @@
+#ifndef _FTL_CUDA_POINTS_HPP_
+#define _FTL_CUDA_POINTS_HPP_
+
+#include <ftl/cuda_common.hpp>
+#include <ftl/rgbd/camera.hpp>
+#include <ftl/cuda_matrix_util.hpp>
+
+namespace ftl {
+namespace cuda {
+
+void point_cloud(ftl::cuda::TextureObject<float4> &output, ftl::cuda::TextureObject<float> &depth, const ftl::rgbd::Camera ¶ms, const float4x4 &pose, cudaStream_t stream);
+
+}
+}
+
+#endif // _FTL_CUDA_POINTS_HPP_
diff --git a/components/renderers/cpp/include/ftl/cuda/weighting.hpp b/components/renderers/cpp/include/ftl/cuda/weighting.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9498d0508605087306db2658b2a1ae1943cde536
--- /dev/null
+++ b/components/renderers/cpp/include/ftl/cuda/weighting.hpp
@@ -0,0 +1,23 @@
+#ifndef _FTL_CUDA_WEIGHTING_HPP_
+#define _FTL_CUDA_WEIGHTING_HPP_
+
+namespace ftl {
+namespace cuda {
+
+/*
+ * Guennebaud, G.; Gross, M. Algebraic point set surfaces. ACMTransactions on Graphics Vol. 26, No. 3, Article No. 23, 2007.
+ * Used in: FusionMLS: Highly dynamic 3D reconstruction with consumer-grade RGB-D cameras
+ * r = distance between points
+ * h = smoothing parameter in meters (default 4cm)
+ */
+__device__ inline float spatialWeighting(float r, float h) {
+ if (r >= h) return 0.0f;
+ float rh = r / h;
+ rh = 1.0f - rh*rh;
+ return rh*rh*rh*rh;
+}
+
+}
+}
+
+#endif // _FTL_CUDA_WEIGHTING_HPP_
diff --git a/components/renderers/cpp/include/ftl/display.hpp b/components/renderers/cpp/include/ftl/display.hpp
deleted file mode 100644
index 05ae0bf11e5ebc3a5b8d54339bc85166effbb4a9..0000000000000000000000000000000000000000
--- a/components/renderers/cpp/include/ftl/display.hpp
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#ifndef _FTL_DISPLAY_HPP_
-#define _FTL_DISPLAY_HPP_
-
-#include <ftl/config.h>
-#include <ftl/configurable.hpp>
-#include "../../../rgbd-sources/include/ftl/rgbd/camera.hpp"
-
-#include <nlohmann/json.hpp>
-#include <opencv2/opencv.hpp>
-#include "opencv2/highgui.hpp"
-
-#if defined HAVE_PCL
-#include <pcl/common/common_headers.h>
-#include <pcl/visualization/pcl_visualizer.h>
-#endif // HAVE_PCL
-
-namespace ftl {
-
-/**
- * Multiple local display options for disparity or point clouds.
- */
-class Display : public ftl::Configurable {
- private:
- std::string name_;
- public:
- enum style_t {
- STYLE_NORMAL, STYLE_DISPARITY, STYLE_DEPTH
- };
-
- public:
- explicit Display(nlohmann::json &config, std::string name);
- ~Display();
-
- bool render(const cv::Mat &rgb, const cv::Mat &depth, const ftl::rgbd::Camera &p);
-
-#if defined HAVE_PCL
- bool render(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr);
-#endif // HAVE_PCL
- bool render(const cv::Mat &img, style_t s=STYLE_NORMAL);
-
- bool active() const;
-
- bool hasDisplays();
-
- void wait(int ms);
-
- void onKey(const std::function<void(int)> &h) { key_handlers_.push_back(h); }
-
- private:
-#if defined HAVE_VIZ
- cv::viz::Viz3d *window_;
-#endif // HAVE_VIZ
-
-#if defined HAVE_PCL
- pcl::visualization::PCLVisualizer::Ptr pclviz_;
-#endif // HAVE_PCL
-
- bool active_;
- std::vector<std::function<void(int)>> key_handlers_;
-};
-};
-
-#endif // _FTL_DISPLAY_HPP_
-
diff --git a/components/renderers/cpp/include/ftl/render/renderer.hpp b/components/renderers/cpp/include/ftl/render/renderer.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..1871b9f9f2a8e1fda0766e1c2e74d2169f47f3fa
--- /dev/null
+++ b/components/renderers/cpp/include/ftl/render/renderer.hpp
@@ -0,0 +1,35 @@
+#ifndef _FTL_RENDER_RENDERER_HPP_
+#define _FTL_RENDER_RENDERER_HPP_
+
+#include <ftl/configurable.hpp>
+#include <ftl/rgbd/virtual.hpp>
+#include <ftl/cuda_common.hpp>
+
+namespace ftl {
+namespace render {
+
+/**
+ * Abstract class for all renderers. A renderer takes some 3D scene and
+ * generates a virtual camera perspective of that scene. The scene might be
+ * based upon a point cloud, or an entirely virtual mesh or procedural scene.
+ * It is intended that multiple scenes can be rendered into a single virtual
+ * view using a compositing renderer, such a renderer accepting any kind of
+ * renderer for compositing and hence relying on this base class.
+ */
+class Renderer : public ftl::Configurable {
+ public:
+ explicit Renderer(nlohmann::json &config) : Configurable(config) {};
+ virtual ~Renderer() {};
+
+ /**
+ * Generate a single virtual camera frame. The frame takes its pose from
+ * the virtual camera object passed, and writes the result into the
+ * virtual camera.
+ */
+ virtual bool render(ftl::rgbd::VirtualSource *, ftl::rgbd::Frame &, cudaStream_t)=0;
+};
+
+}
+}
+
+#endif // _FTL_RENDER_RENDERER_HPP_
diff --git a/applications/reconstruct/src/splat_params.hpp b/components/renderers/cpp/include/ftl/render/splat_params.hpp
similarity index 86%
rename from applications/reconstruct/src/splat_params.hpp
rename to components/renderers/cpp/include/ftl/render/splat_params.hpp
index 8ae5bf345e4e0d348c414cc1ce7bb52190d5ffff..4f9c8882b161d7774388e8d9fff7337cb1d6e685 100644
--- a/applications/reconstruct/src/splat_params.hpp
+++ b/components/renderers/cpp/include/ftl/render/splat_params.hpp
@@ -3,7 +3,7 @@
#include <ftl/cuda_util.hpp>
#include <ftl/cuda_matrix_util.hpp>
-#include <ftl/depth_camera_params.hpp>
+#include <ftl/rgbd/camera.hpp>
namespace ftl {
namespace render {
@@ -18,10 +18,10 @@ struct __align__(16) SplatParams {
float4x4 m_viewMatrixInverse;
uint m_flags;
- float voxelSize;
+ //float voxelSize;
float depthThreshold;
- DepthCameraParams camera;
+ ftl::rgbd::Camera camera;
};
}
diff --git a/applications/reconstruct/src/splat_render.hpp b/components/renderers/cpp/include/ftl/render/splat_render.hpp
similarity index 57%
rename from applications/reconstruct/src/splat_render.hpp
rename to components/renderers/cpp/include/ftl/render/splat_render.hpp
index 828db11fad6bbb5e3aeeae0899bc15549fab1708..2cbb82a8183a3a6269a3888134a57144c93050ca 100644
--- a/applications/reconstruct/src/splat_render.hpp
+++ b/components/renderers/cpp/include/ftl/render/splat_render.hpp
@@ -1,14 +1,9 @@
#ifndef _FTL_RECONSTRUCTION_SPLAT_HPP_
#define _FTL_RECONSTRUCTION_SPLAT_HPP_
-#include <ftl/configurable.hpp>
-#include <ftl/rgbd/source.hpp>
-#include <ftl/depth_camera.hpp>
-#include <ftl/voxel_scene.hpp>
-//#include <ftl/ray_cast_util.hpp>
-#include <ftl/cuda_common.hpp>
-
-#include "splat_params.hpp"
+#include <ftl/render/renderer.hpp>
+#include <ftl/rgbd/frameset.hpp>
+#include <ftl/render/splat_params.hpp>
namespace ftl {
namespace render {
@@ -21,26 +16,30 @@ namespace render {
* on a separate machine or at a later time, the advantage being to save local
* processing resources and that the first pass result may compress better.
*/
-class Splatter {
+class Splatter : public ftl::render::Renderer {
public:
- explicit Splatter(ftl::voxhash::SceneRep *scene);
+ explicit Splatter(nlohmann::json &config, ftl::rgbd::FrameSet *fs);
~Splatter();
- void render(int64_t ts, ftl::rgbd::Source *src, cudaStream_t stream=0);
+ bool render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, cudaStream_t stream=0) override;
+ //void setOutputDevice(int);
- void setOutputDevice(int);
+ protected:
+ void renderChannel(ftl::render::SplatParams ¶ms, ftl::rgbd::Frame &out, const ftl::rgbd::Channel &channel, cudaStream_t stream);
private:
int device_;
- ftl::cuda::TextureObject<int> depth1_;
+ /*ftl::cuda::TextureObject<int> depth1_;
ftl::cuda::TextureObject<int> depth3_;
ftl::cuda::TextureObject<uchar4> colour1_;
ftl::cuda::TextureObject<float4> colour_tmp_;
ftl::cuda::TextureObject<float> depth2_;
ftl::cuda::TextureObject<uchar4> colour2_;
- ftl::cuda::TextureObject<float4> normal1_;
- SplatParams params_;
- ftl::voxhash::SceneRep *scene_;
+ ftl::cuda::TextureObject<float4> normal1_;*/
+ //SplatParams params_;
+
+ ftl::rgbd::Frame temp_;
+ ftl::rgbd::FrameSet *scene_;
};
}
diff --git a/components/renderers/cpp/include/ftl/rgbd_display.hpp b/components/renderers/cpp/include/ftl/rgbd_display.hpp
deleted file mode 100644
index 9c1be76fb5f38a584d3032d50b6ef046f0d0b605..0000000000000000000000000000000000000000
--- a/components/renderers/cpp/include/ftl/rgbd_display.hpp
+++ /dev/null
@@ -1,47 +0,0 @@
-#ifndef _FTL_RGBD_DISPLAY_HPP_
-#define _FTL_RGBD_DISPLAY_HPP_
-
-#include <nlohmann/json.hpp>
-#include <ftl/rgbd/source.hpp>
-
-using MouseAction = std::function<void(int, int, int, int)>;
-
-namespace ftl {
-namespace rgbd {
-
-class Display : public ftl::Configurable {
- public:
- explicit Display(nlohmann::json &);
- Display(nlohmann::json &, Source *);
- ~Display();
-
- void setSource(Source *src) { source_ = src; }
- void update();
-
- bool active() const { return active_; }
-
- void onKey(const std::function<void(int)> &h) { key_handlers_.push_back(h); }
-
- void wait(int ms);
-
- private:
- Source *source_;
- std::string name_;
- std::vector<std::function<void(int)>> key_handlers_;
- Eigen::Vector3d eye_;
- Eigen::Vector3d centre_;
- Eigen::Vector3d up_;
- Eigen::Vector3d lookPoint_;
- float lerpSpeed_;
- bool active_;
- MouseAction mouseaction_;
-
- static int viewcount__;
-
- void init();
-};
-
-}
-}
-
-#endif // _FTL_RGBD_DISPLAY_HPP_
diff --git a/applications/reconstruct/include/ftl/matrix_conversion.hpp b/components/renderers/cpp/include/ftl/utility/matrix_conversion.hpp
similarity index 100%
rename from applications/reconstruct/include/ftl/matrix_conversion.hpp
rename to components/renderers/cpp/include/ftl/utility/matrix_conversion.hpp
diff --git a/applications/reconstruct/src/dibr.cu b/components/renderers/cpp/src/dibr.cu
similarity index 98%
rename from applications/reconstruct/src/dibr.cu
rename to components/renderers/cpp/src/dibr.cu
index a7ba0e9df91637c687eda13d1048f323f72a30f4..66428ce3086e42b6a3e81c667ae126314d910c98 100644
--- a/applications/reconstruct/src/dibr.cu
+++ b/components/renderers/cpp/src/dibr.cu
@@ -1,6 +1,6 @@
#include "splat_render_cuda.hpp"
#include "depth_camera_cuda.hpp"
-#include <cuda_runtime.h>
+//#include <cuda_runtime.h>
#include <ftl/cuda_matrix_util.hpp>
@@ -158,8 +158,7 @@ __global__ void OLD_dibr_visibility_kernel(TextureObject<int> depth, int cam, Sp
const int upsample = min(UPSAMPLE_MAX, int((r) * params.camera.fx / camPos.z));
// Not on screen so stop now...
- if (screenPos.x + upsample < 0 || screenPos.y + upsample < 0 ||
- screenPos.x - upsample >= depth.width() || screenPos.y - upsample >= depth.height()) return;
+ if (screenPos.x - upsample >= depth.width() || screenPos.y - upsample >= depth.height()) return;
// TODO:(Nick) Check depth buffer and don't do anything if already hidden?
@@ -181,7 +180,7 @@ __global__ void OLD_dibr_visibility_kernel(TextureObject<int> depth, int cam, Sp
// and depth remains within the bounds.
// How to find min and max depths?
- float ld = nearest.z;
+ //float ld = nearest.z;
// TODO: (Nick) Estimate depth using points plane, but needs better normals.
//float t;
@@ -241,7 +240,7 @@ __global__ void OLD_dibr_visibility_kernel(TextureObject<int> depth, int cam, Sp
}*/
//nearest = params.camera.kinectDepthToSkeleton(screenPos.x+u,screenPos.y+v,d); // ld + (d - ld)*0.8f
- ld = d;
+ //ld = d;
}
//}
}
@@ -287,8 +286,7 @@ __global__ void OLD_dibr_visibility_kernel(TextureObject<int> depth, int cam, Sp
const int upsample = min(UPSAMPLE_MAX, int((4.0f*r) * params.camera.fx / camPos.z));
// Not on screen so stop now...
- if (screenPos.x + upsample < 0 || screenPos.y + upsample < 0 ||
- screenPos.x - upsample >= depth.width() || screenPos.y - upsample >= depth.height()) return;
+ if (screenPos.x - upsample >= depth.width() || screenPos.y - upsample >= depth.height()) return;
// TODO:(Nick) Check depth buffer and don't do anything if already hidden?
@@ -575,7 +573,7 @@ __global__ void dibr_attribute_contrib_kernel(
const ftl::voxhash::DepthCameraCUDA &camera = c_cameras[cam];
const int tid = (threadIdx.x + threadIdx.y * blockDim.x);
- const int warp = tid / WARP_SIZE;
+ //const int warp = tid / WARP_SIZE;
const int x = (blockIdx.x*blockDim.x + threadIdx.x) / WARP_SIZE;
const int y = blockIdx.y*blockDim.y + threadIdx.y;
@@ -592,8 +590,7 @@ __global__ void dibr_attribute_contrib_kernel(
const int upsample = 8; //min(UPSAMPLE_MAX, int((5.0f*r) * params.camera.fx / camPos.z));
// Not on screen so stop now...
- if (screenPos.x < 0 || screenPos.y < 0 ||
- screenPos.x >= depth_in.width() || screenPos.y >= depth_in.height()) return;
+ if (screenPos.x >= depth_in.width() || screenPos.y >= depth_in.height()) return;
// Is this point near the actual surface and therefore a contributor?
const float d = ((float)depth_in.tex2D((int)screenPos.x, (int)screenPos.y)/1000.0f);
@@ -722,7 +719,7 @@ void ftl::cuda::dibr(const TextureObject<int> &depth_out,
cudaSafeCall(cudaDeviceSynchronize());
#endif
- int i=3;
+ //int i=3;
bool noSplatting = params.m_flags & ftl::render::kNoSplatting;
diff --git a/components/renderers/cpp/src/display.cpp b/components/renderers/cpp/src/display.cpp
deleted file mode 100644
index 0f838df751d0c0a315f4d0acca9798d2d7741066..0000000000000000000000000000000000000000
--- a/components/renderers/cpp/src/display.cpp
+++ /dev/null
@@ -1,287 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#include <loguru.hpp>
-
-#include <ftl/display.hpp>
-#include <ftl/utility/opencv_to_pcl.hpp>
-
-using ftl::Display;
-using cv::Mat;
-using cv::Vec3f;
-
-Display::Display(nlohmann::json &config, std::string name) : ftl::Configurable(config) {
- name_ = name;
-#if defined HAVE_VIZ
- window_ = new cv::viz::Viz3d("FTL: " + name);
- window_->setBackgroundColor(cv::viz::Color::white());
-#endif // HAVE_VIZ
-
- //cv::namedWindow("Image", cv::WINDOW_KEEPRATIO);
-
-#if defined HAVE_PCL
- if (value("points", false)) {
- pclviz_ = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer ("FTL Cloud: " + name));
- pclviz_->setBackgroundColor (255, 255, 255);
- pclviz_->addCoordinateSystem (1.0);
- pclviz_->setShowFPS(true);
- pclviz_->initCameraParameters ();
-
- pclviz_->registerPointPickingCallback(
- [](const pcl::visualization::PointPickingEvent& event, void* viewer_void) {
- if (event.getPointIndex () == -1) return;
- float x, y, z;
- event.getPoint(x, y, z);
- LOG(INFO) << "( " << x << ", " << y << ", " << z << ")";
- }, (void*) &pclviz_);
-
- pclviz_->registerKeyboardCallback (
- [](const pcl::visualization::KeyboardEvent &event, void* viewer_void) {
- auto viewer = *static_cast<pcl::visualization::PCLVisualizer::Ptr*>(viewer_void);
- pcl::visualization::Camera cam;
- viewer->getCameraParameters(cam);
-
- Eigen::Vector3f pos(cam.pos[0], cam.pos[1], cam.pos[2]);
- Eigen::Vector3f focal(cam.focal[0], cam.focal[1], cam.focal[2]);
- Eigen::Vector3f dir = focal - pos; //.normalize();
- dir.normalize();
-
- const float speed = 40.0f;
-
- if (event.getKeySym() == "Up") {
- pos += speed*dir;
- focal += speed*dir;
- } else if (event.getKeySym() == "Down") {
- pos -= speed*dir;
- focal -= speed*dir;
- } else if (event.getKeySym() == "Left") {
- Eigen::Matrix3f m = Eigen::AngleAxisf(-0.5f*M_PI, Eigen::Vector3f::UnitY()).toRotationMatrix();
- dir = m*dir;
- pos += speed*dir;
- focal += speed*dir;
- } else if (event.getKeySym() == "Right") {
- Eigen::Matrix3f m = Eigen::AngleAxisf(0.5f*M_PI, Eigen::Vector3f::UnitY()).toRotationMatrix();
- dir = m*dir;
- pos += speed*dir;
- focal += speed*dir;
- }
-
-
- cam.pos[0] = pos[0];
- cam.pos[1] = pos[1];
- cam.pos[2] = pos[2];
- cam.focal[0] = focal[0];
- cam.focal[1] = focal[1];
- cam.focal[2] = focal[2];
- viewer->setCameraParameters(cam);
-
- }, (void*)&pclviz_);
- }
-#endif // HAVE_PCL
-
- active_ = true;
-}
-
-Display::~Display() {
- #if defined HAVE_VIZ
- delete window_;
- #endif // HAVE_VIZ
-}
-
-#ifdef HAVE_PCL
-/**
- * Convert an OpenCV RGB and Depth Mats to a PCL XYZRGB point cloud.
- */
-static pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgbdToPointXYZ(const cv::Mat &rgb, const cv::Mat &depth, const ftl::rgbd::Camera &p) {
- const double CX = p.cx;
- const double CY = p.cy;
- const double FX = p.fx;
- const double FY = p.fy;
-
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
- point_cloud_ptr->width = rgb.cols * rgb.rows;
- point_cloud_ptr->height = 1;
-
- for(int i=0;i<rgb.rows;i++) {
- const float *sptr = depth.ptr<float>(i);
- for(int j=0;j<rgb.cols;j++) {
- float d = sptr[j] * 1000.0f;
-
- pcl::PointXYZRGB point;
- point.x = (((double)j + CX) / FX) * d;
- point.y = (((double)i + CY) / FY) * d;
- point.z = d;
-
- if (point.x == INFINITY || point.y == INFINITY || point.z > 20000.0f || point.z < 0.04f) {
- point.x = 0.0f; point.y = 0.0f; point.z = 0.0f;
- }
-
- cv::Point3_<uchar> prgb = rgb.at<cv::Point3_<uchar>>(i, j);
- uint32_t rgb = (static_cast<uint32_t>(prgb.z) << 16 | static_cast<uint32_t>(prgb.y) << 8 | static_cast<uint32_t>(prgb.x));
- point.rgb = *reinterpret_cast<float*>(&rgb);
-
- point_cloud_ptr -> points.push_back(point);
- }
- }
-
- return point_cloud_ptr;
-}
-#endif // HAVE_PCL
-
-bool Display::render(const cv::Mat &rgb, const cv::Mat &depth, const ftl::rgbd::Camera &p) {
- Mat idepth;
-
- if (value("points", false) && rgb.rows != 0) {
-#if defined HAVE_PCL
- auto pc = rgbdToPointXYZ(rgb, depth, p);
-
- pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pc);
- if (!pclviz_->updatePointCloud<pcl::PointXYZRGB> (pc, rgb, "reconstruction")) {
- pclviz_->addPointCloud<pcl::PointXYZRGB> (pc, rgb, "reconstruction");
- pclviz_->setCameraPosition(-878.0, -71.0, -2315.0, -0.1, -0.99, 0.068, 0.0, -1.0, 0.0);
- pclviz_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "reconstruction");
- }
-#elif defined HAVE_VIZ
- //cv::Mat Q_32F;
- //calibrate_.getQ().convertTo(Q_32F, CV_32F);
- /*cv::Mat_<cv::Vec3f> XYZ(depth.rows, depth.cols); // Output point cloud
- reprojectImageTo3D(depth+20.0f, XYZ, q, true);
-
- // Remove all invalid pixels from point cloud
- XYZ.setTo(Vec3f(NAN, NAN, NAN), depth == 0.0f);
-
- cv::viz::WCloud cloud_widget = cv::viz::WCloud(XYZ, rgb);
- cloud_widget.setRenderingProperty(cv::viz::POINT_SIZE, 2);
-
- window_->showWidget("coosys", cv::viz::WCoordinateSystem());
- window_->showWidget("Depth", cloud_widget);
-
- //window_->spinOnce(40, true);*/
-
-#else // HAVE_VIZ
-
- LOG(ERROR) << "Need OpenCV Viz module to display points";
-
-#endif // HAVE_VIZ
- }
-
- if (value("left", false)) {
- if (value("crosshair", false)) {
- cv::line(rgb, cv::Point(0, rgb.rows/2), cv::Point(rgb.cols-1, rgb.rows/2), cv::Scalar(0,0,255), 1);
- cv::line(rgb, cv::Point(rgb.cols/2, 0), cv::Point(rgb.cols/2, rgb.rows-1), cv::Scalar(0,0,255), 1);
- }
- cv::namedWindow("Left: " + name_, cv::WINDOW_KEEPRATIO);
- cv::imshow("Left: " + name_, rgb);
- }
- if (value("right", false)) {
- /*if (config_["crosshair"]) {
- cv::line(rgbr, cv::Point(0, rgbr.rows/2), cv::Point(rgbr.cols-1, rgbr.rows/2), cv::Scalar(0,0,255), 1);
- cv::line(rgbr, cv::Point(rgbr.cols/2, 0), cv::Point(rgbr.cols/2, rgbr.rows-1), cv::Scalar(0,0,255), 1);
- }
- cv::namedWindow("Right: " + name_, cv::WINDOW_KEEPRATIO);
- cv::imshow("Right: " + name_, rgbr);*/
- }
-
- if (value("disparity", false)) {
- /*Mat depth32F = (focal * (float)l.cols * base_line) / depth;
- normalize(depth32F, depth32F, 0, 255, NORM_MINMAX, CV_8U);
- cv::imshow("Depth", depth32F);
- if(cv::waitKey(10) == 27){
- //exit if ESC is pressed
- active_ = false;
- }*/
- } else if (value("depth", false)) {
- if (value("flip_vert", false)) {
- cv::flip(depth, idepth, 0);
- } else {
- idepth = depth;
- }
-
- idepth.convertTo(idepth, CV_8U, 255.0f / 10.0f); // TODO(nick)
-
- applyColorMap(idepth, idepth, cv::COLORMAP_JET);
- cv::imshow("Depth: " + name_, idepth);
- //if(cv::waitKey(40) == 27) {
- // exit if ESC is pressed
- // active_ = false;
- //}
- }
-
- return true;
-}
-
-#if defined HAVE_PCL
-bool Display::render(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr pc) {
- pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pc);
- if (pclviz_ && !pclviz_->updatePointCloud<pcl::PointXYZRGB> (pc, rgb, "reconstruction")) {
- pclviz_->addPointCloud<pcl::PointXYZRGB> (pc, rgb, "reconstruction");
- pclviz_->setCameraPosition(-878.0, -71.0, -2315.0, -0.1, -0.99, 0.068, 0.0, -1.0, 0.0);
- pclviz_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "reconstruction");
- }
- return true;
-}
-#endif // HAVE_PCL
-bool Display::render(const cv::Mat &img, style_t s) {
- if (s == STYLE_NORMAL) {
- cv::imshow("Image", img);
- } else if (s == STYLE_DISPARITY) {
- Mat idepth;
-
- if (value("flip_vert", false)) {
- cv::flip(img, idepth, 0);
- } else {
- idepth = img;
- }
-
- idepth.convertTo(idepth, CV_8U, 255.0f / 256.0f);
-
- applyColorMap(idepth, idepth, cv::COLORMAP_JET);
- cv::imshow("Disparity", idepth);
- }
-
- return true;
-}
-
-bool Display::hasDisplays() {
- return value("depth", false) || value("left", false) || value("right", false) || value("points", false);
-}
-
-void Display::wait(int ms) {
- if (value("points", false)) {
- #if defined HAVE_PCL
- if (pclviz_) pclviz_->spinOnce(20);
- #elif defined HAVE_VIZ
- window_->spinOnce(1, true);
- #endif // HAVE_VIZ
- }
-
- if (value("depth", false) || value("left", false) || value("right", false)) {
- while (true) {
- int key = cv::waitKey(ms);
-
- if(key == 27) {
- // exit if ESC is pressed
- active_ = false;
- } else if (key == -1) {
- return;
- } else {
- ms = 1;
- for (auto &h : key_handlers_) {
- h(key);
- }
- }
- }
- }
-}
-
-bool Display::active() const {
- #if defined HAVE_PCL
- return active_ && (!pclviz_ || !pclviz_->wasStopped());
- #elif defined HAVE_VIZ
- return active_ && !window_->wasStopped();
- #else
- return active_;
- #endif
-}
-
diff --git a/applications/reconstruct/src/mls_cuda.hpp b/components/renderers/cpp/src/mls_cuda.hpp
similarity index 100%
rename from applications/reconstruct/src/mls_cuda.hpp
rename to components/renderers/cpp/src/mls_cuda.hpp
diff --git a/components/renderers/cpp/src/points.cu b/components/renderers/cpp/src/points.cu
new file mode 100644
index 0000000000000000000000000000000000000000..39764e4c8aba523caf2758262d9f41f8782ac9dc
--- /dev/null
+++ b/components/renderers/cpp/src/points.cu
@@ -0,0 +1,28 @@
+#include <ftl/cuda/points.hpp>
+
+#define T_PER_BLOCK 8
+
+__global__ void point_cloud_kernel(ftl::cuda::TextureObject<float4> output, ftl::cuda::TextureObject<float> depth, ftl::rgbd::Camera params, float4x4 pose)
+{
+ const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
+ const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+ if (x < params.width && y < params.height) {
+ float d = depth.tex2D((int)x, (int)y);
+
+ output(x,y) = (d >= params.minDepth && d <= params.maxDepth) ?
+ make_float4(pose * params.screenToCam(x, y, d), 0.0f) :
+ make_float4(MINF, MINF, MINF, MINF);
+ }
+}
+
+void ftl::cuda::point_cloud(ftl::cuda::TextureObject<float4> &output, ftl::cuda::TextureObject<float> &depth, const ftl::rgbd::Camera ¶ms, const float4x4 &pose, cudaStream_t stream) {
+ const dim3 gridSize((params.width + T_PER_BLOCK - 1)/T_PER_BLOCK, (params.height + T_PER_BLOCK - 1)/T_PER_BLOCK);
+ const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
+
+ point_cloud_kernel<<<gridSize, blockSize, 0, stream>>>(output, depth, params, pose);
+
+#ifdef _DEBUG
+ cudaSafeCall(cudaDeviceSynchronize());
+#endif
+}
diff --git a/components/renderers/cpp/src/rgbd_display.cpp b/components/renderers/cpp/src/rgbd_display.cpp
deleted file mode 100644
index a0d79f8aeeb42b0a0a1fd281c5f3e9065b43780c..0000000000000000000000000000000000000000
--- a/components/renderers/cpp/src/rgbd_display.cpp
+++ /dev/null
@@ -1,129 +0,0 @@
-#include <ftl/rgbd_display.hpp>
-#include <opencv2/opencv.hpp>
-
-using ftl::rgbd::Source;
-using ftl::rgbd::Display;
-using std::string;
-using cv::Mat;
-
-int Display::viewcount__ = 0;
-
-template<class T>
-Eigen::Matrix<T,4,4> lookAt
-(
- Eigen::Matrix<T,3,1> const & eye,
- Eigen::Matrix<T,3,1> const & center,
- Eigen::Matrix<T,3,1> const & up
-)
-{
- typedef Eigen::Matrix<T,4,4> Matrix4;
- typedef Eigen::Matrix<T,3,1> Vector3;
-
- Vector3 f = (center - eye).normalized();
- Vector3 u = up.normalized();
- Vector3 s = f.cross(u).normalized();
- u = s.cross(f);
-
- Matrix4 res;
- res << s.x(),s.y(),s.z(),-s.dot(eye),
- u.x(),u.y(),u.z(),-u.dot(eye),
- -f.x(),-f.y(),-f.z(),f.dot(eye),
- 0,0,0,1;
-
- return res;
-}
-
-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);
-}
-
-Display::Display(nlohmann::json &config) : ftl::Configurable(config) {
- name_ = value("name", string("View [")+std::to_string(viewcount__)+string("]"));
- viewcount__++;
-
- init();
-}
-
-Display::Display(nlohmann::json &config, Source *source)
- : ftl::Configurable(config) {
- name_ = value("name", string("View [")+std::to_string(viewcount__)+string("]"));
- viewcount__++;
- init();
-}
-
-Display::~Display() {
-
-}
-
-void Display::init() {
- active_ = true;
- source_ = nullptr;
- cv::namedWindow(name_, cv::WINDOW_KEEPRATIO);
-
- eye_ = Eigen::Vector3d(0.0, 0.0, 0.0);
- centre_ = Eigen::Vector3d(0.0, 0.0, -4.0);
- up_ = Eigen::Vector3d(0,1.0,0);
- lookPoint_ = Eigen::Vector3d(0.0,0.0,-4.0);
- lerpSpeed_ = 0.4f;
-
- // Keyboard camera controls
- onKey([this](int key) {
- //LOG(INFO) << "Key = " << key;
- if (key == 81 || key == 83) {
- Eigen::Quaternion<double> q; q = Eigen::AngleAxis<double>((key == 81) ? 0.01 : -0.01, up_);
- eye_ = (q * (eye_ - centre_)) + centre_;
- } else if (key == 84 || key == 82) {
- double scalar = (key == 84) ? 0.99 : 1.01;
- eye_ = ((eye_ - centre_) * scalar) + centre_;
- }
- });
-
- // TODO(Nick) Calculate "camera" properties of viewport.
- mouseaction_ = [this]( int event, int ux, int uy, int) {
- //LOG(INFO) << "Mouse " << ux << "," << uy;
- if (event == 1 && source_) { // click
- Eigen::Vector4d camPos = source_->point(ux,uy);
- camPos *= -1.0f;
- Eigen::Vector4d worldPos = source_->getPose() * camPos;
- lookPoint_ = Eigen::Vector3d(worldPos[0],worldPos[1],worldPos[2]);
- LOG(INFO) << "Depth at click = " << -camPos[2];
- }
- };
- ::setMouseAction(name_, mouseaction_);
-}
-
-void Display::wait(int ms) {
- while (true) {
- int key = cv::waitKey(ms);
-
- if(key == 27) {
- // exit if ESC is pressed
- active_ = false;
- } else if (key == -1) {
- return;
- } else {
- ms = 1;
- for (auto &h : key_handlers_) {
- h(key);
- }
- }
- }
-}
-
-void Display::update() {
- if (!source_) return;
-
- centre_ += (lookPoint_ - centre_) * (lerpSpeed_ * 0.1f);
- Eigen::Matrix4d viewPose = lookAt<double>(eye_,centre_,up_).inverse();
- source_->setPose(viewPose);
-
- Mat rgb, depth;
- source_->grab();
- source_->getFrames(rgb, depth);
- if (rgb.rows > 0) cv::imshow(name_, rgb);
- wait(1);
-}
diff --git a/components/renderers/cpp/src/splat_render.cpp b/components/renderers/cpp/src/splat_render.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1b39ccebf69e589372ab2944cb907bb03d1dbdd8
--- /dev/null
+++ b/components/renderers/cpp/src/splat_render.cpp
@@ -0,0 +1,221 @@
+#include <ftl/render/splat_render.hpp>
+#include <ftl/utility/matrix_conversion.hpp>
+#include "splatter_cuda.hpp"
+#include <ftl/cuda/points.hpp>
+
+#include <opencv2/core/cuda_stream_accessor.hpp>
+
+using ftl::render::Splatter;
+using ftl::rgbd::Channel;
+using ftl::rgbd::Channels;
+using ftl::rgbd::Format;
+using cv::cuda::GpuMat;
+
+Splatter::Splatter(nlohmann::json &config, ftl::rgbd::FrameSet *fs) : ftl::render::Renderer(config), scene_(fs) {
+
+}
+
+Splatter::~Splatter() {
+
+}
+
+void Splatter::renderChannel(
+ ftl::render::SplatParams ¶ms, ftl::rgbd::Frame &out,
+ const Channel &channel, cudaStream_t stream)
+{
+ cv::cuda::Stream cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
+ temp_.get<GpuMat>(Channel::Depth).setTo(cv::Scalar(0x7FFFFFFF), cvstream);
+ temp_.get<GpuMat>(Channel::Depth2).setTo(cv::Scalar(0x7FFFFFFF), cvstream);
+ temp_.get<GpuMat>(Channel::Colour).setTo(cv::Scalar(0.0f,0.0f,0.0f,0.0f), cvstream);
+ temp_.get<GpuMat>(Channel::Contribution).setTo(cv::Scalar(0.0f), cvstream);
+
+ bool is_float = ftl::rgbd::isFloatChannel(channel);
+
+ // Render each camera into virtual view
+ for (size_t i=0; i < scene_->frames.size(); ++i) {
+ auto &f = scene_->frames[i];
+ auto *s = scene_->sources[i];
+
+ if (f.empty(Channel::Depth + Channel::Colour)) {
+ LOG(ERROR) << "Missing required channel";
+ continue;
+ }
+
+ // Needs to create points channel first?
+ if (!f.hasChannel(Channel::Points)) {
+ //LOG(INFO) << "Creating points... " << s->parameters().width;
+
+ auto &t = f.createTexture<float4>(Channel::Points, Format<float4>(f.get<GpuMat>(Channel::Colour).size()));
+ auto pose = MatrixConversion::toCUDA(s->getPose().cast<float>()); //.inverse());
+ ftl::cuda::point_cloud(t, f.createTexture<float>(Channel::Depth), s->parameters(), pose, stream);
+
+ //LOG(INFO) << "POINTS Added";
+ }
+
+ ftl::cuda::dibr_merge(
+ f.createTexture<float4>(Channel::Points),
+ temp_.getTexture<int>(Channel::Depth),
+ params, stream
+ );
+
+ //LOG(INFO) << "DIBR DONE";
+ }
+
+ // TODO: Add the depth splatting step..
+
+ temp_.createTexture<float4>(Channel::Colour);
+ temp_.createTexture<float>(Channel::Contribution);
+
+ // Accumulate attribute contributions for each pixel
+ for (auto &f : scene_->frames) {
+ // Convert colour from BGR to BGRA if needed
+ if (f.get<GpuMat>(Channel::Colour).type() == CV_8UC3) {
+ // Convert to 4 channel colour
+ auto &col = f.get<GpuMat>(Channel::Colour);
+ GpuMat tmp(col.size(), CV_8UC4);
+ cv::cuda::swap(col, tmp);
+ cv::cuda::cvtColor(tmp,col, cv::COLOR_BGR2BGRA);
+ }
+
+ if (is_float) {
+ ftl::cuda::dibr_attribute(
+ f.createTexture<float>(channel),
+ f.createTexture<float4>(Channel::Points),
+ temp_.getTexture<int>(Channel::Depth),
+ temp_.getTexture<float4>(Channel::Colour),
+ temp_.getTexture<float>(Channel::Contribution),
+ params, stream
+ );
+ } else if (channel == Channel::Colour || channel == Channel::Right) {
+ ftl::cuda::dibr_attribute(
+ f.createTexture<uchar4>(Channel::Colour),
+ f.createTexture<float4>(Channel::Points),
+ temp_.getTexture<int>(Channel::Depth),
+ temp_.getTexture<float4>(Channel::Colour),
+ temp_.getTexture<float>(Channel::Contribution),
+ params, stream
+ );
+ } else {
+ ftl::cuda::dibr_attribute(
+ f.createTexture<uchar4>(channel),
+ f.createTexture<float4>(Channel::Points),
+ temp_.getTexture<int>(Channel::Depth),
+ temp_.getTexture<float4>(Channel::Colour),
+ temp_.getTexture<float>(Channel::Contribution),
+ params, stream
+ );
+ }
+ }
+
+ if (is_float) {
+ // Normalise attribute contributions
+ ftl::cuda::dibr_normalise(
+ temp_.createTexture<float4>(Channel::Colour),
+ out.createTexture<float>(channel),
+ temp_.createTexture<float>(Channel::Contribution),
+ stream
+ );
+ } else {
+ // Normalise attribute contributions
+ ftl::cuda::dibr_normalise(
+ temp_.createTexture<float4>(Channel::Colour),
+ out.createTexture<uchar4>(channel),
+ temp_.createTexture<float>(Channel::Contribution),
+ stream
+ );
+ }
+}
+
+bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, cudaStream_t stream) {
+ SHARED_LOCK(scene_->mtx, lk);
+ if (!src->isReady()) return false;
+
+ const auto &camera = src->parameters();
+
+ //cudaSafeCall(cudaSetDevice(scene_->getCUDADevice()));
+
+ // Create all the required channels
+ out.create<GpuMat>(Channel::Depth, Format<float>(camera.width, camera.height));
+ out.create<GpuMat>(Channel::Colour, Format<uchar4>(camera.width, camera.height));
+
+ // FIXME: Use source resolutions, not virtual resolution
+ temp_.create<GpuMat>(Channel::Colour, Format<float4>(camera.width, camera.height));
+ temp_.create<GpuMat>(Channel::Colour2, Format<uchar4>(camera.width, camera.height));
+ temp_.create<GpuMat>(Channel::Contribution, Format<float>(camera.width, camera.height));
+ temp_.create<GpuMat>(Channel::Depth, Format<int>(camera.width, camera.height));
+ temp_.create<GpuMat>(Channel::Depth2, Format<int>(camera.width, camera.height));
+ temp_.create<GpuMat>(Channel::Normals, Format<float4>(camera.width, camera.height));
+
+ cv::cuda::Stream cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
+
+ // Create buffers if they don't exist
+ /*if ((unsigned int)depth1_.width() != camera.width || (unsigned int)depth1_.height() != camera.height) {
+ depth1_ = ftl::cuda::TextureObject<int>(camera.width, camera.height);
+ }
+ if ((unsigned int)depth3_.width() != camera.width || (unsigned int)depth3_.height() != camera.height) {
+ depth3_ = ftl::cuda::TextureObject<int>(camera.width, camera.height);
+ }
+ if ((unsigned int)colour1_.width() != camera.width || (unsigned int)colour1_.height() != camera.height) {
+ colour1_ = ftl::cuda::TextureObject<uchar4>(camera.width, camera.height);
+ }
+ if ((unsigned int)colour_tmp_.width() != camera.width || (unsigned int)colour_tmp_.height() != camera.height) {
+ colour_tmp_ = ftl::cuda::TextureObject<float4>(camera.width, camera.height);
+ }
+ if ((unsigned int)normal1_.width() != camera.width || (unsigned int)normal1_.height() != camera.height) {
+ normal1_ = ftl::cuda::TextureObject<float4>(camera.width, camera.height);
+ }
+ if ((unsigned int)depth2_.width() != camera.width || (unsigned int)depth2_.height() != camera.height) {
+ depth2_ = ftl::cuda::TextureObject<float>(camera.width, camera.height);
+ }
+ if ((unsigned int)colour2_.width() != camera.width || (unsigned int)colour2_.height() != camera.height) {
+ colour2_ = ftl::cuda::TextureObject<uchar4>(camera.width, camera.height);
+ }*/
+
+ // Parameters object to pass to CUDA describing the camera
+ SplatParams params;
+ params.m_flags = 0;
+ if (src->value("splatting", true) == false) params.m_flags |= ftl::render::kNoSplatting;
+ if (src->value("upsampling", true) == false) params.m_flags |= ftl::render::kNoUpsampling;
+ if (src->value("texturing", true) == false) params.m_flags |= ftl::render::kNoTexturing;
+ params.m_viewMatrix = MatrixConversion::toCUDA(src->getPose().cast<float>().inverse());
+ params.m_viewMatrixInverse = MatrixConversion::toCUDA(src->getPose().cast<float>());
+ params.camera = camera;
+
+ // Clear all channels to 0 or max depth
+
+ out.get<GpuMat>(Channel::Depth).setTo(cv::Scalar(1000.0f), cvstream);
+ out.get<GpuMat>(Channel::Colour).setTo(cv::Scalar(76,76,76), cvstream);
+
+ //LOG(INFO) << "Render ready: " << camera.width << "," << camera.height;
+
+ temp_.createTexture<int>(Channel::Depth);
+
+ renderChannel(params, out, Channel::Colour, stream);
+
+ Channel chan = src->getChannel();
+ if (chan == Channel::Depth)
+ {
+ temp_.get<GpuMat>(Channel::Depth).convertTo(out.get<GpuMat>(Channel::Depth), CV_32F, 1.0f / 1000.0f, cvstream);
+ }
+ else if (chan == Channel::Contribution)
+ {
+ cv::cuda::swap(temp_.get<GpuMat>(Channel::Contribution), out.create<GpuMat>(Channel::Contribution));
+ }
+ else if (chan == Channel::Right)
+ {
+ Eigen::Affine3f transform(Eigen::Translation3f(camera.baseline,0.0f,0.0f));
+ Eigen::Matrix4f matrix = src->getPose().cast<float>() * transform.matrix();
+ params.m_viewMatrix = MatrixConversion::toCUDA(matrix.inverse());
+ params.m_viewMatrixInverse = MatrixConversion::toCUDA(matrix);
+
+ out.create<GpuMat>(Channel::Right, Format<uchar4>(camera.width, camera.height));
+ out.get<GpuMat>(Channel::Right).setTo(cv::Scalar(76,76,76), cvstream);
+ renderChannel(params, out, Channel::Right, stream);
+ }
+
+ return true;
+}
+
+//void Splatter::setOutputDevice(int device) {
+// device_ = device;
+//}
diff --git a/components/renderers/cpp/src/splatter.cu b/components/renderers/cpp/src/splatter.cu
new file mode 100644
index 0000000000000000000000000000000000000000..3b1ae4b47ef0fe6b29b15d1fa20fdc9a0fd0b9bb
--- /dev/null
+++ b/components/renderers/cpp/src/splatter.cu
@@ -0,0 +1,304 @@
+#include <ftl/render/splat_params.hpp>
+#include "splatter_cuda.hpp"
+#include <ftl/rgbd/camera.hpp>
+#include <ftl/cuda_common.hpp>
+
+#include <ftl/cuda/weighting.hpp>
+
+#define T_PER_BLOCK 8
+#define UPSAMPLE_FACTOR 1.8f
+#define WARP_SIZE 32
+#define DEPTH_THRESHOLD 0.05f
+#define UPSAMPLE_MAX 60
+#define MAX_ITERATIONS 32 // Note: Must be multiple of 32
+#define SPATIAL_SMOOTHING 0.005f
+
+using ftl::cuda::TextureObject;
+using ftl::render::SplatParams;
+
+/*
+ * Pass 1: Directly render each camera into virtual view but with no upsampling
+ * for sparse points.
+ */
+ __global__ void dibr_merge_kernel(TextureObject<float4> points, TextureObject<int> depth, SplatParams params) {
+ const int x = blockIdx.x*blockDim.x + threadIdx.x;
+ const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+ const float3 worldPos = make_float3(points.tex2D(x, y));
+ if (worldPos.x == MINF) return;
+
+ // Find the virtual screen position of current point
+ const float3 camPos = params.m_viewMatrix * worldPos;
+ if (camPos.z < params.camera.minDepth) return;
+ if (camPos.z > params.camera.maxDepth) return;
+
+ const float d = camPos.z;
+
+ const uint2 screenPos = params.camera.camToScreen<uint2>(camPos);
+ const unsigned int cx = screenPos.x;
+ const unsigned int cy = screenPos.y;
+ if (d > params.camera.minDepth && d < params.camera.maxDepth && cx < depth.width() && cy < depth.height()) {
+ // Transform estimated point to virtual cam space and output z
+ atomicMin(&depth(cx,cy), d * 1000.0f);
+ }
+}
+
+void ftl::cuda::dibr_merge(TextureObject<float4> &points, TextureObject<int> &depth, SplatParams params, cudaStream_t stream) {
+ const dim3 gridSize((depth.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (depth.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
+ const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
+
+ dibr_merge_kernel<<<gridSize, blockSize, 0, stream>>>(points, depth, params);
+ cudaSafeCall( cudaGetLastError() );
+}
+
+//==============================================================================
+
+__device__ inline float4 make_float4(const uchar4 &c) {
+ return make_float4(c.x,c.y,c.z,c.w);
+}
+
+
+#define ENERGY_THRESHOLD 0.1f
+#define SMOOTHING_MULTIPLIER_A 10.0f // For surface search
+#define SMOOTHING_MULTIPLIER_B 4.0f // For z contribution
+#define SMOOTHING_MULTIPLIER_C 4.0f // For colour contribution
+
+/*
+ * Pass 2: Accumulate attribute contributions if the points pass a visibility test.
+ */
+__global__ void dibr_attribute_contrib_kernel(
+ TextureObject<uchar4> colour_in, // Original colour image
+ TextureObject<float4> points, // Original 3D points
+ TextureObject<int> depth_in, // Virtual depth map
+ TextureObject<float4> colour_out, // Accumulated output
+ //TextureObject<float4> normal_out,
+ TextureObject<float> contrib_out,
+ SplatParams params) {
+
+ //const ftl::voxhash::DepthCameraCUDA &camera = c_cameras[cam];
+
+ const int tid = (threadIdx.x + threadIdx.y * blockDim.x);
+ //const int warp = tid / WARP_SIZE;
+ const int x = (blockIdx.x*blockDim.x + threadIdx.x) / WARP_SIZE;
+ const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+ const float3 worldPos = make_float3(points.tex2D(x, y));
+ //const float3 normal = make_float3(tex2D<float4>(camera.normal, x, y));
+ if (worldPos.x == MINF) return;
+ //const float r = (camera.poseInverse * worldPos).z / camera.params.fx;
+
+ const float3 camPos = params.m_viewMatrix * worldPos;
+ if (camPos.z < params.camera.minDepth) return;
+ if (camPos.z > params.camera.maxDepth) return;
+ const uint2 screenPos = params.camera.camToScreen<uint2>(camPos);
+
+ const int upsample = 8; //min(UPSAMPLE_MAX, int((5.0f*r) * params.camera.fx / camPos.z));
+
+ // Not on screen so stop now...
+ if (screenPos.x >= depth_in.width() || screenPos.y >= depth_in.height()) return;
+
+ // Is this point near the actual surface and therefore a contributor?
+ const float d = ((float)depth_in.tex2D((int)screenPos.x, (int)screenPos.y)/1000.0f);
+ //if (abs(d - camPos.z) > DEPTH_THRESHOLD) return;
+
+ // TODO:(Nick) Should just one thread load these to shared mem?
+ const float4 colour = make_float4(colour_in.tex2D(x, y));
+ //const float4 normal = tex2D<float4>(camera.normal, x, y);
+
+ // Each thread in warp takes an upsample point and updates corresponding depth buffer.
+ const int lane = tid % WARP_SIZE;
+ for (int i=lane; i<upsample*upsample; i+=WARP_SIZE) {
+ const float u = (i % upsample) - (upsample / 2);
+ const float v = (i / upsample) - (upsample / 2);
+
+ // Use the depth buffer to determine this pixels 3D position in camera space
+ const float d = ((float)depth_in.tex2D(screenPos.x+u, screenPos.y+v)/1000.0f);
+ const float3 nearest = params.camera.screenToCam((int)(screenPos.x+u),(int)(screenPos.y+v),d);
+
+ // What is contribution of our current point at this pixel?
+ const float weight = ftl::cuda::spatialWeighting(length(nearest - camPos), SMOOTHING_MULTIPLIER_C*(nearest.z/params.camera.fx));
+ if (screenPos.x+u < colour_out.width() && screenPos.y+v < colour_out.height() && weight > 0.0f) { // TODO: Use confidence threshold here
+ const float4 wcolour = colour * weight;
+ //const float4 wnormal = normal * weight;
+
+ //printf("Z %f\n", d);
+
+ // Add this points contribution to the pixel buffer
+ atomicAdd((float*)&colour_out(screenPos.x+u, screenPos.y+v), wcolour.x);
+ atomicAdd((float*)&colour_out(screenPos.x+u, screenPos.y+v)+1, wcolour.y);
+ atomicAdd((float*)&colour_out(screenPos.x+u, screenPos.y+v)+2, wcolour.z);
+ atomicAdd((float*)&colour_out(screenPos.x+u, screenPos.y+v)+3, wcolour.w);
+ //atomicAdd((float*)&normal_out(screenPos.x+u, screenPos.y+v), wnormal.x);
+ //atomicAdd((float*)&normal_out(screenPos.x+u, screenPos.y+v)+1, wnormal.y);
+ //atomicAdd((float*)&normal_out(screenPos.x+u, screenPos.y+v)+2, wnormal.z);
+ //atomicAdd((float*)&normal_out(screenPos.x+u, screenPos.y+v)+3, wnormal.w);
+ atomicAdd(&contrib_out(screenPos.x+u, screenPos.y+v), weight);
+ }
+ }
+}
+
+__global__ void dibr_attribute_contrib_kernel(
+ TextureObject<float> colour_in, // Original colour image
+ TextureObject<float4> points, // Original 3D points
+ TextureObject<int> depth_in, // Virtual depth map
+ TextureObject<float4> colour_out, // Accumulated output
+ //TextureObject<float4> normal_out,
+ TextureObject<float> contrib_out,
+ SplatParams params) {
+
+ //const ftl::voxhash::DepthCameraCUDA &camera = c_cameras[cam];
+
+ const int tid = (threadIdx.x + threadIdx.y * blockDim.x);
+ //const int warp = tid / WARP_SIZE;
+ const int x = (blockIdx.x*blockDim.x + threadIdx.x) / WARP_SIZE;
+ const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+ const float3 worldPos = make_float3(points.tex2D(x, y));
+ //const float3 normal = make_float3(tex2D<float4>(camera.normal, x, y));
+ if (worldPos.x == MINF) return;
+ //const float r = (camera.poseInverse * worldPos).z / camera.params.fx;
+
+ const float3 camPos = params.m_viewMatrix * worldPos;
+ if (camPos.z < params.camera.minDepth) return;
+ if (camPos.z > params.camera.maxDepth) return;
+ const uint2 screenPos = params.camera.camToScreen<uint2>(camPos);
+
+ const int upsample = 8; //min(UPSAMPLE_MAX, int((5.0f*r) * params.camera.fx / camPos.z));
+
+ // Not on screen so stop now...
+ if (screenPos.x >= depth_in.width() || screenPos.y >= depth_in.height()) return;
+
+ // Is this point near the actual surface and therefore a contributor?
+ const float d = ((float)depth_in.tex2D((int)screenPos.x, (int)screenPos.y)/1000.0f);
+ //if (abs(d - camPos.z) > DEPTH_THRESHOLD) return;
+
+ // TODO:(Nick) Should just one thread load these to shared mem?
+ const float colour = (colour_in.tex2D(x, y));
+ //const float4 normal = tex2D<float4>(camera.normal, x, y);
+
+ // Each thread in warp takes an upsample point and updates corresponding depth buffer.
+ const int lane = tid % WARP_SIZE;
+ for (int i=lane; i<upsample*upsample; i+=WARP_SIZE) {
+ const float u = (i % upsample) - (upsample / 2);
+ const float v = (i / upsample) - (upsample / 2);
+
+ // Use the depth buffer to determine this pixels 3D position in camera space
+ const float d = ((float)depth_in.tex2D(screenPos.x+u, screenPos.y+v)/1000.0f);
+ const float3 nearest = params.camera.screenToCam((int)(screenPos.x+u),(int)(screenPos.y+v),d);
+
+ // What is contribution of our current point at this pixel?
+ const float weight = ftl::cuda::spatialWeighting(length(nearest - camPos), SMOOTHING_MULTIPLIER_C*(nearest.z/params.camera.fx));
+ if (screenPos.x+u < colour_out.width() && screenPos.y+v < colour_out.height() && weight > 0.0f) { // TODO: Use confidence threshold here
+ const float wcolour = colour * weight;
+ //const float4 wnormal = normal * weight;
+
+ //printf("Z %f\n", d);
+
+ // Add this points contribution to the pixel buffer
+ atomicAdd((float*)&colour_out(screenPos.x+u, screenPos.y+v), wcolour);
+ atomicAdd(&contrib_out(screenPos.x+u, screenPos.y+v), weight);
+ }
+ }
+}
+
+void ftl::cuda::dibr_attribute(
+ TextureObject<uchar4> &colour_in, // Original colour image
+ TextureObject<float4> &points, // Original 3D points
+ TextureObject<int> &depth_in, // Virtual depth map
+ TextureObject<float4> &colour_out, // Accumulated output
+ //TextureObject<float4> normal_out,
+ TextureObject<float> &contrib_out,
+ SplatParams ¶ms, cudaStream_t stream) {
+ const dim3 gridSize((depth_in.width() + 2 - 1)/2, (depth_in.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
+ const dim3 blockSize(2*WARP_SIZE, T_PER_BLOCK);
+
+ dibr_attribute_contrib_kernel<<<gridSize, blockSize, 0, stream>>>(
+ colour_in,
+ points,
+ depth_in,
+ colour_out,
+ contrib_out,
+ params
+ );
+ cudaSafeCall( cudaGetLastError() );
+}
+
+void ftl::cuda::dibr_attribute(
+ TextureObject<float> &colour_in, // Original colour image
+ TextureObject<float4> &points, // Original 3D points
+ TextureObject<int> &depth_in, // Virtual depth map
+ TextureObject<float4> &colour_out, // Accumulated output
+ //TextureObject<float4> normal_out,
+ TextureObject<float> &contrib_out,
+ SplatParams ¶ms, cudaStream_t stream) {
+ const dim3 gridSize((depth_in.width() + 2 - 1)/2, (depth_in.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
+ const dim3 blockSize(2*WARP_SIZE, T_PER_BLOCK);
+
+ dibr_attribute_contrib_kernel<<<gridSize, blockSize, 0, stream>>>(
+ colour_in,
+ points,
+ depth_in,
+ colour_out,
+ contrib_out,
+ params
+ );
+ cudaSafeCall( cudaGetLastError() );
+}
+
+//==============================================================================
+
+__global__ void dibr_normalise_kernel(
+ TextureObject<float4> colour_in,
+ TextureObject<uchar4> colour_out,
+ //TextureObject<float4> normals,
+ TextureObject<float> contribs) {
+ const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
+ const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+ if (x < colour_in.width() && y < colour_in.height()) {
+ const float4 colour = colour_in.tex2D((int)x,(int)y);
+ //const float4 normal = normals.tex2D((int)x,(int)y);
+ const float contrib = contribs.tex2D((int)x,(int)y);
+
+ if (contrib > 0.0f) {
+ colour_out(x,y) = make_uchar4(colour.x / contrib, colour.y / contrib, colour.z / contrib, 0);
+ //normals(x,y) = normal / contrib;
+ }
+ }
+}
+
+__global__ void dibr_normalise_kernel(
+ TextureObject<float4> colour_in,
+ TextureObject<float> colour_out,
+ //TextureObject<float4> normals,
+ TextureObject<float> contribs) {
+ const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
+ const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+ if (x < colour_in.width() && y < colour_in.height()) {
+ const float4 colour = colour_in.tex2D((int)x,(int)y);
+ //const float4 normal = normals.tex2D((int)x,(int)y);
+ const float contrib = contribs.tex2D((int)x,(int)y);
+
+ if (contrib > 0.0f) {
+ colour_out(x,y) = colour.x / contrib;
+ //normals(x,y) = normal / contrib;
+ }
+ }
+}
+
+void ftl::cuda::dibr_normalise(TextureObject<float4> &colour_in, TextureObject<uchar4> &colour_out, TextureObject<float> &contribs, cudaStream_t stream) {
+ const dim3 gridSize((colour_in.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (colour_in.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
+ const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
+
+ dibr_normalise_kernel<<<gridSize, blockSize, 0, stream>>>(colour_in, colour_out, contribs);
+ cudaSafeCall( cudaGetLastError() );
+}
+
+void ftl::cuda::dibr_normalise(TextureObject<float4> &colour_in, TextureObject<float> &colour_out, TextureObject<float> &contribs, cudaStream_t stream) {
+ const dim3 gridSize((colour_in.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (colour_in.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
+ const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
+
+ dibr_normalise_kernel<<<gridSize, blockSize, 0, stream>>>(colour_in, colour_out, contribs);
+ cudaSafeCall( cudaGetLastError() );
+}
diff --git a/components/renderers/cpp/src/splatter_cuda.hpp b/components/renderers/cpp/src/splatter_cuda.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..8c57d58486c04aff5960f215c6a6308719e6af7b
--- /dev/null
+++ b/components/renderers/cpp/src/splatter_cuda.hpp
@@ -0,0 +1,47 @@
+#ifndef _FTL_RECONSTRUCTION_SPLAT_CUDA_HPP_
+#define _FTL_RECONSTRUCTION_SPLAT_CUDA_HPP_
+
+#include <ftl/cuda_common.hpp>
+#include <ftl/render/splat_params.hpp>
+
+namespace ftl {
+namespace cuda {
+ void dibr_merge(
+ ftl::cuda::TextureObject<float4> &points,
+ ftl::cuda::TextureObject<int> &depth,
+ ftl::render::SplatParams params,
+ cudaStream_t stream);
+
+ void dibr_attribute(
+ ftl::cuda::TextureObject<uchar4> &in, // Original colour image
+ ftl::cuda::TextureObject<float4> &points, // Original 3D points
+ ftl::cuda::TextureObject<int> &depth_in, // Virtual depth map
+ ftl::cuda::TextureObject<float4> &out, // Accumulated output
+ //TextureObject<float4> normal_out,
+ ftl::cuda::TextureObject<float> &contrib_out,
+ ftl::render::SplatParams ¶ms, cudaStream_t stream);
+
+ void dibr_attribute(
+ ftl::cuda::TextureObject<float> &in, // Original colour image
+ ftl::cuda::TextureObject<float4> &points, // Original 3D points
+ ftl::cuda::TextureObject<int> &depth_in, // Virtual depth map
+ ftl::cuda::TextureObject<float4> &out, // Accumulated output
+ //TextureObject<float4> normal_out,
+ ftl::cuda::TextureObject<float> &contrib_out,
+ ftl::render::SplatParams ¶ms, cudaStream_t stream);
+
+ void dibr_normalise(
+ ftl::cuda::TextureObject<float4> &in,
+ ftl::cuda::TextureObject<uchar4> &out,
+ ftl::cuda::TextureObject<float> &contribs,
+ cudaStream_t stream);
+
+ void dibr_normalise(
+ ftl::cuda::TextureObject<float4> &in,
+ ftl::cuda::TextureObject<float> &out,
+ ftl::cuda::TextureObject<float> &contribs,
+ cudaStream_t stream);
+}
+}
+
+#endif // _FTL_RECONSTRUCTION_SPLAT_CUDA_HPP_
diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt
index ff7d290981c1a84c5ee211219ae0651f85c58c77..2b056d009a73dd7ee82adaedbf03bb98194d636f 100644
--- a/components/rgbd-sources/CMakeLists.txt
+++ b/components/rgbd-sources/CMakeLists.txt
@@ -4,6 +4,7 @@ set(RGBDSRC
src/disparity.cpp
src/source.cpp
src/frame.cpp
+ src/frameset.cpp
src/stereovideo.cpp
src/middlebury_source.cpp
src/net.cpp
@@ -17,6 +18,7 @@ set(RGBDSRC
src/cb_segmentation.cpp
src/abr.cpp
src/offilter.cpp
+ src/virtual.cpp
)
if (HAVE_REALSENSE)
@@ -37,6 +39,7 @@ endif (LIBSGM_FOUND)
if (CUDA_FOUND)
list(APPEND RGBDSRC
src/algorithms/disp2depth.cu
+ src/algorithms/offilter.cu
# "src/algorithms/opencv_cuda_bm.cpp"
# "src/algorithms/opencv_cuda_bp.cpp"
# "src/algorithms/rtcensus.cu"
diff --git a/components/rgbd-sources/include/ftl/offilter.hpp b/components/rgbd-sources/include/ftl/offilter.hpp
index 4c4fbbb9837ef39e80f9aad68c62ae5d82d4671e..6aece39aab241dfc7605dfb208d7d30ba6135509 100644
--- a/components/rgbd-sources/include/ftl/offilter.hpp
+++ b/components/rgbd-sources/include/ftl/offilter.hpp
@@ -1,8 +1,10 @@
#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>
@@ -12,23 +14,15 @@ namespace rgbd {
class OFDisparityFilter {
public:
- OFDisparityFilter() : n_max_(0), threshold_(0.0), size_(0, 0) {} // TODO: invalid state
+ OFDisparityFilter() : n_max_(0), threshold_(0.0) {}
OFDisparityFilter(cv::Size size, int n_frames, float threshold);
- void filter(cv::Mat &disp, const cv::Mat &rgb);
+ void filter(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream);
private:
- int n_;
int n_max_;
float threshold_;
- cv::Size size_;
- cv::Mat disp_;
- cv::Mat gray_;
-
- cv::Mat flowxy_;
- cv::Mat flowxy_up_;
-
- cv::Ptr<cv::cuda::NvidiaOpticalFlow_1_0> nvof_;
+ cv::cuda::GpuMat disp_old_;
};
}
diff --git a/components/rgbd-sources/include/ftl/rgbd/camera.hpp b/components/rgbd-sources/include/ftl/rgbd/camera.hpp
index 8acad9c41d6c4950398cde7d9f44ab8ae0bc08b6..e245be1ea44f3e7e8503f585bf2c387fef821a38 100644
--- a/components/rgbd-sources/include/ftl/rgbd/camera.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/camera.hpp
@@ -2,23 +2,69 @@
#ifndef _FTL_RGBD_CAMERA_PARAMS_HPP_
#define _FTL_RGBD_CAMERA_PARAMS_HPP_
+#include <vector_types.h>
+#include <cuda_runtime.h>
+#include <ftl/cuda_util.hpp>
+
namespace ftl{
namespace rgbd {
-struct Camera {
- double fx;
- double fy;
- double cx;
- double cy;
- unsigned int width;
- unsigned int height;
- double minDepth;
- double maxDepth;
- double baseline;
- double doffs;
+/**
+ * All properties associated with cameras. This structure is designed to
+ * operate on CPU and GPU.
+ */
+struct __align__(16) Camera {
+ double fx; // Focal length X
+ double fy; // Focal length Y (usually same as fx)
+ double cx; // Principle point Y
+ double cy; // Principle point Y
+ unsigned int width; // Pixel width
+ unsigned int height; // Pixel height
+ double minDepth; // Near clip in meters
+ double maxDepth; // Far clip in meters
+ double baseline; // For stereo pair
+ double doffs; // Disparity offset
+
+ /**
+ * Convert camera coordinates into screen coordinates.
+ */
+ template <typename T> __device__ T camToScreen(const float3 &pos) const;
+
+ /**
+ * Convert screen plus depth into camera coordinates.
+ */
+ __device__ float3 screenToCam(uint ux, uint uy, float depth) const;
};
};
};
+// ---- IMPLEMENTATIONS --------------------------------------------------------
+
+template <> __device__
+inline float2 ftl::rgbd::Camera::camToScreen<float2>(const float3 &pos) const {
+ return make_float2(
+ pos.x*fx/pos.z - cx,
+ pos.y*fy/pos.z - cy);
+}
+
+template <> __device__
+inline int2 ftl::rgbd::Camera::camToScreen<int2>(const float3 &pos) const {
+ float2 pImage = camToScreen<float2>(pos);
+ return make_int2(pImage + make_float2(0.5f, 0.5f));
+}
+
+template <> __device__
+inline uint2 ftl::rgbd::Camera::camToScreen<uint2>(const float3 &pos) const {
+ int2 p = camToScreen<int2>(pos);
+ return make_uint2(p.x, p.y);
+}
+
+__device__
+inline float3 ftl::rgbd::Camera::screenToCam(uint ux, uint uy, float depth) const {
+ const float x = ((float)ux+cx) / fx;
+ const float y = ((float)uy+cy) / fy;
+ return make_float3(depth*x, depth*y, depth);
+}
+
#endif
diff --git a/components/rgbd-sources/include/ftl/rgbd/channels.hpp b/components/rgbd-sources/include/ftl/rgbd/channels.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9bf731a5319fa47c501a91e09f1e2acc48c5a4a8
--- /dev/null
+++ b/components/rgbd-sources/include/ftl/rgbd/channels.hpp
@@ -0,0 +1,124 @@
+#ifndef _FTL_RGBD_CHANNELS_HPP_
+#define _FTL_RGBD_CHANNELS_HPP_
+
+#include <bitset>
+#include <msgpack.hpp>
+
+namespace ftl {
+namespace rgbd {
+
+enum struct Channel : int {
+ None = -1,
+ Colour = 0, // 8UC3 or 8UC4
+ Left = 0,
+ Depth = 1, // 32S or 32F
+ Right = 2, // 8UC3 or 8UC4
+ Colour2 = 2,
+ Disparity = 3,
+ Depth2 = 3,
+ Deviation = 4,
+ Normals = 5, // 32FC4
+ Points = 6, // 32FC4
+ Confidence = 7, // 32F
+ Contribution = 7, // 32F
+ EnergyVector, // 32FC4
+ Flow, // 32F
+ Energy, // 32F
+ LeftGray,
+ RightGray,
+ Overlay1
+};
+
+class Channels {
+ public:
+
+ class iterator {
+ public:
+ iterator(const Channels &c, unsigned int ix) : channels_(c), ix_(ix) { }
+ iterator operator++();
+ iterator operator++(int junk);
+ inline ftl::rgbd::Channel operator*() { return static_cast<Channel>(static_cast<int>(ix_)); }
+ //ftl::rgbd::Channel operator->() { return ptr_; }
+ inline bool operator==(const iterator& rhs) { return ix_ == rhs.ix_; }
+ inline bool operator!=(const iterator& rhs) { return ix_ != rhs.ix_; }
+ private:
+ const Channels &channels_;
+ unsigned int ix_;
+ };
+
+ inline Channels() { mask = 0; }
+ inline explicit Channels(unsigned int m) { mask = m; }
+ inline explicit Channels(Channel c) { mask = (c == Channel::None) ? 0 : 0x1 << static_cast<unsigned int>(c); }
+ inline Channels &operator=(Channel c) { mask = (c == Channel::None) ? 0 : 0x1 << static_cast<unsigned int>(c); return *this; }
+ inline Channels operator|(Channel c) const { return (c == Channel::None) ? Channels(mask) : Channels(mask | (0x1 << static_cast<unsigned int>(c))); }
+ inline Channels operator+(Channel c) const { return (c == Channel::None) ? Channels(mask) : Channels(mask | (0x1 << static_cast<unsigned int>(c))); }
+ inline Channels &operator|=(Channel c) { mask |= (c == Channel::None) ? 0 : (0x1 << static_cast<unsigned int>(c)); return *this; }
+ inline Channels &operator+=(Channel c) { mask |= (c == Channel::None) ? 0 : (0x1 << static_cast<unsigned int>(c)); return *this; }
+ inline Channels &operator-=(Channel c) { mask &= ~((c == Channel::None) ? 0 : (0x1 << static_cast<unsigned int>(c))); return *this; }
+ inline Channels &operator+=(unsigned int c) { mask |= (0x1 << c); return *this; }
+ inline Channels &operator-=(unsigned int c) { mask &= ~(0x1 << c); return *this; }
+
+ inline bool has(Channel c) const {
+ return (c == Channel::None) ? true : mask & (0x1 << static_cast<unsigned int>(c));
+ }
+
+ inline bool has(unsigned int c) const {
+ return mask & (0x1 << c);
+ }
+
+ inline iterator begin() { return iterator(*this, 0); }
+ inline iterator end() { return iterator(*this, 32); }
+
+ inline operator unsigned int() { return mask; }
+ inline operator bool() { return mask > 0; }
+ inline operator Channel() {
+ if (mask == 0) return Channel::None;
+ int ix = 0;
+ int tmask = mask;
+ while (!(tmask & 0x1) && ++ix < 32) tmask >>= 1;
+ return static_cast<Channel>(ix);
+ }
+
+ inline size_t count() { return std::bitset<32>(mask).count(); }
+ inline void clear() { mask = 0; }
+
+ static const size_t kMax = 32;
+
+ static Channels All();
+
+ private:
+ unsigned int mask;
+};
+
+inline Channels::iterator Channels::iterator::operator++() { Channels::iterator i = *this; while (++ix_ < 32 && !channels_.has(ix_)); return i; }
+inline Channels::iterator Channels::iterator::operator++(int junk) { while (++ix_ < 32 && !channels_.has(ix_)); return *this; }
+
+inline Channels Channels::All() {
+ return Channels(0xFFFFFFFFu);
+}
+
+static const Channels kNoChannels;
+static const Channels kAllChannels(0xFFFFFFFFu);
+
+inline bool isFloatChannel(ftl::rgbd::Channel chan) {
+ switch (chan) {
+ case Channel::Depth :
+ case Channel::Energy : return true;
+ default : return false;
+ }
+}
+
+}
+}
+
+MSGPACK_ADD_ENUM(ftl::rgbd::Channel);
+
+inline ftl::rgbd::Channels operator|(ftl::rgbd::Channel a, ftl::rgbd::Channel b) {
+ return ftl::rgbd::Channels(a) | b;
+}
+
+inline ftl::rgbd::Channels operator+(ftl::rgbd::Channel a, ftl::rgbd::Channel b) {
+ return ftl::rgbd::Channels(a) | b;
+}
+
+#endif // _FTL_RGBD_CHANNELS_HPP_
diff --git a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp b/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
index 29edf722fe4a3eaac36c76f7c861797f8ff71a49..e98ff38aacd4cf0731ef96b67ecc85732d4c0c7f 100644
--- a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
@@ -2,6 +2,7 @@
#define _FTL_RGBD_DETAIL_SOURCE_HPP_
#include <Eigen/Eigen>
+#include <ftl/cuda_util.hpp>
#include <opencv2/opencv.hpp>
#include <ftl/rgbd/camera.hpp>
#include <ftl/rgbd/frame.hpp>
@@ -55,7 +56,7 @@ class Source {
virtual bool isReady() { return false; };
virtual void setPose(const Eigen::Matrix4d &pose) { };
- virtual Camera parameters(channel_t) { return params_; };
+ virtual Camera parameters(ftl::rgbd::Channel) { return params_; };
protected:
capability_t capabilities_;
diff --git a/components/rgbd-sources/include/ftl/rgbd/format.hpp b/components/rgbd-sources/include/ftl/rgbd/format.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..2e86aaf96a5f64d13065385ddee360fd80ca9f8c
--- /dev/null
+++ b/components/rgbd-sources/include/ftl/rgbd/format.hpp
@@ -0,0 +1,52 @@
+#ifndef _FTL_RGBD_FORMAT_HPP_
+#define _FTL_RGBD_FORMAT_HPP_
+
+#include <opencv2/core.hpp>
+#include <opencv2/core/cuda.hpp>
+#include <ftl/cuda_util.hpp>
+#include <ftl/codecs/bitrates.hpp>
+#include <ftl/traits.hpp>
+
+namespace ftl {
+namespace rgbd {
+
+struct FormatBase {
+ FormatBase(size_t w, size_t h, int t) : width(w), height(h), cvType(t) {}
+
+ size_t width; // Width in pixels
+ size_t height; // Height in pixels
+ int cvType; // OpenCV Mat type
+
+ inline bool empty() const { return width == 0 || height == 0; }
+ inline cv::Size size() const { return cv::Size(width, height); }
+};
+
+template <typename T>
+struct Format : public ftl::rgbd::FormatBase {
+ Format() : FormatBase(0,0,0) {}
+
+ Format(size_t w, size_t h) : FormatBase(
+ w, h, ftl::traits::OpenCVType<T>::value) {}
+
+ explicit Format(ftl::codecs::definition_t d) : FormatBase(
+ ftl::codecs::getWidth(d),
+ ftl::codecs::getHeight(d),
+ ftl::traits::OpenCVType<T>::value) {}
+
+ explicit Format(const cv::Size &s) : FormatBase(
+ s.width,
+ s.height,
+ ftl::traits::OpenCVType<T>::value) {}
+
+ explicit Format(const cv::InputArray &a) : FormatBase(
+ a.cols,
+ a.rows,
+ ftl::traits::OpenCVType<T>::value) {
+ CHECK(cvType == a.type());
+ }
+};
+
+}
+}
+
+#endif // _FTL_RGBD_FORMAT_HPP_
diff --git a/components/rgbd-sources/include/ftl/rgbd/frame.hpp b/components/rgbd-sources/include/ftl/rgbd/frame.hpp
index 227913e5de056f15380390201da96b66ec23e27f..252ff271de36336938d51d616cdd5a9e87d52187 100644
--- a/components/rgbd-sources/include/ftl/rgbd/frame.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/frame.hpp
@@ -3,133 +3,253 @@
#define _FTL_RGBD_FRAME_HPP_
#include <ftl/configuration.hpp>
+#include <ftl/exception.hpp>
#include <opencv2/core.hpp>
#include <opencv2/core/cuda.hpp>
+#include <opencv2/core/cuda_stream_accessor.hpp>
-namespace ftl {
-namespace rgbd {
-
-typedef unsigned int channel_t;
-
-static const channel_t kChanNone = 0;
-static const channel_t kChanLeft = 0x0001; // CV_8UC3
-static const channel_t kChanDepth = 0x0002; // CV_32FC1
-static const channel_t kChanRight = 0x0004; // CV_8UC3
-static const channel_t kChanDisparity = 0x0008; // CV_32FC1
-static const channel_t kChanDeviation = 0x0010;
-static const channel_t kChanNormals = 0x0020;
-static const channel_t kChanConfidence = 0x0040;
-static const channel_t kChanFlow = 0x0080; // CV_16SC2 (format 10.5) from NVOF
-static const channel_t kChanEnergy = 0x0100;
+#include <ftl/rgbd/channels.hpp>
+#include <ftl/rgbd/format.hpp>
+#include <ftl/codecs/bitrates.hpp>
-// should l/r gray be removed (not that expensive to re-calculate if needed)?
-static const channel_t kChanLeftGray = 0x0200; // CV_8UC1
-static const channel_t kChanRightGray = 0x0400; // CV_8UC1
+#include <ftl/cuda_common.hpp>
-static const channel_t kChanOverlay1 = 0x1000;
+#include <type_traits>
+#include <array>
-// maximum number of available channels
-static const unsigned int n_channels = 13;
-
-inline bool isFloatChannel(ftl::rgbd::channel_t chan) {
- return (chan == ftl::rgbd::kChanDepth || chan == ftl::rgbd::kChanEnergy);
-}
+namespace ftl {
+namespace rgbd {
// TODO: interpolation for scaling depends on channel type;
// NN for depth/disparity/optflow, linear/cubic/etc. for RGB
class Frame;
+class Source;
+/**
+ * Manage a set of image channels corresponding to a single camera frame.
+ */
class Frame {
public:
- Frame() : channels_host_(n_channels),
- channels_gpu_(n_channels),
- available_(n_channels, 0)
- {}
+ Frame() : src_(nullptr) {}
+ explicit Frame(ftl::rgbd::Source *src) : src_(src) {}
+
+ inline ftl::rgbd::Source *source() const { return src_; }
+
+ // Prevent frame copy, instead use a move.
+ //Frame(const Frame &)=delete;
+ //Frame &operator=(const Frame &)=delete;
+
+ void download(ftl::rgbd::Channel c, cv::cuda::Stream stream);
+ void upload(ftl::rgbd::Channel c, cv::cuda::Stream stream);
+ void download(ftl::rgbd::Channels c, cv::cuda::Stream stream);
+ void upload(ftl::rgbd::Channels c, cv::cuda::Stream stream);
+
+ inline void download(ftl::rgbd::Channel c, cudaStream_t stream=0) { download(c, cv::cuda::StreamAccessor::wrapStream(stream)); };
+ inline void upload(ftl::rgbd::Channel c, cudaStream_t stream=0) { upload(c, cv::cuda::StreamAccessor::wrapStream(stream)); };
+ inline void download(ftl::rgbd::Channels c, cudaStream_t stream=0) { download(c, cv::cuda::StreamAccessor::wrapStream(stream)); };
+ inline void upload(ftl::rgbd::Channels c, cudaStream_t stream=0) { upload(c, cv::cuda::StreamAccessor::wrapStream(stream)); };
+
+ /**
+ * Perform a buffer swap of the selected channels. This is intended to be
+ * a copy from `this` to the passed frame object but by buffer swap
+ * instead of memory copy, meaning `this` may become invalid afterwards.
+ */
+ void swapTo(ftl::rgbd::Channels, Frame &);
+
+ /**
+ * Create a channel with a given format. This will discard any existing
+ * data associated with the channel and ensure all data structures and
+ * memory allocations match the new format.
+ */
+ template <typename T> T &create(ftl::rgbd::Channel c, const ftl::rgbd::FormatBase &f);
- /* @brief Reset all channels without releasing memory.
+ /**
+ * Create a channel but without any format.
*/
- void reset()
- {
- std::fill(available_.begin(), available_.end(), 0);
+ template <typename T> T &create(ftl::rgbd::Channel c);
+
+ /**
+ * Create a CUDA texture object for a channel. This version takes a format
+ * argument to also create (or recreate) the associated GpuMat.
+ */
+ template <typename T>
+ ftl::cuda::TextureObject<T> &createTexture(ftl::rgbd::Channel c, const ftl::rgbd::Format<T> &f);
+
+ /**
+ * Create a CUDA texture object for a channel. With this version the GpuMat
+ * must already exist and be of the correct type.
+ */
+ template <typename T>
+ ftl::cuda::TextureObject<T> &createTexture(ftl::rgbd::Channel c);
+
+ /**
+ * Reset all channels without releasing memory.
+ */
+ void reset();
+
+ bool empty(ftl::rgbd::Channels c);
+
+ inline bool empty(ftl::rgbd::Channel c) {
+ auto &m = _get(c);
+ return !hasChannel(c) || (m.host.empty() && m.gpu.empty());
}
- /* @brief Is there valid data in channel (either host or gpu).
+ /**
+ * Is there valid data in channel (either host or gpu).
*/
- bool hasChannel(const ftl::rgbd::channel_t& channel)
- {
- return available_[_channelIdx(channel)];
+ inline bool hasChannel(ftl::rgbd::Channel channel) const {
+ return channels_.has(channel);
}
- /* @brief Method to get reference to the channel content
+ inline ftl::rgbd::Channels getChannels() const { return channels_; }
+
+ /**
+ * Is the channel data currently located on GPU. This also returns false if
+ * the channel does not exist.
+ */
+ inline bool isGPU(ftl::rgbd::Channel channel) const {
+ return channels_.has(channel) && gpu_.has(channel);
+ }
+
+ /**
+ * Is the channel data currently located on CPU memory. This also returns
+ * false if the channel does not exist.
+ */
+ inline bool isCPU(ftl::rgbd::Channel channel) const {
+ return channels_.has(channel) && !gpu_.has(channel);
+ }
+
+ /**
+ * Method to get reference to the channel content.
* @param Channel type
- * @param CUDA stream
- * @returns Const reference to channel data
+ * @return Const reference to channel data
*
* Result is valid only if hasChannel() is true. Host/Gpu transfer is
- * performed, if necessary, but only once unless channel contents is
- * changed by calling setChannel(). Return value valid only if
- * hasChannel(channel) is true.
+ * performed, if necessary, but with a warning since an explicit upload or
+ * download should be used.
*/
- template <typename T> const T& getChannel(const ftl::rgbd::channel_t& channel, cv::cuda::Stream& stream);
- template <typename T> const T& getChannel(const ftl::rgbd::channel_t& channel);
+ template <typename T> const T& get(ftl::rgbd::Channel channel) const;
- /* @brief Method to set/modify channel content
+ /**
+ * Method to get reference to the channel content.
* @param Channel type
- * @returns Reference to channel data
- *
- * Returns non-const reference to channel memory. Invalidates other copies
- * of the data (host/gpu) for the specified channel, next time getChannel()
- * is called a memory transfer may occur.
+ * @return Reference to channel data
*
- * NOTE: If user of setChannel<T>() wants to modify contents instead of
- * replacing them, getChannel<T>() needs to be called first to
- * ensure there is valid contents in the returned reference!
- * (TODO: interface could be improved)
+ * Result is valid only if hasChannel() is true. Host/Gpu transfer is
+ * performed, if necessary, but with a warning since an explicit upload or
+ * download should be used.
*/
- template <typename T> T& setChannel(const ftl::rgbd::channel_t& channel);
+ template <typename T> T& get(ftl::rgbd::Channel channel);
+
+ template <typename T> const ftl::cuda::TextureObject<T> &getTexture(ftl::rgbd::Channel) const;
+ template <typename T> ftl::cuda::TextureObject<T> &getTexture(ftl::rgbd::Channel);
private:
+ struct ChannelData {
+ cv::Mat host;
+ cv::cuda::GpuMat gpu;
+ ftl::cuda::TextureObjectBase tex;
+ };
- static size_t _channelIdx(const ftl::rgbd::channel_t& channel)
- {
- switch(channel)
- {
- case kChanNone: return 0;
- case kChanLeft: return 1;
- case kChanDepth: return 2;
- case kChanRight: return 3;
- case kChanDisparity: return 4;
- case kChanDeviation: return 5;
- case kChanNormals: return 6;
- case kChanConfidence: return 7;
- case kChanFlow: return 8;
- case kChanEnergy: return 9;
- case kChanLeftGray: return 11;
- case kChanRightGray: return 12;
- // should not happen (error); returned index is kChanNone
- default: return 0;
- }
- }
+ std::array<ChannelData, Channels::kMax> data_;
- std::vector<cv::Mat> channels_host_;
- std::vector<cv::cuda::GpuMat> channels_gpu_;
+ ftl::rgbd::Channels channels_; // Does it have a channel
+ ftl::rgbd::Channels gpu_; // Is the channel on a GPU
- // bitmasks for each channel stored in available_
- static const uint mask_host = 1;
- static const uint mask_gpu = 2;
+ ftl::rgbd::Source *src_;
- std::vector<uint> available_;
+ inline ChannelData &_get(ftl::rgbd::Channel c) { return data_[static_cast<unsigned int>(c)]; }
+ inline const ChannelData &_get(ftl::rgbd::Channel c) const { return data_[static_cast<unsigned int>(c)]; }
};
-template<> const cv::Mat& Frame::getChannel(const ftl::rgbd::channel_t& channel, cv::cuda::Stream& stream);
-template<> const cv::cuda::GpuMat& Frame::getChannel(const ftl::rgbd::channel_t& channel, cv::cuda::Stream& stream);
+// Specialisations
+
+template<> const cv::Mat& Frame::get(ftl::rgbd::Channel channel) const;
+template<> const cv::cuda::GpuMat& Frame::get(ftl::rgbd::Channel channel) const;
+template<> cv::Mat& Frame::get(ftl::rgbd::Channel channel);
+template<> cv::cuda::GpuMat& Frame::get(ftl::rgbd::Channel channel);
+
+template <> cv::Mat &Frame::create(ftl::rgbd::Channel c, const ftl::rgbd::FormatBase &);
+template <> cv::cuda::GpuMat &Frame::create(ftl::rgbd::Channel c, const ftl::rgbd::FormatBase &);
+template <> cv::Mat &Frame::create(ftl::rgbd::Channel c);
+template <> cv::cuda::GpuMat &Frame::create(ftl::rgbd::Channel c);
+
+template <typename T>
+ftl::cuda::TextureObject<T> &Frame::getTexture(ftl::rgbd::Channel c) {
+ if (!channels_.has(c)) throw ftl::exception("Texture channel does not exist");
+ if (!gpu_.has(c)) throw ftl::exception("Texture channel is not on GPU");
+
+ auto &m = _get(c);
+
+ if (m.tex.cvType() != ftl::traits::OpenCVType<T>::value || m.tex.width() != m.gpu.cols || m.tex.height() != m.gpu.rows || m.gpu.type() != m.tex.cvType()) {
+ throw ftl::exception("Texture has not been created properly for this channel");
+ }
+
+ return ftl::cuda::TextureObject<T>::cast(m.tex);
+}
+
+template <typename T>
+ftl::cuda::TextureObject<T> &Frame::createTexture(ftl::rgbd::Channel c, const ftl::rgbd::Format<T> &f) {
+ if (!channels_.has(c)) channels_ += c;
+ if (!gpu_.has(c)) gpu_ += c;
-template<> const cv::Mat& Frame::getChannel(const ftl::rgbd::channel_t& channel);
-template<> const cv::cuda::GpuMat& Frame::getChannel(const ftl::rgbd::channel_t& channel);
+ auto &m = _get(c);
+
+ if (f.empty()) {
+ throw ftl::exception("createTexture needs a non-empty format");
+ } else {
+ m.gpu.create(f.size(), f.cvType);
+ }
+
+ if (m.gpu.type() != ftl::traits::OpenCVType<T>::value) {
+ throw ftl::exception("Texture type does not match underlying data type");
+ }
-template<> cv::Mat& Frame::setChannel(const ftl::rgbd::channel_t& channel);
-template<> cv::cuda::GpuMat& Frame::setChannel(const ftl::rgbd::channel_t& channel);
+ // TODO: Check tex cvType
+
+ if (m.tex.devicePtr() == nullptr) {
+ LOG(INFO) << "Creating texture object";
+ m.tex = ftl::cuda::TextureObject<T>(m.gpu);
+ } else if (m.tex.cvType() != ftl::traits::OpenCVType<T>::value || m.tex.width() != m.gpu.cols || m.tex.height() != m.gpu.rows) {
+ LOG(INFO) << "Recreating texture object";
+ m.tex.free();
+ m.tex = ftl::cuda::TextureObject<T>(m.gpu);
+ }
+
+ return ftl::cuda::TextureObject<T>::cast(m.tex);
+}
+
+template <typename T>
+ftl::cuda::TextureObject<T> &Frame::createTexture(ftl::rgbd::Channel c) {
+ if (!channels_.has(c)) throw ftl::exception("createTexture needs a format if the channel does not exist");
+
+ auto &m = _get(c);
+
+ if (isCPU(c) && !m.host.empty()) {
+ m.gpu.create(m.host.size(), m.host.type());
+ // TODO: Should this upload to GPU or not?
+ //gpu_ += c;
+ } else if (isCPU(c) || (isGPU(c) && m.gpu.empty())) {
+ throw ftl::exception("createTexture needs a format if no memory is allocated");
+ }
+
+ if (m.gpu.type() != ftl::traits::OpenCVType<T>::value) {
+ throw ftl::exception("Texture type does not match underlying data type");
+ }
+
+ // TODO: Check tex cvType
+
+ if (m.tex.devicePtr() == nullptr) {
+ LOG(INFO) << "Creating texture object";
+ m.tex = ftl::cuda::TextureObject<T>(m.gpu);
+ } else if (m.tex.cvType() != ftl::traits::OpenCVType<T>::value || m.tex.width() != m.gpu.cols || m.tex.height() != m.gpu.rows || m.tex.devicePtr() != m.gpu.data) {
+ m.tex.free();
+ m.tex = ftl::cuda::TextureObject<T>(m.gpu);
+ }
+
+ return ftl::cuda::TextureObject<T>::cast(m.tex);
+}
}
}
diff --git a/components/rgbd-sources/include/ftl/rgbd/frameset.hpp b/components/rgbd-sources/include/ftl/rgbd/frameset.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..2fa39e2eacf19339860e98fa98df44f687ac64c7
--- /dev/null
+++ b/components/rgbd-sources/include/ftl/rgbd/frameset.hpp
@@ -0,0 +1,37 @@
+#ifndef _FTL_RGBD_FRAMESET_HPP_
+#define _FTL_RGBD_FRAMESET_HPP_
+
+#include <ftl/threads.hpp>
+#include <ftl/rgbd/frame.hpp>
+
+#include <opencv2/opencv.hpp>
+#include <vector>
+
+namespace ftl {
+namespace rgbd {
+
+class Source;
+
+/**
+ * Represents a set of synchronised frames, each with two channels. This is
+ * used to collect all frames from multiple computers that have the same
+ * timestamp.
+ */
+struct FrameSet {
+ int64_t timestamp; // Millisecond timestamp of all frames
+ std::vector<Source*> sources; // All source objects involved.
+ std::vector<ftl::rgbd::Frame> frames;
+ std::atomic<int> count; // Number of valid frames
+ std::atomic<unsigned int> mask; // Mask of all sources that contributed
+ bool stale; // True if buffers have been invalidated
+ SHARED_MUTEX mtx;
+
+ void upload(ftl::rgbd::Channels, cudaStream_t stream=0);
+ void download(ftl::rgbd::Channels, cudaStream_t stream=0);
+ void swapTo(ftl::rgbd::FrameSet &);
+};
+
+}
+}
+
+#endif // _FTL_RGBD_FRAMESET_HPP_
diff --git a/components/rgbd-sources/include/ftl/rgbd/group.hpp b/components/rgbd-sources/include/ftl/rgbd/group.hpp
index 000eea0ae60303d816e09298613ddc30f0a8146a..0ded29e80b7d2fa01ad656c2fbb3b8865b726a70 100644
--- a/components/rgbd-sources/include/ftl/rgbd/group.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/group.hpp
@@ -1,8 +1,11 @@
#ifndef _FTL_RGBD_GROUP_HPP_
#define _FTL_RGBD_GROUP_HPP_
+#include <ftl/cuda_util.hpp>
#include <ftl/threads.hpp>
#include <ftl/timer.hpp>
+#include <ftl/rgbd/frame.hpp>
+#include <ftl/rgbd/frameset.hpp>
#include <opencv2/opencv.hpp>
#include <vector>
@@ -12,22 +15,6 @@ namespace rgbd {
class Source;
-/**
- * Represents a set of synchronised frames, each with two channels. This is
- * used to collect all frames from multiple computers that have the same
- * timestamp.
- */
-struct FrameSet {
- int64_t timestamp; // Millisecond timestamp of all frames
- std::vector<Source*> sources; // All source objects involved.
- std::vector<cv::Mat> channel1; // RGB
- std::vector<cv::Mat> channel2; // Depth (usually)
- std::atomic<int> count; // Number of valid frames
- std::atomic<unsigned int> mask; // Mask of all sources that contributed
- bool stale; // True if buffers have been invalidated
- SHARED_MUTEX mtx;
-};
-
// Allows a latency of 20 frames maximum
static const size_t kFrameBufferSize = 20;
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index 1106d76220e676a3e7e36c5db18db9596a0f91d9..0ee163add0009023ec24e6df6bd18a1da927af1e 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp
@@ -1,10 +1,11 @@
#ifndef _FTL_RGBD_SOURCE_HPP_
#define _FTL_RGBD_SOURCE_HPP_
+#include <ftl/cuda_util.hpp>
#include <ftl/configuration.hpp>
#include <ftl/rgbd/camera.hpp>
#include <ftl/threads.hpp>
-//#include <ftl/net/universe.hpp>
+#include <ftl/net/universe.hpp>
#include <ftl/uri.hpp>
#include <ftl/rgbd/detail/source.hpp>
#include <opencv2/opencv.hpp>
@@ -25,6 +26,7 @@ namespace rgbd {
static inline bool isValidDepth(float d) { return (d > 0.01f) && (d < 39.99f); }
class SnapshotReader;
+class VirtualSource;
/**
* RGBD Generic data source configurable entity. This class hides the
@@ -39,6 +41,7 @@ class Source : public ftl::Configurable {
public:
template <typename T, typename... ARGS>
friend T *ftl::config::create(ftl::config::json_t &, ARGS ...);
+ friend class VirtualSource;
//template <typename T, typename... ARGS>
//friend T *ftl::config::create(ftl::Configurable *, const std::string &, ARGS ...);
@@ -50,11 +53,11 @@ class Source : public ftl::Configurable {
Source(const Source&)=delete;
Source &operator=(const Source&) =delete;
- private:
+ protected:
explicit Source(ftl::config::json_t &cfg);
Source(ftl::config::json_t &cfg, ftl::rgbd::SnapshotReader *);
Source(ftl::config::json_t &cfg, ftl::net::Universe *net);
- ~Source();
+ virtual ~Source();
public:
/**
@@ -65,9 +68,9 @@ class Source : public ftl::Configurable {
/**
* Change the second channel source.
*/
- bool setChannel(channel_t c);
+ bool setChannel(ftl::rgbd::Channel c);
- channel_t getChannel() const { return channel_; }
+ ftl::rgbd::Channel getChannel() const { return channel_; }
/**
* Perform the hardware or virtual frame grab operation. This should be
@@ -120,17 +123,6 @@ class Source : public ftl::Configurable {
*/
void getDepth(cv::Mat &d);
- /**
- * Write frames into source buffers from an external renderer. Virtual
- * sources do not have an internal generator of frames but instead have
- * their data provided from an external rendering class. This function only
- * works when there is no internal generator.
- */
- void writeFrames(int64_t ts, const cv::Mat &rgb, const cv::Mat &depth);
- void writeFrames(int64_t ts, const ftl::cuda::TextureObject<uchar4> &rgb, const ftl::cuda::TextureObject<uint> &depth, cudaStream_t stream);
- void writeFrames(int64_t ts, const ftl::cuda::TextureObject<uchar4> &rgb, const ftl::cuda::TextureObject<float> &depth, cudaStream_t stream);
- void writeFrames(int64_t ts, const ftl::cuda::TextureObject<uchar4> &rgb, const ftl::cuda::TextureObject<uchar4> &rgb2, cudaStream_t stream);
-
int64_t timestamp() const { return timestamp_; }
/**
@@ -151,7 +143,7 @@ class Source : public ftl::Configurable {
else return params_;
}
- const Camera parameters(channel_t) const;
+ const Camera parameters(ftl::rgbd::Channel) const;
cv::Mat cameraMatrix() const;
@@ -213,7 +205,7 @@ class Source : public ftl::Configurable {
void removeCallback() { callback_ = nullptr; }
- private:
+ protected:
detail::Source *impl_;
cv::Mat rgb_;
cv::Mat depth_;
@@ -224,7 +216,7 @@ class Source : public ftl::Configurable {
SHARED_MUTEX mutex_;
bool paused_;
bool bullet_;
- channel_t channel_;
+ ftl::rgbd::Channel channel_;
cudaStream_t stream_;
int64_t timestamp_;
std::function<void(int64_t, cv::Mat &, cv::Mat &)> callback_;
diff --git a/components/rgbd-sources/include/ftl/rgbd/streamer.hpp b/components/rgbd-sources/include/ftl/rgbd/streamer.hpp
index 3f6f1c4fa35bfcb4a97877a2d46d53230e2acbec..7c6e6f479afe022cdacefbabf9098e276e0c9f79 100644
--- a/components/rgbd-sources/include/ftl/rgbd/streamer.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/streamer.hpp
@@ -117,6 +117,8 @@ class Streamer : public ftl::Configurable {
void wait();
+ void setLatency(int l) { group_.setLatency(l); }
+
/**
* Alternative to calling run(), it will operate a single frame capture,
* compress and stream cycle.
diff --git a/components/rgbd-sources/include/ftl/rgbd/virtual.hpp b/components/rgbd-sources/include/ftl/rgbd/virtual.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ed3530258d64b14427802f8e56375635ab26a24a
--- /dev/null
+++ b/components/rgbd-sources/include/ftl/rgbd/virtual.hpp
@@ -0,0 +1,30 @@
+#ifndef _FTL_RGBD_VIRTUAL_HPP_
+#define _FTL_RGBD_VIRTUAL_HPP_
+
+#include <ftl/rgbd/source.hpp>
+
+namespace ftl {
+namespace rgbd {
+
+class VirtualSource : public ftl::rgbd::Source {
+ public:
+ explicit VirtualSource(ftl::config::json_t &cfg);
+ ~VirtualSource();
+
+ void onRender(const std::function<void(ftl::rgbd::Frame &)> &);
+
+ void setTimestamp(int64_t ts) { timestamp_ = ts; }
+
+ /**
+ * Write frames into source buffers from an external renderer. Virtual
+ * sources do not have an internal generator of frames but instead have
+ * their data provided from an external rendering class. This function only
+ * works when there is no internal generator.
+ */
+ //void write(int64_t ts, ftl::rgbd::Frame &frame, cudaStream_t stream=0);
+};
+
+}
+}
+
+#endif // _FTL_RGBD_VIRTUAL_HPP_
diff --git a/components/rgbd-sources/include/qsort.h b/components/rgbd-sources/include/qsort.h
new file mode 100644
index 0000000000000000000000000000000000000000..62a76b836c1b13861fc8a5a12ff0fc0eb7936f8a
--- /dev/null
+++ b/components/rgbd-sources/include/qsort.h
@@ -0,0 +1,186 @@
+/*
+ * Copyright (c) 2013, 2017 Alexey Tourbin
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+/*
+ * This is a traditional Quicksort implementation which mostly follows
+ * [Sedgewick 1978]. Sorting is performed entirely on array indices,
+ * while actual access to the array elements is abstracted out with the
+ * user-defined `LESS` and `SWAP` primitives.
+ *
+ * Synopsis:
+ * QSORT(N, LESS, SWAP);
+ * where
+ * N - the number of elements in A[];
+ * LESS(i, j) - compares A[i] to A[j];
+ * SWAP(i, j) - exchanges A[i] with A[j].
+ */
+
+#ifndef QSORT_H
+#define QSORT_H
+
+/* Sort 3 elements. */
+#define Q_SORT3(q_a1, q_a2, q_a3, Q_LESS, Q_SWAP) \
+do { \
+ if (Q_LESS(q_a2, q_a1)) { \
+ if (Q_LESS(q_a3, q_a2)) \
+ Q_SWAP(q_a1, q_a3); \
+ else { \
+ Q_SWAP(q_a1, q_a2); \
+ if (Q_LESS(q_a3, q_a2)) \
+ Q_SWAP(q_a2, q_a3); \
+ } \
+ } \
+ else if (Q_LESS(q_a3, q_a2)) { \
+ Q_SWAP(q_a2, q_a3); \
+ if (Q_LESS(q_a2, q_a1)) \
+ Q_SWAP(q_a1, q_a2); \
+ } \
+} while (0)
+
+/* Partition [q_l,q_r] around a pivot. After partitioning,
+ * [q_l,q_j] are the elements that are less than or equal to the pivot,
+ * while [q_i,q_r] are the elements greater than or equal to the pivot. */
+#define Q_PARTITION(q_l, q_r, q_i, q_j, Q_UINT, Q_LESS, Q_SWAP) \
+do { \
+ /* The middle element, not to be confused with the median. */ \
+ Q_UINT q_m = q_l + ((q_r - q_l) >> 1); \
+ /* Reorder the second, the middle, and the last items. \
+ * As [Edelkamp Weiss 2016] explain, using the second element \
+ * instead of the first one helps avoid bad behaviour for \
+ * decreasingly sorted arrays. This method is used in recent \
+ * versions of gcc's std::sort, see gcc bug 58437#c13, although \
+ * the details are somewhat different (cf. #c14). */ \
+ Q_SORT3(q_l + 1, q_m, q_r, Q_LESS, Q_SWAP); \
+ /* Place the median at the beginning. */ \
+ Q_SWAP(q_l, q_m); \
+ /* Partition [q_l+2, q_r-1] around the median which is in q_l. \
+ * q_i and q_j are initially off by one, they get decremented \
+ * in the do-while loops. */ \
+ q_i = q_l + 1; q_j = q_r; \
+ while (1) { \
+ do q_i++; while (Q_LESS(q_i, q_l)); \
+ do q_j--; while (Q_LESS(q_l, q_j)); \
+ if (q_i >= q_j) break; /* Sedgewick says "until j < i" */ \
+ Q_SWAP(q_i, q_j); \
+ } \
+ /* Compensate for the i==j case. */ \
+ q_i = q_j + 1; \
+ /* Put the median to its final place. */ \
+ Q_SWAP(q_l, q_j); \
+ /* The median is not part of the left subfile. */ \
+ q_j--; \
+} while (0)
+
+/* Insertion sort is applied to small subfiles - this is contrary to
+ * Sedgewick's suggestion to run a separate insertion sort pass after
+ * the partitioning is done. The reason I don't like a separate pass
+ * is that it triggers extra comparisons, because it can't see that the
+ * medians are already in their final positions and need not be rechecked.
+ * Since I do not assume that comparisons are cheap, I also do not try
+ * to eliminate the (q_j > q_l) boundary check. */
+#define Q_INSERTION_SORT(q_l, q_r, Q_UINT, Q_LESS, Q_SWAP) \
+do { \
+ Q_UINT q_i, q_j; \
+ /* For each item starting with the second... */ \
+ for (q_i = q_l + 1; q_i <= q_r; q_i++) \
+ /* move it down the array so that the first part is sorted. */ \
+ for (q_j = q_i; q_j > q_l && (Q_LESS(q_j, q_j - 1)); q_j--) \
+ Q_SWAP(q_j, q_j - 1); \
+} while (0)
+
+/* When the size of [q_l,q_r], i.e. q_r-q_l+1, is greater than or equal to
+ * Q_THRESH, the algorithm performs recursive partitioning. When the size
+ * drops below Q_THRESH, the algorithm switches to insertion sort.
+ * The minimum valid value is probably 5 (with 5 items, the second and
+ * the middle items, the middle itself being rounded down, are distinct). */
+#define Q_THRESH 16
+
+/* The main loop. */
+#define Q_LOOP(Q_UINT, Q_N, Q_LESS, Q_SWAP) \
+do { \
+ Q_UINT q_l = 0; \
+ Q_UINT q_r = (Q_N) - 1; \
+ Q_UINT q_sp = 0; /* the number of frames pushed to the stack */ \
+ struct { Q_UINT q_l, q_r; } \
+ /* On 32-bit platforms, to sort a "char[3GB+]" array, \
+ * it may take full 32 stack frames. On 64-bit CPUs, \
+ * though, the address space is limited to 48 bits. \
+ * The usage is further reduced if Q_N has a 32-bit type. */ \
+ q_st[sizeof(Q_UINT) > 4 && sizeof(Q_N) > 4 ? 48 : 32]; \
+ while (1) { \
+ if (q_r - q_l + 1 >= Q_THRESH) { \
+ Q_UINT q_i, q_j; \
+ Q_PARTITION(q_l, q_r, q_i, q_j, Q_UINT, Q_LESS, Q_SWAP); \
+ /* Now have two subfiles: [q_l,q_j] and [q_i,q_r]. \
+ * Dealing with them depends on which one is bigger. */ \
+ if (q_j - q_l >= q_r - q_i) \
+ Q_SUBFILES(q_l, q_j, q_i, q_r); \
+ else \
+ Q_SUBFILES(q_i, q_r, q_l, q_j); \
+ } \
+ else { \
+ Q_INSERTION_SORT(q_l, q_r, Q_UINT, Q_LESS, Q_SWAP); \
+ /* Pop subfiles from the stack, until it gets empty. */ \
+ if (q_sp == 0) break; \
+ q_sp--; \
+ q_l = q_st[q_sp].q_l; \
+ q_r = q_st[q_sp].q_r; \
+ } \
+ } \
+} while (0)
+
+/* The missing part: dealing with subfiles.
+ * Assumes that the first subfile is not smaller than the second. */
+#define Q_SUBFILES(q_l1, q_r1, q_l2, q_r2) \
+do { \
+ /* If the second subfile is only a single element, it needs \
+ * no further processing. The first subfile will be processed \
+ * on the next iteration (both subfiles cannot be only a single \
+ * element, due to Q_THRESH). */ \
+ if (q_l2 == q_r2) { \
+ q_l = q_l1; \
+ q_r = q_r1; \
+ } \
+ else { \
+ /* Otherwise, both subfiles need processing. \
+ * Push the larger subfile onto the stack. */ \
+ q_st[q_sp].q_l = q_l1; \
+ q_st[q_sp].q_r = q_r1; \
+ q_sp++; \
+ /* Process the smaller subfile on the next iteration. */ \
+ q_l = q_l2; \
+ q_r = q_r2; \
+ } \
+} while (0)
+
+/* And now, ladies and gentlemen, may I proudly present to you... */
+#define QSORT(Q_N, Q_LESS, Q_SWAP) \
+do { \
+ if ((Q_N) > 1) \
+ /* We could check sizeof(Q_N) and use "unsigned", but at least \
+ * on x86_64, this has the performance penalty of up to 5%. */ \
+ Q_LOOP(unsigned long, Q_N, Q_LESS, Q_SWAP); \
+} while (0)
+
+#endif
+
+/* ex:set ts=8 sts=4 sw=4 noet: */
\ No newline at end of file
diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
index 782338fc2be41d56a4d6fcd4f4920f6d920e663f..5f8921bda0e562bcc26b454d750a35294ed75537 100644
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
@@ -7,6 +7,8 @@
using ftl::algorithms::FixstarsSGM;
using cv::Mat;
using cv::cuda::GpuMat;
+using ftl::rgbd::Channel;
+using ftl::rgbd::Format;
//static ftl::Disparity::Register fixstarssgm("libsgm", FixstarsSGM::create);
@@ -78,14 +80,9 @@ void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream)
cv::cuda::cvtColor(rgb, gray, cv::COLOR_BGR2GRAY, 0, stream);
}*/
- const auto &l = frame.getChannel<GpuMat>(ftl::rgbd::kChanLeft, stream);
- const auto &r = frame.getChannel<GpuMat>(ftl::rgbd::kChanRight, stream);
- auto &disp = frame.setChannel<GpuMat>(ftl::rgbd::kChanDisparity);
-
- if (disp.size() != l.size())
- {
- disp = GpuMat(l.size(), CV_32FC1);
- }
+ 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_)
@@ -131,11 +128,7 @@ void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream)
dispt_scaled.convertTo(disp, CV_32F, 1.0f / 16.0f, stream);
#ifdef HAVE_OPTFLOW
- if (use_off_)
- {
- frame.getChannel<Mat>(ftl::rgbd::kChanDisparity);
- off_.filter(frame.setChannel<Mat>(ftl::rgbd::kChanDisparity), Mat(lbw_));
- }
+ if (use_off_) { off_.filter(frame, stream); }
#endif
}
diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
index 039db0c874d8d816b91d447a6254c79fd5a43304..d2c0c1d2a8fd459695bb02c9f66f17e423dc9fa5 100644
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
@@ -5,6 +5,7 @@
#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>
diff --git a/components/rgbd-sources/src/algorithms/offilter.cu b/components/rgbd-sources/src/algorithms/offilter.cu
new file mode 100644
index 0000000000000000000000000000000000000000..6feee5ae1daf9fc8b4b165370dbb8646b1d7b8a1
--- /dev/null
+++ b/components/rgbd-sources/src/algorithms/offilter.cu
@@ -0,0 +1,91 @@
+#include <ftl/cuda_common.hpp>
+#include <ftl/rgbd/camera.hpp>
+#include <opencv2/core/cuda_stream_accessor.hpp>
+#include <qsort.h>
+
+__device__ void quicksort(float A[], size_t n)
+{
+ float tmp;
+ #define LESS(i, j) A[i] < A[j]
+ #define SWAP(i, j) tmp = A[i], A[i] = A[j], A[j] = tmp
+ QSORT(n, LESS, SWAP);
+}
+
+template<typename T>
+__device__ static bool inline isValidDisparity(T d) { return (0.0 < d) && (d < 256.0); } // TODO
+
+static const int max_history = 32; // TODO dynamic shared memory
+
+__global__ void temporal_median_filter_kernel(
+ cv::cuda::PtrStepSz<float> disp,
+ cv::cuda::PtrStepSz<int16_t> optflow,
+ cv::cuda::PtrStepSz<float> history,
+ int n_max,
+ int16_t threshold, // fixed point 10.5
+ float granularity // 4 for Turing
+)
+{
+ float sorted[max_history]; // TODO: dynamic shared memory
+ for (STRIDE_Y(y, disp.rows)) {
+ for (STRIDE_X(x, disp.cols)) {
+
+ int flowx = optflow(round(y / granularity), 2 * round(x / granularity));
+ int flowy = optflow(round(y / granularity), 2 * round(x / granularity) + 1);
+
+ if ((abs(flowx) + abs(flowy)) > threshold)
+ {
+ // last element in history[x][y][t]
+ history(y, (x + 1) * n_max - 1) = 0.0;
+ return;
+ }
+
+ int count = history(y, (x + 1) * n_max - 1);
+ int n = count % (n_max - 1);
+
+ if (isValidDisparity(disp(y, x)))
+ {
+ history(y, (x + 1) * n_max - 1) += 1.0;
+ count++;
+ history(y, x * n_max + n) = disp(y, x);
+ }
+
+ int n_end = count;
+ if (n_end >= n_max) { n_end = n_max - 1; }
+
+ if (n_end != 0)
+ {
+ for (size_t i = 0; i < n_end; i++)
+ {
+ sorted[i] = history(y, x * n_max + i);
+ }
+
+ quicksort(sorted, n_end);
+ disp(y, x) = sorted[n_end / 2];
+ }
+ }}
+}
+
+namespace ftl {
+namespace cuda {
+
+void optflow_filter(cv::cuda::GpuMat &disp, const cv::cuda::GpuMat &optflow,
+ cv::cuda::GpuMat &history, int n, float threshold,
+ cv::cuda::Stream &stream)
+{
+ dim3 grid(1, 1, 1);
+ dim3 threads(128, 1, 1);
+ grid.x = cv::cuda::device::divUp(disp.cols, 128);
+ grid.y = cv::cuda::device::divUp(disp.rows, 1);
+
+ // TODO: dynamic shared memory
+ temporal_median_filter_kernel<<<grid, threads, 0, cv::cuda::StreamAccessor::getStream(stream)>>>
+ ( disp, optflow, history, n,
+ round(threshold * (1 << 5)), // TODO: documentation; 10.5 format
+ 4 // TODO: (4 pixels granularity for Turing)
+ );
+
+ cudaSafeCall(cudaGetLastError());
+}
+
+}
+}
\ No newline at end of file
diff --git a/components/rgbd-sources/src/calibrate.cpp b/components/rgbd-sources/src/calibrate.cpp
index df7fac455b68d8ee326957053ae99da946aa941f..3940520d2374661449c376020cb98c5802e2c674 100644
--- a/components/rgbd-sources/src/calibrate.cpp
+++ b/components/rgbd-sources/src/calibrate.cpp
@@ -78,21 +78,24 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
return false;
}
+
+ cv::Size calib_size;
{
vector<Mat> K, D;
fs["K"] >> K;
fs["D"] >> D;
+ fs["resolution"] >> calib_size;
- K[0].copyTo(M1_);
- K[1].copyTo(M2_);
+ K[0].copyTo(K1_);
+ K[1].copyTo(K2_);
D[0].copyTo(D1_);
D[1].copyTo(D2_);
}
fs.release();
- CHECK(M1_.size() == Size(3, 3));
- CHECK(M2_.size() == Size(3, 3));
+ CHECK(K1_.size() == Size(3, 3));
+ CHECK(K2_.size() == Size(3, 3));
CHECK(D1_.size() == Size(5, 1));
CHECK(D2_.size() == Size(5, 1));
@@ -113,45 +116,54 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
fs["R"] >> R_;
fs["T"] >> T_;
+
+ /* re-calculate rectification from camera parameters
fs["R1"] >> R1_;
fs["R2"] >> R2_;
fs["P1"] >> P1_;
fs["P2"] >> P2_;
fs["Q"] >> Q_;
-
+ */
fs.release();
img_size_ = img_size;
- // TODO: normalize calibration
- double scale_x = ((double) img_size.width) / 1280.0;
- double scale_y = ((double) img_size.height) / 720.0;
+ if (calib_size.empty())
+ {
+ LOG(WARNING) << "Calibration resolution missing!";
+ }
+ else
+ {
+ double scale_x = ((double) img_size.width) / ((double) calib_size.width);
+ double scale_y = ((double) img_size.height) / ((double) calib_size.height);
- Mat scale(cv::Size(3, 3), CV_64F, 0.0);
- scale.at<double>(0, 0) = scale_x;
- scale.at<double>(1, 1) = scale_y;
- scale.at<double>(2, 2) = 1.0;
+ Mat scale(cv::Size(3, 3), CV_64F, 0.0);
+ scale.at<double>(0, 0) = scale_x;
+ scale.at<double>(1, 1) = scale_y;
+ scale.at<double>(2, 2) = 1.0;
+
+ K1_ = scale * K1_;
+ K2_ = scale * K2_;
+ }
- M1_ = scale * M1_;
- M2_ = scale * M2_;
- P1_ = scale * P1_;
- P2_ = scale * P2_;
+ double alpha = value("alpha", 0.0);
+ cv::stereoRectify(K1_, D1_, K2_, D2_, img_size_, R_, T_, R1_, R2_, P1_, P2_, Q_, 0, alpha);
+ /* scaling not required as rectification is performed from scaled values
Q_.at<double>(0, 3) = Q_.at<double>(0, 3) * scale_x;
Q_.at<double>(1, 3) = Q_.at<double>(1, 3) * scale_y;
Q_.at<double>(2, 3) = Q_.at<double>(2, 3) * scale_x; // TODO: scaling?
Q_.at<double>(3, 3) = Q_.at<double>(3, 3) * scale_x;
+ */
// cv::cuda::remap() works only with CV_32FC1
- initUndistortRectifyMap(M1_, D1_, R1_, P1_, img_size_, CV_32FC1, map1.first, map2.first);
- initUndistortRectifyMap(M2_, D2_, R2_, P2_, img_size_, CV_32FC1, map1.second, map2.second);
+ initUndistortRectifyMap(K1_, D1_, R1_, P1_, img_size_, CV_32FC1, map1.first, map2.first);
+ initUndistortRectifyMap(K2_, D2_, R2_, P2_, img_size_, CV_32FC1, map1.second, map2.second);
return true;
}
void Calibrate::updateCalibration(const ftl::rgbd::Camera &p) {
- std::pair<Mat, Mat> map1, map2;
-
Q_.at<double>(3, 2) = 1.0 / p.baseline;
Q_.at<double>(2, 3) = p.fx;
Q_.at<double>(0, 3) = p.cx;
@@ -178,17 +190,19 @@ void Calibrate::_updateIntrinsics() {
// no rectification
R1 = Mat::eye(Size(3, 3), CV_64FC1);
R2 = R1;
- P1 = M1_;
- P2 = M2_;
+ P1 = Mat::zeros(Size(4, 3), CV_64FC1);
+ P2 = Mat::zeros(Size(4, 3), CV_64FC1);
+ K1_.copyTo(Mat(P1, cv::Rect(0, 0, 3, 3)));
+ K2_.copyTo(Mat(P2, cv::Rect(0, 0, 3, 3)));
}
// Set correct camera matrices for
// getCameraMatrix(), getCameraMatrixLeft(), getCameraMatrixRight()
- C_l_ = P1;
- C_r_ = P2;
+ Kl_ = Mat(P1, cv::Rect(0, 0, 3, 3));
+ Kr_ = Mat(P1, cv::Rect(0, 0, 3, 3));
- initUndistortRectifyMap(M1_, D1_, R1, P1, img_size_, CV_32FC1, map1_.first, map2_.first);
- initUndistortRectifyMap(M2_, D2_, R2, P2, img_size_, CV_32FC1, map1_.second, map2_.second);
+ initUndistortRectifyMap(K1_, D1_, R1, P1, img_size_, CV_32FC1, map1_.first, map2_.first);
+ initUndistortRectifyMap(K2_, D2_, R2, P2, img_size_, CV_32FC1, map1_.second, map2_.second);
// CHECK Is this thread safe!!!!
map1_gpu_.first.upload(map1_.first);
diff --git a/components/rgbd-sources/src/calibrate.hpp b/components/rgbd-sources/src/calibrate.hpp
index 7f6b262c1e7afcd2852f65a6d62333b5389f7615..4561b90a79129dd5a0d46d9d54bd005147d766a7 100644
--- a/components/rgbd-sources/src/calibrate.hpp
+++ b/components/rgbd-sources/src/calibrate.hpp
@@ -54,8 +54,9 @@ class Calibrate : public ftl::Configurable {
* a 3D point cloud.
*/
const cv::Mat &getQ() const { return Q_; }
- const cv::Mat &getCameraMatrixLeft() { return C_l_; }
- const cv::Mat &getCameraMatrixRight() { return C_r_; }
+
+ const cv::Mat &getCameraMatrixLeft() { return Kl_; }
+ const cv::Mat &getCameraMatrixRight() { return Kr_; }
const cv::Mat &getCameraMatrix() { return getCameraMatrixLeft(); }
private:
@@ -70,12 +71,17 @@ private:
std::pair<cv::cuda::GpuMat, cv::cuda::GpuMat> map1_gpu_;
std::pair<cv::cuda::GpuMat, cv::cuda::GpuMat> map2_gpu_;
- cv::Mat Q_;
+ // parameters for rectification, see cv::stereoRectify() documentation
cv::Mat R_, T_, R1_, P1_, R2_, P2_;
- cv::Mat M1_, D1_, M2_, D2_;
- cv::Mat C_l_;
- cv::Mat C_r_;
+ // disparity to depth matrix
+ cv::Mat Q_;
+
+ // intrinsic paramters and distortion coefficients
+ cv::Mat K1_, D1_, K2_, D2_;
+
+ cv::Mat Kl_;
+ cv::Mat Kr_;
cv::Size img_size_;
};
diff --git a/components/rgbd-sources/src/cuda_algorithms.hpp b/components/rgbd-sources/src/cuda_algorithms.hpp
index 0aa7399c0a24035bdbbb0442b3c429ddd03d145c..439c16cfc21fef08086bb69b7e84b1c8b49fec74 100644
--- a/components/rgbd-sources/src/cuda_algorithms.hpp
+++ b/components/rgbd-sources/src/cuda_algorithms.hpp
@@ -41,6 +41,9 @@ namespace cuda {
void disparity_to_depth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth,
const ftl::rgbd::Camera &c, cv::cuda::Stream &stream);
+ void optflow_filter(cv::cuda::GpuMat &disp, const cv::cuda::GpuMat &optflow,
+ cv::cuda::GpuMat &history, int n_max, float threshold,
+ cv::cuda::Stream &stream);
}
}
diff --git a/components/rgbd-sources/src/disparity.hpp b/components/rgbd-sources/src/disparity.hpp
index 2ab8223ca97f14752265be86ccf0ace0554eb20e..44215871d37b2944c08d072d63afd5bf871082e4 100644
--- a/components/rgbd-sources/src/disparity.hpp
+++ b/components/rgbd-sources/src/disparity.hpp
@@ -48,10 +48,11 @@ class Disparity : public ftl::Configurable {
virtual void compute(Frame &frame, cv::cuda::Stream &stream)=0;
virtual void compute(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &disp, cv::cuda::Stream &stream)
{
- ftl::rgbd::Frame frame;
- frame.setChannel<cv::cuda::GpuMat>(kChanLeft) = l;
- frame.setChannel<cv::cuda::GpuMat>(kChanRight) = r;
- frame.setChannel<cv::cuda::GpuMat>(kChanDisparity) = disp;
+ // FIXME: What were these for?
+ //ftl::rgbd::Frame frame;
+ //frame.create<cv::cuda::GpuMat>(ftl::rgbd::Channel::Left) = l;
+ //frame.create<cv::cuda::GpuMat>(ftl::rgbd::Channel::Right) = r;
+ //frame.create<cv::cuda::GpuMat>(ftl::rgbd::Channel::Disparity) = disp;
}
/**
diff --git a/components/rgbd-sources/src/frame.cpp b/components/rgbd-sources/src/frame.cpp
index 8145280cc83d24347d9bb2fc07a5a9546bbe5dd7..a56a19355526d9cdcdf25faaecdc67c05d09469d 100644
--- a/components/rgbd-sources/src/frame.cpp
+++ b/components/rgbd-sources/src/frame.cpp
@@ -1,68 +1,201 @@
#include <ftl/rgbd/frame.hpp>
-namespace ftl {
-namespace rgbd {
-
-template<> const cv::Mat& Frame::getChannel(const ftl::rgbd::channel_t& channel, cv::cuda::Stream &stream)
-{
- size_t idx = _channelIdx(channel);
- if (!(available_[idx] & mask_host))
- {
- if (available_[idx] & mask_gpu)
- {
- channels_gpu_[idx].download(channels_host_[idx], stream);
- available_[idx] |= mask_host;
+using ftl::rgbd::Frame;
+using ftl::rgbd::Channels;
+using ftl::rgbd::Channel;
+
+static cv::Mat none;
+static cv::cuda::GpuMat noneGPU;
+
+void Frame::reset() {
+ channels_.clear();
+ gpu_.clear();
+}
+
+void Frame::download(Channel c, cv::cuda::Stream stream) {
+ download(Channels(c), stream);
+}
+
+void Frame::upload(Channel c, cv::cuda::Stream stream) {
+ upload(Channels(c), stream);
+}
+
+void Frame::download(Channels c, cv::cuda::Stream stream) {
+ for (size_t i=0u; i<Channels::kMax; ++i) {
+ if (c.has(i) && channels_.has(i) && gpu_.has(i)) {
+ data_[i].gpu.download(data_[i].host, stream);
+ gpu_ -= i;
}
}
-
- return channels_host_[idx];
}
-template<> const cv::Mat& Frame::getChannel(const ftl::rgbd::channel_t& channel)
-{
- auto &stream = cv::cuda::Stream::Null();
- auto &retval = getChannel<cv::Mat>(channel, stream);
- stream.waitForCompletion();
- return retval;
+void Frame::upload(Channels c, cv::cuda::Stream stream) {
+ for (size_t i=0u; i<Channels::kMax; ++i) {
+ if (c.has(i) && channels_.has(i) && !gpu_.has(i)) {
+ data_[i].gpu.upload(data_[i].host, stream);
+ gpu_ += i;
+ }
+ }
}
-template<> cv::Mat& Frame::setChannel(const ftl::rgbd::channel_t& channel)
-{
- size_t idx = _channelIdx(channel);
- available_[idx] = mask_host;
- return channels_host_[idx];
+bool Frame::empty(ftl::rgbd::Channels channels) {
+ for (auto c : channels) {
+ if (empty(c)) return true;
+ }
+ return false;
}
-template<> const cv::cuda::GpuMat& Frame::getChannel(const ftl::rgbd::channel_t& channel, cv::cuda::Stream &stream)
-{
- size_t idx = _channelIdx(channel);
- if (!(available_[idx] & mask_gpu))
- {
- if (available_[idx] & mask_host)
- {
- channels_gpu_[idx].upload(channels_host_[idx], stream);
- available_[idx] |= mask_gpu;
+void Frame::swapTo(ftl::rgbd::Channels channels, Frame &f) {
+ f.reset();
+
+ // For all channels in this frame object
+ for (auto c : channels_) {
+ // Should we swap this channel?
+ if (channels.has(c)) {
+ // Does 'f' have this channel?
+ //if (!f.hasChannel(c)) {
+ // No, so create it first
+ // FIXME: Allocate the memory as well?
+ if (isCPU(c)) f.create<cv::Mat>(c);
+ else f.create<cv::cuda::GpuMat>(c);
+ //}
+
+ auto &m1 = _get(c);
+ auto &m2 = f._get(c);
+
+ cv::swap(m1.host, m2.host);
+ cv::cuda::swap(m1.gpu, m2.gpu);
+
+ auto temptex = std::move(m2.tex);
+ m2.tex = std::move(m1.tex);
+ m1.tex = std::move(temptex);
}
}
-
- return channels_gpu_[idx];
}
-template<> const cv::cuda::GpuMat& Frame::getChannel(const ftl::rgbd::channel_t& channel)
-{
- auto &stream = cv::cuda::Stream::Null();
- auto &retval = getChannel<cv::cuda::GpuMat>(channel, stream);
- stream.waitForCompletion();
- return retval;
+template<> cv::Mat& Frame::get(ftl::rgbd::Channel channel) {
+ if (channel == Channel::None) {
+ DLOG(WARNING) << "Cannot get the None channel from a Frame";
+ none.release();
+ return none;
+ }
+
+ if (isGPU(channel)) {
+ download(Channels(channel));
+ LOG(WARNING) << "Getting GPU channel on CPU without explicit 'download'";
+ }
+
+ // Add channel if not already there
+ if (!channels_.has(channel)) {
+ throw ftl::exception("Frame channel does not exist");
+ }
+
+ return _get(channel).host;
+}
+
+template<> cv::cuda::GpuMat& Frame::get(ftl::rgbd::Channel channel) {
+ if (channel == Channel::None) {
+ DLOG(WARNING) << "Cannot get the None channel from a Frame";
+ noneGPU.release();
+ return noneGPU;
+ }
+
+ if (isCPU(channel)) {
+ upload(Channels(channel));
+ LOG(WARNING) << "Getting CPU channel on GPU without explicit 'upload'";
+ }
+
+ // Add channel if not already there
+ if (!channels_.has(channel)) {
+ throw ftl::exception("Frame channel does not exist");
+ }
+
+ return _get(channel).gpu;
+}
+
+template<> const cv::Mat& Frame::get(ftl::rgbd::Channel channel) const {
+ if (channel == Channel::None) {
+ LOG(FATAL) << "Cannot get the None channel from a Frame";
+ }
+
+ if (isGPU(channel)) {
+ LOG(FATAL) << "Getting GPU channel on CPU without explicit 'download'";
+ }
+
+ if (!channels_.has(channel)) throw ftl::exception("Frame channel does not exist");
+
+ return _get(channel).host;
+}
+
+template<> const cv::cuda::GpuMat& Frame::get(ftl::rgbd::Channel channel) const {
+ if (channel == Channel::None) {
+ LOG(FATAL) << "Cannot get the None channel from a Frame";
+ }
+
+ if (isCPU(channel)) {
+ LOG(FATAL) << "Getting CPU channel on GPU without explicit 'upload'";
+ }
+
+ // Add channel if not already there
+ if (!channels_.has(channel)) {
+ throw ftl::exception("Frame channel does not exist");
+ }
+
+ return _get(channel).gpu;
+}
+
+template <> cv::Mat &Frame::create(ftl::rgbd::Channel c, const ftl::rgbd::FormatBase &f) {
+ if (c == Channel::None) {
+ throw ftl::exception("Cannot create a None channel");
+ }
+ channels_ += c;
+ gpu_ -= c;
+
+ auto &m = _get(c).host;
+
+ if (!f.empty()) {
+ m.create(f.size(), f.cvType);
+ }
+
+ return m;
+}
+
+template <> cv::cuda::GpuMat &Frame::create(ftl::rgbd::Channel c, const ftl::rgbd::FormatBase &f) {
+ if (c == Channel::None) {
+ throw ftl::exception("Cannot create a None channel");
+ }
+ channels_ += c;
+ gpu_ += c;
+
+ auto &m = _get(c).gpu;
+
+ if (!f.empty()) {
+ m.create(f.size(), f.cvType);
+ }
+
+ return m;
}
-template<> cv::cuda::GpuMat& Frame::setChannel(const ftl::rgbd::channel_t& channel)
-{
- size_t idx = _channelIdx(channel);
- available_[idx] = mask_gpu;
- return channels_gpu_[idx];
+template <> cv::Mat &Frame::create(ftl::rgbd::Channel c) {
+ if (c == Channel::None) {
+ throw ftl::exception("Cannot create a None channel");
+ }
+ channels_ += c;
+ gpu_ -= c;
+
+ auto &m = _get(c).host;
+ return m;
}
+template <> cv::cuda::GpuMat &Frame::create(ftl::rgbd::Channel c) {
+ if (c == Channel::None) {
+ throw ftl::exception("Cannot create a None channel");
+ }
+ channels_ += c;
+ gpu_ += c;
+
+ auto &m = _get(c).gpu;
+ return m;
}
-}
\ No newline at end of file
+
diff --git a/components/rgbd-sources/src/frameset.cpp b/components/rgbd-sources/src/frameset.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9b9a807d8599c23141b6c3806546bf8038ef30f9
--- /dev/null
+++ b/components/rgbd-sources/src/frameset.cpp
@@ -0,0 +1,39 @@
+#include <ftl/rgbd/frameset.hpp>
+
+using ftl::rgbd::FrameSet;
+using ftl::rgbd::Channels;
+using ftl::rgbd::Channel;
+
+void FrameSet::upload(ftl::rgbd::Channels c, cudaStream_t stream) {
+ for (auto &f : frames) {
+ f.upload(c, stream);
+ }
+}
+
+void FrameSet::download(ftl::rgbd::Channels c, cudaStream_t stream) {
+ for (auto &f : frames) {
+ f.download(c, stream);
+ }
+}
+
+void FrameSet::swapTo(ftl::rgbd::FrameSet &fs) {
+ UNIQUE_LOCK(fs.mtx, lk);
+
+ //if (fs.frames.size() != frames.size()) {
+ // Assume "this" is correct and "fs" is not.
+ fs.sources.clear();
+ for (auto s : sources) fs.sources.push_back(s);
+ fs.frames.resize(frames.size());
+ //}
+
+ fs.timestamp = timestamp;
+ fs.count = static_cast<int>(count);
+ fs.stale = stale;
+ fs.mask = static_cast<unsigned int>(mask);
+
+ for (size_t i=0; i<frames.size(); ++i) {
+ frames[i].swapTo(Channels::All(), fs.frames[i]);
+ }
+
+ stale = true;
+}
diff --git a/components/rgbd-sources/src/group.cpp b/components/rgbd-sources/src/group.cpp
index e61289220ad92fff8e278905a075f01d93b6ad9a..96ca3a82fd3e306656f047d4971ce4a29b00fe48 100644
--- a/components/rgbd-sources/src/group.cpp
+++ b/components/rgbd-sources/src/group.cpp
@@ -10,6 +10,7 @@ using ftl::rgbd::kFrameBufferSize;
using std::vector;
using std::chrono::milliseconds;
using std::this_thread::sleep_for;
+using ftl::rgbd::Channel;
Group::Group() : framesets_(kFrameBufferSize), head_(0) {
framesets_[0].timestamp = -1;
@@ -52,6 +53,8 @@ void Group::addSource(ftl::rgbd::Source *src) {
src->setCallback([this,ix,src](int64_t timestamp, cv::Mat &rgb, cv::Mat &depth) {
if (timestamp == 0) return;
+ auto chan = src->getChannel();
+
//LOG(INFO) << "SRC CB: " << timestamp << " (" << framesets_[head_].timestamp << ")";
UNIQUE_LOCK(mutex_, lk);
@@ -73,11 +76,15 @@ void Group::addSource(ftl::rgbd::Source *src) {
//LOG(INFO) << "Adding frame: " << ix << " for " << timestamp;
// Ensure channels match source mat format
- fs.channel1[ix].create(rgb.size(), rgb.type());
- fs.channel2[ix].create(depth.size(), depth.type());
+ //fs.channel1[ix].create(rgb.size(), rgb.type());
+ //fs.channel2[ix].create(depth.size(), depth.type());
+ fs.frames[ix].create<cv::Mat>(Channel::Colour, Format<uchar3>(rgb.size())); //.create(rgb.size(), rgb.type());
+ if (chan != Channel::None) fs.frames[ix].create<cv::Mat>(chan, ftl::rgbd::FormatBase(depth.cols, depth.rows, depth.type())); //.create(depth.size(), depth.type());
- cv::swap(rgb, fs.channel1[ix]);
- cv::swap(depth, fs.channel2[ix]);
+ //cv::swap(rgb, fs.channel1[ix]);
+ //cv::swap(depth, fs.channel2[ix]);
+ cv::swap(rgb, fs.frames[ix].get<cv::Mat>(Channel::Colour));
+ if (chan != Channel::None) cv::swap(depth, fs.frames[ix].get<cv::Mat>(chan));
++fs.count;
fs.mask |= (1 << ix);
@@ -271,8 +278,9 @@ void Group::_addFrameset(int64_t timestamp) {
framesets_[head_].count = 0;
framesets_[head_].mask = 0;
framesets_[head_].stale = false;
- framesets_[head_].channel1.resize(sources_.size());
- framesets_[head_].channel2.resize(sources_.size());
+ //framesets_[head_].channel1.resize(sources_.size());
+ //framesets_[head_].channel2.resize(sources_.size());
+ framesets_[head_].frames.resize(sources_.size());
if (framesets_[head_].sources.size() != sources_.size()) {
framesets_[head_].sources.clear();
@@ -301,8 +309,9 @@ void Group::_addFrameset(int64_t timestamp) {
framesets_[head_].count = 0;
framesets_[head_].mask = 0;
framesets_[head_].stale = false;
- framesets_[head_].channel1.resize(sources_.size());
- framesets_[head_].channel2.resize(sources_.size());
+ //framesets_[head_].channel1.resize(sources_.size());
+ //framesets_[head_].channel2.resize(sources_.size());
+ framesets_[head_].frames.resize(sources_.size());
if (framesets_[head_].sources.size() != sources_.size()) {
framesets_[head_].sources.clear();
diff --git a/components/rgbd-sources/src/middlebury_source.cpp b/components/rgbd-sources/src/middlebury_source.cpp
index a707bf9fab06a212b4146065b414999d21bc9adb..e82167fdcf6b4e2bfd1a38a00b50e3cdceeb2aef 100644
--- a/components/rgbd-sources/src/middlebury_source.cpp
+++ b/components/rgbd-sources/src/middlebury_source.cpp
@@ -16,14 +16,13 @@ MiddleburySource::MiddleburySource(ftl::rgbd::Source *host)
static bool loadMiddleburyCalib(const std::string &filename, ftl::rgbd::Camera ¶ms, double scaling) {
FILE* fp = fopen(filename.c_str(), "r");
- char buff[512];
- float cam0[3][3];
+ float cam0[3][3] = {};
float cam1[3][3];
- float doffs;
- float baseline;
- int width;
- int height;
+ float doffs = 0.0f;
+ float baseline = 0.0f;
+ int width = 0;
+ int height = 0;
int ndisp;
int isint;
int vmin;
@@ -31,8 +30,8 @@ static bool loadMiddleburyCalib(const std::string &filename, ftl::rgbd::Camera &
float dyavg;
float dymax;
- if (fp != nullptr)
- {
+ if (fp != nullptr) {
+ char buff[512];
if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "cam0 = [%f %f %f; %f %f %f; %f %f %f]\n", &cam0[0][0], &cam0[0][1], &cam0[0][2], &cam0[1][0], &cam0[1][1], &cam0[1][2], &cam0[2][0], &cam0[2][1], &cam0[2][2]);
if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "cam1 = [%f %f %f; %f %f %f; %f %f %f]\n", &cam1[0][0], &cam1[0][1], &cam1[0][2], &cam1[1][0], &cam1[1][1], &cam1[1][2], &cam1[2][0], &cam1[2][1], &cam1[2][2]);
if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "doffs = %f\n", &doffs);
@@ -122,7 +121,7 @@ MiddleburySource::MiddleburySource(ftl::rgbd::Source *host, const string &dir)
mask_l_ = (mask_l == 0);
if (!host_->getConfig()["disparity"].is_object()) {
- host_->getConfig()["disparity"] = {{"algorithm","libsgm"}};
+ host_->getConfig()["disparity"] = ftl::config::json_t{{"algorithm","libsgm"}};
}
disp_ = Disparity::create(host_, "disparity");
diff --git a/components/rgbd-sources/src/middlebury_source.hpp b/components/rgbd-sources/src/middlebury_source.hpp
index 21c7d1f1c07d96130be44156fd17666868ab739b..d273d23a66d67c6618c0ac4a2062a780d9a3bddb 100644
--- a/components/rgbd-sources/src/middlebury_source.hpp
+++ b/components/rgbd-sources/src/middlebury_source.hpp
@@ -15,7 +15,7 @@ class Disparity;
class MiddleburySource : public detail::Source {
public:
- MiddleburySource(ftl::rgbd::Source *);
+ explicit MiddleburySource(ftl::rgbd::Source *);
MiddleburySource(ftl::rgbd::Source *, const std::string &dir);
~MiddleburySource() {};
diff --git a/components/rgbd-sources/src/net.cpp b/components/rgbd-sources/src/net.cpp
index 5c3c7ecb5dc3a11d0b4f96471ebe2eb8beef53d8..ba485c9402d888be0636902f3962cce2dd1e85ed 100644
--- a/components/rgbd-sources/src/net.cpp
+++ b/components/rgbd-sources/src/net.cpp
@@ -20,6 +20,7 @@ using std::vector;
using std::this_thread::sleep_for;
using std::chrono::milliseconds;
using std::tuple;
+using ftl::rgbd::Channel;
// ===== NetFrameQueue =========================================================
@@ -69,7 +70,7 @@ void NetFrameQueue::freeFrame(NetFrame &f) {
// ===== NetSource =============================================================
-bool NetSource::_getCalibration(Universe &net, const UUID &peer, const string &src, ftl::rgbd::Camera &p, ftl::rgbd::channel_t chan) {
+bool NetSource::_getCalibration(Universe &net, const UUID &peer, const string &src, ftl::rgbd::Camera &p, ftl::rgbd::Channel chan) {
try {
while(true) {
auto [cap,buf] = net.call<tuple<unsigned int,vector<unsigned char>>>(peer_, "source_details", src, chan);
@@ -227,11 +228,11 @@ void NetSource::_recvPacket(short ttimeoff, const ftl::codecs::StreamPacket &spk
int64_t now = std::chrono::time_point_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()).time_since_epoch().count();
if (!active_) return;
- const ftl::rgbd::channel_t chan = host_->getChannel();
+ const ftl::rgbd::Channel chan = host_->getChannel();
int rchan = spkt.channel & 0x1;
// Ignore any unwanted second channel
- if (chan == ftl::rgbd::kChanNone && rchan > 0) {
+ if (chan == ftl::rgbd::Channel::None && rchan > 0) {
LOG(INFO) << "Unwanted channel";
//return;
// TODO: Allow decode to be skipped
@@ -324,8 +325,8 @@ void NetSource::setPose(const Eigen::Matrix4d &pose) {
//Source::setPose(pose);
}
-ftl::rgbd::Camera NetSource::parameters(ftl::rgbd::channel_t chan) {
- if (chan == ftl::rgbd::kChanRight) {
+ftl::rgbd::Camera NetSource::parameters(ftl::rgbd::Channel chan) {
+ if (chan == ftl::rgbd::Channel::Right) {
auto uri = host_->get<string>("uri");
if (!uri) return params_;
@@ -340,7 +341,7 @@ ftl::rgbd::Camera NetSource::parameters(ftl::rgbd::channel_t chan) {
void NetSource::_updateURI() {
UNIQUE_LOCK(mutex_,lk);
active_ = false;
- prev_chan_ = ftl::rgbd::kChanNone;
+ prev_chan_ = ftl::rgbd::Channel::None;
auto uri = host_->get<string>("uri");
if (uri_.size() > 0) {
@@ -355,7 +356,7 @@ void NetSource::_updateURI() {
}
peer_ = *p;
- has_calibration_ = _getCalibration(*host_->getNet(), peer_, *uri, params_, ftl::rgbd::kChanLeft);
+ has_calibration_ = _getCalibration(*host_->getNet(), peer_, *uri, params_, ftl::rgbd::Channel::Left);
host_->getNet()->bind(*uri, [this](short ttimeoff, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
//if (chunk == -1) {
@@ -395,7 +396,7 @@ bool NetSource::compute(int n, int b) {
// Send k frames before end to prevent unwanted pause
// Unless only a single frame is requested
if ((N_ <= maxN_/2 && maxN_ > 1) || N_ == 0) {
- const ftl::rgbd::channel_t chan = host_->getChannel();
+ const ftl::rgbd::Channel chan = host_->getChannel();
N_ = maxN_;
diff --git a/components/rgbd-sources/src/net.hpp b/components/rgbd-sources/src/net.hpp
index 236ac6969759c7531c65681504b27aacae985d9d..51f31861fa3c9c39ea0cb53217e0fa3f764aeef3 100644
--- a/components/rgbd-sources/src/net.hpp
+++ b/components/rgbd-sources/src/net.hpp
@@ -39,7 +39,7 @@ class NetSource : public detail::Source {
bool isReady();
void setPose(const Eigen::Matrix4d &pose);
- Camera parameters(channel_t chan);
+ Camera parameters(ftl::rgbd::Channel chan);
void reset();
@@ -57,7 +57,7 @@ class NetSource : public detail::Source {
int minB_;
int maxN_;
int default_quality_;
- ftl::rgbd::channel_t prev_chan_;
+ ftl::rgbd::Channel prev_chan_;
ftl::rgbd::detail::ABRController abr_;
int last_bitrate_;
@@ -77,7 +77,7 @@ class NetSource : public detail::Source {
NetFrameQueue queue_;
- bool _getCalibration(ftl::net::Universe &net, const ftl::UUID &peer, const std::string &src, ftl::rgbd::Camera &p, ftl::rgbd::channel_t chan);
+ bool _getCalibration(ftl::net::Universe &net, const ftl::UUID &peer, const std::string &src, ftl::rgbd::Camera &p, ftl::rgbd::Channel chan);
void _recv(const std::vector<unsigned char> &jpg, const std::vector<unsigned char> &d);
void _recvPacket(short ttimeoff, const ftl::codecs::StreamPacket &, const ftl::codecs::Packet &);
//void _recvChunk(int64_t frame, short ttimeoff, uint8_t bitrate, int chunk, const std::vector<unsigned char> &jpg, const std::vector<unsigned char> &d);
diff --git a/components/rgbd-sources/src/offilter.cpp b/components/rgbd-sources/src/offilter.cpp
index 03db4807a7f9fa045708d3cf17bf32556b7bf5b5..466aa9249517b91ac54726e821401e85272aa082 100644
--- a/components/rgbd-sources/src/offilter.cpp
+++ b/components/rgbd-sources/src/offilter.cpp
@@ -1,4 +1,5 @@
#include "ftl/offilter.hpp"
+#include "cuda_algorithms.hpp"
#ifdef HAVE_OPTFLOW
@@ -14,101 +15,27 @@ using std::vector;
template<typename T> static bool inline isValidDisparity(T d) { return (0.0 < d) && (d < 256.0); } // TODO
OFDisparityFilter::OFDisparityFilter(Size size, int n_frames, float threshold) :
- n_(0), n_max_(n_frames), threshold_(threshold), size_(size)
+ n_max_(n_frames + 1), threshold_(threshold)
{
+ CHECK((n_max_ > 1) && (n_max_ <= 32)) << "History length must be between 0 and 31!";
+ disp_old_ = cv::cuda::GpuMat(cv::Size(size.width * n_max_, size.height), CV_32FC1);
- disp_ = Mat::zeros(cv::Size(size.width * n_frames, size.height), CV_64FC1);
- gray_ = Mat::zeros(size, CV_8UC1);
-
- nvof_ = cv::cuda::NvidiaOpticalFlow_1_0::create(size.width, size.height,
+ /*nvof_ = cv::cuda::NvidiaOpticalFlow_1_0::create(size.width, size.height,
cv::cuda::NvidiaOpticalFlow_1_0::NV_OF_PERF_LEVEL_SLOW,
- true, false, false, 0);
+ true, false, false, 0);*/
}
-void OFDisparityFilter::filter(Mat &disp, const Mat &gray)
+void OFDisparityFilter::filter(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream)
{
-
- const int n = n_;
- n_ = (n_ + 1) % n_max_;
-
- nvof_->calc(gray, gray_, flowxy_);
- nvof_->upSampler( flowxy_, size_.width, size_.height,
- nvof_->getGridSize(), flowxy_up_);
-
- CHECK(disp.type() == CV_32FC1);
- CHECK(gray.type() == CV_8UC1);
- CHECK(flowxy_up_.type() == CV_32FC2);
-
- gray.copyTo(gray_);
-
- vector<float> values(n_max_);
-
- for (int y = 0; y < size_.height; y++)
- {
- float *D = disp_.ptr<float>(y);
- float *d = disp.ptr<float>(y);
- float *flow = flowxy_up_.ptr<float>(y);
-
- for (int x = 0; x < size_.width; x++)
- {
- const float flow_l1 = abs(flow[2*x]) + abs(flow[2*x + 1]);
-
- if (flow_l1 < threshold_)
- {
- values.clear();
-
- if (isValidDisparity(d[x]))
- {
- bool updated = false;
- for (int i = 0; i < n_max_; i++)
- {
- float &val = D[n_max_ * x + (n_max_ - i + n) % n_max_];
- if (!isValidDisparity(val))
- {
- val = d[x];
- updated = true;
- }
- }
- if (!updated) { D[n_max_ * x + n] = d[x]; }
- }
-
- for (int i = 0; i < n_max_; i++)
- {
- float &val = D[n_max_ * x + i];
- if (isValidDisparity(val)) { values.push_back(val); }
- }
-
- if (values.size() > 0) {
- const auto median_it = values.begin() + values.size() / 2;
- std::nth_element(values.begin(), median_it , values.end());
- d[x] = *median_it;
- }
-
- /*
- if (isValidDepth(d[x]) && isValidDepth(D[x]))
- {
- D[x] = D[x] * 0.66 + d[x] * (1.0 - 0.66);
- }
- if (isValidDepth(D[x]))
- {
- d[x] = D[x];
- }
- else
- {
- D[x] = d[x];
- }
- */
- }
- else
- {
- for (int i = 0; i < n_max_; i++)
- {
- D[n_max_ * x + i] = 0.0;
- }
- }
- }
- }
+ frame.upload(Channel::Flow, stream);
+ const cv::cuda::GpuMat &optflow = frame.get<cv::cuda::GpuMat>(Channel::Flow);
+ //frame.get<cv::cuda::GpuMat>(Channel::Disparity);
+ stream.waitForCompletion();
+ if (optflow.empty()) { return; }
+
+ cv::cuda::GpuMat &disp = frame.create<cv::cuda::GpuMat>(Channel::Disparity);
+ ftl::cuda::optflow_filter(disp, optflow, disp_old_, n_max_, threshold_, stream);
}
#endif // HAVE_OPTFLOW
diff --git a/components/rgbd-sources/src/realsense_source.cpp b/components/rgbd-sources/src/realsense_source.cpp
index df4c0fe2535426ac52808ea985911968efb74e15..b458aa3e77c8557ee828a8f71431bb5e4e665066 100644
--- a/components/rgbd-sources/src/realsense_source.cpp
+++ b/components/rgbd-sources/src/realsense_source.cpp
@@ -57,6 +57,9 @@ bool RealsenseSource::compute(int n, int b) {
cv::Mat tmp(cv::Size((int)w, (int)h), CV_16UC1, (void*)depth.get_data(), depth.get_stride_in_bytes());
tmp.convertTo(depth_, CV_32FC1, scale_);
rgb_ = cv::Mat(cv::Size(w, h), CV_8UC4, (void*)rscolour_.get_data(), cv::Mat::AUTO_STEP);
+
+ auto cb = host_->callback();
+ if (cb) cb(timestamp_, rgb_, depth_);
return true;
}
diff --git a/components/rgbd-sources/src/realsense_source.hpp b/components/rgbd-sources/src/realsense_source.hpp
index 4eb182032ffa67cbe121993ed981398a57e0470a..371d305b7d27fc73ad85bba83965f58dcd28c45b 100644
--- a/components/rgbd-sources/src/realsense_source.hpp
+++ b/components/rgbd-sources/src/realsense_source.hpp
@@ -14,7 +14,7 @@ namespace detail {
class RealsenseSource : public ftl::rgbd::detail::Source {
public:
- RealsenseSource(ftl::rgbd::Source *host);
+ explicit RealsenseSource(ftl::rgbd::Source *host);
~RealsenseSource();
bool capture(int64_t ts) { timestamp_ = ts; return true; }
diff --git a/components/rgbd-sources/src/snapshot.cpp b/components/rgbd-sources/src/snapshot.cpp
index 8ce89fa080392eb755278f42d92c60a7b0fa0f1e..7a80ee677c6275f2446d0f2b404e3bc778745d08 100644
--- a/components/rgbd-sources/src/snapshot.cpp
+++ b/components/rgbd-sources/src/snapshot.cpp
@@ -169,7 +169,7 @@ bool SnapshotWriter::addRGBD(size_t source, const cv::Mat &rgb, const cv::Mat &d
void SnapshotWriter::writeIndex() {
FileStorage fs(".yml", FileStorage::WRITE + FileStorage::MEMORY);
- vector<string> channels = {"time", "rgb_left", "depth_left"};
+ vector<string> channels{"time", "rgb_left", "depth_left"};
fs << "sources" << sources_;
fs << "params" <<params_;
diff --git a/components/rgbd-sources/src/snapshot_source.hpp b/components/rgbd-sources/src/snapshot_source.hpp
index 1200f460cfb0ed68cc7cc1573b66c9135c605404..de1b0df48be79df732f51144226f5c7e6d2f0478 100644
--- a/components/rgbd-sources/src/snapshot_source.hpp
+++ b/components/rgbd-sources/src/snapshot_source.hpp
@@ -13,7 +13,7 @@ namespace detail {
class SnapshotSource : public detail::Source {
public:
- SnapshotSource(ftl::rgbd::Source *);
+ explicit SnapshotSource(ftl::rgbd::Source *);
SnapshotSource(ftl::rgbd::Source *, ftl::rgbd::Snapshot &snapshot, const std::string &id);
~SnapshotSource() {};
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 10c9210f183c8f51d0b1e5424f0e4e441321cd86..35d23f27ad7edac18d3e3e02247296f1382be5e2 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -25,10 +25,11 @@ using ftl::rgbd::detail::NetSource;
using ftl::rgbd::detail::ImageSource;
using ftl::rgbd::detail::MiddleburySource;
using ftl::rgbd::capability_t;
+using ftl::rgbd::Channel;
Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matrix4d::Identity()), net_(nullptr) {
impl_ = nullptr;
- params_ = {0};
+ params_ = {};
stream_ = 0;
timestamp_ = 0;
reset();
@@ -41,7 +42,7 @@ Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matri
Source::Source(ftl::config::json_t &cfg, ftl::net::Universe *net) : Configurable(cfg), pose_(Eigen::Matrix4d::Identity()), net_(net) {
impl_ = nullptr;
- params_ = {0};
+ params_ = {};
stream_ = 0;
timestamp_ = 0;
reset();
@@ -227,7 +228,7 @@ capability_t Source::getCapabilities() const {
void Source::reset() {
UNIQUE_LOCK(mutex_,lk);
- channel_ = kChanNone;
+ channel_ = Channel::None;
if (impl_) delete impl_;
impl_ = _createImplementation();
}
@@ -272,71 +273,6 @@ bool Source::compute(int N, int B) {
return false;
}
-void Source::writeFrames(int64_t ts, const cv::Mat &rgb, const cv::Mat &depth) {
- if (!impl_) {
- UNIQUE_LOCK(mutex_,lk);
- rgb.copyTo(rgb_);
- depth.copyTo(depth_);
- timestamp_ = ts;
- }
-}
-
-void Source::writeFrames(int64_t ts, const ftl::cuda::TextureObject<uchar4> &rgb, const ftl::cuda::TextureObject<uint> &depth, cudaStream_t stream) {
- if (!impl_) {
- UNIQUE_LOCK(mutex_,lk);
- timestamp_ = ts;
- rgb_.create(rgb.height(), rgb.width(), CV_8UC4);
- cudaSafeCall(cudaMemcpy2DAsync(rgb_.data, rgb_.step, rgb.devicePtr(), rgb.pitch(), rgb_.cols * sizeof(uchar4), rgb_.rows, cudaMemcpyDeviceToHost, stream));
- depth_.create(depth.height(), depth.width(), CV_32SC1);
- cudaSafeCall(cudaMemcpy2DAsync(depth_.data, depth_.step, depth.devicePtr(), depth.pitch(), depth_.cols * sizeof(uint), depth_.rows, cudaMemcpyDeviceToHost, stream));
- //cudaSafeCall(cudaStreamSynchronize(stream)); // TODO:(Nick) Don't wait here.
- stream_ = stream;
- //depth_.convertTo(depth_, CV_32F, 1.0f / 1000.0f);
- } else {
- LOG(ERROR) << "writeFrames cannot be done on this source: " << getURI();
- }
-}
-
-void Source::writeFrames(int64_t ts, const ftl::cuda::TextureObject<uchar4> &rgb, const ftl::cuda::TextureObject<float> &depth, cudaStream_t stream) {
- if (!impl_) {
- UNIQUE_LOCK(mutex_,lk);
- timestamp_ = ts;
- rgb.download(rgb_, stream);
- //rgb_.create(rgb.height(), rgb.width(), CV_8UC4);
- //cudaSafeCall(cudaMemcpy2DAsync(rgb_.data, rgb_.step, rgb.devicePtr(), rgb.pitch(), rgb_.cols * sizeof(uchar4), rgb_.rows, cudaMemcpyDeviceToHost, stream));
- depth.download(depth_, stream);
- //depth_.create(depth.height(), depth.width(), CV_32FC1);
- //cudaSafeCall(cudaMemcpy2DAsync(depth_.data, depth_.step, depth.devicePtr(), depth.pitch(), depth_.cols * sizeof(float), depth_.rows, cudaMemcpyDeviceToHost, stream));
-
- stream_ = stream;
- cudaSafeCall(cudaStreamSynchronize(stream_));
- cv::cvtColor(rgb_,rgb_, cv::COLOR_BGRA2BGR);
- cv::cvtColor(rgb_,rgb_, cv::COLOR_Lab2BGR);
-
- if (callback_) callback_(timestamp_, rgb_, depth_);
- }
-}
-
-void Source::writeFrames(int64_t ts, const ftl::cuda::TextureObject<uchar4> &rgb, const ftl::cuda::TextureObject<uchar4> &rgb2, cudaStream_t stream) {
- if (!impl_) {
- UNIQUE_LOCK(mutex_,lk);
- timestamp_ = ts;
- rgb.download(rgb_, stream);
- //rgb_.create(rgb.height(), rgb.width(), CV_8UC4);
- //cudaSafeCall(cudaMemcpy2DAsync(rgb_.data, rgb_.step, rgb.devicePtr(), rgb.pitch(), rgb_.cols * sizeof(uchar4), rgb_.rows, cudaMemcpyDeviceToHost, stream));
- rgb2.download(depth_, stream);
- //depth_.create(depth.height(), depth.width(), CV_32FC1);
- //cudaSafeCall(cudaMemcpy2DAsync(depth_.data, depth_.step, depth.devicePtr(), depth.pitch(), depth_.cols * sizeof(float), depth_.rows, cudaMemcpyDeviceToHost, stream));
-
- stream_ = stream;
- cudaSafeCall(cudaStreamSynchronize(stream_));
- cv::cvtColor(rgb_,rgb_, cv::COLOR_BGRA2BGR);
- cv::cvtColor(rgb_,rgb_, cv::COLOR_Lab2BGR);
- cv::cvtColor(depth_,depth_, cv::COLOR_BGRA2BGR);
- cv::cvtColor(depth_,depth_, cv::COLOR_Lab2BGR);
- }
-}
-
bool Source::thumbnail(cv::Mat &t) {
if (!impl_ && stream_ != 0) {
cudaSafeCall(cudaStreamSynchronize(stream_));
@@ -360,13 +296,13 @@ bool Source::thumbnail(cv::Mat &t) {
return !thumb_.empty();
}
-bool Source::setChannel(ftl::rgbd::channel_t c) {
+bool Source::setChannel(ftl::rgbd::Channel c) {
channel_ = c;
// FIXME:(Nick) Verify channel is supported by this source...
return true;
}
-const ftl::rgbd::Camera Source::parameters(ftl::rgbd::channel_t chan) const {
+const ftl::rgbd::Camera Source::parameters(ftl::rgbd::Channel chan) const {
return (impl_) ? impl_->parameters(chan) : parameters();
}
diff --git a/components/rgbd-sources/src/stereovideo.cpp b/components/rgbd-sources/src/stereovideo.cpp
index ebc72d854e8d35a480f3d4ef5873e8b58d5bb086..6573f74f4d7cf1f3761f98a66c68c6963e10af31 100644
--- a/components/rgbd-sources/src/stereovideo.cpp
+++ b/components/rgbd-sources/src/stereovideo.cpp
@@ -12,6 +12,7 @@
using ftl::rgbd::detail::Calibrate;
using ftl::rgbd::detail::LocalSource;
using ftl::rgbd::detail::StereoVideoSource;
+using ftl::rgbd::Channel;
using std::string;
StereoVideoSource::StereoVideoSource(ftl::rgbd::Source *host)
@@ -135,8 +136,8 @@ void StereoVideoSource::init(const string &file)
ready_ = true;
}
-ftl::rgbd::Camera StereoVideoSource::parameters(ftl::rgbd::channel_t chan) {
- if (chan == ftl::rgbd::kChanRight) {
+ftl::rgbd::Camera StereoVideoSource::parameters(Channel chan) {
+ if (chan == Channel::Right) {
cv::Mat q = calib_->getCameraMatrixRight();
ftl::rgbd::Camera params = {
q.at<double>(0,0), // Fx
@@ -175,8 +176,8 @@ bool StereoVideoSource::capture(int64_t ts) {
bool StereoVideoSource::retrieve() {
auto &frame = frames_[0];
frame.reset();
- auto &left = frame.setChannel<cv::cuda::GpuMat>(ftl::rgbd::kChanLeft);
- auto &right = frame.setChannel<cv::cuda::GpuMat>(ftl::rgbd::kChanRight);
+ auto &left = frame.create<cv::cuda::GpuMat>(Channel::Left);
+ auto &right = frame.create<cv::cuda::GpuMat>(Channel::Right);
lsrc_->get(left, right, calib_, stream2_);
#ifdef HAVE_OPTFLOW
@@ -184,17 +185,18 @@ bool StereoVideoSource::retrieve() {
if (use_optflow_)
{
- auto &left_gray = frame.setChannel<cv::cuda::GpuMat>(kChanLeftGray);
- auto &right_gray = frame.setChannel<cv::cuda::GpuMat>(kChanRightGray);
+ auto &left_gray = frame.create<cv::cuda::GpuMat>(Channel::LeftGray);
+ auto &right_gray = frame.create<cv::cuda::GpuMat>(Channel::RightGray);
cv::cuda::cvtColor(left, left_gray, cv::COLOR_BGR2GRAY, 0, stream2_);
cv::cuda::cvtColor(right, right_gray, cv::COLOR_BGR2GRAY, 0, stream2_);
- if (frames_[1].hasChannel(kChanLeftGray))
+ if (frames_[1].hasChannel(Channel::LeftGray))
{
- auto &left_gray_prev = frame.getChannel<cv::cuda::GpuMat>(kChanLeftGray, stream2_);
- auto &optflow = frame.setChannel<cv::cuda::GpuMat>(kChanFlow);
- nvof_->calc(left_gray, left_gray_prev, optflow_, stream2_);
+ //frames_[1].download(Channel::LeftGray);
+ auto &left_gray_prev = frames_[1].get<cv::cuda::GpuMat>(Channel::LeftGray);
+ auto &optflow = frame.create<cv::cuda::GpuMat>(Channel::Flow);
+ nvof_->calc(left_gray, left_gray_prev, optflow, stream2_);
// nvof_->upSampler() isn't implemented with CUDA
// cv::cuda::resize() does not work wiht 2-channel input
// cv::cuda::resize(optflow_, optflow, left.size(), 0.0, 0.0, cv::INTER_NEAREST, stream2_);
@@ -207,32 +209,33 @@ bool StereoVideoSource::retrieve() {
}
void StereoVideoSource::swap() {
- auto tmp = frames_[0];
- frames_[0] = frames_[1];
- frames_[1] = tmp;
+ auto tmp = std::move(frames_[0]);
+ frames_[0] = std::move(frames_[1]);
+ frames_[1] = std::move(tmp);
}
bool StereoVideoSource::compute(int n, int b) {
auto &frame = frames_[1];
- auto &left = frame.getChannel<cv::cuda::GpuMat>(ftl::rgbd::kChanLeft);
- auto &right = frame.getChannel<cv::cuda::GpuMat>(ftl::rgbd::kChanRight);
+ auto &left = frame.get<cv::cuda::GpuMat>(Channel::Left);
+ auto &right = frame.get<cv::cuda::GpuMat>(Channel::Right);
- const ftl::rgbd::channel_t chan = host_->getChannel();
+ const ftl::rgbd::Channel chan = host_->getChannel();
if (left.empty() || right.empty()) return false;
- if (chan == ftl::rgbd::kChanDepth) {
+ if (chan == Channel::Depth) {
disp_->compute(frame, stream_);
- auto &disp = frame.getChannel<cv::cuda::GpuMat>(ftl::rgbd::kChanDisparity);
- auto &depth = frame.setChannel<cv::cuda::GpuMat>(ftl::rgbd::kChanDepth);
+ auto &disp = frame.get<cv::cuda::GpuMat>(Channel::Disparity);
+ auto &depth = frame.create<cv::cuda::GpuMat>(Channel::Depth);
if (depth.empty()) depth = cv::cuda::GpuMat(left.size(), CV_32FC1);
ftl::cuda::disparity_to_depth(disp, depth, params_, stream_);
left.download(rgb_, stream_);
depth.download(depth_, stream_);
+ //frame.download(Channel::Left + Channel::Depth);
stream_.waitForCompletion(); // TODO:(Nick) Move to getFrames
- } else if (chan == ftl::rgbd::kChanRight) {
+ } else if (chan == Channel::Right) {
left.download(rgb_, stream_);
right.download(depth_, stream_);
stream_.waitForCompletion(); // TODO:(Nick) Move to getFrames
diff --git a/components/rgbd-sources/src/stereovideo.hpp b/components/rgbd-sources/src/stereovideo.hpp
index 9fe3ca529f2f32300cb85aa47bd958b78f78984c..9d3325e1ac27ec544abeb409149cb89817dc29d2 100644
--- a/components/rgbd-sources/src/stereovideo.hpp
+++ b/components/rgbd-sources/src/stereovideo.hpp
@@ -31,7 +31,7 @@ class StereoVideoSource : public detail::Source {
bool retrieve();
bool compute(int n, int b);
bool isReady();
- Camera parameters(channel_t chan);
+ Camera parameters(ftl::rgbd::Channel chan);
//const cv::Mat &getRight() const { return right_; }
diff --git a/components/rgbd-sources/src/streamer.cpp b/components/rgbd-sources/src/streamer.cpp
index 4a9ccf529fbfcfac285312e1651d5a4b292fb849..676cf58cf3045111cf966d78d587c0e918a351e7 100644
--- a/components/rgbd-sources/src/streamer.cpp
+++ b/components/rgbd-sources/src/streamer.cpp
@@ -17,6 +17,7 @@ using ftl::rgbd::detail::StreamClient;
using ftl::rgbd::detail::ABRController;
using ftl::codecs::definition_t;
using ftl::codecs::device_t;
+using ftl::rgbd::Channel;
using ftl::net::Universe;
using std::string;
using std::list;
@@ -46,11 +47,11 @@ Streamer::Streamer(nlohmann::json &config, Universe *net)
hq_devices_ = (value("disable_hardware_encode", false)) ? device_t::Software : device_t::Any;
//group_.setFPS(value("fps", 20));
- group_.setLatency(10);
+ group_.setLatency(4);
compress_level_ = value("compression", 1);
- net->bind("find_stream", [this](const std::string &uri) -> optional<UUID> {
+ net->bind("find_stream", [this](const std::string &uri) -> optional<ftl::UUID> {
SHARED_LOCK(mutex_,slk);
if (sources_.find(uri) != sources_.end()) {
@@ -91,7 +92,7 @@ Streamer::Streamer(nlohmann::json &config, Universe *net)
});
// Allow remote users to access camera calibration matrix
- net->bind("source_details", [this](const std::string &uri, ftl::rgbd::channel_t chan) -> tuple<unsigned int,vector<unsigned char>> {
+ net->bind("source_details", [this](const std::string &uri, ftl::rgbd::Channel chan) -> tuple<unsigned int,vector<unsigned char>> {
vector<unsigned char> buf;
SHARED_LOCK(mutex_,slk);
@@ -110,11 +111,11 @@ Streamer::Streamer(nlohmann::json &config, Universe *net)
_addClient(source, N, rate, peer, dest);
});
- net->bind("set_channel", [this](const string &uri, unsigned int chan) {
+ net->bind("set_channel", [this](const string &uri, Channel chan) {
SHARED_LOCK(mutex_,slk);
if (sources_.find(uri) != sources_.end()) {
- sources_[uri]->src->setChannel((ftl::rgbd::channel_t)chan);
+ sources_[uri]->src->setChannel(chan);
}
});
@@ -365,9 +366,10 @@ void Streamer::_process(ftl::rgbd::FrameSet &fs) {
if (!src) continue;
if (!fs.sources[j]->isReady()) continue;
if (src->clientCount == 0) continue;
- if (fs.channel1[j].empty() || (fs.sources[j]->getChannel() != ftl::rgbd::kChanNone && fs.channel2[j].empty())) continue;
+ //if (fs.channel1[j].empty() || (fs.sources[j]->getChannel() != ftl::rgbd::kChanNone && fs.channel2[j].empty())) continue;
+ if (!fs.frames[j].hasChannel(Channel::Colour) || !fs.frames[j].hasChannel(fs.sources[j]->getChannel())) continue;
- bool hasChan2 = fs.sources[j]->getChannel() != ftl::rgbd::kChanNone;
+ bool hasChan2 = fs.sources[j]->getChannel() != Channel::None;
totalclients += src->clientCount;
@@ -387,14 +389,14 @@ void Streamer::_process(ftl::rgbd::FrameSet &fs) {
// Receiver only waits for channel 1 by default
// TODO: Each encode could be done in own thread
if (hasChan2) {
- enc2->encode(fs.channel2[j], src->hq_bitrate, [this,src,hasChan2](const ftl::codecs::Packet &blk){
+ enc2->encode(fs.frames[j].get<cv::Mat>(fs.sources[j]->getChannel()), src->hq_bitrate, [this,src,hasChan2](const ftl::codecs::Packet &blk){
_transmitPacket(src, blk, 1, hasChan2, true);
});
} else {
if (enc2) enc2->reset();
}
- enc1->encode(fs.channel1[j], src->hq_bitrate, [this,src,hasChan2](const ftl::codecs::Packet &blk){
+ enc1->encode(fs.frames[j].get<cv::Mat>(Channel::Colour), src->hq_bitrate, [this,src,hasChan2](const ftl::codecs::Packet &blk){
_transmitPacket(src, blk, 0, hasChan2, true);
});
}
@@ -415,14 +417,14 @@ void Streamer::_process(ftl::rgbd::FrameSet &fs) {
// Important to send channel 2 first if needed...
// Receiver only waits for channel 1 by default
if (hasChan2) {
- enc2->encode(fs.channel2[j], src->lq_bitrate, [this,src,hasChan2](const ftl::codecs::Packet &blk){
+ enc2->encode(fs.frames[j].get<cv::Mat>(fs.sources[j]->getChannel()), src->lq_bitrate, [this,src,hasChan2](const ftl::codecs::Packet &blk){
_transmitPacket(src, blk, 1, hasChan2, false);
});
} else {
if (enc2) enc2->reset();
}
- enc1->encode(fs.channel1[j], src->lq_bitrate, [this,src,hasChan2](const ftl::codecs::Packet &blk){
+ enc1->encode(fs.frames[j].get<cv::Mat>(Channel::Colour), src->lq_bitrate, [this,src,hasChan2](const ftl::codecs::Packet &blk){
_transmitPacket(src, blk, 0, hasChan2, false);
});
}
@@ -494,7 +496,7 @@ void Streamer::_transmitPacket(StreamSource *src, const ftl::codecs::Packet &pkt
frame_no_,
static_cast<uint8_t>((chan & 0x1) | ((hasChan2) ? 0x2 : 0x0))
};
-
+ LOG(INFO) << "codec:" << (int) pkt.codec;
// Lock to prevent clients being added / removed
//SHARED_LOCK(src->mutex,lk);
auto c = src->clients.begin();
diff --git a/components/rgbd-sources/src/virtual.cpp b/components/rgbd-sources/src/virtual.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0e6db973884a3c8361fcaae764cc1688d2434d9d
--- /dev/null
+++ b/components/rgbd-sources/src/virtual.cpp
@@ -0,0 +1,144 @@
+#include <ftl/rgbd/virtual.hpp>
+
+using ftl::rgbd::VirtualSource;
+using ftl::rgbd::Source;
+using ftl::rgbd::Channel;
+
+class VirtualImpl : public ftl::rgbd::detail::Source {
+ public:
+ explicit VirtualImpl(ftl::rgbd::Source *host, const ftl::rgbd::Camera ¶ms) : ftl::rgbd::detail::Source(host) {
+ params_ = params;
+ capabilities_ = ftl::rgbd::kCapMovable | ftl::rgbd::kCapVideo | ftl::rgbd::kCapStereo;
+ }
+
+ ~VirtualImpl() {
+
+ }
+
+ bool capture(int64_t ts) override {
+ timestamp_ = ts;
+ return true;
+ }
+
+ bool retrieve() override {
+ return true;
+ }
+
+ bool compute(int n, int b) override {
+ if (callback) {
+ frame.reset();
+
+ try {
+ callback(frame);
+ } catch (std::exception &e) {
+ LOG(ERROR) << "Exception in render callback: " << e.what();
+ } catch (...) {
+ LOG(ERROR) << "Unknown exception in render callback";
+ }
+
+ if (frame.hasChannel(Channel::Colour)) {
+ frame.download(Channel::Colour);
+ cv::swap(frame.get<cv::Mat>(Channel::Colour), rgb_);
+ } else {
+ LOG(ERROR) << "Channel 1 frame in rendering";
+ }
+
+ if ((host_->getChannel() != Channel::None) &&
+ frame.hasChannel(host_->getChannel())) {
+ frame.download(host_->getChannel());
+ cv::swap(frame.get<cv::Mat>(host_->getChannel()), depth_);
+ } else {
+ LOG(ERROR) << "Channel 2 frame in rendering";
+ }
+
+ auto cb = host_->callback();
+ if (cb) cb(timestamp_, rgb_, depth_);
+ }
+ return true;
+ }
+
+ bool isReady() override { return true; }
+
+ std::function<void(ftl::rgbd::Frame &)> callback;
+ ftl::rgbd::Frame frame;
+};
+
+VirtualSource::VirtualSource(ftl::config::json_t &cfg) : Source(cfg) {
+ auto params = params_;
+ impl_ = new VirtualImpl(this, params);
+}
+
+VirtualSource::~VirtualSource() {
+
+}
+
+void VirtualSource::onRender(const std::function<void(ftl::rgbd::Frame &)> &f) {
+ dynamic_cast<VirtualImpl*>(impl_)->callback = f;
+}
+
+/*
+void Source::writeFrames(int64_t ts, const cv::Mat &rgb, const cv::Mat &depth) {
+ if (!impl_) {
+ UNIQUE_LOCK(mutex_,lk);
+ rgb.copyTo(rgb_);
+ depth.copyTo(depth_);
+ timestamp_ = ts;
+ }
+}
+
+void Source::writeFrames(int64_t ts, const ftl::cuda::TextureObject<uchar4> &rgb, const ftl::cuda::TextureObject<uint> &depth, cudaStream_t stream) {
+ if (!impl_) {
+ UNIQUE_LOCK(mutex_,lk);
+ timestamp_ = ts;
+ rgb_.create(rgb.height(), rgb.width(), CV_8UC4);
+ cudaSafeCall(cudaMemcpy2DAsync(rgb_.data, rgb_.step, rgb.devicePtr(), rgb.pitch(), rgb_.cols * sizeof(uchar4), rgb_.rows, cudaMemcpyDeviceToHost, stream));
+ depth_.create(depth.height(), depth.width(), CV_32SC1);
+ cudaSafeCall(cudaMemcpy2DAsync(depth_.data, depth_.step, depth.devicePtr(), depth.pitch(), depth_.cols * sizeof(uint), depth_.rows, cudaMemcpyDeviceToHost, stream));
+ //cudaSafeCall(cudaStreamSynchronize(stream)); // TODO:(Nick) Don't wait here.
+ stream_ = stream;
+ //depth_.convertTo(depth_, CV_32F, 1.0f / 1000.0f);
+ } else {
+ LOG(ERROR) << "writeFrames cannot be done on this source: " << getURI();
+ }
+}
+
+void Source::writeFrames(int64_t ts, const ftl::cuda::TextureObject<uchar4> &rgb, const ftl::cuda::TextureObject<float> &depth, cudaStream_t stream) {
+ if (!impl_) {
+ UNIQUE_LOCK(mutex_,lk);
+ timestamp_ = ts;
+ rgb.download(rgb_, stream);
+ //rgb_.create(rgb.height(), rgb.width(), CV_8UC4);
+ //cudaSafeCall(cudaMemcpy2DAsync(rgb_.data, rgb_.step, rgb.devicePtr(), rgb.pitch(), rgb_.cols * sizeof(uchar4), rgb_.rows, cudaMemcpyDeviceToHost, stream));
+ depth.download(depth_, stream);
+ //depth_.create(depth.height(), depth.width(), CV_32FC1);
+ //cudaSafeCall(cudaMemcpy2DAsync(depth_.data, depth_.step, depth.devicePtr(), depth.pitch(), depth_.cols * sizeof(float), depth_.rows, cudaMemcpyDeviceToHost, stream));
+
+ stream_ = stream;
+ cudaSafeCall(cudaStreamSynchronize(stream_));
+ cv::cvtColor(rgb_,rgb_, cv::COLOR_BGRA2BGR);
+ cv::cvtColor(rgb_,rgb_, cv::COLOR_Lab2BGR);
+
+ if (callback_) callback_(timestamp_, rgb_, depth_);
+ }
+}
+
+void Source::writeFrames(int64_t ts, const ftl::cuda::TextureObject<uchar4> &rgb, const ftl::cuda::TextureObject<uchar4> &rgb2, cudaStream_t stream) {
+ if (!impl_) {
+ UNIQUE_LOCK(mutex_,lk);
+ timestamp_ = ts;
+ rgb.download(rgb_, stream);
+ //rgb_.create(rgb.height(), rgb.width(), CV_8UC4);
+ //cudaSafeCall(cudaMemcpy2DAsync(rgb_.data, rgb_.step, rgb.devicePtr(), rgb.pitch(), rgb_.cols * sizeof(uchar4), rgb_.rows, cudaMemcpyDeviceToHost, stream));
+ rgb2.download(depth_, stream);
+ //depth_.create(depth.height(), depth.width(), CV_32FC1);
+ //cudaSafeCall(cudaMemcpy2DAsync(depth_.data, depth_.step, depth.devicePtr(), depth.pitch(), depth_.cols * sizeof(float), depth_.rows, cudaMemcpyDeviceToHost, stream));
+
+ stream_ = stream;
+ cudaSafeCall(cudaStreamSynchronize(stream_));
+ cv::cvtColor(rgb_,rgb_, cv::COLOR_BGRA2BGR);
+ cv::cvtColor(rgb_,rgb_, cv::COLOR_Lab2BGR);
+ cv::cvtColor(depth_,depth_, cv::COLOR_BGRA2BGR);
+ cv::cvtColor(depth_,depth_, cv::COLOR_Lab2BGR);
+ }
+}
+*/
\ No newline at end of file
diff --git a/components/rgbd-sources/test/CMakeLists.txt b/components/rgbd-sources/test/CMakeLists.txt
index 96e1441c5da6134eb17070d77e5ce9dff3a355f1..78bb6cec7e8c411ffbfe982a1b65320f56439bd5 100644
--- a/components/rgbd-sources/test/CMakeLists.txt
+++ b/components/rgbd-sources/test/CMakeLists.txt
@@ -5,6 +5,29 @@ add_executable(source_unit
)
target_include_directories(source_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../include")
target_link_libraries(source_unit
- ftlcommon Eigen3::Eigen ${CUDA_LIBRARIES})
+ ftlcommon ftlcodecs ftlnet Eigen3::Eigen ${CUDA_LIBRARIES})
add_test(SourceUnitTest source_unit)
+
+### Channel Unit ###############################################################
+add_executable(channel_unit
+ ./tests.cpp
+ ./channel_unit.cpp
+)
+target_include_directories(channel_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../include")
+target_link_libraries(channel_unit
+ ftlcommon)
+
+add_test(ChannelUnitTest channel_unit)
+
+### Frame Unit #################################################################
+add_executable(frame_unit
+ ./tests.cpp
+ ./frame_unit.cpp
+ ../src/frame.cpp
+)
+target_include_directories(frame_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../include")
+target_link_libraries(frame_unit
+ ftlcommon ftlcodecs)
+
+add_test(FrameUnitTest frame_unit)
diff --git a/components/rgbd-sources/test/channel_unit.cpp b/components/rgbd-sources/test/channel_unit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..25171678540f1970088d84e1e842865a4249455e
--- /dev/null
+++ b/components/rgbd-sources/test/channel_unit.cpp
@@ -0,0 +1,88 @@
+#include "catch.hpp"
+#include <ftl/rgbd/channels.hpp>
+
+using ftl::rgbd::Channel;
+using ftl::rgbd::Channels;
+
+TEST_CASE("channel casting", "") {
+ SECTION("cast channel to channels") {
+ Channels cs(Channel::Depth);
+
+ REQUIRE( (unsigned int)cs > 0 );
+ REQUIRE( cs.count() == 1 );
+ }
+
+ SECTION("cast channels to channel") {
+ Channels cs(Channel::Depth);
+ Channel c = (Channel)cs;
+
+ REQUIRE( c == Channel::Depth );
+ }
+}
+
+TEST_CASE("Channel or-ing", "") {
+ SECTION("Add channel to channel mask") {
+ Channels cs(Channel::Depth);
+
+ cs |= Channel::Right;
+
+ REQUIRE( (cs.count() == 2) );
+ REQUIRE( cs.has(Channel::Right) );
+ REQUIRE( cs.has(Channel::Depth) );
+ }
+
+ SECTION("Combine multiple channels in assignment") {
+ Channels cs;
+
+ cs = Channel::Right | Channel::Flow | Channel::Left;
+
+ REQUIRE( (cs.count() == 3) );
+ REQUIRE( cs.has(Channel::Right) );
+ REQUIRE( cs.has(Channel::Flow) );
+ REQUIRE( cs.has(Channel::Left) );
+ }
+
+ SECTION("Combine multiple channels at init") {
+ Channels cs = Channel::Right | Channel::Flow | Channel::Left;
+
+ REQUIRE( (cs.count() == 3) );
+ REQUIRE( cs.has(Channel::Right) );
+ REQUIRE( cs.has(Channel::Flow) );
+ REQUIRE( cs.has(Channel::Left) );
+ }
+}
+
+TEST_CASE("Channel adding", "") {
+ SECTION("Add channel to channel mask") {
+ Channels cs(Channel::Depth);
+
+ cs += Channel::Right;
+
+ REQUIRE( (cs.count() == 2) );
+ REQUIRE( cs.has(Channel::Right) );
+ REQUIRE( cs.has(Channel::Depth) );
+ }
+
+ SECTION("Combine multiple channels in assignment") {
+ Channels cs;
+
+ cs = Channel::Right + Channel::Flow + Channel::Left;
+
+ REQUIRE( (cs.count() == 3) );
+ REQUIRE( cs.has(Channel::Right) );
+ REQUIRE( cs.has(Channel::Flow) );
+ REQUIRE( cs.has(Channel::Left) );
+ }
+}
+
+TEST_CASE("Channel subtracting", "") {
+ SECTION("Remove channel from channel mask") {
+ Channels cs = Channel::Right | Channel::Flow | Channel::Left;
+
+ cs -= Channel::Flow;
+
+ REQUIRE( (cs.count() == 2) );
+ REQUIRE( cs.has(Channel::Right) );
+ REQUIRE( cs.has(Channel::Left) );
+ }
+}
diff --git a/components/rgbd-sources/test/frame_unit.cpp b/components/rgbd-sources/test/frame_unit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6ad528a28fd677e207b05362c0021f7d302784dc
--- /dev/null
+++ b/components/rgbd-sources/test/frame_unit.cpp
@@ -0,0 +1,283 @@
+#include "catch.hpp"
+#include <ftl/rgbd/frame.hpp>
+
+using ftl::rgbd::Frame;
+using ftl::rgbd::Channel;
+using ftl::rgbd::Channels;
+using ftl::rgbd::Format;
+
+TEST_CASE("Frame::create() cpu mat", "") {
+ SECTION("in empty channel with format") {
+ Frame f;
+ auto &m = f.create<cv::Mat>(Channel::Colour, Format<float4>(200,200));
+
+ REQUIRE( m.type() == CV_32FC4 );
+ REQUIRE( m.cols == 200 );
+ REQUIRE( m.rows == 200 );
+ }
+
+ SECTION("in non-empty channel with format") {
+ Frame f;
+ f.create<cv::Mat>(Channel::Colour, Format<float>(200,100));
+ auto &m = f.create<cv::Mat>(Channel::Colour, Format<float4>(200,200));
+
+ REQUIRE( m.type() == CV_32FC4 );
+ REQUIRE( m.cols == 200 );
+ REQUIRE( m.rows == 200 );
+ }
+}
+
+TEST_CASE("Frame::get()", "") {
+ SECTION("get a non-existant host channel") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ f.get<cv::Mat>(Channel::Colour);
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( hadexception );
+ }
+
+ SECTION("get a non-existant gpu channel") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ f.get<cv::cuda::GpuMat>(Channel::Colour);
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( hadexception );
+ }
+
+ SECTION("get a valid host channel") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ f.create<cv::Mat>(Channel::Colour, Format<uchar3>(1024,1024));
+ auto &m = f.get<cv::Mat>(Channel::Colour);
+
+ REQUIRE( m.type() == CV_8UC3 );
+ REQUIRE( m.cols == 1024 );
+ REQUIRE( m.rows == 1024 );
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( !hadexception );
+ }
+
+ SECTION("get a valid gpu channel") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ f.create<cv::cuda::GpuMat>(Channel::Colour, Format<uchar3>(1024,1024));
+ auto &m = f.get<cv::cuda::GpuMat>(Channel::Colour);
+
+ REQUIRE( m.type() == CV_8UC3 );
+ REQUIRE( m.cols == 1024 );
+ REQUIRE( m.rows == 1024 );
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( !hadexception );
+ }
+
+ SECTION("get a cpu mat from gpu channel") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ f.create<cv::cuda::GpuMat>(Channel::Colour, Format<uchar3>(1024,1024));
+ REQUIRE( f.isGPU(Channel::Colour) );
+
+ auto &m = f.get<cv::Mat>(Channel::Colour);
+
+ REQUIRE( f.isCPU(Channel::Colour) );
+ REQUIRE( m.type() == CV_8UC3 );
+ REQUIRE( m.cols == 1024 );
+ REQUIRE( m.rows == 1024 );
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( !hadexception );
+ }
+
+ SECTION("get a gpu mat from cpu channel") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ f.create<cv::Mat>(Channel::Colour, Format<uchar3>(1024,1024));
+ REQUIRE( f.isCPU(Channel::Colour) );
+
+ auto &m = f.get<cv::cuda::GpuMat>(Channel::Colour);
+
+ REQUIRE( f.isGPU(Channel::Colour) );
+ REQUIRE( m.type() == CV_8UC3 );
+ REQUIRE( m.cols == 1024 );
+ REQUIRE( m.rows == 1024 );
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( !hadexception );
+ }
+}
+
+TEST_CASE("Frame::createTexture()", "") {
+ SECTION("Missing format and no existing mat") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ f.createTexture<float>(Channel::Depth);
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( hadexception );
+ }
+
+ SECTION("Missing format but with existing host mat") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ f.create<cv::Mat>(Channel::Depth, Format<float>(100,100));
+ auto &t = f.createTexture<float>(Channel::Depth);
+
+ REQUIRE( t.width() == 100 );
+ REQUIRE( t.cvType() == CV_32FC1 );
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( !hadexception );
+ }
+
+ SECTION("Missing format but with incorrect existing host mat") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ f.create<cv::Mat>(Channel::Depth, Format<uchar4>(100,100));
+ f.createTexture<float>(Channel::Depth);
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( hadexception );
+ }
+
+ SECTION("With format and no existing mat") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ auto &t = f.createTexture<float>(Channel::Depth, Format<float>(1024,1024));
+ REQUIRE( t.cvType() == CV_32FC1 );
+ REQUIRE( t.cudaTexture() > 0 );
+ REQUIRE( t.devicePtr() != nullptr );
+
+ auto &m = f.get<cv::cuda::GpuMat>(Channel::Depth);
+ REQUIRE( m.data == reinterpret_cast<uchar*>(t.devicePtr()) );
+ REQUIRE( m.type() == CV_32FC1 );
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( !hadexception );
+ }
+
+ SECTION("Unchanged type is same texture object") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ auto &t = f.createTexture<float>(Channel::Depth, Format<float>(1024,1024));
+ REQUIRE( t.cvType() == CV_32FC1 );
+
+ auto tex = t.cudaTexture();
+ float *ptr = t.devicePtr();
+
+ REQUIRE( ptr != nullptr );
+
+ auto &t2 = f.createTexture<float>(Channel::Depth, Format<float>(1024,1024));
+
+ REQUIRE( tex == t2.cudaTexture() );
+ REQUIRE( ptr == t2.devicePtr() );
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( !hadexception );
+ }
+}
+
+TEST_CASE("Frame::getTexture()", "") {
+ SECTION("Missing texture") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ f.getTexture<float>(Channel::Depth);
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( hadexception );
+ }
+
+ SECTION("Texture of incorrect type") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ f.createTexture<uchar4>(Channel::Depth, Format<uchar4>(100,100));
+ f.getTexture<float>(Channel::Depth);
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( hadexception );
+ }
+
+ SECTION("Valid texture get") {
+ Frame f;
+ bool hadexception = false;
+
+ try {
+ f.createTexture<uchar4>(Channel::Colour, Format<uchar4>(100,100));
+ auto &t = f.getTexture<uchar4>(Channel::Colour);
+
+ REQUIRE( t.cvType() == CV_8UC4 );
+ REQUIRE( t.width() == 100 );
+ } catch (ftl::exception &e) {
+ hadexception = true;
+ }
+
+ REQUIRE( !hadexception );
+ }
+}
+
+TEST_CASE("Frame::swapTo()", "") {
+ SECTION("Single host channel to empty frame") {
+ Frame f1;
+ Frame f2;
+
+ f1.create<cv::Mat>(Channel::Colour, Format<uchar3>(100,100));
+ f1.swapTo(Channels::All(), f2);
+
+ REQUIRE( f2.hasChannel(Channel::Colour) );
+ REQUIRE( (f2.get<cv::Mat>(Channel::Colour).cols == 100) );
+ }
+}
diff --git a/components/rgbd-sources/test/source_unit.cpp b/components/rgbd-sources/test/source_unit.cpp
index 4b14bee1887c956a2b646009b82d779e663b97d7..dca38be2ffb6e06b8be416c8de71a7a799c77867 100644
--- a/components/rgbd-sources/test/source_unit.cpp
+++ b/components/rgbd-sources/test/source_unit.cpp
@@ -14,7 +14,7 @@ class Snapshot {};
class SnapshotReader {
public:
- SnapshotReader(const std::string &) {}
+ explicit SnapshotReader(const std::string &) {}
Snapshot readArchive() { return Snapshot(); };
};
@@ -22,7 +22,7 @@ namespace detail {
class ImageSource : public ftl::rgbd::detail::Source {
public:
- ImageSource(ftl::rgbd::Source *host) : ftl::rgbd::detail::Source(host) {
+ explicit ImageSource(ftl::rgbd::Source *host) : ftl::rgbd::detail::Source(host) {
last_type = "image";
}
ImageSource(ftl::rgbd::Source *host, const std::string &f) : ftl::rgbd::detail::Source(host) {
@@ -37,7 +37,7 @@ class ImageSource : public ftl::rgbd::detail::Source {
class StereoVideoSource : public ftl::rgbd::detail::Source {
public:
- StereoVideoSource(ftl::rgbd::Source *host) : ftl::rgbd::detail::Source(host) {
+ explicit StereoVideoSource(ftl::rgbd::Source *host) : ftl::rgbd::detail::Source(host) {
last_type = "video";
}
StereoVideoSource(ftl::rgbd::Source *host, const std::string &f) : ftl::rgbd::detail::Source(host) {
@@ -52,7 +52,7 @@ class StereoVideoSource : public ftl::rgbd::detail::Source {
class NetSource : public ftl::rgbd::detail::Source {
public:
- NetSource(ftl::rgbd::Source *host) : ftl::rgbd::detail::Source(host) {
+ explicit NetSource(ftl::rgbd::Source *host) : ftl::rgbd::detail::Source(host) {
last_type = "net";
}
@@ -76,7 +76,7 @@ class SnapshotSource : public ftl::rgbd::detail::Source {
class RealsenseSource : public ftl::rgbd::detail::Source {
public:
- RealsenseSource(ftl::rgbd::Source *host) : ftl::rgbd::detail::Source(host) {
+ explicit RealsenseSource(ftl::rgbd::Source *host) : ftl::rgbd::detail::Source(host) {
last_type = "realsense";
}
@@ -122,11 +122,11 @@ using ftl::rgbd::Source;
using ftl::config::json_t;
TEST_CASE("ftl::create<Source>(cfg)", "[rgbd]") {
- json_t global = {{"$id","ftl://test"}};
+ json_t global = json_t{{"$id","ftl://test"}};
ftl::config::configure(global);
SECTION("with valid image file uri") {
- json_t cfg = {
+ json_t cfg = json_t{
{"$id","ftl://test/1"},
{"uri","file://" FTL_SOURCE_DIRECTORY "/components/rgbd-sources/test/data/image.png"}
};
@@ -139,7 +139,7 @@ TEST_CASE("ftl::create<Source>(cfg)", "[rgbd]") {
}
SECTION("with valid video file uri") {
- json_t cfg = {
+ json_t cfg = json_t{
{"$id","ftl://test/2"},
{"uri","file://" FTL_SOURCE_DIRECTORY "/components/rgbd-sources/test/data/video.mp4"}
};
@@ -152,7 +152,7 @@ TEST_CASE("ftl::create<Source>(cfg)", "[rgbd]") {
}
SECTION("with valid net uri") {
- json_t cfg = {
+ json_t cfg = json_t{
{"$id","ftl://test/2"},
{"uri","ftl://utu.fi/dummy"}
};
@@ -165,7 +165,7 @@ TEST_CASE("ftl::create<Source>(cfg)", "[rgbd]") {
}
SECTION("with an invalid uri") {
- json_t cfg = {
+ json_t cfg = json_t{
{"$id","ftl://test/2"},
{"uri","not a uri"}
};
@@ -177,7 +177,7 @@ TEST_CASE("ftl::create<Source>(cfg)", "[rgbd]") {
}
SECTION("with an invalid file uri") {
- json_t cfg = {
+ json_t cfg = json_t{
{"$id","ftl://test/2"},
{"uri","file:///not/a/file"}
};
@@ -189,7 +189,7 @@ TEST_CASE("ftl::create<Source>(cfg)", "[rgbd]") {
}
SECTION("with a missing file") {
- json_t cfg = {
+ json_t cfg = json_t{
{"$id","ftl://test/2"},
{"uri","file:///data/image2.png"}
};
@@ -202,11 +202,11 @@ TEST_CASE("ftl::create<Source>(cfg)", "[rgbd]") {
}
TEST_CASE("Source::set(uri)", "[rgbd]") {
- json_t global = {{"$id","ftl://test"}};
+ json_t global = json_t{{"$id","ftl://test"}};
ftl::config::configure(global);
SECTION("change to different valid URI type") {
- json_t cfg = {
+ json_t cfg = json_t{
{"$id","ftl://test/1"},
{"uri","file://" FTL_SOURCE_DIRECTORY "/components/rgbd-sources/test/data/image.png"}
};
diff --git a/components/scene-sources/include/ftl/scene/framescene.hpp b/components/scene-sources/include/ftl/scene/framescene.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0235a60331899ca125514e0905c845bafd1e466c
--- /dev/null
+++ b/components/scene-sources/include/ftl/scene/framescene.hpp
@@ -0,0 +1,28 @@
+#ifndef _FTL_SCENE_FRAMESCENE_HPP_
+#define _FTL_SCENE_FRAMESCENE_HPP_
+
+#include <ftl/scene/scene.hpp>
+
+namespace ftl {
+namespace scene {
+
+/**
+ * A scene represented internally as a set of image frames that together
+ * define a point cloud.
+ */
+class FrameScene : public ftl::scene::Scene {
+ public:
+ FrameScene();
+ ~FrameScene();
+
+ bool update(ftl::rgbd::FrameSet &);
+
+ bool render(ftl::rgbd::Source *, ftl::rgbd::Frame &);
+ bool encode(std::vector<uint8_t> &);
+ bool decode(const std::vector<uint8_t> &);
+};
+
+}
+}
+
+#endif // _FTL_SCENE_FRAMESCENE_HPP_
diff --git a/components/scene-sources/include/ftl/scene/scene.hpp b/components/scene-sources/include/ftl/scene/scene.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..856819cf403982d54ea3fd12a3cd195b9d437919
--- /dev/null
+++ b/components/scene-sources/include/ftl/scene/scene.hpp
@@ -0,0 +1,21 @@
+#ifndef _FTL_RECONSTRUCT_SCENE_HPP_
+#define _FTL_RECONSTRUCT_SCENE_HPP_
+
+namespace ftl {
+namespace scene {
+
+class Scene {
+ public:
+ Scene();
+ virtual ~Scene();
+
+ virtual bool render(ftl::rgbd::Source *, ftl::rgbd::Frame &)=0;
+
+ virtual bool encode(std::vector<uint8_t> &)=0;
+ virtual bool decode(const std::vector<uint8_t> &)=0;
+};
+
+} // scene
+} // ftl
+
+#endif // _FTL_RECONSTRUCT_SCENE_HPP_
diff --git a/config/config_vision.jsonc b/config/config_vision.jsonc
new file mode 100644
index 0000000000000000000000000000000000000000..b73446cefe06aaaf3663b8fe2823822878b5a1a8
--- /dev/null
+++ b/config/config_vision.jsonc
@@ -0,0 +1,129 @@
+{
+ //"$id": "ftl://utu.fi",
+ "$schema": "",
+ "calibrations": {
+ "default": {
+ "use_intrinsics": true,
+ "use_extrinsics": true,
+ "alpha": 0.0
+ }
+ },
+
+ "disparity": {
+ "libsgm": {
+ "algorithm": "libsgm",
+ "width": 1280,
+ "height": 720,
+ "use_cuda": true,
+ "minimum": 0,
+ "maximum": 256,
+ "tau": 0.0,
+ "gamma": 0.0,
+ "window_size": 5,
+ "sigma": 1.5,
+ "lambda": 8000.0,
+ "uniqueness": 0.65,
+ "use_filter": true,
+ "P1": 8,
+ "P2": 130,
+ "filter_radius": 11,
+ "filter_iter": 2,
+ "use_off": true,
+ "off_size": 24,
+ "off_threshold": 0.75,
+ "T": 60,
+ "T_add": 0,
+ "T_del": 25,
+ "T_x" : 3.0,
+ "alpha" : 0.6,
+ "beta" : 1.7,
+ "epsilon" : 15.0
+ },
+
+ "rtcensus": {
+ "algorithm": "rtcensus",
+ "use_cuda": true,
+ "minimum": 0,
+ "maximum": 256,
+ "tau": 0.0,
+ "gamma": 0.0,
+ "window_size": 5,
+ "sigma": 1.5,
+ "lambda": 8000.0,
+ "use_filter": true,
+ "filter_radius": 3,
+ "filter_iter": 4
+ }
+ },
+
+ "sources": {
+ "stereocam": {
+ "uri": "device:video",
+ "feed": {
+ "flip": false,
+ "nostereo": false,
+ "scale": 1.0,
+ "flip_vert": false,
+ "max_fps": 500,
+ "width": 1280,
+ "height": 720,
+ "crosshair": false
+ },
+ "use_optflow" : true,
+ "calibration": { "$ref": "#calibrations/default" },
+ "disparity": { "$ref": "#disparity/libsgm" }
+ },
+ "stereovid": {},
+ "localhost": {}
+
+ },
+
+ "vision_default": {
+ "fps": 20,
+ "source": { "$ref": "#sources/stereocam" },
+ "middlebury": { "$ref": "#middlebury/none" },
+ "display": { "$ref": "#displays/none" },
+ "net": { "$ref": "#net/default_vision" },
+ "stream": { }
+ },
+
+ // Listen to localhost
+ "net": {
+ "default_vision": {
+ "listen": "tcp://*:9001",
+ "peers": [],
+ "tcp_send_buffer": 100000 //204800
+ },
+ "default_reconstruct": {
+ "listen": "tcp://*:9002",
+ "peers": []
+ }
+ },
+
+ "displays": {
+ "none": {
+ "flip_vert": false,
+ "disparity": false,
+ "points": false,
+ "depth": false,
+ "left": false,
+ "right": false
+ },
+ "left": {
+ "flip_vert": false,
+ "disparity": false,
+ "points": false,
+ "depth": false,
+ "left": true,
+ "right": false
+ }
+ },
+
+ "middlebury": {
+ "none": {
+ "dataset": "",
+ "threshold": 10.0,
+ "scale": 0.25
+ }
+ }
+}
diff --git a/web-service/server/src/index.js b/web-service/server/src/index.js
index 0e4062f95f6ee1e9c79498d8e5f55b3b165b21c4..6dd821d19282cbe97dba5eaf131d39bf4226ccaa 100644
--- a/web-service/server/src/index.js
+++ b/web-service/server/src/index.js
@@ -46,8 +46,8 @@ function RGBDClient(peer, N, rate, dest) {
/**
* Actually send a frame over network to the client.
*/
-RGBDClient.prototype.push = function(uri, frame, ttime, chunk, rgb, depth) {
- this.peer.send(uri, frame, ttime, chunk, rgb, depth);
+RGBDClient.prototype.push = function(uri, latency, spacket, packet) {
+ this.peer.send(uri, latency, spacket, packet);
this.txcount++;
}
@@ -72,14 +72,23 @@ function RGBDStream(uri, peer) {
this.rxmax = 10;
// Add RPC handler to receive frames from the source
- peer.bind(uri, (frame, ttime, chunk, rgb, depth) => {
+ peer.bind(uri, (latency, spacket, packet) => {
// Forward frames to all clients
- this.pushFrames(frame, ttime, chunk, rgb, depth);
+ this.pushFrames(latency, spacket, packet);
this.rxcount++;
if (this.rxcount >= this.rxmax && this.clients.length > 0) {
this.subscribe();
}
});
+
+ /*peer.bind(uri, (frame, ttime, chunk, rgb, depth) => {
+ // Forward frames to all clients
+ this.pushFrames(frame, ttime, chunk, rgb, depth);
+ this.rxcount++;
+ if (this.rxcount >= this.rxmax && this.clients.length > 0) {
+ this.subscribe();
+ }
+ });*/
}
RGBDStream.prototype.addClient = function(peer, N, rate, dest) {
@@ -99,15 +108,19 @@ RGBDStream.prototype.subscribe = function() {
this.rxcount = 0;
this.rxmax = 10;
//console.log("Subscribe to ", this.uri);
- this.peer.send("get_stream", this.uri, 10, 0, [Peer.uuid], this.uri);
+ // TODO: Don't hard code 9 here, instead use 9 for thumbnails and 0 for
+ // the video...
+ this.peer.send("get_stream", this.uri, 10, 9, [Peer.uuid], this.uri);
}
-RGBDStream.prototype.pushFrames = function(frame, ttime, chunk, rgb, depth) {
- this.rgb = rgb;
- this.depth = depth;
+RGBDStream.prototype.pushFrames = function(latency, spacket, packet) {
+ if (spacket[1] & 0x1) this.depth = packet[4];
+ else this.rgb = packet[4];
+
+ console.log("Frame = ", packet[0], packet[1]);
for (let i=0; i < this.clients.length; i++) {
- this.clients[i].push(this.uri, frame, ttime, chunk, rgb, depth);
+ this.clients[i].push(this.uri, latency, spacket, packet);
}
let i=0;