diff --git a/modules/ar/CMakeLists.txt b/modules/ar/CMakeLists.txt index 4e026793f4..1844714daa 100644 --- a/modules/ar/CMakeLists.txt +++ b/modules/ar/CMakeLists.txt @@ -195,7 +195,7 @@ if(USE_PANDA3D) set(PANDA3D_MODULE_SOURCES vpPanda3DBaseRenderer.cpp vpPanda3DGeometryRenderer.cpp vpPanda3DRGBRenderer.cpp vpPanda3DRenderParameters.cpp - vpPanda3DRendererSet.cpp + vpPanda3DRendererSet.cpp vpPanda3DPostProcessFilter.cpp vpPanda3DCommonFilters.cpp ) foreach(panda_src_name ${PANDA3D_MODULE_SOURCES}) vp_set_source_file_compile_flag(src/panda3d-simulator/${panda_src_name} ${PANDA3D_CXX_FLAGS}) diff --git a/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h index e45d1e8390..cf9594fa39 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h @@ -56,7 +56,7 @@ class VISP_EXPORT vpPanda3DBaseRenderer { public: vpPanda3DBaseRenderer(const std::string &rendererName) - : m_name(rendererName), m_framework(nullptr), m_window(nullptr), m_camera(nullptr) + : m_name(rendererName), m_renderOrder(-100), m_framework(nullptr), m_window(nullptr), m_camera(nullptr) { setVerticalSyncEnabled(false); } @@ -121,12 +121,28 @@ class VISP_EXPORT vpPanda3DBaseRenderer } } - // If renderer is already initialize, modify camera properties + // If renderer is already initialized, modify camera properties if (m_camera != nullptr) { m_renderParameters.setupPandaCamera(m_camera); } } + /** + * @brief Returns true if this renderer process 3D data and its scene root can be interacted with. + * + * This value could be false, if for instance it is redefined in a subclass that performs postprocessing on a texture. + */ + virtual bool isRendering3DScene() const { return true; } + + /** + * @brief Get the rendering order of this renderer. + * If a renderer A has a lower order value than B, it will be rendered before B. + * This is useful, if for instance, B is a postprocessing filter that depends on the result of B. + * + * @return int + */ + int getRenderOrder() const { return m_renderOrder; } + /** * @brief Set the camera's pose. * The pose is specified using the ViSP convention (Y-down right handed). @@ -232,6 +248,8 @@ class VISP_EXPORT vpPanda3DBaseRenderer void printStructure(); + virtual GraphicsOutput *getMainOutputBuffer() { return nullptr; } + protected: /** @@ -254,13 +272,14 @@ class VISP_EXPORT vpPanda3DBaseRenderer */ virtual void setupRenderTarget() { } + const static vpHomogeneousMatrix VISP_T_PANDA; //! Homogeneous transformation matrix to convert from the Panda coordinate system (right-handed Z-up) to the ViSP coordinate system (right-handed Y-Down) const static vpHomogeneousMatrix PANDA_T_VISP; //! Inverse of VISP_T_PANDA - protected: const std::string m_name; //! name of the renderer + int m_renderOrder; //! Rendering priority for this renderer and its buffers. A lower value will be rendered first. Should be used when calling make_output in setupRenderTarget() std::shared_ptr m_framework; //! Pointer to the active panda framework PT(WindowFramework) m_window; //! Pointer to owning window, which can create buffers etc. It is not necessarily visible. vpPanda3DRenderParameters m_renderParameters; //! Rendering parameters diff --git a/modules/ar/include/visp3/ar/vpPanda3DCommonFilters.h b/modules/ar/include/visp3/ar/vpPanda3DCommonFilters.h new file mode 100644 index 0000000000..6fbf7abab1 --- /dev/null +++ b/modules/ar/include/visp3/ar/vpPanda3DCommonFilters.h @@ -0,0 +1,24 @@ +#ifndef vpPanda3DCommonFilters_h +#define vpPanda3DCommonFilters_h + +#include + +#if defined(VISP_HAVE_PANDA3D) + +#include + +class vpPanda3DRGBRenderer; + +class VISP_EXPORT vpPanda3DLuminanceFilter : public vpPanda3DPostProcessFilter +{ +public: + vpPanda3DLuminanceFilter(const std::string &name, std::shared_ptr &inputRenderer, bool isOutput); + FrameBufferProperties getBufferProperties() const vp_override; + void getRender(vpImage &I) const; + +private: + static const char *FRAGMENT_SHADER; + +}; +#endif +#endif diff --git a/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h index 7f304c979a..ecf96705ff 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h @@ -81,6 +81,7 @@ class VISP_EXPORT vpPanda3DGeometryRenderer : public vpPanda3DBaseRenderer void getRender(vpImage &depth) const; + GraphicsOutput *getMainOutputBuffer() vp_override { return m_normalDepthBuffer; } protected: void setupScene() vp_override; diff --git a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h new file mode 100644 index 0000000000..963ac815dc --- /dev/null +++ b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h @@ -0,0 +1,47 @@ +#ifndef vpPanda3DPostProcessFilter_h +#define vpPanda3DPostProcessFilter_h + +#include + +#if defined(VISP_HAVE_PANDA3D) +#include +#include "cardMaker.h" +#include "orthographicLens.h" + + +class VISP_EXPORT vpPanda3DPostProcessFilter : public vpPanda3DBaseRenderer +{ +public: + vpPanda3DPostProcessFilter(const std::string &name, const std::shared_ptr &inputRenderer, bool isOutput, std::string fragmentShader) + : vpPanda3DBaseRenderer(name), m_inputRenderer(inputRenderer), m_isOutput(isOutput), m_fragmentShader(fragmentShader) + { + m_renderOrder = m_inputRenderer->getRenderOrder() + 1; + } + bool isRendering3DScene() const vp_override + { + return false; + } + +protected: + void setupScene() vp_override; + + void setupCamera() vp_override; + + void setupRenderTarget() vp_override; + + void setRenderParameters(const vpPanda3DRenderParameters ¶ms) vp_override; + + virtual FrameBufferProperties getBufferProperties() const = 0; + + std::shared_ptr m_inputRenderer; + bool m_isOutput; //! Whether this filter is an output to be used and should be copied to ram + std::string m_fragmentShader; + PT(Shader) shader; + Texture *m_texture; + GraphicsOutput *m_buffer; + + static const char *FILTER_VERTEX_SHADER; +}; + +#endif +#endif diff --git a/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h index e4d9d7a983..39f6431ae2 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h @@ -58,6 +58,8 @@ class VISP_EXPORT vpPanda3DRGBRenderer : public vpPanda3DBaseRenderer, public vp void addNodeToScene(const NodePath &object) vp_override; + GraphicsOutput *getMainOutputBuffer() vp_override { return m_colorBuffer; } + protected: void setupScene() vp_override; void setupRenderTarget() vp_override; diff --git a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h index 48dc8fc4c6..6602d7b5b6 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h @@ -47,8 +47,10 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp virtual ~vpPanda3DRendererSet(); /** - * @brief Initialize the framework and propagate the created panda3D framework to the subrenderers + * @brief Initialize the framework and propagate the created panda3D framework to the subrenderers. * + * The subrenderers will be initialized in the order of their priority as defined by vpPanda3DBaseRenderer::getRenderOrder + * Thus, if a renderer B depends on A for its render, and if B.getRenderOrder() > A.getRenderOrder() it can rely on A being initialized when B.initFromParent is called (along with the setupCamera, setupRenderTarget). */ void initFramework() vp_override; @@ -110,12 +112,20 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp */ vpHomogeneousMatrix getNodePose(NodePath &object) vp_override; + /** + * \warn this method is not supported and will throw + */ void addNodeToScene(const NodePath &object) vp_override; void setRenderParameters(const vpPanda3DRenderParameters ¶ms) vp_override; void addLight(const vpPanda3DLight &light) vp_override; + /** + * @brief Add a new subrenderer + * + * @param renderer + */ void addSubRenderer(std::shared_ptr renderer); template diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DCommonFilters.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DCommonFilters.cpp new file mode 100644 index 0000000000..98062831e4 --- /dev/null +++ b/modules/ar/src/panda3d-simulator/vpPanda3DCommonFilters.cpp @@ -0,0 +1,56 @@ +#include +#include + +#if defined(VISP_HAVE_PANDA3D) + +const char *vpPanda3DLuminanceFilter::FRAGMENT_SHADER = R"shader( +#version 330 + +in vec2 texcoords; + + +out vec4 p3d_FragData; +uniform sampler2D p3d_Texture0; + +void main() { + vec4 v = texture(p3d_Texture0, texcoords); + p3d_FragData.b = 0.299 * v.r + 0.587 * v.g + 0.114 * v.b; +} + +)shader"; + + +vpPanda3DLuminanceFilter::vpPanda3DLuminanceFilter(const std::string &name, std::shared_ptr &inputRenderer, bool isOutput) + : vpPanda3DPostProcessFilter(name, inputRenderer, isOutput, std::string(vpPanda3DLuminanceFilter::FRAGMENT_SHADER)) +{ + +} +FrameBufferProperties vpPanda3DLuminanceFilter::getBufferProperties() const +{ + FrameBufferProperties fbp; + fbp.set_depth_bits(0); + fbp.set_rgba_bits(0, 0, 8, 0); + fbp.set_float_color(false); + return fbp; +} +void vpPanda3DLuminanceFilter::getRender(vpImage &I) const +{ + if (!m_isOutput) { + throw vpException(vpException::fatalError, "Tried to fetch output of a postprocessing filter that was configured as an intermediate output"); + } + unsigned indexMultiplier = m_texture->get_num_components(); // we ask for only 8 bits image, but we may get an rgb image + I.resize(m_texture->get_y_size(), m_texture->get_x_size()); + unsigned char *data = (unsigned char *)(&(m_texture->get_ram_image().front())); + if (indexMultiplier != 1) { + for (unsigned int i = 0; i < I.getSize(); ++i) { + I.bitmap[i] = data[i * indexMultiplier]; + } + } + else { + memcpy(I.bitmap, &data[0], I.getSize() * sizeof(unsigned char)); + } +} + + + +#endif diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp new file mode 100644 index 0000000000..662f10011d --- /dev/null +++ b/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp @@ -0,0 +1,114 @@ +#include + + +#if defined(VISP_HAVE_PANDA3D) + +const char *vpPanda3DPostProcessFilter::FILTER_VERTEX_SHADER = R"shader( +#version 330 +in vec4 p3d_Vertex; +uniform mat4 p3d_ModelViewProjectionMatrix; +in vec2 p3d_MultiTexCoord0; +out vec2 texcoords; + +void main() +{ + gl_Position = p3d_ModelViewProjectionMatrix * p3d_Vertex; + texcoords = p3d_MultiTexCoord0; +} +)shader"; + + +void vpPanda3DPostProcessFilter::setupScene() +{ + CardMaker cm("cm"); + cm.set_frame_fullscreen_quad(); + m_renderRoot = NodePath(cm.generate()); // Render root is a 2D rectangle + m_renderRoot.set_depth_test(false); + m_renderRoot.set_depth_write(false); + GraphicsOutput *buffer = m_inputRenderer->getMainOutputBuffer(); + if (buffer == nullptr) { + throw vpException(vpException::fatalError, + "Cannot add a postprocess filter to a renderer that does not define getMainOutputBuffer()"); + } + shader = Shader::make(Shader::ShaderLanguage::SL_GLSL, + FILTER_VERTEX_SHADER, + m_fragmentShader); + m_renderRoot.set_shader(shader); + std::cout << m_fragmentShader << std::endl; + m_renderRoot.set_texture(buffer->get_texture()); + m_renderRoot.set_attrib(LightRampAttrib::make_identity()); +} + +void vpPanda3DPostProcessFilter::setupCamera() +{ + m_cameraPath = m_window->make_camera(); + m_camera = (Camera *)m_cameraPath.node(); + PT(OrthographicLens) lens = new OrthographicLens(); + lens->set_film_size(2, 2); + lens->set_film_offset(0, 0); + lens->set_near_far(-1000, 1000); + m_camera->set_lens(lens); + m_cameraPath = m_renderRoot.attach_new_node(m_camera); + m_camera->set_scene(m_renderRoot); +} + +void vpPanda3DPostProcessFilter::setupRenderTarget() +{ + + if (m_window == nullptr) { + throw vpException(vpException::fatalError, "Cannot setup render target when window is null"); + } + FrameBufferProperties fbp = getBufferProperties(); + WindowProperties win_prop; + win_prop.set_size(m_renderParameters.getImageWidth(), m_renderParameters.getImageHeight()); + + // Don't open a window - force it to be an offscreen buffer. + int flags = GraphicsPipe::BF_refuse_window | GraphicsPipe::BF_resizeable; + GraphicsOutput *windowOutput = m_window->get_graphics_output(); + GraphicsEngine *engine = windowOutput->get_engine(); + GraphicsStateGuardian *gsg = windowOutput->get_gsg(); + GraphicsPipe *pipe = windowOutput->get_pipe(); + m_buffer = engine->make_output(pipe, m_name, m_renderOrder, + fbp, win_prop, flags, + gsg, windowOutput); + if (m_buffer == nullptr) { + throw vpException(vpException::fatalError, "Could not create buffer"); + } + m_buffers.push_back(m_buffer); + m_buffer->set_inverted(gsg->get_copy_texture_inverted()); + m_texture = new Texture(); + //fbp.setup_color_texture(m_texture); + m_buffer->add_render_texture(m_texture, m_isOutput ? GraphicsOutput::RenderTextureMode::RTM_copy_ram : GraphicsOutput::RenderTextureMode::RTM_copy_texture); + m_buffer->set_clear_color(LColor(0.f)); + m_buffer->set_clear_color_active(true); + DisplayRegion *region = m_buffer->make_display_region(); + if (region == nullptr) { + throw vpException(vpException::fatalError, "Could not create display region"); + } + region->set_camera(m_cameraPath); + region->set_clear_color(LColor(0.f)); +} + +void vpPanda3DPostProcessFilter::setRenderParameters(const vpPanda3DRenderParameters ¶ms) +{ + m_renderParameters = params; + unsigned int previousH = m_renderParameters.getImageHeight(), previousW = m_renderParameters.getImageWidth(); + bool resize = previousH != params.getImageHeight() || previousW != params.getImageWidth(); + + m_renderParameters = params; + + if (resize) { + for (GraphicsOutput *buffer: m_buffers) { + //buffer->get_type().is_derived_from() + GraphicsBuffer *buf = dynamic_cast(buffer); + if (buf == nullptr) { + throw vpException(vpException::fatalError, "Panda3D: could not cast to GraphicsBuffer when rendering."); + } + else { + buf->set_size(m_renderParameters.getImageWidth(), m_renderParameters.getImageHeight()); + } + } + } +} + +#endif diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp index 775e8612a4..257c0ba760 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp @@ -267,7 +267,7 @@ void vpPanda3DRGBRenderer::setupRenderTarget() GraphicsEngine *engine = windowOutput->get_engine(); GraphicsStateGuardian *gsg = windowOutput->get_gsg(); GraphicsPipe *pipe = windowOutput->get_pipe(); - m_colorBuffer = engine->make_output(pipe, "Color Buffer", -100, + m_colorBuffer = engine->make_output(pipe, "Color Buffer", m_renderOrder, fbp, win_prop, flags, gsg, windowOutput); if (m_colorBuffer == nullptr) { diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp index 991031dd4e..5002a113e3 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp @@ -78,7 +78,9 @@ void vpPanda3DRendererSet::initFramework() void vpPanda3DRendererSet::setCameraPose(const vpHomogeneousMatrix &wTc) { for (std::shared_ptr &renderer: m_subRenderers) { - renderer->setCameraPose(wTc); + if (renderer->isRendering3DScene()) { + renderer->setCameraPose(wTc); + } } } @@ -87,13 +89,18 @@ vpHomogeneousMatrix vpPanda3DRendererSet::getCameraPose() if (m_subRenderers.size() == 0) { throw vpException(vpException::fatalError, "cannot get the pose of an object if no subrenderer is present"); } + if (!m_subRenderers[0]->isRendering3DScene()) { + throw vpException(vpException::fatalError, "This renderer set only contains a non-3D renderer."); + } return m_subRenderers[0]->getCameraPose(); } void vpPanda3DRendererSet::setNodePose(const std::string &name, const vpHomogeneousMatrix &wTo) { for (std::shared_ptr &renderer: m_subRenderers) { - renderer->setNodePose(name, wTo); + if (renderer->isRendering3DScene()) { + renderer->setNodePose(name, wTo); + } } } @@ -107,6 +114,9 @@ vpHomogeneousMatrix vpPanda3DRendererSet::getNodePose(const std::string &name) if (m_subRenderers.size() == 0) { throw vpException(vpException::fatalError, "cannot get the pose of an object if no subrenderer is present"); } + if (!m_subRenderers[0]->isRendering3DScene()) { + throw vpException(vpException::fatalError, "This renderer set only contains a non-3D renderer."); + } return m_subRenderers[0]->getNodePose(name); } @@ -118,7 +128,9 @@ vpHomogeneousMatrix vpPanda3DRendererSet::getNodePose(NodePath &object) void vpPanda3DRendererSet::addNodeToScene(const NodePath &object) { for (std::shared_ptr &renderer: m_subRenderers) { - renderer->addNodeToScene(object); + if (renderer->isRendering3DScene()) { + renderer->addNodeToScene(object); + } } } @@ -147,7 +159,19 @@ void vpPanda3DRendererSet::addSubRenderer(std::shared_ptr throw vpException(vpException::badValue, "Cannot have two subrenderers with the same name"); } } - m_subRenderers.push_back(renderer); + std::vector>::const_iterator it = m_subRenderers.begin(); + while (it != m_subRenderers.end()) { + if ((*it)->getRenderOrder() > renderer->getRenderOrder()) { + break; + } + ++it; + } + m_subRenderers.insert(it, renderer); + for (const auto r: m_subRenderers) { + std::cout << r->getName() << " "; + } + std::cout << std::endl; + renderer->setRenderParameters(m_renderParameters); if (m_framework != nullptr) { renderer->initFromParent(m_framework, m_window); diff --git a/tutorial/ar/tutorial-panda3d-renderer.cpp b/tutorial/ar/tutorial-panda3d-renderer.cpp index c85a05be44..2165bb8044 100644 --- a/tutorial/ar/tutorial-panda3d-renderer.cpp +++ b/tutorial/ar/tutorial-panda3d-renderer.cpp @@ -17,6 +17,7 @@ #include #include #include +#include void displayNormals(const vpImage &normalsImage, vpImage &normalDisplayImage) @@ -63,6 +64,7 @@ int main(int argc, const char **argv) bool stepByStep = false; bool debug = false; bool showLightContrib = false; + bool showCanny = false; char *modelPathCstr = nullptr; vpParseArgv::vpArgvInfo argTable[] = { @@ -74,6 +76,9 @@ int main(int argc, const char **argv) "Show frames step by step."}, {"-specular", vpParseArgv::ARGV_CONSTANT_BOOL, (char *) nullptr, (char *)&showLightContrib, "Show frames step by step."}, + {"-canny", vpParseArgv::ARGV_CONSTANT_BOOL, (char *) nullptr, (char *)&showCanny, + "Show frames step by step."}, + {"-debug", vpParseArgv::ARGV_CONSTANT_BOOL, (char *) nullptr, (char *)&debug, "Show Opengl/Panda3D debug message."}, {"-h", vpParseArgv::ARGV_HELP, (char *) nullptr, (char *) nullptr, @@ -106,6 +111,8 @@ int main(int argc, const char **argv) std::shared_ptr cameraRenderer = std::shared_ptr(new vpPanda3DGeometryRenderer(vpPanda3DGeometryRenderer::vpRenderType::CAMERA_NORMALS)); std::shared_ptr rgbRenderer = std::shared_ptr(new vpPanda3DRGBRenderer()); std::shared_ptr rgbDiffuseRenderer = std::shared_ptr(new vpPanda3DRGBRenderer(false)); + std::shared_ptr grayscaleFilter = std::make_shared("toGrayscale", rgbRenderer, true); + renderer.addSubRenderer(geometryRenderer); renderer.addSubRenderer(cameraRenderer); @@ -113,7 +120,9 @@ int main(int argc, const char **argv) if (showLightContrib) { renderer.addSubRenderer(rgbDiffuseRenderer); } - + if (showCanny) { + renderer.addSubRenderer(grayscaleFilter); + } renderer.setVerticalSyncEnabled(false); renderer.setAbortOnPandaError(true); @@ -149,6 +158,7 @@ int main(int argc, const char **argv) vpImage colorImage(h, w); vpImage colorDiffuseOnly(h, w); vpImage lightDifference(h, w); + vpImage cannyImage(h, w); vpImage normalDisplayImage(h, w); vpImage cameraNormalDisplayImage(h, w); @@ -173,9 +183,12 @@ int main(int argc, const char **argv) DisplayCls dImageDiff; if (showLightContrib) { - dImageDiff.init(lightDifference, w * 2 + 90, 0, "Specular/reflectance contribution"); + dImageDiff.init(lightDifference, w * 2 + 80, 0, "Specular/reflectance contribution"); + } + DisplayCls dCanny; + if (showCanny) { + dCanny.init(cannyImage, w * 2 + 80, h + 80, "Canny"); } - renderer.renderFrame(); bool end = false; bool firstFrame = true; @@ -197,6 +210,9 @@ int main(int argc, const char **argv) if (showLightContrib) { renderer.getRenderer(rgbDiffuseRenderer->getName())->getRender(colorDiffuseOnly); } + if (showCanny) { + renderer.getRenderer()->getRender(cannyImage); + } const double beforeConvert = vpTime::measureTimeMs(); @@ -206,6 +222,11 @@ int main(int argc, const char **argv) if (showLightContrib) { displayLightDifference(colorImage, colorDiffuseOnly, lightDifference); } + vpDisplay::display(cannyImage); + vpDisplay::flush(cannyImage); + + + vpDisplay::display(colorImage); vpDisplay::displayText(colorImage, 15, 15, "Click to quit", vpColor::red);