Skip to content
Snippets Groups Projects
Commit ce61931a authored by Nicolas Pope's avatar Nicolas Pope
Browse files

Move the splat render

parent 3cd02bc1
No related branches found
No related tags found
1 merge request!109Resolves #173 remove voxel code
Hide whitespace changes
Inline Side-by-side
Showing
with 16 additions and 531 deletions
components/renderers/cpp/include/ftl/display.hpp deleted 100644 → 0
+ 0
− 68
View file @ 3cd02bc1
/*
* 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_
components/renderers/cpp/include/ftl/render/renderer.hpp 0 → 100644
+ 16
− 0
View file @ ce61931a
#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_
components/renderers/cpp/include/ftl/rgbd_display.hpp deleted 100644 → 0
+ 0
− 47
View file @ 3cd02bc1
#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_
components/renderers/cpp/src/display.cpp deleted 100644 → 0
+ 0
− 287
View file @ 3cd02bc1
/*
* 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
}
components/renderers/cpp/src/rgbd_display.cpp deleted 100644 → 0
+ 0
− 129
View file @ 3cd02bc1
#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);
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment