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..d8817ad9192de3e31a83108e531dfb009490a066 --- /dev/null +++ b/components/renderers/cpp/include/ftl/render/renderer.hpp @@ -0,0 +1,16 @@ +#ifndef _FTL_RENDER_RENDERER_HPP_ +#define _FTL_RENDER_RENDERER_HPP_ + +namespace ftl { +namespace render { + +class Renderer : public ftl::Configurable { + public: + Renderer(); + virtual ~Renderer(); +}; + +} +} + +#endif // _FTL_RENDER_RENDERER_HPP_ diff --git a/applications/reconstruct/src/splat_render.hpp b/components/renderers/cpp/include/ftl/render/splat_render.hpp similarity index 100% rename from applications/reconstruct/src/splat_render.hpp rename to components/renderers/cpp/include/ftl/render/splat_render.hpp 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/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/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/applications/reconstruct/src/splat_params.hpp b/components/renderers/cpp/src/splat_params.hpp similarity index 100% rename from applications/reconstruct/src/splat_params.hpp rename to components/renderers/cpp/src/splat_params.hpp diff --git a/applications/reconstruct/src/splat_render.cpp b/components/renderers/cpp/src/splat_render.cpp similarity index 100% rename from applications/reconstruct/src/splat_render.cpp rename to components/renderers/cpp/src/splat_render.cpp diff --git a/applications/reconstruct/src/splat_render_cuda.hpp b/components/renderers/cpp/src/splat_render_cuda.hpp similarity index 100% rename from applications/reconstruct/src/splat_render_cuda.hpp rename to components/renderers/cpp/src/splat_render_cuda.hpp