From f562950280109bb67326dcce51c0eac94e3df801 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Tue, 6 Aug 2024 17:15:22 +0200 Subject: [PATCH 01/25] merge sinatrack changes --- .../include/visp3/ar/vpPanda3DBaseRenderer.h | 61 +++---- .../visp3/ar/vpPanda3DGeometryRenderer.h | 1 + modules/ar/include/visp3/ar/vpPanda3DLight.h | 1 - .../visp3/ar/vpPanda3DPostProcessFilter.h | 1 + .../include/visp3/ar/vpPanda3DRGBRenderer.h | 2 + .../include/visp3/ar/vpPanda3DRendererSet.h | 22 ++- .../vpPanda3DBaseRenderer.cpp | 106 ++++++++++- .../vpPanda3DCommonFilters.cpp | 2 +- .../vpPanda3DGeometryRenderer.cpp | 10 +- .../vpPanda3DPostProcessFilter.cpp | 6 +- .../vpPanda3DRGBRenderer.cpp | 1 - .../vpPanda3DRendererSet.cpp | 42 ++++- .../core/include/visp3/core/vpImageFilter.h | 25 +++ .../visp3/core/vpUKSigmaDrawerSimplex.h | 94 ++++++++++ .../math/kalman/vpUKSigmaDrawerSimplex.cpp | 103 +++++++++++ .../klt/include/visp3/klt/vpKltOpencv.h | 2 +- modules/tracker/klt/src/vpKltOpencv.cpp | 2 +- .../tracker/me/include/visp3/me/vpMeSite.h | 37 +++- .../tracker/me/src/moving-edges/vpMeSite.cpp | 166 ++++++++++++++---- tutorial/ar/tutorial-panda3d-renderer.cpp | 32 +++- 20 files changed, 606 insertions(+), 110 deletions(-) create mode 100644 modules/core/include/visp3/core/vpUKSigmaDrawerSimplex.h create mode 100644 modules/core/src/math/kalman/vpUKSigmaDrawerSimplex.cpp diff --git a/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h index 2c69edb298..f2ca28f2f4 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h @@ -71,17 +71,14 @@ class VISP_EXPORT vpPanda3DBaseRenderer * Will also perform the renderer setup (scene, camera and render targets) */ virtual void initFramework(); - - /** - * @brief - * - * @param framework - * @param window - */ - void initFromParent(std::shared_ptr framework, std::shared_ptr window); + virtual void initFromParent(std::shared_ptr framework, std::shared_ptr window); + virtual void initFromParent(const vpPanda3DBaseRenderer &renderer); + virtual void beforeFrameRendered() { } virtual void renderFrame(); + virtual void afterFrameRendered() { } + /** * @brief Get the name of the renderer @@ -90,6 +87,8 @@ class VISP_EXPORT vpPanda3DBaseRenderer */ const std::string &getName() const { return m_name; } + void setName(const std::string &name) { m_name = name; } + /** * @brief Get the scene root * @@ -101,31 +100,7 @@ class VISP_EXPORT vpPanda3DBaseRenderer * * @param params the new rendering parameters */ - virtual void setRenderParameters(const vpPanda3DRenderParameters ¶ms) - { - 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()); - } - } - } - - // If renderer is already initialized, modify camera properties - if (m_camera != nullptr) { - m_renderParameters.setupPandaCamera(m_camera); - } - } + virtual void setRenderParameters(const vpPanda3DRenderParameters ¶ms); /** * @brief Returns true if this renderer process 3D data and its scene root can be interacted with. @@ -143,6 +118,15 @@ class VISP_EXPORT vpPanda3DBaseRenderer */ int getRenderOrder() const { return m_renderOrder; } + void setRenderOrder(int order) + { + int previousOrder = m_renderOrder; + m_renderOrder = order; + for (GraphicsOutput *buffer: m_buffers) { + buffer->set_sort(buffer->get_sort() + (order - previousOrder)); + } + } + /** * @brief Set the camera's pose. * The pose is specified using the ViSP convention (Y-down right handed). @@ -206,8 +190,10 @@ class VISP_EXPORT vpPanda3DBaseRenderer * @param name name of the node that should be used to compute near and far values. * @param near resulting near clipping plane distance * @param far resulting far clipping plane distance + * @param fast Whether to use the axis align bounding box to compute the clipping planes. + * This is faster than reprojecting the full geometry in the camera frame */ - void computeNearAndFarPlanesFromNode(const std::string &name, float &near, float &far); + void computeNearAndFarPlanesFromNode(const std::string &name, float &near, float &far, bool fast); /** * @brief Load a 3D object. To load an .obj file, Panda3D must be compiled with assimp support. @@ -250,8 +236,13 @@ class VISP_EXPORT vpPanda3DBaseRenderer virtual GraphicsOutput *getMainOutputBuffer() { return nullptr; } + virtual void enableSharedDepthBuffer(vpPanda3DBaseRenderer &sourceBuffer); + + + protected: + /** * @brief Initialize the scene for this specific renderer. * @@ -278,7 +269,7 @@ class VISP_EXPORT vpPanda3DBaseRenderer protected: - const std::string m_name; //! name of the renderer + 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 std::shared_ptr m_window; //! Pointer to owning window, which can create buffers etc. It is not necessarily visible. diff --git a/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h index b043145960..a452a04cab 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h @@ -84,6 +84,7 @@ class VISP_EXPORT vpPanda3DGeometryRenderer : public vpPanda3DBaseRenderer GraphicsOutput *getMainOutputBuffer() VP_OVERRIDE { return m_normalDepthBuffer; } + protected: void setupScene() VP_OVERRIDE; void setupRenderTarget() VP_OVERRIDE; diff --git a/modules/ar/include/visp3/ar/vpPanda3DLight.h b/modules/ar/include/visp3/ar/vpPanda3DLight.h index 0e5fa38e8f..c41e535a6b 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DLight.h +++ b/modules/ar/include/visp3/ar/vpPanda3DLight.h @@ -209,7 +209,6 @@ class VISP_EXPORT vpPanda3DDirectionalLight : public vpPanda3DLight PT(DirectionalLight) light = new DirectionalLight(m_name); light->set_color(LColor(m_color.R, m_color.G, m_color.B, 1)); vpColVector dir = vpPanda3DBaseRenderer::vispVectorToPanda(m_direction); - std::cout << m_direction << ", " << dir << std::endl; light->set_direction(LVector3f(m_direction[0], m_direction[1], m_direction[2])); NodePath np = scene.attach_new_node(light); scene.set_light(np); diff --git a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h index 2304919ef6..06cf135a71 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h +++ b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h @@ -69,6 +69,7 @@ class VISP_EXPORT vpPanda3DPostProcessFilter : public vpPanda3DBaseRenderer GraphicsOutput *getMainOutputBuffer() VP_OVERRIDE { return m_buffer; } + protected: virtual void setupScene() VP_OVERRIDE; diff --git a/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h index 25a3d6ce20..43c4ae546b 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h @@ -98,8 +98,10 @@ class VISP_EXPORT vpPanda3DRGBRenderer : public vpPanda3DBaseRenderer, public vp protected: + void setupScene() VP_OVERRIDE; void setupRenderTarget() VP_OVERRIDE; + virtual std::string makeFragmentShader(bool hasTexture, bool specular); private: diff --git a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h index 9c0bded81e..8caba0e837 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h @@ -72,6 +72,9 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp */ void initFramework() VP_OVERRIDE; + void initFromParent(std::shared_ptr framework, std::shared_ptr window) VP_OVERRIDE; + void initFromParent(const vpPanda3DBaseRenderer &renderer) VP_OVERRIDE; + /** * @brief Set the pose of the camera, using the ViSP convention. This change is propagated to all subrenderers * @@ -134,7 +137,7 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp */ void addNodeToScene(const NodePath &object) VP_OVERRIDE; - void setRenderParameters(const vpPanda3DRenderParameters ¶ms) VP_OVERRIDE; + virtual void setRenderParameters(const vpPanda3DRenderParameters ¶ms) VP_OVERRIDE; void addLight(const vpPanda3DLight &light) VP_OVERRIDE; @@ -147,6 +150,9 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp */ void addSubRenderer(std::shared_ptr renderer); + void enableSharedDepthBuffer(vpPanda3DBaseRenderer &sourceBuffer) vp_override; + + /** * @brief Retrieve the first subrenderer with the specified template type. * @@ -185,12 +191,24 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp return nullptr; } + void beforeFrameRendered() vp_override + { + for (std::shared_ptr &renderer: m_subRenderers) { + renderer->beforeFrameRendered(); + } + } + void afterFrameRendered() vp_override + { + for (std::shared_ptr &renderer: m_subRenderers) { + renderer->afterFrameRendered(); + } + } + protected: void setupScene() VP_OVERRIDE { } void setupCamera() VP_OVERRIDE { } -private: std::vector> m_subRenderers; }; END_VISP_NAMESPACE diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp index 13e9a6f9f0..b8b7174c1d 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp @@ -36,6 +36,8 @@ #include "load_prc_file.h" #include +#include "boundingSphere.h" +#include "boundingBox.h" BEGIN_VISP_NAMESPACE const vpHomogeneousMatrix vpPanda3DBaseRenderer::VISP_T_PANDA({ @@ -46,6 +48,9 @@ const vpHomogeneousMatrix vpPanda3DBaseRenderer::VISP_T_PANDA({ }); const vpHomogeneousMatrix vpPanda3DBaseRenderer::PANDA_T_VISP(vpPanda3DBaseRenderer::VISP_T_PANDA.inverse()); + + + void vpPanda3DBaseRenderer::initFramework() { if (m_framework.use_count() > 0) { @@ -83,6 +88,12 @@ void vpPanda3DBaseRenderer::initFromParent(std::shared_ptr frame setupRenderTarget(); } +void vpPanda3DBaseRenderer::initFromParent(const vpPanda3DBaseRenderer &renderer) +{ + initFromParent(renderer.m_framework, renderer.m_window); +} + + void vpPanda3DBaseRenderer::setupScene() { m_renderRoot = m_window->get_render().attach_new_node(m_name); @@ -101,7 +112,35 @@ void vpPanda3DBaseRenderer::setupCamera() void vpPanda3DBaseRenderer::renderFrame() { + beforeFrameRendered(); m_framework->get_graphics_engine()->render_frame(); + afterFrameRendered(); +} + +void vpPanda3DBaseRenderer::setRenderParameters(const vpPanda3DRenderParameters ¶ms) +{ + 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()); + } + } + } + + // If renderer is already initialized, modify camera properties + if (m_camera != nullptr) { + m_renderParameters.setupPandaCamera(m_camera); + } } void vpPanda3DBaseRenderer::setCameraPose(const vpHomogeneousMatrix &wTc) @@ -146,6 +185,7 @@ vpHomogeneousMatrix vpPanda3DBaseRenderer::getNodePose(const std::string &name) vpHomogeneousMatrix vpPanda3DBaseRenderer::getNodePose(NodePath &object) { + const LPoint3 pos = object.get_pos(); const LQuaternion quat = object.get_quat(); const vpTranslationVector t(pos[0], pos[1], pos[2]); @@ -153,7 +193,7 @@ vpHomogeneousMatrix vpPanda3DBaseRenderer::getNodePose(NodePath &object) return vpHomogeneousMatrix(t, q) * PANDA_T_VISP; } -void vpPanda3DBaseRenderer::computeNearAndFarPlanesFromNode(const std::string &name, float &nearV, float &farV) +void vpPanda3DBaseRenderer::computeNearAndFarPlanesFromNode(const std::string &name, float &nearV, float &farV, bool fast) { if (m_camera == nullptr) { throw vpException(vpException::notInitialized, "Cannot compute planes when the camera is not initialized"); @@ -162,16 +202,70 @@ void vpPanda3DBaseRenderer::computeNearAndFarPlanesFromNode(const std::string &n if (object.is_empty()) { throw vpException(vpException::badValue, "Node %s was not found", name.c_str()); } - LPoint3 minP, maxP; - object.calc_tight_bounds(minP, maxP, m_cameraPath); - nearV = vpMath::maximum(0.f, minP.get_y()); - farV = vpMath::maximum(nearV, maxP.get_y()); + if (!fast) { + LPoint3 minP, maxP; + + object.calc_tight_bounds(minP, maxP, m_cameraPath); + nearV = vpMath::maximum(0.f, minP.get_y()); + farV = vpMath::maximum(nearV, maxP.get_y()); + } + else { + const BoundingVolume *volume = object.node()->get_bounds(); + if (volume->get_type() == BoundingSphere::get_class_type()) { + const BoundingSphere *sphere = (const BoundingSphere *)volume; + const LPoint3 center = sphere->get_center(); + const float distCenter = (center -m_cameraPath.get_pos()).length(); + nearV = vpMath::maximum(0.f, distCenter - sphere->get_radius()); + farV = vpMath::maximum(nearV, distCenter + sphere->get_radius()); + } + else if (volume->get_type() == BoundingBox::get_class_type()) { + const vpHomogeneousMatrix wTcam = getCameraPose(); + const vpHomogeneousMatrix wTobj = getNodePose(object) * vpPanda3DBaseRenderer::PANDA_T_VISP; + const vpHomogeneousMatrix camTobj = wTcam.inverse() * wTobj; + const BoundingBox *box = (const BoundingBox *)volume; + double minZ = std::numeric_limits::max(), maxZ = 0.0; + + for (unsigned int i = 0; i < 8; ++i) { + const LPoint3 p = box->get_point(i); + vpColVector cp = camTobj * vpColVector({ p.get_x(), -p.get_z(), p.get_y(), 1.0 }); + double Z = cp[2] / cp[3]; + if (Z < minZ) { + minZ = Z; + } + if (Z > maxZ) { + maxZ = Z; + } + } + nearV = minZ; + farV = maxZ; + } + else { + throw vpException(vpException::fatalError, "Unhandled bounding volume %s type returned by Panda3d", volume->get_type().get_name().c_str()); + } + } +} + +void vpPanda3DBaseRenderer::enableSharedDepthBuffer(vpPanda3DBaseRenderer &sourceBuffer) +{ + if (isRendering3DScene()) { + GraphicsOutput *buffer = getMainOutputBuffer(); + if (buffer != nullptr) { + buffer->set_clear_depth_active(false); + if (!buffer->share_depth_buffer(sourceBuffer.getMainOutputBuffer())) { + throw vpException(vpException::fatalError, "Could not share depth buffer!"); + } + } + } } NodePath vpPanda3DBaseRenderer::loadObject(const std::string &nodeName, const std::string &modelPath) { NodePath model = m_window->load_model(m_framework->get_models(), modelPath); - std::cout << "After loading model" << std::endl; + for (int i = 0; i < model.get_num_children(); ++i) { + model.get_child(i).clear_transform(); + + } + model.detach_node(); model.set_name(nodeName); return model; diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DCommonFilters.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DCommonFilters.cpp index 5ef57c4b48..232a2302eb 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DCommonFilters.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DCommonFilters.cpp @@ -171,7 +171,7 @@ void main() { sum_v += pix * kernel_v[i]; } - vec2 orientationAndValid = sum_h * sum_h + sum_v * sum_v > 0 ? vec2(atan(sum_v/sum_h), 1.f) : vec2(0.f, 0.f); + vec2 orientationAndValid = sum_h * sum_h + sum_v * sum_v > 0 ? vec2(-atan(sum_v/sum_h), 1.f) : vec2(0.f, 0.f); p3d_FragData = vec4(sum_h, sum_v, orientationAndValid.x, orientationAndValid.y); } else { p3d_FragData = vec4(0.f, 0.f, 0.f, 0.f); diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp index 7700089823..4efbc003b5 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp @@ -81,6 +81,8 @@ void main() oNormal = normalize(p3d_Normal); oNormal.yz = oNormal.zy; oNormal.y = -oNormal.y; + // vec3 on = vec3(oNormal.z, -oNormal.x, -oNormal.y); + // oNormal = on; vec4 cs_position = p3d_ModelViewMatrix * p3d_Vertex; distToCamera = -cs_position.z; } @@ -153,12 +155,12 @@ void vpPanda3DGeometryRenderer::setupRenderTarget() 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; + int flags = GraphicsPipe::BF_refuse_window | GraphicsPipe::BF_resizeable | GraphicsPipe::BF_refuse_parasite; GraphicsOutput *windowOutput = m_window->get_graphics_output(); GraphicsEngine *engine = windowOutput->get_engine(); GraphicsPipe *pipe = windowOutput->get_pipe(); - m_normalDepthBuffer = engine->make_output(pipe, renderTypeToName(m_renderType), -100, fbp, win_prop, flags, + m_normalDepthBuffer = engine->make_output(pipe, renderTypeToName(m_renderType), m_renderOrder, fbp, win_prop, flags, windowOutput->get_gsg(), windowOutput); if (m_normalDepthBuffer == nullptr) { @@ -172,7 +174,7 @@ void vpPanda3DGeometryRenderer::setupRenderTarget() m_normalDepthBuffer->set_inverted(windowOutput->get_gsg()->get_copy_texture_inverted()); fbp.setup_color_texture(m_normalDepthTexture); m_normalDepthTexture->set_format(Texture::F_rgba32); - m_normalDepthBuffer->add_render_texture(m_normalDepthTexture, GraphicsOutput::RenderTextureMode::RTM_copy_ram); + m_normalDepthBuffer->add_render_texture(m_normalDepthTexture, GraphicsOutput::RenderTextureMode::RTM_copy_ram, GraphicsOutput::RenderTexturePlane::RTP_color); m_normalDepthBuffer->set_clear_color(LColor(0.f)); m_normalDepthBuffer->set_clear_color_active(true); DisplayRegion *region = m_normalDepthBuffer->make_display_region(); @@ -185,6 +187,7 @@ void vpPanda3DGeometryRenderer::setupRenderTarget() void vpPanda3DGeometryRenderer::getRender(vpImage &normals, vpImage &depth) const { + normals.resize(m_normalDepthTexture->get_y_size(), m_normalDepthTexture->get_x_size()); depth.resize(m_normalDepthTexture->get_y_size(), m_normalDepthTexture->get_x_size()); if (m_normalDepthTexture->get_component_type() != Texture::T_float) { @@ -220,7 +223,6 @@ void vpPanda3DGeometryRenderer::getRender(vpImage &normals) const float *data = (float *)(&(m_normalDepthTexture->get_ram_image().front())); data = data + rowIncrement * (normals.getHeight() - 1); rowIncrement = -rowIncrement; - for (unsigned int i = 0; i < normals.getHeight(); ++i) { vpRGBf *normalRow = normals[i]; for (unsigned int j = 0; j < normals.getWidth(); ++j) { diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp index 2daa1c9af8..23da614824 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp @@ -66,7 +66,6 @@ void vpPanda3DPostProcessFilter::setupScene() m_fragmentShader); m_renderRoot.set_shader(m_shader); m_renderRoot.set_shader_input("dp", LVector2f(1.0 / buffer->get_texture()->get_x_size(), 1.0 / buffer->get_texture()->get_y_size())); - std::cout << m_fragmentShader << std::endl; m_renderRoot.set_texture(buffer->get_texture()); m_renderRoot.set_attrib(LightRampAttrib::make_identity()); } @@ -123,7 +122,6 @@ void vpPanda3DPostProcessFilter::setupRenderTarget() 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(); @@ -161,11 +159,11 @@ void vpPanda3DPostProcessFilter::getRenderBasic(vpImage &I) const rowIncrement = -rowIncrement; for (unsigned int i = 0; i < I.getHeight(); ++i) { - data += rowIncrement; unsigned char *colorRow = I[i]; for (unsigned int j = 0; j < I.getWidth(); ++j) { colorRow[j] = data[j * numComponents]; } + data += rowIncrement; } } @@ -184,13 +182,13 @@ void vpPanda3DPostProcessFilter::getRenderBasic(vpImage &I) const rowIncrement = -rowIncrement; for (unsigned int i = 0; i < I.getHeight(); ++i) { - data += rowIncrement; vpRGBf *colorRow = I[i]; for (unsigned int j = 0; j < I.getWidth(); ++j) { colorRow[j].B = data[j * numComponents]; colorRow[j].G = data[j * numComponents + 1]; colorRow[j].R = data[j * numComponents + 2]; } + data += rowIncrement; } } diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp index ab6800fdaf..f4abc3648b 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp @@ -274,7 +274,6 @@ void vpPanda3DRGBRenderer::setBackgroundImage(const vpImage &background) //m_backgroundTexture = TexturePool::load_texture("/home/sfelton/IMG_20230221_165330430.jpg"); unsigned char *data = (unsigned char *)m_backgroundTexture->modify_ram_image(); - std::cout << m_backgroundTexture->get_x_size() << ", " << m_backgroundTexture->get_y_size() << std::endl; for (unsigned int i = 0; i < background.getHeight(); ++i) { const vpRGBa *srcRow = background[background.getHeight() - (i + 1)]; unsigned char *destRow = data + i * background.getWidth() * 4; diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp index 10ed67cdff..50d9a1604c 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp @@ -48,7 +48,16 @@ void vpPanda3DRendererSet::initFramework() // load_prc_file_data("", "load-display p3tinydisplay"); // load_prc_file_data("", "color-bits 32 32 32"); load_prc_file_data("", "gl-version 3 2"); - + // load_prc_file_data("", "auto-flip 1"); + // load_prc_file_data("", "sync-video 0"); + // load_prc_file_data("", "multisamples 1\n" + // // "background-color 0.0 0.0 0.0 0.0\n" + // // "load-file-type p3assimp\n" + // // "transform-cache 0\n" + // // "state-cache 0\n" + // // "audio-library-name null\n" + // "model-cache-dir\n" + // "notify-level-glgsg spam"); if (m_framework.use_count() > 0) { @@ -74,6 +83,22 @@ void vpPanda3DRendererSet::initFramework() } } +void vpPanda3DRendererSet::initFromParent(std::shared_ptr framework, std::shared_ptr window) +{ + vpPanda3DBaseRenderer::initFromParent(framework, window); + for (std::shared_ptr &renderer: m_subRenderers) { + renderer->initFromParent(m_framework, m_window); + } +} + +void vpPanda3DRendererSet::initFromParent(const vpPanda3DBaseRenderer &renderer) +{ + vpPanda3DBaseRenderer::initFromParent(renderer); + for (std::shared_ptr &r: m_subRenderers) { + r->initFromParent(*this); + } +} + void vpPanda3DRendererSet::setCameraPose(const vpHomogeneousMatrix &wTc) { @@ -167,10 +192,6 @@ void vpPanda3DRendererSet::addSubRenderer(std::shared_ptr ++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) { @@ -179,10 +200,19 @@ void vpPanda3DRendererSet::addSubRenderer(std::shared_ptr } } -END_VISP_NAMESPACE +void vpPanda3DRendererSet::enableSharedDepthBuffer(vpPanda3DBaseRenderer &sourceBuffer) +{ + for (std::shared_ptr &subRenderer: m_subRenderers) { + if (subRenderer.get() != &sourceBuffer) { + subRenderer->enableSharedDepthBuffer(sourceBuffer); + } + } +} + #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_ar.a(vpPanda3DRendererSet.cpp.o) has no symbols void dummy_vpPanda3DRendererSet() { }; +>>>>>>> upstream/master #endif diff --git a/modules/core/include/visp3/core/vpImageFilter.h b/modules/core/include/visp3/core/vpImageFilter.h index 4039856615..758d6c34a5 100644 --- a/modules/core/include/visp3/core/vpImageFilter.h +++ b/modules/core/include/visp3/core/vpImageFilter.h @@ -561,6 +561,31 @@ class VISP_EXPORT vpImageFilter } } + /** + * @brief Apply a filter at a given image location + * + * @tparam FilterType Image and filter types: double or float + * @param I The input image + * @param row the row coordinate where the filter should be applied + * @param col the column coordinate where the filter shoud be applied + * @param M the filter + */ + template + static FilterType filter(const vpImage &I, const vpArray2D &M, unsigned int row, unsigned int col) + { + const unsigned int size_y = M.getRows(), size_x = M.getCols(); + const unsigned int half_size_y = size_y / 2, half_size_x = size_x / 2; + FilterType corr = 0; + + for (unsigned int a = 0; a < size_y; ++a) { + for (unsigned int b = 0; b < size_x; ++b) { + FilterType val = static_cast(I[row - half_size_y + a][col - half_size_x + b]); // Correlation + corr += M[a][b] * val; + } + } + return corr; + } + #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher template static void filter(const vpImage &I, vpImage &If, const vpArray2D &M, bool convolve = false) = delete; diff --git a/modules/core/include/visp3/core/vpUKSigmaDrawerSimplex.h b/modules/core/include/visp3/core/vpUKSigmaDrawerSimplex.h new file mode 100644 index 0000000000..1dce5b22cc --- /dev/null +++ b/modules/core/include/visp3/core/vpUKSigmaDrawerSimplex.h @@ -0,0 +1,94 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2024 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + * + * Description: + * Display a point cloud using PCL library. + */ + +#ifndef VP_UK_SIGMA_DRAWER_SIMPLEX_H +#define VP_UK_SIGMA_DRAWER_SIMPLEX_H + +#include + +#include + +#include +#include + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +BEGIN_VISP_NAMESPACE +/*! + \class vpUKSigmaDrawerSimplex + \ingroup group_core_kalman +*/ +class VISP_EXPORT vpUKSigmaDrawerSimplex : public vpUKSigmaDrawerAbstract +{ +public: + typedef vpUnscentedKalman::vpAddSubFunction vpAddSubFunction; + + /** + * \brief Construct a new vpUKSigmaDrawerMerwe object. + * + * \param[in] n The size of the state vector. + * \param[in] w0 the weighting term, between 0 and 1 + * \param[in] addFunc Addition function expressed in the state space. + */ + vpUKSigmaDrawerSimplex(int n, double w0, const vpAddSubFunction &addFunc = vpUnscentedKalman::simpleAdd); + + /** + * Destructor that does nothing. + */ + virtual ~vpUKSigmaDrawerSimplex() { } + + /** + * \brief Draw the sigma points according to the current mean and covariance of the state + * of the Unscented Kalman filter. + * + * \param[in] mean The current mean of the state of the UKF. + * \param[in] covariance The current process covariance of the UKF. + * @return std::vector The sigma points. + */ + virtual std::vector drawSigmaPoints(const vpColVector &mean, const vpMatrix &covariance) override; + + /** + * \brief Computed the weigths that correspond to the sigma poitns that have been drawn. + * + * \return vpSigmaPointsWeights The weights that correspond to the sigma points. + */ + virtual vpSigmaPointsWeights computeWeights() override; +protected: + + double m_w0; /*!< Weighting term, between zero and one.*/ + std::vector m_ws; + std::vector m_chis; + vpAddSubFunction m_addFunc; /*!< Addition function expressed in the state space.*/ +}; +END_VISP_NAMESPACE +#endif +#endif diff --git a/modules/core/src/math/kalman/vpUKSigmaDrawerSimplex.cpp b/modules/core/src/math/kalman/vpUKSigmaDrawerSimplex.cpp new file mode 100644 index 0000000000..d0c7a2d9c2 --- /dev/null +++ b/modules/core/src/math/kalman/vpUKSigmaDrawerSimplex.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + * + * Description: + * Kalman filtering. + * +*****************************************************************************/ + +/*! + \file vpUKSigmaDrawerSimplex.cpp + \brief Sigma points drawer following the simplex method from Julier. +*/ + +#include + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +BEGIN_VISP_NAMESPACE +vpUKSigmaDrawerSimplex::vpUKSigmaDrawerSimplex(int n, double w0, const vpAddSubFunction &addFunc) + : vpUKSigmaDrawerAbstract(n), + m_w0(w0), m_addFunc(addFunc), + m_chis(n + 1, vpColVector(n)), + m_ws(n + 1) +{ + // m_ws[0] = m_w0; + m_ws[0] = (1.0 - m_w0) / std::pow(2.0, m_n); + m_ws[1] = m_ws[0]; + for (unsigned int i = 2; i < m_n + 1; ++i) { + m_ws[i] = std::pow(2.0, i - 1) * m_ws[0]; + } + +} + +std::vector vpUKSigmaDrawerSimplex::drawSigmaPoints(const vpColVector &mean, const vpMatrix &covariance) +{ + const unsigned int nbSigmaPoints = m_n + 1; + std::vector sigmaPoints(nbSigmaPoints); + vpMatrix chi(m_n, nbSigmaPoints, 0.0); + chi[0][0] = 0.0; + chi[0][1] = (-1.0) / sqrt(2.0 * m_ws[0]); + chi[0][2] = (1.0) / sqrt(2.0 * m_ws[0]); + for (unsigned int j = 1; j < m_n; ++j) { + chi[j][0] = 0.0; + for (unsigned int i = 1; i < j + 1; ++i) { + chi[j][i] = (-1.0) / sqrt(2.0 * m_ws[j - 1]); + } + chi[j][j + 1] = 1.0 / sqrt(2.0 * m_ws[j - 1]); + } + + const vpMatrix cholesky = covariance.cholesky(); + for (unsigned int i = 0; i < nbSigmaPoints; ++i) { + std::cout << "Cholesky = " << cholesky <(range)); + + + // Insert into a map, where the key is the sorting criterion (negative likelihood or contrast diff) + // and the key is the ME site + its computed likelihood and contrast. + // After computation: iterating on the map is guaranteed to be done with the keys being sorted according to the criterion. + // Multimap allows to have multiple values (sites) with the same key (likelihood/contrast diff) + // Only the candidates that are above the threshold are kept + std::multimap> candidates; + + const double contrast_max = 1 + me.getMu2(); + const double contrast_min = 1 - me.getMu1(); + + const double threshold = computeFinalThreshold(me); + + // First step: compute likelihoods and contrasts for all queries + if (test_contrast) { + for (unsigned int n = 0; n < numQueries; ++n) { + vpMeSite &query = list_query_pixels[n]; + // convolution results + const double convolution_ = query.convolution(I, &me); + // luminance ratio of reference pixel to potential correspondent pixel + // the luminance must be similar, hence the ratio value should + // lay between, for instance, 0.5 and 1.5 (parameter tolerance) + const double likelihood = fabs(convolution_ + m_convlt); + + query.m_convlt = convolution_; + const double contrast = convolution_ / m_convlt; + candidates.insert({ fabs(1.0 - contrast), {&query, likelihood, contrast} }); + } + } + else { // test on likelihood only + for (unsigned int n = 0; n < numQueries; ++n) { + // convolution results + vpMeSite &query = list_query_pixels[n]; + const double convolution_ = query.convolution(I, &me); + const double likelihood = fabs(2 * convolution_); + query.m_convlt = convolution_; + candidates.insert({ -likelihood, {&query, likelihood, 0.0} }); + } + } + // Take first numCandidates hypotheses: map is sorted according to the likelihood/contrast difference so we can just + // iterate from the start + outputHypotheses.resize(numCandidates); + std::multimap>::iterator it = candidates.begin(); + if (test_contrast) { + for (unsigned int i = 0; i < numCandidates; ++i, ++it) { + outputHypotheses[i] = *(std::get<0>(it->second)); + outputHypotheses[i].m_normGradient = vpMath::sqr(outputHypotheses[i].m_convlt); + const double likelihood = std::get<1>(it->second); + const double contrast = std::get<2>(it->second); + + if (likelihood > threshold) { + if (contrast <= contrast_min || contrast >= contrast_max) { + outputHypotheses[i].m_state = vpMeSiteState::CONTRAST; + } + else { + outputHypotheses[i].m_state = vpMeSiteState::NO_SUPPRESSION; + } + } + else { + outputHypotheses[i].m_state = vpMeSiteState::THRESHOLD; + } + } + } + else { + for (unsigned int i = 0; i < numCandidates; ++i, ++it) { + outputHypotheses[i] = *(std::get<0>(it->second)); + const double likelihood = std::get<1>(it->second); + if (likelihood > threshold) { + outputHypotheses[i].m_state = vpMeSiteState::NO_SUPPRESSION; + } + else { + outputHypotheses[i].m_state = vpMeSiteState::THRESHOLD; + } + } + } + + const vpMeSite &bestMatch = outputHypotheses[0]; + + + if (bestMatch.m_state != NO_SUPPRESSION) { + if ((m_selectDisplay == RANGE_RESULT) || (m_selectDisplay == RESULT)) { + + vpDisplay::displayPoint(I, bestMatch.m_i, bestMatch.m_j, vpColor::red); + } + *this = outputHypotheses[0]; + } + else { + if ((m_selectDisplay == RANGE_RESULT) || (m_selectDisplay == RESULT)) { + vpDisplay::displayPoint(I, bestMatch.m_i, bestMatch.m_j, vpColor::green); + } + m_normGradient = 0; + } + + delete[] list_query_pixels; } int vpMeSite::operator!=(const vpMeSite &m) { return ((m.m_i != m_i) || (m.m_j != m_j)); } -void vpMeSite::display(const vpImage &I) { vpMeSite::display(I, m_ifloat, m_jfloat, m_state); } -void vpMeSite::display(const vpImage &I) { vpMeSite::display(I, m_ifloat, m_jfloat, m_state); } +void vpMeSite::display(const vpImage &I) const { vpMeSite::display(I, m_ifloat, m_jfloat, m_state); } + +void vpMeSite::display(const vpImage &I) const { vpMeSite::display(I, m_ifloat, m_jfloat, m_state); } // Static functions diff --git a/tutorial/ar/tutorial-panda3d-renderer.cpp b/tutorial/ar/tutorial-panda3d-renderer.cpp index a4d26b8793..3c1212fb93 100644 --- a/tutorial/ar/tutorial-panda3d-renderer.cpp +++ b/tutorial/ar/tutorial-panda3d-renderer.cpp @@ -14,6 +14,7 @@ #include #include + #include #include @@ -102,12 +103,13 @@ int main(int argc, const char **argv) bool showLightContrib = false; bool showCanny = false; char *modelPathCstr = nullptr; - char *backgroundPathCstr = nullptr; + char *backgroundPathCstr = nullptr; vpParseArgv::vpArgvInfo argTable[] = { {"-model", vpParseArgv::ARGV_STRING, (char *) nullptr, (char *)&modelPathCstr, "Path to the model to load."}, + {"-background", vpParseArgv::ARGV_STRING, (char *) nullptr, (char *)&backgroundPathCstr, "Path to the background image to load for the rgb renderer."}, {"-step", vpParseArgv::ARGV_CONSTANT_BOOL, (char *) nullptr, (char *)&stepByStep, @@ -130,6 +132,16 @@ int main(int argc, const char **argv) return (false); } + if (PStatClient::is_connected()) { + PStatClient::disconnect(); + } + + std::string host = ""; // Empty = default config var value + int port = -1; // -1 = default config var value + if (!PStatClient::connect(host, port)) { + std::cout << "Could not connect to PStat server." << std::endl; + } + std::string modelPath; if (modelPathCstr) { modelPath = modelPathCstr; @@ -137,6 +149,7 @@ int main(int argc, const char **argv) else { modelPath = "data/suzanne.bam"; } + std::string backgroundPath; if (backgroundPathCstr) { backgroundPath = backgroundPathCstr; @@ -144,7 +157,8 @@ int main(int argc, const char **argv) const std::string objectName = "object"; //! [Renderer set] - vpPanda3DRenderParameters renderParams(vpCameraParameters(300, 300, 160, 120), 240, 320, 0.01, 10.0); + double factor = 0.5; + vpPanda3DRenderParameters renderParams(vpCameraParameters(600 * factor, 600 * factor, 320 * factor, 240 * factor), int(480 * factor), int(640 * factor), 0.01, 10.0); vpPanda3DRendererSet renderer(renderParams); renderer.setRenderParameters(renderParams); renderer.setVerticalSyncEnabled(false); @@ -160,8 +174,7 @@ int main(int argc, const char **argv) std::shared_ptr rgbRenderer = std::make_shared(); std::shared_ptr rgbDiffuseRenderer = std::make_shared(false); std::shared_ptr grayscaleFilter = std::make_shared("toGrayscale", rgbRenderer, false); - std::shared_ptr blurFilter = std::make_shared("blur", grayscaleFilter, false); - std::shared_ptr cannyFilter = std::make_shared("canny", blurFilter, true, 10.f); + std::shared_ptr cannyFilter = std::make_shared("canny", grayscaleFilter, true, 10.f); //! [Subrenderers init] //! [Adding subrenderers] @@ -173,7 +186,6 @@ int main(int argc, const char **argv) } if (showCanny) { renderer.addSubRenderer(grayscaleFilter); - renderer.addSubRenderer(blurFilter); renderer.addSubRenderer(cannyFilter); } std::cout << "Initializing Panda3D rendering framework" << std::endl; @@ -193,6 +205,12 @@ int main(int argc, const char **argv) vpPanda3DDirectionalLight dlight("Directional", vpRGBf(2.0f), vpColVector({ 1.0, 1.0, 0.0 })); renderer.addLight(dlight); + //! [Scene configuration] + + rgbRenderer->printStructure(); + + unsigned h = renderParams.getImageHeight(), w = renderParams.getImageWidth(); + if (!backgroundPath.empty()) { vpImage background; vpImageIo::read(background, backgroundPath); @@ -203,9 +221,9 @@ int main(int argc, const char **argv) std::cout << "Setting camera pose" << std::endl; renderer.setCameraPose(vpHomogeneousMatrix(0.0, 0.0, -0.3, 0.0, 0.0, 0.0)); + //! [Scene configuration] - unsigned h = renderParams.getImageHeight(), w = renderParams.getImageWidth(); std::cout << "Creating display and data images" << std::endl; vpImage normalsImage; vpImage cameraNormalsImage; @@ -252,7 +270,7 @@ int main(int argc, const char **argv) while (!end) { float nearV = 0, farV = 0; const double beforeComputeBB = vpTime::measureTimeMs(); - rgbRenderer->computeNearAndFarPlanesFromNode(objectName, nearV, farV); + rgbRenderer->computeNearAndFarPlanesFromNode(objectName, nearV, farV, true); renderParams.setClippingDistance(nearV, farV); renderer.setRenderParameters(renderParams); //std::cout << "Update clipping plane took " << vpTime::measureTimeMs() - beforeComputeBB << std::endl; From d845b405e2d78ebbc285f5e3af950dc45e7cb720 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Fri, 23 Aug 2024 14:09:40 +0200 Subject: [PATCH 02/25] Remove attempts at UKF, fix some stuff for python --- .../include/visp3/ar/vpPanda3DRendererSet.h | 6 +- .../vpPanda3DPostProcessFilter.cpp | 2 +- .../visp3/core/vpUKSigmaDrawerSimplex.h | 94 ---------------- .../math/kalman/vpUKSigmaDrawerSimplex.cpp | 103 ------------------ .../generator/visp_python_bindgen/header.py | 5 + .../generator/visp_python_bindgen/methods.py | 3 + 6 files changed, 12 insertions(+), 201 deletions(-) delete mode 100644 modules/core/include/visp3/core/vpUKSigmaDrawerSimplex.h delete mode 100644 modules/core/src/math/kalman/vpUKSigmaDrawerSimplex.cpp diff --git a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h index 8caba0e837..0e31530f7c 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h @@ -150,7 +150,7 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp */ void addSubRenderer(std::shared_ptr renderer); - void enableSharedDepthBuffer(vpPanda3DBaseRenderer &sourceBuffer) vp_override; + void enableSharedDepthBuffer(vpPanda3DBaseRenderer &sourceBuffer) VP_OVERRIDE; /** @@ -191,13 +191,13 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp return nullptr; } - void beforeFrameRendered() vp_override + void beforeFrameRendered() VP_OVERRIDE { for (std::shared_ptr &renderer: m_subRenderers) { renderer->beforeFrameRendered(); } } - void afterFrameRendered() vp_override + void afterFrameRendered() VP_OVERRIDE { for (std::shared_ptr &renderer: m_subRenderers) { renderer->afterFrameRendered(); diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp index 23da614824..8fe3021075 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp @@ -109,7 +109,7 @@ void vpPanda3DPostProcessFilter::setupRenderTarget() //m_buffer->set_inverted(true); 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->add_render_texture(m_texture, m_isOutput ? GraphicsOutput::RenderTextureMode::RTM_copy_ram : GraphicsOutput::RenderTextureMode::RTM_bind_or_copy); m_buffer->set_clear_color(LColor(0.f)); m_buffer->set_clear_color_active(true); DisplayRegion *region = m_buffer->make_display_region(); diff --git a/modules/core/include/visp3/core/vpUKSigmaDrawerSimplex.h b/modules/core/include/visp3/core/vpUKSigmaDrawerSimplex.h deleted file mode 100644 index 1dce5b22cc..0000000000 --- a/modules/core/include/visp3/core/vpUKSigmaDrawerSimplex.h +++ /dev/null @@ -1,94 +0,0 @@ -/* - * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2024 by Inria. All rights reserved. - * - * This software is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * See the file LICENSE.txt at the root directory of this source - * distribution for additional information about the GNU GPL. - * - * For using ViSP with software that can not be combined with the GNU - * GPL, please contact Inria about acquiring a ViSP Professional - * Edition License. - * - * See https://visp.inria.fr for more information. - * - * This software was developed at: - * Inria Rennes - Bretagne Atlantique - * Campus Universitaire de Beaulieu - * 35042 Rennes Cedex - * France - * - * If you have questions regarding the use of this file, please contact - * Inria at visp@inria.fr - * - * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE - * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * - * Description: - * Display a point cloud using PCL library. - */ - -#ifndef VP_UK_SIGMA_DRAWER_SIMPLEX_H -#define VP_UK_SIGMA_DRAWER_SIMPLEX_H - -#include - -#include - -#include -#include - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) -BEGIN_VISP_NAMESPACE -/*! - \class vpUKSigmaDrawerSimplex - \ingroup group_core_kalman -*/ -class VISP_EXPORT vpUKSigmaDrawerSimplex : public vpUKSigmaDrawerAbstract -{ -public: - typedef vpUnscentedKalman::vpAddSubFunction vpAddSubFunction; - - /** - * \brief Construct a new vpUKSigmaDrawerMerwe object. - * - * \param[in] n The size of the state vector. - * \param[in] w0 the weighting term, between 0 and 1 - * \param[in] addFunc Addition function expressed in the state space. - */ - vpUKSigmaDrawerSimplex(int n, double w0, const vpAddSubFunction &addFunc = vpUnscentedKalman::simpleAdd); - - /** - * Destructor that does nothing. - */ - virtual ~vpUKSigmaDrawerSimplex() { } - - /** - * \brief Draw the sigma points according to the current mean and covariance of the state - * of the Unscented Kalman filter. - * - * \param[in] mean The current mean of the state of the UKF. - * \param[in] covariance The current process covariance of the UKF. - * @return std::vector The sigma points. - */ - virtual std::vector drawSigmaPoints(const vpColVector &mean, const vpMatrix &covariance) override; - - /** - * \brief Computed the weigths that correspond to the sigma poitns that have been drawn. - * - * \return vpSigmaPointsWeights The weights that correspond to the sigma points. - */ - virtual vpSigmaPointsWeights computeWeights() override; -protected: - - double m_w0; /*!< Weighting term, between zero and one.*/ - std::vector m_ws; - std::vector m_chis; - vpAddSubFunction m_addFunc; /*!< Addition function expressed in the state space.*/ -}; -END_VISP_NAMESPACE -#endif -#endif diff --git a/modules/core/src/math/kalman/vpUKSigmaDrawerSimplex.cpp b/modules/core/src/math/kalman/vpUKSigmaDrawerSimplex.cpp deleted file mode 100644 index d0c7a2d9c2..0000000000 --- a/modules/core/src/math/kalman/vpUKSigmaDrawerSimplex.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. - * - * This software is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * See the file LICENSE.txt at the root directory of this source - * distribution for additional information about the GNU GPL. - * - * For using ViSP with software that can not be combined with the GNU - * GPL, please contact Inria about acquiring a ViSP Professional - * Edition License. - * - * See https://visp.inria.fr for more information. - * - * This software was developed at: - * Inria Rennes - Bretagne Atlantique - * Campus Universitaire de Beaulieu - * 35042 Rennes Cedex - * France - * - * If you have questions regarding the use of this file, please contact - * Inria at visp@inria.fr - * - * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE - * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * - * Description: - * Kalman filtering. - * -*****************************************************************************/ - -/*! - \file vpUKSigmaDrawerSimplex.cpp - \brief Sigma points drawer following the simplex method from Julier. -*/ - -#include - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) -BEGIN_VISP_NAMESPACE -vpUKSigmaDrawerSimplex::vpUKSigmaDrawerSimplex(int n, double w0, const vpAddSubFunction &addFunc) - : vpUKSigmaDrawerAbstract(n), - m_w0(w0), m_addFunc(addFunc), - m_chis(n + 1, vpColVector(n)), - m_ws(n + 1) -{ - // m_ws[0] = m_w0; - m_ws[0] = (1.0 - m_w0) / std::pow(2.0, m_n); - m_ws[1] = m_ws[0]; - for (unsigned int i = 2; i < m_n + 1; ++i) { - m_ws[i] = std::pow(2.0, i - 1) * m_ws[0]; - } - -} - -std::vector vpUKSigmaDrawerSimplex::drawSigmaPoints(const vpColVector &mean, const vpMatrix &covariance) -{ - const unsigned int nbSigmaPoints = m_n + 1; - std::vector sigmaPoints(nbSigmaPoints); - vpMatrix chi(m_n, nbSigmaPoints, 0.0); - chi[0][0] = 0.0; - chi[0][1] = (-1.0) / sqrt(2.0 * m_ws[0]); - chi[0][2] = (1.0) / sqrt(2.0 * m_ws[0]); - for (unsigned int j = 1; j < m_n; ++j) { - chi[j][0] = 0.0; - for (unsigned int i = 1; i < j + 1; ++i) { - chi[j][i] = (-1.0) / sqrt(2.0 * m_ws[j - 1]); - } - chi[j][j + 1] = 1.0 / sqrt(2.0 * m_ws[j - 1]); - } - - const vpMatrix cholesky = covariance.cholesky(); - for (unsigned int i = 0; i < nbSigmaPoints; ++i) { - std::cout << "Cholesky = " << cholesky < framework, std::shared_ptr window); virtual void initFromParent(const vpPanda3DBaseRenderer &renderer); - virtual void beforeFrameRendered() { } virtual void renderFrame(); virtual void afterFrameRendered() { } - /** * @brief Get the name of the renderer * diff --git a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h index 0e31530f7c..725b3a366e 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h @@ -71,7 +71,6 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp * 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; - void initFromParent(std::shared_ptr framework, std::shared_ptr window) VP_OVERRIDE; void initFromParent(const vpPanda3DBaseRenderer &renderer) VP_OVERRIDE; diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp index 50d9a1604c..40c7e56ca0 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp @@ -39,15 +39,16 @@ vpPanda3DRendererSet::vpPanda3DRendererSet(const vpPanda3DRenderParameters &rend { m_renderParameters = renderParameters; load_prc_file_data("", "textures-power-2 none"); + load_prc_file_data("", "gl-version 3 2"); + load_prc_file_data("", "no-singular-invert"); } void vpPanda3DRendererSet::initFramework() { - // load_prc_file_data("", "load-display p3tinydisplay"); // load_prc_file_data("", "color-bits 32 32 32"); - load_prc_file_data("", "gl-version 3 2"); + // load_prc_file_data("", "auto-flip 1"); // load_prc_file_data("", "sync-video 0"); // load_prc_file_data("", "multisamples 1\n" @@ -94,7 +95,7 @@ void vpPanda3DRendererSet::initFromParent(std::shared_ptr framew void vpPanda3DRendererSet::initFromParent(const vpPanda3DBaseRenderer &renderer) { vpPanda3DBaseRenderer::initFromParent(renderer); - for (std::shared_ptr &r: m_subRenderers) { + for (std::shared_ptr &renderer: m_subRenderers) { r->initFromParent(*this); } } diff --git a/tutorial/ar/tutorial-panda3d-renderer.cpp b/tutorial/ar/tutorial-panda3d-renderer.cpp index 3c1212fb93..f8732c4e51 100644 --- a/tutorial/ar/tutorial-panda3d-renderer.cpp +++ b/tutorial/ar/tutorial-panda3d-renderer.cpp @@ -157,12 +157,13 @@ int main(int argc, const char **argv) const std::string objectName = "object"; //! [Renderer set] - double factor = 0.5; + double factor = 1.0; vpPanda3DRenderParameters renderParams(vpCameraParameters(600 * factor, 600 * factor, 320 * factor, 240 * factor), int(480 * factor), int(640 * factor), 0.01, 10.0); + unsigned h = renderParams.getImageHeight(), w = renderParams.getImageWidth(); vpPanda3DRendererSet renderer(renderParams); renderer.setRenderParameters(renderParams); renderer.setVerticalSyncEnabled(false); - renderer.setAbortOnPandaError(true); + renderer.setAbortOnPandaError(false); if (debug) { renderer.enableDebugLog(); } @@ -173,7 +174,7 @@ int main(int argc, const char **argv) std::shared_ptr cameraRenderer = std::make_shared(vpPanda3DGeometryRenderer::vpRenderType::CAMERA_NORMALS); std::shared_ptr rgbRenderer = std::make_shared(); std::shared_ptr rgbDiffuseRenderer = std::make_shared(false); - std::shared_ptr grayscaleFilter = std::make_shared("toGrayscale", rgbRenderer, false); + std::shared_ptr grayscaleFilter = std::make_shared("toGrayscale", rgbRenderer, true); std::shared_ptr cannyFilter = std::make_shared("canny", grayscaleFilter, true, 10.f); //! [Subrenderers init] @@ -192,6 +193,7 @@ int main(int argc, const char **argv) renderer.initFramework(); //! [Adding subrenderers] + //! [Scene configuration] NodePath object = renderer.loadObject(objectName, modelPath); renderer.addNodeToScene(object); @@ -204,12 +206,8 @@ int main(int argc, const char **argv) vpPanda3DDirectionalLight dlight("Directional", vpRGBf(2.0f), vpColVector({ 1.0, 1.0, 0.0 })); renderer.addLight(dlight); - //! [Scene configuration] - rgbRenderer->printStructure(); - - unsigned h = renderParams.getImageHeight(), w = renderParams.getImageWidth(); if (!backgroundPath.empty()) { vpImage background; @@ -219,9 +217,7 @@ int main(int argc, const char **argv) rgbRenderer->printStructure(); - std::cout << "Setting camera pose" << std::endl; - renderer.setCameraPose(vpHomogeneousMatrix(0.0, 0.0, -0.3, 0.0, 0.0, 0.0)); - + renderer.setCameraPose(vpHomogeneousMatrix(0.0, 0.0, -5.0, 0.0, 0.0, 0.0)); //! [Scene configuration] std::cout << "Creating display and data images" << std::endl; @@ -249,35 +245,39 @@ int main(int argc, const char **argv) #elif defined(VISP_HAVE_D3D9) using DisplayCls = vpDisplayD3D; #endif - - DisplayCls dNormals(normalDisplayImage, 0, 0, "normals in world space"); - DisplayCls dNormalsCamera(cameraNormalDisplayImage, 0, h + 80, "normals in camera space"); - DisplayCls dDepth(depthDisplayImage, w + 80, 0, "depth"); - DisplayCls dColor(colorImage, w + 80, h + 80, "color"); + unsigned int padding = 80; + DisplayCls dNormals(normalDisplayImage, 0, 0, "normals in object space"); + DisplayCls dNormalsCamera(cameraNormalDisplayImage, 0, h + padding, "normals in camera space"); + DisplayCls dDepth(depthDisplayImage, w + padding, 0, "depth"); + DisplayCls dColor(colorImage, w + padding, h + padding, "color"); DisplayCls dImageDiff; if (showLightContrib) { - dImageDiff.init(lightDifference, w * 2 + 80, 0, "Specular/reflectance contribution"); + dImageDiff.init(lightDifference, w * 2 + padding, 0, "Specular/reflectance contribution"); } DisplayCls dCanny; if (showCanny) { - dCanny.init(cannyImage, w * 2 + 80, h + 80, "Canny"); + dCanny.init(cannyImage, w * 2 + padding, h + padding, "Canny"); } renderer.renderFrame(); bool end = false; bool firstFrame = true; std::vector renderTime, fetchTime, displayTime; while (!end) { - float nearV = 0, farV = 0; const double beforeComputeBB = vpTime::measureTimeMs(); - rgbRenderer->computeNearAndFarPlanesFromNode(objectName, nearV, farV, true); + //! [Updating render parameters] + float nearV = 0, farV = 0; + geometryRenderer->computeNearAndFarPlanesFromNode(objectName, nearV, farV, true); renderParams.setClippingDistance(nearV, farV); renderer.setRenderParameters(renderParams); - //std::cout << "Update clipping plane took " << vpTime::measureTimeMs() - beforeComputeBB << std::endl; + //! [Updating render parameters] const double beforeRender = vpTime::measureTimeMs(); + //! [Render frame] renderer.renderFrame(); + //! [Render frame] const double beforeFetch = vpTime::measureTimeMs(); + //! [Fetch render] renderer.getRenderer(geometryRenderer->getName())->getRender(normalsImage, depthImage); renderer.getRenderer(cameraRenderer->getName())->getRender(cameraNormalsImage); renderer.getRenderer(rgbRenderer->getName())->getRender(colorImage); @@ -287,8 +287,9 @@ int main(int argc, const char **argv) if (showCanny) { renderer.getRenderer()->getRender(cannyRawData); } - + //! [Fetch render] const double beforeConvert = vpTime::measureTimeMs(); + //! [Display] displayNormals(normalsImage, normalDisplayImage); displayNormals(cameraNormalsImage, cameraNormalDisplayImage); displayDepth(depthImage, depthDisplayImage, nearV, farV); @@ -301,6 +302,7 @@ int main(int argc, const char **argv) vpDisplay::display(colorImage); vpDisplay::displayText(colorImage, 15, 15, "Click to quit", vpColor::red); + //! [Display] if (stepByStep) { vpDisplay::displayText(colorImage, 50, 15, "Next frame: space", vpColor::red); @@ -324,10 +326,12 @@ int main(int argc, const char **argv) } } const double afterAll = vpTime::measureTimeMs(); + //! [Move object] const double delta = (afterAll - beforeRender) / 1000.0; const vpHomogeneousMatrix wTo = renderer.getNodePose(objectName); const vpHomogeneousMatrix oToo = vpExponentialMap::direct(vpColVector({ 0.0, 0.0, 0.0, 0.0, vpMath::rad(20.0), 0.0 }), delta); renderer.setNodePose(objectName, wTo * oToo); + //! [Move object] } if (renderTime.size() > 0) { std::cout << "Render time: " << vpMath::getMean(renderTime) << "ms +- " << vpMath::getStdev(renderTime) << "ms" << std::endl; From ccc47d21fddd4e2b6384f8e5b09843a3764a8ace Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Tue, 27 Aug 2024 12:46:34 +0200 Subject: [PATCH 04/25] Trying to replace alternated ram fetch with a unified block of extract_texture_data --- .../include/visp3/ar/vpPanda3DBaseRenderer.h | 12 +++++++--- .../visp3/ar/vpPanda3DPostProcessFilter.h | 13 +++++++++++ .../include/visp3/ar/vpPanda3DRendererSet.h | 12 +++++++++- .../vpPanda3DBaseRenderer.cpp | 6 ++--- .../vpPanda3DGeometryRenderer.cpp | 22 +++++++++---------- .../vpPanda3DPostProcessFilter.cpp | 2 +- .../vpPanda3DRGBRenderer.cpp | 19 +++++++++------- .../vpPanda3DRendererSet.cpp | 9 ++++---- tutorial/ar/tutorial-panda3d-renderer.cpp | 4 ++-- 9 files changed, 65 insertions(+), 34 deletions(-) diff --git a/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h index b5c04d80a3..d72c12255f 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h @@ -71,12 +71,18 @@ class VISP_EXPORT vpPanda3DBaseRenderer * Will also perform the renderer setup (scene, camera and render targets) */ virtual void initFramework(); - virtual void initFromParent(std::shared_ptr framework, std::shared_ptr window); + virtual void initFromParent(std::shared_ptr framework, PointerTo window); virtual void initFromParent(const vpPanda3DBaseRenderer &renderer); virtual void beforeFrameRendered() { } virtual void renderFrame(); - virtual void afterFrameRendered() { } + virtual void afterFrameRendered() + { + GraphicsOutput *mainBuffer = getMainOutputBuffer(); + if (mainBuffer != nullptr) { + m_framework->get_graphics_engine()->extract_texture_data(mainBuffer->get_texture(), mainBuffer->get_gsg()); + } + } /** * @brief Get the name of the renderer @@ -270,7 +276,7 @@ class VISP_EXPORT vpPanda3DBaseRenderer 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 - std::shared_ptr m_window; //! Pointer to owning window, which can create buffers etc. It is not necessarily visible. + PointerTo m_window; //! Pointer to owning window, which can create buffers etc. It is not necessarily visible. vpPanda3DRenderParameters m_renderParameters; //! Rendering parameters NodePath m_renderRoot; //! Node containing all the objects and the camera for this renderer PointerTo m_camera; diff --git a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h index 06cf135a71..4eb1d3ef7b 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h +++ b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h @@ -62,6 +62,12 @@ class VISP_EXPORT vpPanda3DPostProcessFilter : public vpPanda3DBaseRenderer m_renderOrder = m_inputRenderer->getRenderOrder() + 1; } + ~vpPanda3DPostProcessFilter() + { + // delete m_texture; + // delete m_buffer; + } + bool isRendering3DScene() const VP_OVERRIDE { return false; @@ -69,6 +75,13 @@ class VISP_EXPORT vpPanda3DPostProcessFilter : public vpPanda3DBaseRenderer GraphicsOutput *getMainOutputBuffer() VP_OVERRIDE { return m_buffer; } + void afterFrameRendered() VP_OVERRIDE + { + if (m_isOutput) { + vpPanda3DBaseRenderer::afterFrameRendered(); + } + } + protected: virtual void setupScene() VP_OVERRIDE; diff --git a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h index 725b3a366e..ed47e66e54 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h @@ -71,7 +71,7 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp * 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; - void initFromParent(std::shared_ptr framework, std::shared_ptr window) VP_OVERRIDE; + void initFromParent(std::shared_ptr framework, PointerTo window) VP_OVERRIDE; void initFromParent(const vpPanda3DBaseRenderer &renderer) VP_OVERRIDE; /** @@ -198,9 +198,19 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp } void afterFrameRendered() VP_OVERRIDE { + double t1 = vpTime::measureTimeMs(); for (std::shared_ptr &renderer: m_subRenderers) { renderer->afterFrameRendered(); } + std::cout << "Render to texture took: " << vpTime::measureTimeMs() - t1 << std::endl; + // std::vector textures; + // for (std::shared_ptr &renderer: m_subRenderers) { + // textures.push_back(renderer->getMainOutputBuffer()->get_texture()); + // } + + // for (unsigned int i = 0; i < textures.size(); ++i) { + // m_framework->get_graphics_engine()->extract_texture_data(textures[i], m_window->get_graphics_output()->get_gsg()); + // } } protected: diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp index b8b7174c1d..6c2dd1bbeb 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp @@ -62,11 +62,11 @@ void vpPanda3DBaseRenderer::initFramework() WindowProperties winProps; winProps.set_size(LVecBase2i(m_renderParameters.getImageWidth(), m_renderParameters.getImageHeight())); int flags = GraphicsPipe::BF_refuse_window; - m_window = std::shared_ptr(m_framework->open_window(winProps, flags)); + m_window = m_framework->open_window(winProps, flags); // try and reopen with visible window if (m_window == nullptr) { winProps.set_minimized(true); - m_window = std::shared_ptr(m_framework->open_window(winProps, 0)); + m_window = m_framework->open_window(winProps, 0); } if (m_window == nullptr) { throw vpException(vpException::notInitialized, @@ -79,7 +79,7 @@ void vpPanda3DBaseRenderer::initFramework() //m_window->get_display_region_3d()->set_camera(m_cameraPath); } -void vpPanda3DBaseRenderer::initFromParent(std::shared_ptr framework, std::shared_ptr window) +void vpPanda3DBaseRenderer::initFromParent(std::shared_ptr framework, PointerTo window) { m_framework = framework; m_window = window; diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp index 4efbc003b5..d0d51ff6e0 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp @@ -51,14 +51,13 @@ out float distToCamera; void main() { - //gl_Position = ftransform(); - gl_Position = p3d_ModelViewProjectionMatrix * p3d_Vertex; - // View space is Z-up right handed, flip z and y - oNormal = p3d_NormalMatrix * normalize(p3d_Normal); - // oNormal.yz = oNormal.zy; - // oNormal.y = -oNormal.y; - vec4 cs_position = p3d_ModelViewMatrix * p3d_Vertex; + gl_Position = p3d_ModelViewProjectionMatrix * p3d_Vertex; + // View space is Z-up right handed, flip z and y + oNormal = p3d_NormalMatrix * normalize(p3d_Normal); + // oNormal.yz = oNormal.zy; + // oNormal.y = -oNormal.y; + vec4 cs_position = p3d_ModelViewMatrix * p3d_Vertex; distToCamera = -cs_position.z; } )shader"; @@ -104,8 +103,7 @@ void main() vec3 n = normalize(oNormal); //if (!gl_FrontFacing) //n = -n; - p3d_FragData = vec4(n, distToCamera); - + p3d_FragData.bgra = vec4(n, distToCamera); } )shader"; @@ -174,7 +172,7 @@ void vpPanda3DGeometryRenderer::setupRenderTarget() m_normalDepthBuffer->set_inverted(windowOutput->get_gsg()->get_copy_texture_inverted()); fbp.setup_color_texture(m_normalDepthTexture); m_normalDepthTexture->set_format(Texture::F_rgba32); - m_normalDepthBuffer->add_render_texture(m_normalDepthTexture, GraphicsOutput::RenderTextureMode::RTM_copy_ram, GraphicsOutput::RenderTexturePlane::RTP_color); + m_normalDepthBuffer->add_render_texture(m_normalDepthTexture, GraphicsOutput::RenderTextureMode::RTM_copy_texture, GraphicsOutput::RenderTexturePlane::RTP_color); m_normalDepthBuffer->set_clear_color(LColor(0.f)); m_normalDepthBuffer->set_clear_color_active(true); DisplayRegion *region = m_normalDepthBuffer->make_display_region(); @@ -226,9 +224,9 @@ void vpPanda3DGeometryRenderer::getRender(vpImage &normals) const for (unsigned int i = 0; i < normals.getHeight(); ++i) { vpRGBf *normalRow = normals[i]; for (unsigned int j = 0; j < normals.getWidth(); ++j) { - normalRow[j].B = (data[j * 4]); + normalRow[j].R = (data[j * 4]); normalRow[j].G = (data[j * 4 + 1]); - normalRow[j].R = (data[j * 4 + 2]); + normalRow[j].B = (data[j * 4 + 2]); } data += rowIncrement; } diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp index 8fe3021075..0a123b51f5 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp @@ -109,7 +109,7 @@ void vpPanda3DPostProcessFilter::setupRenderTarget() //m_buffer->set_inverted(true); 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_bind_or_copy); + m_buffer->add_render_texture(m_texture, m_isOutput ? GraphicsOutput::RenderTextureMode::RTM_bind_or_copy : 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(); diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp index f4abc3648b..cc429ab31d 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp @@ -195,6 +195,7 @@ void main() p3d_FragData += (p3d_LightSource[i].color * attenuation) * nl * (baseColor * vec4(kd, 1.f) + vec4(specularColor, 1.f)); } + p3d_FragData.bgra = p3d_FragData; } )shader"; @@ -298,13 +299,15 @@ void vpPanda3DRGBRenderer::getRender(vpImage &I) const for (unsigned int i = 0; i < I.getHeight(); ++i) { vpRGBa *colorRow = I[i]; - for (unsigned int j = 0; j < I.getWidth(); ++j) { - // BGRA order in panda3d - colorRow[j].B = data[j * 4]; - colorRow[j].G = data[j * 4 + 1]; - colorRow[j].R = data[j * 4 + 2]; - colorRow[j].A = data[j * 4 + 3]; - } + + memcpy((unsigned char *)(colorRow), data, sizeof(unsigned char) * 4 * I.getWidth()); + // for (unsigned int j = 0; j < I.getWidth(); ++j) { + // // BGRA order in panda3d + // colorRow[j].R = data[j * 4]; + // colorRow[j].G = data[j * 4 + 1]; + // colorRow[j].B = data[j * 4 + 2]; + // colorRow[j].A = data[j * 4 + 3]; + // } data += rowIncrement; } } @@ -348,7 +351,7 @@ void vpPanda3DRGBRenderer::setupRenderTarget() m_colorTexture = new Texture(); fbp.setup_color_texture(m_colorTexture); //m_colorTexture->set_format(Texture::Format::F_srgb_alpha); - m_colorBuffer->add_render_texture(m_colorTexture, GraphicsOutput::RenderTextureMode::RTM_copy_ram); + m_colorBuffer->add_render_texture(m_colorTexture, GraphicsOutput::RenderTextureMode::RTM_copy_texture); m_colorBuffer->set_clear_color(LColor(0.f)); m_colorBuffer->set_clear_color_active(true); DisplayRegion *region = m_colorBuffer->make_display_region(); diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp index 40c7e56ca0..26e2113557 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp @@ -65,14 +65,15 @@ void vpPanda3DRendererSet::initFramework() throw vpException(vpException::notImplementedError, "Panda3D renderer: Reinitializing is not supported!"); } m_framework = std::shared_ptr(new PandaFramework()); + m_framework->open_framework(); WindowProperties winProps; winProps.set_size(LVecBase2i(m_renderParameters.getImageWidth(), m_renderParameters.getImageHeight())); int flags = GraphicsPipe::BF_refuse_window; - m_window = std::shared_ptr(m_framework->open_window(winProps, flags)); + m_window = m_framework->open_window(winProps, flags); if (m_window == nullptr) { winProps.set_minimized(true); - m_window = std::shared_ptr(m_framework->open_window(winProps, 0)); + m_window = m_framework->open_window(winProps, 0); } if (m_window == nullptr) { throw vpException(vpException::fatalError, "Could not open Panda3D window (hidden or visible)"); @@ -84,7 +85,7 @@ void vpPanda3DRendererSet::initFramework() } } -void vpPanda3DRendererSet::initFromParent(std::shared_ptr framework, std::shared_ptr window) +void vpPanda3DRendererSet::initFromParent(std::shared_ptr framework, PointerTo window) { vpPanda3DBaseRenderer::initFromParent(framework, window); for (std::shared_ptr &renderer: m_subRenderers) { @@ -96,7 +97,7 @@ void vpPanda3DRendererSet::initFromParent(const vpPanda3DBaseRenderer &renderer) { vpPanda3DBaseRenderer::initFromParent(renderer); for (std::shared_ptr &renderer: m_subRenderers) { - r->initFromParent(*this); + renderer->initFromParent(*this); } } diff --git a/tutorial/ar/tutorial-panda3d-renderer.cpp b/tutorial/ar/tutorial-panda3d-renderer.cpp index f8732c4e51..46de94959a 100644 --- a/tutorial/ar/tutorial-panda3d-renderer.cpp +++ b/tutorial/ar/tutorial-panda3d-renderer.cpp @@ -163,7 +163,7 @@ int main(int argc, const char **argv) vpPanda3DRendererSet renderer(renderParams); renderer.setRenderParameters(renderParams); renderer.setVerticalSyncEnabled(false); - renderer.setAbortOnPandaError(false); + renderer.setAbortOnPandaError(true); if (debug) { renderer.enableDebugLog(); } @@ -174,7 +174,7 @@ int main(int argc, const char **argv) std::shared_ptr cameraRenderer = std::make_shared(vpPanda3DGeometryRenderer::vpRenderType::CAMERA_NORMALS); std::shared_ptr rgbRenderer = std::make_shared(); std::shared_ptr rgbDiffuseRenderer = std::make_shared(false); - std::shared_ptr grayscaleFilter = std::make_shared("toGrayscale", rgbRenderer, true); + std::shared_ptr grayscaleFilter = std::make_shared("toGrayscale", rgbRenderer, false); std::shared_ptr cannyFilter = std::make_shared("canny", grayscaleFilter, true, 10.f); //! [Subrenderers init] From b08fa4e0db1aefc8059777a11d4787f726556fe7 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Tue, 27 Aug 2024 21:34:52 +0200 Subject: [PATCH 05/25] Remove debug print --- modules/ar/include/visp3/ar/vpPanda3DRendererSet.h | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h index ed47e66e54..e46d55feda 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h @@ -198,19 +198,9 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp } void afterFrameRendered() VP_OVERRIDE { - double t1 = vpTime::measureTimeMs(); for (std::shared_ptr &renderer: m_subRenderers) { renderer->afterFrameRendered(); } - std::cout << "Render to texture took: " << vpTime::measureTimeMs() - t1 << std::endl; - // std::vector textures; - // for (std::shared_ptr &renderer: m_subRenderers) { - // textures.push_back(renderer->getMainOutputBuffer()->get_texture()); - // } - - // for (unsigned int i = 0; i < textures.size(); ++i) { - // m_framework->get_graphics_engine()->extract_texture_data(textures[i], m_window->get_graphics_output()->get_gsg()); - // } } protected: From 475cc343ef616995ddfdf588905169b351a5f2cd Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 28 Aug 2024 12:56:08 +0200 Subject: [PATCH 06/25] rename normals_world shader to normals_object, to better reflect the functionality --- modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h | 2 +- .../ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h index a452a04cab..1106677163 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h @@ -94,7 +94,7 @@ class VISP_EXPORT vpPanda3DGeometryRenderer : public vpPanda3DBaseRenderer Texture *m_normalDepthTexture; GraphicsOutput *m_normalDepthBuffer; - static const char *SHADER_VERT_NORMAL_AND_DEPTH_WORLD; + static const char *SHADER_VERT_NORMAL_AND_DEPTH_OBJECT; static const char *SHADER_VERT_NORMAL_AND_DEPTH_CAMERA; static const char *SHADER_FRAG_NORMAL_AND_DEPTH; diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp index d0d51ff6e0..2086af232d 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp @@ -62,7 +62,7 @@ void main() } )shader"; -const char *vpPanda3DGeometryRenderer::SHADER_VERT_NORMAL_AND_DEPTH_WORLD = R"shader( +const char *vpPanda3DGeometryRenderer::SHADER_VERT_NORMAL_AND_DEPTH_OBJECT = R"shader( #version 330 in vec3 p3d_Normal; @@ -127,7 +127,7 @@ void vpPanda3DGeometryRenderer::setupScene() PT(Shader) shader; if (m_renderType == OBJECT_NORMALS) { shader = Shader::make(Shader::ShaderLanguage::SL_GLSL, - SHADER_VERT_NORMAL_AND_DEPTH_WORLD, + SHADER_VERT_NORMAL_AND_DEPTH_OBJECT, SHADER_FRAG_NORMAL_AND_DEPTH); } else if (m_renderType == CAMERA_NORMALS) { From 6b9e0bfb492105934a45d335f52d7aece478378c Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 28 Aug 2024 13:47:47 +0200 Subject: [PATCH 07/25] small optimization on normal rendering --- .../panda3d-simulator/vpPanda3DGeometryRenderer.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp index d0d51ff6e0..e4201e20f8 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp @@ -53,10 +53,7 @@ void main() { gl_Position = p3d_ModelViewProjectionMatrix * p3d_Vertex; - // View space is Z-up right handed, flip z and y oNormal = p3d_NormalMatrix * normalize(p3d_Normal); - // oNormal.yz = oNormal.zy; - // oNormal.y = -oNormal.y; vec4 cs_position = p3d_ModelViewMatrix * p3d_Vertex; distToCamera = -cs_position.z; } @@ -77,8 +74,7 @@ void main() { //gl_Position = ftransform(); gl_Position = p3d_ModelViewProjectionMatrix * p3d_Vertex; - oNormal = normalize(p3d_Normal); - oNormal.yz = oNormal.zy; + oNormal.xzy = p3d_Normal; oNormal.y = -oNormal.y; // vec3 on = vec3(oNormal.z, -oNormal.x, -oNormal.y); // oNormal = on; @@ -92,8 +88,6 @@ const char *vpPanda3DGeometryRenderer::SHADER_FRAG_NORMAL_AND_DEPTH = R"shader( in vec3 oNormal; in float distToCamera; -out vec4 p3d_FragColor; -out vec4 fragColor; out vec4 p3d_FragData; @@ -101,8 +95,6 @@ out vec4 p3d_FragData; void main() { vec3 n = normalize(oNormal); - //if (!gl_FrontFacing) - //n = -n; p3d_FragData.bgra = vec4(n, distToCamera); } )shader"; From 094e740c9c4ea22099de4a9b81571b29a7c6553f Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 28 Aug 2024 15:36:04 +0200 Subject: [PATCH 08/25] simplify normals shader --- .../panda3d-simulator/vpPanda3DGeometryRenderer.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp index e4201e20f8..53db6fe844 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp @@ -72,12 +72,8 @@ out float distToCamera; void main() { - //gl_Position = ftransform(); gl_Position = p3d_ModelViewProjectionMatrix * p3d_Vertex; - oNormal.xzy = p3d_Normal; - oNormal.y = -oNormal.y; - // vec3 on = vec3(oNormal.z, -oNormal.x, -oNormal.y); - // oNormal = on; + oNormal = vec3(p3d_Normal.y, -p3d_Normal.z, p3d_Normal.x); vec4 cs_position = p3d_ModelViewMatrix * p3d_Vertex; distToCamera = -cs_position.z; } @@ -91,11 +87,9 @@ in float distToCamera; out vec4 p3d_FragData; - void main() { - vec3 n = normalize(oNormal); - p3d_FragData.bgra = vec4(n, distToCamera); + p3d_FragData.bgra = vec4(normalize(oNormal), distToCamera); } )shader"; @@ -164,7 +158,7 @@ void vpPanda3DGeometryRenderer::setupRenderTarget() m_normalDepthBuffer->set_inverted(windowOutput->get_gsg()->get_copy_texture_inverted()); fbp.setup_color_texture(m_normalDepthTexture); m_normalDepthTexture->set_format(Texture::F_rgba32); - m_normalDepthBuffer->add_render_texture(m_normalDepthTexture, GraphicsOutput::RenderTextureMode::RTM_copy_texture, GraphicsOutput::RenderTexturePlane::RTP_color); + m_normalDepthBuffer->add_render_texture(m_normalDepthTexture, GraphicsOutput::RenderTextureMode::RTM_bind_or_copy, GraphicsOutput::RenderTexturePlane::RTP_color); m_normalDepthBuffer->set_clear_color(LColor(0.f)); m_normalDepthBuffer->set_clear_color_active(true); DisplayRegion *region = m_normalDepthBuffer->make_display_region(); From e73c1d7fb2e21f67526572293a0985d855deff19 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 28 Aug 2024 17:12:04 +0200 Subject: [PATCH 09/25] update Clipping distance computation that was wrongly calculated --- .../vpPanda3DBaseRenderer.cpp | 28 ++++++++++++++++--- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp index 6c2dd1bbeb..5dfa155c1d 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp @@ -204,17 +204,37 @@ void vpPanda3DBaseRenderer::computeNearAndFarPlanesFromNode(const std::string &n } if (!fast) { LPoint3 minP, maxP; + double t1 = vpTime::measureTimeMs(); + object.calc_tight_bounds(minP, maxP); + const BoundingBox box(minP, maxP); + float minZ = std::numeric_limits::max(), maxZ = 0.f; + const vpHomogeneousMatrix wTcam = getCameraPose(); + const vpHomogeneousMatrix wTobj = getNodePose(name) * vpPanda3DBaseRenderer::PANDA_T_VISP; + const vpHomogeneousMatrix camTobj = wTcam.inverse() * wTobj; + for (unsigned int i = 0; i < 8; ++i) { + const LPoint3 p = box.get_point(i); + const vpColVector pv = vpColVector({ p.get_x(), -p.get_z(), p.get_y(), 1.0 }); + vpColVector cpV = camTobj * pv; + cpV /= cpV[3]; + float Z = cpV[2]; + if (Z > maxZ) { + maxZ = Z; + } + if (Z < minZ) { + minZ = Z; + } + } + + nearV = minZ; + farV = maxZ; - object.calc_tight_bounds(minP, maxP, m_cameraPath); - nearV = vpMath::maximum(0.f, minP.get_y()); - farV = vpMath::maximum(nearV, maxP.get_y()); } else { const BoundingVolume *volume = object.node()->get_bounds(); if (volume->get_type() == BoundingSphere::get_class_type()) { const BoundingSphere *sphere = (const BoundingSphere *)volume; const LPoint3 center = sphere->get_center(); - const float distCenter = (center -m_cameraPath.get_pos()).length(); + const float distCenter = (center - m_cameraPath.get_pos()).length(); nearV = vpMath::maximum(0.f, distCenter - sphere->get_radius()); farV = vpMath::maximum(nearV, distCenter + sphere->get_radius()); } From d32922ec4271894ce667b6d18615f5a6657fb444 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 28 Aug 2024 18:40:21 +0200 Subject: [PATCH 10/25] Introduce getRender overload to write into a bounding box in an image --- .../visp3/ar/vpPanda3DGeometryRenderer.h | 3 ++ .../vpPanda3DGeometryRenderer.cpp | 42 +++++++++++++++++-- 2 files changed, 42 insertions(+), 3 deletions(-) diff --git a/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h index 1106677163..a33c599553 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h @@ -36,6 +36,7 @@ #if defined(VISP_HAVE_PANDA3D) #include #include +#include #include BEGIN_VISP_NAMESPACE @@ -71,6 +72,8 @@ class VISP_EXPORT vpPanda3DGeometryRenderer : public vpPanda3DBaseRenderer */ void getRender(vpImage &colorData, vpImage &depth) const; + void getRender(vpImage &normals, vpImage &depth, const vpRect &bb, unsigned int h, unsigned w) const; + /** * @brief Get render results into ViSP readable structures. This version only retrieves the normal data * @param colorData Depending on the vpRenderType, normals in the world or camera frame may be stored in this image. diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp index 7af7a60ecc..02a739fafb 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp @@ -34,6 +34,7 @@ #include + BEGIN_VISP_NAMESPACE const char *vpPanda3DGeometryRenderer::SHADER_VERT_NORMAL_AND_DEPTH_CAMERA = R"shader( #version 330 @@ -73,7 +74,7 @@ out float distToCamera; void main() { gl_Position = p3d_ModelViewProjectionMatrix * p3d_Vertex; - oNormal = vec3(p3d_Normal.y, -p3d_Normal.z, p3d_Normal.x); + oNormal = vec3(p3d_Normal.x, -p3d_Normal.z, p3d_Normal.y); vec4 cs_position = p3d_ModelViewMatrix * p3d_Vertex; distToCamera = -cs_position.z; } @@ -187,15 +188,50 @@ void vpPanda3DGeometryRenderer::getRender(vpImage &normals, vpImage &normals, vpImage &depth, const vpRect &bb, unsigned int h, unsigned w) const +{ + normals.resize(h, w); + // memset(normals.bitmap, 0, normals.getSize() * sizeof(vpRGBf)); + depth.resize(normals.getHeight(), normals.getWidth(), 0.f); + // memset(depth.bitmap, 0, normals.getSize()); + + const unsigned top = static_cast(std::max(0.0, bb.getTop())); + const unsigned left = static_cast(std::max(0.0, bb.getLeft())); + const unsigned bottom = static_cast(std::min(static_cast(h), bb.getBottom())); + const unsigned right = static_cast(std::min(static_cast(w), bb.getRight())); + const unsigned numComponents = m_normalDepthTexture->get_num_components(); + const unsigned rowIncrement = m_renderParameters.getImageWidth() * numComponents; // we ask for only 8 bits image, but we may get an rgb image + const float *data = (float *)(&(m_normalDepthTexture->get_ram_image().front())); + // Panda3D stores data upside down + data += rowIncrement * (m_renderParameters.getImageHeight() - 1); + if (numComponents != 4) { + throw vpException(vpException::dimensionError, "Expected panda texture to have 4 components!"); + } + for (unsigned int i = 0; i < m_renderParameters.getImageHeight(); ++i) { + const float *const rowData = data - i * rowIncrement; + vpRGBf *normalRow = normals[top + i]; + float *depthRow = depth[top + i]; +#pragma omp simd + for (unsigned int j = 0; j < m_renderParameters.getImageWidth(); ++j) { + normalRow[left + j].R = (rowData[j * 4]); + normalRow[left + j].G = (rowData[j * 4 + 1]); + normalRow[left + j].B = (rowData[j * 4 + 2]); + depthRow[left + j] = (rowData[j * 4 + 3]); + } + } +} + + void vpPanda3DGeometryRenderer::getRender(vpImage &normals) const { normals.resize(m_normalDepthTexture->get_y_size(), m_normalDepthTexture->get_x_size()); From cc1b82c574712720b007d4c2c01b09b17584c513 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 28 Aug 2024 20:46:27 +0200 Subject: [PATCH 11/25] remove merge tag in render set, add missing include directive --- modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp | 1 - modules/tracker/me/src/moving-edges/vpMeSite.cpp | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp index 26e2113557..7b3600e9a1 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp @@ -216,5 +216,4 @@ void vpPanda3DRendererSet::enableSharedDepthBuffer(vpPanda3DBaseRenderer &source // Work around to avoid warning: libvisp_ar.a(vpPanda3DRendererSet.cpp.o) has no symbols void dummy_vpPanda3DRendererSet() { }; ->>>>>>> upstream/master #endif diff --git a/modules/tracker/me/src/moving-edges/vpMeSite.cpp b/modules/tracker/me/src/moving-edges/vpMeSite.cpp index 990e386d5e..ba4a9473e5 100644 --- a/modules/tracker/me/src/moving-edges/vpMeSite.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeSite.cpp @@ -39,6 +39,7 @@ #include // std::fabs #include // numeric_limits #include +#include #include #include #include From c8bb06a36e9b36b12d7842753b0959e6c5866853 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 28 Aug 2024 21:19:00 +0200 Subject: [PATCH 12/25] clean old comment, add yet another missing header include directive --- .../panda3d-simulator/vpPanda3DRendererSet.cpp | 15 --------------- modules/tracker/me/src/moving-edges/vpMeSite.cpp | 1 + 2 files changed, 1 insertion(+), 15 deletions(-) diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp index 7b3600e9a1..8b6b4bb3b4 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp @@ -46,21 +46,6 @@ vpPanda3DRendererSet::vpPanda3DRendererSet(const vpPanda3DRenderParameters &rend void vpPanda3DRendererSet::initFramework() { - // load_prc_file_data("", "load-display p3tinydisplay"); - // load_prc_file_data("", "color-bits 32 32 32"); - - // load_prc_file_data("", "auto-flip 1"); - // load_prc_file_data("", "sync-video 0"); - // load_prc_file_data("", "multisamples 1\n" - // // "background-color 0.0 0.0 0.0 0.0\n" - // // "load-file-type p3assimp\n" - // // "transform-cache 0\n" - // // "state-cache 0\n" - // // "audio-library-name null\n" - // "model-cache-dir\n" - // "notify-level-glgsg spam"); - - if (m_framework.use_count() > 0) { throw vpException(vpException::notImplementedError, "Panda3D renderer: Reinitializing is not supported!"); } diff --git a/modules/tracker/me/src/moving-edges/vpMeSite.cpp b/modules/tracker/me/src/moving-edges/vpMeSite.cpp index ba4a9473e5..f7d4477afb 100644 --- a/modules/tracker/me/src/moving-edges/vpMeSite.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeSite.cpp @@ -40,6 +40,7 @@ #include // numeric_limits #include #include +#include #include #include #include From 1c653dfff646778bf188e331354facbe4a2b9b5f Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Thu, 29 Aug 2024 13:05:33 +0200 Subject: [PATCH 13/25] Remove tuple usage to be C++98 conforming --- .../tracker/me/src/moving-edges/vpMeSite.cpp | 31 +++++++++++++------ 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/modules/tracker/me/src/moving-edges/vpMeSite.cpp b/modules/tracker/me/src/moving-edges/vpMeSite.cpp index f7d4477afb..b0ddf15af9 100644 --- a/modules/tracker/me/src/moving-edges/vpMeSite.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeSite.cpp @@ -40,7 +40,7 @@ #include // numeric_limits #include #include -#include + #include #include #include @@ -373,13 +373,23 @@ std::vector &outputHypotheses, const unsigned numCandidates) vpMeSite *list_query_pixels = getQueryList(I, static_cast(range)); + struct vpMeSiteHypothesis + { + vpMeSiteHypothesis(vpMeSite *site, double l, double c) : site(site), likelihood(l), contrast(c) + { } + + vpMeSite *site; + double likelihood; + double contrast; + + }; // Insert into a map, where the key is the sorting criterion (negative likelihood or contrast diff) // and the key is the ME site + its computed likelihood and contrast. // After computation: iterating on the map is guaranteed to be done with the keys being sorted according to the criterion. // Multimap allows to have multiple values (sites) with the same key (likelihood/contrast diff) // Only the candidates that are above the threshold are kept - std::multimap> candidates; + std::multimap candidates; const double contrast_max = 1 + me.getMu2(); const double contrast_min = 1 - me.getMu1(); @@ -399,7 +409,7 @@ std::vector &outputHypotheses, const unsigned numCandidates) query.m_convlt = convolution_; const double contrast = convolution_ / m_convlt; - candidates.insert({ fabs(1.0 - contrast), {&query, likelihood, contrast} }); + candidates.insert({ fabs(1.0 - contrast), vpMeSiteHypothesis(&query, likelihood, contrast) }); } } else { // test on likelihood only @@ -409,19 +419,20 @@ std::vector &outputHypotheses, const unsigned numCandidates) const double convolution_ = query.convolution(I, &me); const double likelihood = fabs(2 * convolution_); query.m_convlt = convolution_; - candidates.insert({ -likelihood, {&query, likelihood, 0.0} }); + candidates.insert({ -likelihood, vpMeSiteHypothesis(&query, likelihood, 0.0) }); } } // Take first numCandidates hypotheses: map is sorted according to the likelihood/contrast difference so we can just // iterate from the start outputHypotheses.resize(numCandidates); - std::multimap>::iterator it = candidates.begin(); + + std::multimap::iterator it = candidates.begin(); if (test_contrast) { for (unsigned int i = 0; i < numCandidates; ++i, ++it) { - outputHypotheses[i] = *(std::get<0>(it->second)); + outputHypotheses[i] = *(it->second.site); outputHypotheses[i].m_normGradient = vpMath::sqr(outputHypotheses[i].m_convlt); - const double likelihood = std::get<1>(it->second); - const double contrast = std::get<2>(it->second); + const double likelihood = it->second.likelihood; + const double contrast = it->second.contrast; if (likelihood > threshold) { if (contrast <= contrast_min || contrast >= contrast_max) { @@ -438,8 +449,8 @@ std::vector &outputHypotheses, const unsigned numCandidates) } else { for (unsigned int i = 0; i < numCandidates; ++i, ++it) { - outputHypotheses[i] = *(std::get<0>(it->second)); - const double likelihood = std::get<1>(it->second); + outputHypotheses[i] = *(it->second.site); + const double likelihood = it->second.likelihood; if (likelihood > threshold) { outputHypotheses[i].m_state = vpMeSiteState::NO_SUPPRESSION; } From 3accaa38df6dfbbb86f8e75daaee098be746a81f Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Thu, 29 Aug 2024 13:05:51 +0200 Subject: [PATCH 14/25] Added Display grid functionality --- .../gui/include/visp3/gui/vpDisplayFactory.h | 88 +++++++++++++++++++ 1 file changed, 88 insertions(+) diff --git a/modules/gui/include/visp3/gui/vpDisplayFactory.h b/modules/gui/include/visp3/gui/vpDisplayFactory.h index 16c68f3e10..6456817c06 100644 --- a/modules/gui/include/visp3/gui/vpDisplayFactory.h +++ b/modules/gui/include/visp3/gui/vpDisplayFactory.h @@ -202,8 +202,96 @@ std::shared_ptr createDisplay(vpImage &I, const int winx = -1, con return std::shared_ptr(nullptr); #endif } +namespace impl +{ + +struct GridSettings +{ + unsigned int rows; + unsigned int cols; + unsigned int startY; + unsigned int startX; + unsigned int paddingX; + unsigned int paddingY; +}; + +void makeDisplayGridHelper(std::vector> &res, const GridSettings &settings, +unsigned int currRow, unsigned int currCol, +unsigned int currentPixelX, unsigned int currentPixelY, +unsigned int maxRowHeightPixel) +{ + if (currRow != settings.rows -1 && currCol != settings.cols - 1) { + throw vpException(vpException::dimensionError, "Too few images for the grid size"); + } + +} + +template +void makeDisplayGridHelper(std::vector> &res, +const GridSettings &settings, +unsigned int currRow, unsigned int currCol, +unsigned int currentPixelX, unsigned int currentPixelY, +const unsigned int maxRowHeightPixel, +const std::string &name, vpImage &I, Args&... args) +{ + if (currRow >= settings.rows) { + throw vpException(vpException::dimensionError, "Too many images for the grid size"); + } + if (currCol == settings.cols) { + makeDisplayGridHelper(res, settings, currRow + 1, 0, settings.startX, currentPixelY + maxRowHeightPixel + settings.paddingY, 0, name, I, args...); + } + else { + std::shared_ptr display = vpDisplayFactory::createDisplay(I, currentPixelX, currentPixelY, name); + vpDisplay::display(I); + vpDisplay::flush(I); + res.push_back(display); + makeDisplayGridHelper(res, settings, currRow, currCol + 1, currentPixelX + I.getWidth() + settings.paddingX, currentPixelY, std::max(maxRowHeightPixel, I.getHeight()), args...); + } +} + +} + +/** + * \brief Create a grid of displays, given a set of images. + * All the displays will be initialized in the correct location with the content of the associated image and name. + * All the images should have been initialized before with the correct resolution. + * The display creation and image association will follow a row major order. + * + * \tparam Args A sequence of display name (const std::string&) and ViSP image. + * The name should always come before the image. The image can be vpImage or vpImage + * \param rows Number of rows in the grid + * \param cols Number of columns in the grid + * \param startX the starting left position of the grid + * \param startY the starting top localization of the grid + * \param paddingX Horizontal padding between windows + * \param paddingY Vertical padding between windows + * \param args the name => image => name sequence + * \return std::vector> The allocated displays. + * + * \throws if the grid dimensions and number of images do not match + * + */ +template +std::vector> makeDisplayGrid(unsigned int rows, unsigned int cols, +unsigned int startX, unsigned int startY, +unsigned int paddingX, unsigned int paddingY, +Args&... args) +{ + std::vector> res; + impl::GridSettings settings; + settings.rows = rows; + settings.cols = cols; + settings.paddingX = paddingX; + settings.paddingY = paddingY; + settings.startX = startX; + settings.startY = startY; + makeDisplayGridHelper(res, settings, 0, 0, settings.startX, settings.startY, 0, args...); + return res; +} + #endif } + END_VISP_NAMESPACE #endif From 1d438a78e79b66e32b0e16447e449dc8aebf8624 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Thu, 29 Aug 2024 14:58:06 +0200 Subject: [PATCH 15/25] Move local struct outside of function --- .../tracker/me/src/moving-edges/vpMeSite.cpp | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/modules/tracker/me/src/moving-edges/vpMeSite.cpp b/modules/tracker/me/src/moving-edges/vpMeSite.cpp index b0ddf15af9..ac779cb8b4 100644 --- a/modules/tracker/me/src/moving-edges/vpMeSite.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeSite.cpp @@ -48,6 +48,18 @@ BEGIN_VISP_NAMESPACE #ifndef DOXYGEN_SHOULD_SKIP_THIS + +struct vpMeSiteHypothesis +{ + vpMeSiteHypothesis(vpMeSite *site, double l, double c) : site(site), likelihood(l), contrast(c) + { } + + vpMeSite *site; + double likelihood; + double contrast; + +}; + static bool horsImage(int i, int j, int half, int rows, int cols) { int half_1 = half + 1; @@ -373,17 +385,6 @@ std::vector &outputHypotheses, const unsigned numCandidates) vpMeSite *list_query_pixels = getQueryList(I, static_cast(range)); - struct vpMeSiteHypothesis - { - vpMeSiteHypothesis(vpMeSite *site, double l, double c) : site(site), likelihood(l), contrast(c) - { } - - vpMeSite *site; - double likelihood; - double contrast; - - }; - // Insert into a map, where the key is the sorting criterion (negative likelihood or contrast diff) // and the key is the ME site + its computed likelihood and contrast. // After computation: iterating on the map is guaranteed to be done with the keys being sorted according to the criterion. From 8e71f15843da97bef77b84e3ef71562a080661da Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Thu, 29 Aug 2024 15:34:56 +0200 Subject: [PATCH 16/25] Further fixes for C++98 --- modules/tracker/me/src/moving-edges/vpMeSite.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/modules/tracker/me/src/moving-edges/vpMeSite.cpp b/modules/tracker/me/src/moving-edges/vpMeSite.cpp index ac779cb8b4..7b1e230ac9 100644 --- a/modules/tracker/me/src/moving-edges/vpMeSite.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeSite.cpp @@ -410,7 +410,7 @@ std::vector &outputHypotheses, const unsigned numCandidates) query.m_convlt = convolution_; const double contrast = convolution_ / m_convlt; - candidates.insert({ fabs(1.0 - contrast), vpMeSiteHypothesis(&query, likelihood, contrast) }); + candidates.insert(std::pair(fabs(1.0 - contrast), vpMeSiteHypothesis(&query, likelihood, contrast))); } } else { // test on likelihood only @@ -420,7 +420,7 @@ std::vector &outputHypotheses, const unsigned numCandidates) const double convolution_ = query.convolution(I, &me); const double likelihood = fabs(2 * convolution_); query.m_convlt = convolution_; - candidates.insert({ -likelihood, vpMeSiteHypothesis(&query, likelihood, 0.0) }); + candidates.insert(std::pair(-likelihood, vpMeSiteHypothesis(&query, likelihood, 0.0))); } } // Take first numCandidates hypotheses: map is sorted according to the likelihood/contrast difference so we can just @@ -437,14 +437,14 @@ std::vector &outputHypotheses, const unsigned numCandidates) if (likelihood > threshold) { if (contrast <= contrast_min || contrast >= contrast_max) { - outputHypotheses[i].m_state = vpMeSiteState::CONTRAST; + outputHypotheses[i].m_state = CONTRAST; } else { - outputHypotheses[i].m_state = vpMeSiteState::NO_SUPPRESSION; + outputHypotheses[i].m_state = NO_SUPPRESSION; } } else { - outputHypotheses[i].m_state = vpMeSiteState::THRESHOLD; + outputHypotheses[i].m_state = THRESHOLD; } } } @@ -453,10 +453,10 @@ std::vector &outputHypotheses, const unsigned numCandidates) outputHypotheses[i] = *(it->second.site); const double likelihood = it->second.likelihood; if (likelihood > threshold) { - outputHypotheses[i].m_state = vpMeSiteState::NO_SUPPRESSION; + outputHypotheses[i].m_state = NO_SUPPRESSION; } else { - outputHypotheses[i].m_state = vpMeSiteState::THRESHOLD; + outputHypotheses[i].m_state = THRESHOLD; } } } From 9378dffee77ce278f0576087e77af8a3f46d3a4b Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Fri, 30 Aug 2024 12:54:50 +0200 Subject: [PATCH 17/25] Add move assignment operator, fixed the copy operator using pass by value, resulting in an uneeded copy --- modules/core/include/visp3/core/vpImage.h | 6 ++- .../include/visp3/core/vpImage_operators.h | 43 ++++++++++++++++++- 2 files changed, 46 insertions(+), 3 deletions(-) diff --git a/modules/core/include/visp3/core/vpImage.h b/modules/core/include/visp3/core/vpImage.h index 5cedde9c8e..20f678c217 100644 --- a/modules/core/include/visp3/core/vpImage.h +++ b/modules/core/include/visp3/core/vpImage.h @@ -314,7 +314,11 @@ template class vpImage vpImage operator-(const vpImage &B) const; //! Copy operator - vpImage &operator=(vpImage other); + vpImage &operator=(const vpImage &other); +#if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher +//! move constructor + vpImage &operator=(vpImage &&other); +#endif vpImage &operator=(const Type &v); bool operator==(const vpImage &I) const; diff --git a/modules/core/include/visp3/core/vpImage_operators.h b/modules/core/include/visp3/core/vpImage_operators.h index fbddbdd292..050848ed08 100644 --- a/modules/core/include/visp3/core/vpImage_operators.h +++ b/modules/core/include/visp3/core/vpImage_operators.h @@ -182,15 +182,54 @@ inline std::ostream &operator<<(std::ostream &s, const vpImage &I) /*! \brief Copy operator */ -template vpImage &vpImage::operator=(vpImage other) +template vpImage &vpImage::operator=(const vpImage &other) { - swap(*this, other); + + resize(other.height, other.width); + memcpy(static_cast(bitmap), static_cast(other.bitmap), other.npixels * sizeof(Type)); + if (other.display != nullptr) { + display = other.display; + } + height = other.height; + width = other.width; + npixels = other.npixels; + hasOwnership = other.hasOwnership; + + return *this; +} + +#if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher + +template vpImage &vpImage::operator=(vpImage &&other) +{ + + if (row != nullptr) { + delete[] row; + row = other.row; + } + if (bitmap != nullptr && hasOwnership) { + delete[] bitmap; + } + bitmap = other.bitmap; if (other.display != nullptr) { display = other.display; } + height = other.height; + width = other.width; + npixels = other.npixels; + hasOwnership = other.hasOwnership; + + other.bitmap = nullptr; + other.display = nullptr; + other.npixels = 0; + other.width = 0; + other.height = 0; + other.row = nullptr; + other.hasOwnership = false; return *this; } +#endif /*! \brief = operator : Set all the element of the bitmap to a given value \e From 62debb68e26005f9333683e4aa484df4f4d9d162 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Fri, 30 Aug 2024 14:08:16 +0200 Subject: [PATCH 18/25] Fix move operator --- modules/core/include/visp3/core/vpImage_operators.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/core/include/visp3/core/vpImage_operators.h b/modules/core/include/visp3/core/vpImage_operators.h index 050848ed08..f6ff9824ae 100644 --- a/modules/core/include/visp3/core/vpImage_operators.h +++ b/modules/core/include/visp3/core/vpImage_operators.h @@ -195,6 +195,7 @@ template vpImage &vpImage::operator=(const vpImage vpImage &vpImage::operator=(vpImage &&ot if (row != nullptr) { delete[] row; - row = other.row; } + row = other.row; if (bitmap != nullptr && hasOwnership) { delete[] bitmap; } From 7dc52e39d0edc30c65bcc01f4366ddbd7dc13b98 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Fri, 30 Aug 2024 14:17:41 +0200 Subject: [PATCH 19/25] Fix potential ownership issue on assignement operator with const ref --- modules/core/include/visp3/core/vpImage_operators.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/modules/core/include/visp3/core/vpImage_operators.h b/modules/core/include/visp3/core/vpImage_operators.h index f6ff9824ae..be9ae68025 100644 --- a/modules/core/include/visp3/core/vpImage_operators.h +++ b/modules/core/include/visp3/core/vpImage_operators.h @@ -190,11 +190,6 @@ template vpImage &vpImage::operator=(const vpImage Date: Fri, 30 Aug 2024 15:20:01 +0200 Subject: [PATCH 20/25] Introduce flag in JSON argument parser --- .../include/visp3/io/vpJsonArgumentParser.h | 21 ++++- modules/io/src/tools/vpJsonArgumentParser.cpp | 76 +++++++++++++++++-- modules/io/test/testJsonArgumentParser.cpp | 59 ++++++++------ 3 files changed, 126 insertions(+), 30 deletions(-) diff --git a/modules/io/include/visp3/io/vpJsonArgumentParser.h b/modules/io/include/visp3/io/vpJsonArgumentParser.h index 9a3db027b2..61defd5949 100644 --- a/modules/io/include/visp3/io/vpJsonArgumentParser.h +++ b/modules/io/include/visp3/io/vpJsonArgumentParser.h @@ -136,6 +136,13 @@ BEGIN_VISP_NAMESPACE class VISP_EXPORT vpJsonArgumentParser { public: + + enum vpJsonArgumentType + { + WITH_FIELD = 0, + FLAG = 1 + }; + /** * @brief Create a new argument parser, that can take into account both a JSON configuration file and command line arguments. * @@ -181,7 +188,8 @@ class VISP_EXPORT vpJsonArgumentParser template vpJsonArgumentParser &addArgument(const std::string &name, T ¶meter, const bool required = true, const std::string &help = "No description") { - const auto getter = [name, this](nlohmann::json &j, bool create) -> nlohmann::json * { + argumentType[name] = WITH_FIELD; + const auto getter = [name, this](nlohmann::json &j, bool create) -> nlohmann::json *{ size_t pos = 0; nlohmann::json *f = &j; std::string token; @@ -246,6 +254,16 @@ class VISP_EXPORT vpJsonArgumentParser return *this; } + /** + * @brief Add an argument that acts as a flag when specified on the command line. When this flag is specified, the boolean passed in argument will be inverted. + * + * @param name Name of the flag. + * @param parameter The boolean to modify when the flag is specified + * @param help The description of the argument. + * @return vpJsonArgumentParser& returns self, allowing argument definition chaining + */ + vpJsonArgumentParser &addFlag(const std::string &name, bool ¶meter, const std::string &help = "No description"); + /** * @brief Parse the arguments. * @@ -260,6 +278,7 @@ class VISP_EXPORT vpJsonArgumentParser std::string jsonFileArgumentName; // Name of the argument that points to the json file: ./program --config settings.json. Here jsonFileArgumentName == "--config" std::string nestSeparator; // JSON nesting delimiter character. Used to access JSON nested objects from a single string std::map> parsers; // Functions that update the variables with the values contained in the JSON document (should be used after calling updaters) + std::map argumentType; // Update the base json document with command line arguments std::map> updaters; // Update the base json document with command line arguments std::map> helpers; // Functions that output the usage and description of command line arguments: used when the help flag is given as argument nlohmann::json exampleJson; // Example JSON argument file: displayed when user calls for help diff --git a/modules/io/src/tools/vpJsonArgumentParser.cpp b/modules/io/src/tools/vpJsonArgumentParser.cpp index bfe68f06c2..a1ca1efd86 100644 --- a/modules/io/src/tools/vpJsonArgumentParser.cpp +++ b/modules/io/src/tools/vpJsonArgumentParser.cpp @@ -94,6 +94,63 @@ std::string vpJsonArgumentParser::help() const return ss.str(); } +vpJsonArgumentParser &vpJsonArgumentParser::addFlag(const std::string &name, bool ¶meter, const std::string &help) +{ + argumentType[name] = FLAG; + const auto getter = [name, this](nlohmann::json &j, bool create) -> nlohmann::json *{ + size_t pos = 0; + nlohmann::json *f = &j; + std::string token; + std::string name_copy = name; + + while ((pos = name_copy.find(nestSeparator)) != std::string::npos) { + token = name_copy.substr(0, pos); + + name_copy.erase(0, pos + nestSeparator.length()); + if (create && !f->contains(token)) { + (*f)[token] = {}; + } + else if (!f->contains(token)) { + return nullptr; + } + f = &(f->at(token)); + } + if (create && !f->contains(name_copy)) { + (*f)[name_copy] = {}; + } + else if (!f->contains(name_copy)) { + return nullptr; + } + f = &(f->at(name_copy)); + return f; + }; + + parsers[name] = [¶meter, getter, name](nlohmann::json &j) { + const nlohmann::json *field = getter(j, false); + const bool fieldHasNoValue = field == nullptr || (field != nullptr && field->is_null()); + if (!fieldHasNoValue && field->type() == json::value_t::boolean && (*field) == true) { + parameter = !parameter; + } + }; + + updaters[name] = [getter, parameter](nlohmann::json &j, const std::string &) { + nlohmann::json *field = getter(j, true); + *field = true; + }; + + helpers[name] = [help, parameter]() -> std::string { + std::stringstream ss; + nlohmann::json repr = parameter; + ss << help << std::endl << "Default: " << repr; + return ss.str(); + }; + + nlohmann::json *exampleField = getter(exampleJson, true); + *exampleField = parameter; + + return *this; +} + void vpJsonArgumentParser::parse(int argc, const char *argv[]) { json j; @@ -132,14 +189,19 @@ void vpJsonArgumentParser::parse(int argc, const char *argv[]) } if (parsers.find(arg) != parsers.end()) { - if (i < argc - 1) { - updaters[arg](j, std::string(argv[i + 1])); - ++i; + if (argumentType[arg] == WITH_FIELD) { + if (i < argc - 1) { + updaters[arg](j, std::string(argv[i + 1])); + ++i; + } + else { + std::stringstream ss; + ss << "Argument " << arg << " was passed but no value was provided" << std::endl; + throw vpException(vpException::ioError, ss.str()); + } } - else { - std::stringstream ss; - ss << "Argument " << arg << " was passed but no value was provided" << std::endl; - throw vpException(vpException::ioError, ss.str()); + else if (argumentType[arg] == FLAG) { + updaters[arg](j, std::string()); } } else { diff --git a/modules/io/test/testJsonArgumentParser.cpp b/modules/io/test/testJsonArgumentParser.cpp index 9678b4b205..02735d197b 100644 --- a/modules/io/test/testJsonArgumentParser.cpp +++ b/modules/io/test/testJsonArgumentParser.cpp @@ -118,9 +118,10 @@ SCENARIO("Parsing arguments from JSON file", "[json]") {"b", 2.0}, {"c", "a string"}, {"d", true}, - {"e", { - {"a", 5} - }} + {"flag", true}, + {"flagDefaultTrue", true}, + {"e", {{"a", 5} } + } }; saveJson(j, jsonPath); @@ -129,6 +130,10 @@ SCENARIO("Parsing arguments from JSON file", "[json]") std::string c = ""; bool d = false; int ea = 4; + bool flag = false; + bool flagInitialValue = flag; + + bool invertedFlag = true; WHEN("Declaring a parser with all parameters required") { vpJsonArgumentParser parser("A program", "--config", "/"); @@ -136,7 +141,9 @@ SCENARIO("Parsing arguments from JSON file", "[json]") .addArgument("b", b, true) .addArgument("c", c, true) .addArgument("d", d, true) - .addArgument("e/a", ea, true); + .addArgument("e/a", ea, true) + .addFlag("flag", flag) + .addFlag("flagDefaultTrue", invertedFlag); THEN("Calling the parser without any argument fails") { @@ -162,7 +169,8 @@ SCENARIO("Parsing arguments from JSON file", "[json]") REQUIRE(c == j["c"]); REQUIRE(d == j["d"]); REQUIRE(ea == j["e"]["a"]); - + REQUIRE(flag != flagInitialValue); + REQUIRE(invertedFlag != true); } THEN("Calling the parser by specifying the json argument but leaving the file path empty throws an error") { @@ -177,26 +185,30 @@ SCENARIO("Parsing arguments from JSON file", "[json]") { const int argc = 3; for (const auto &jsonElem : j.items()) { - modifyJson([&jsonElem](json &j) { j.erase(jsonElem.key()); }); - const char *argv[] = { - "program", - "--config", - jsonPath.c_str() - }; - REQUIRE_THROWS(parser.parse(argc, argv)); + if (jsonElem.key().rfind("flag", 0) != 0) { + modifyJson([&jsonElem](json &j) { j.erase(jsonElem.key()); }); + const char *argv[] = { + "program", + "--config", + jsonPath.c_str() + }; + REQUIRE_THROWS(parser.parse(argc, argv)); + } } } THEN("Calling the parser with only the json file but setting a random field to null throws an error") { const int argc = 3; for (const auto &jsonElem : j.items()) { - modifyJson([&jsonElem](json &j) { j[jsonElem.key()] = nullptr; }); - const char *argv[] = { - "program", - "--config", - jsonPath.c_str() - }; - REQUIRE_THROWS(parser.parse(argc, argv)); + if (jsonElem.key().rfind("flag", 0) != 0) { + modifyJson([&jsonElem](json &j) { j[jsonElem.key()] = nullptr; }); + const char *argv[] = { + "program", + "--config", + jsonPath.c_str() + }; + REQUIRE_THROWS(parser.parse(argc, argv)); + } } } THEN("Calling the parser with an invalid json file path throws an error") @@ -223,7 +235,8 @@ SCENARIO("Parsing arguments from JSON file", "[json]") "b", std::to_string(newb), "c", newc, "d", newdstr, - "e/a", std::to_string(newea) + "e/a", std::to_string(newea), + "flag" }; int argc; std::vector argv; @@ -234,6 +247,7 @@ SCENARIO("Parsing arguments from JSON file", "[json]") REQUIRE(c == newc); REQUIRE(d == newd); REQUIRE(ea == newea); + REQUIRE(flag != flagInitialValue); } THEN("Calling the parser with JSON and command line argument works") @@ -244,7 +258,8 @@ SCENARIO("Parsing arguments from JSON file", "[json]") "program", "--config", jsonPath, "a", std::to_string(newa), - "b", std::to_string(newb) + "b", std::to_string(newb), + "flagDefaultTrue" }; int argc; std::vector argv; @@ -255,7 +270,7 @@ SCENARIO("Parsing arguments from JSON file", "[json]") REQUIRE(c == j["c"]); REQUIRE(d == j["d"]); REQUIRE(ea == j["e"]["a"]); - + REQUIRE(invertedFlag == false); } THEN("Calling the parser with a missing argument value throws an error") { From 51c7ccafcb96d74447fb5f302eb8d40177c1d694 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 3 Sep 2024 08:47:28 +0200 Subject: [PATCH 21/25] Clean code by removing empty lines and code indentation --- .../ar/include/visp3/ar/vpPanda3DBaseRenderer.h | 5 ----- .../include/visp3/ar/vpPanda3DPostProcessFilter.h | 2 -- modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h | 4 ---- modules/ar/include/visp3/ar/vpPanda3DRendererSet.h | 3 ++- .../panda3d-simulator/vpPanda3DBaseRenderer.cpp | 7 ------- .../vpPanda3DGeometryRenderer.cpp | 13 ++++--------- .../src/panda3d-simulator/vpPanda3DRendererSet.cpp | 3 --- modules/core/include/visp3/core/vpImage.h | 2 +- modules/core/include/visp3/core/vpImageFilter.h | 4 ++-- modules/gui/include/visp3/gui/vpDisplayFactory.h | 14 +++++--------- modules/io/include/visp3/io/vpJsonArgumentParser.h | 8 ++++---- .../generator/visp_python_bindgen/methods.py | 5 ----- modules/tracker/mbt/src/vpMbGenericTracker.cpp | 2 +- modules/tracker/me/include/visp3/me/vpMeSite.h | 3 ++- modules/tracker/me/src/moving-edges/vpMeSite.cpp | 1 - tutorial/ar/tutorial-panda3d-renderer.cpp | 2 -- 16 files changed, 21 insertions(+), 57 deletions(-) diff --git a/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h index d72c12255f..d0844a4625 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h @@ -242,11 +242,8 @@ class VISP_EXPORT vpPanda3DBaseRenderer virtual void enableSharedDepthBuffer(vpPanda3DBaseRenderer &sourceBuffer); - - protected: - /** * @brief Initialize the scene for this specific renderer. * @@ -267,11 +264,9 @@ 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: 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() diff --git a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h index 4eb1d3ef7b..abb27399ff 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h +++ b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h @@ -82,7 +82,6 @@ class VISP_EXPORT vpPanda3DPostProcessFilter : public vpPanda3DBaseRenderer } } - protected: virtual void setupScene() VP_OVERRIDE; @@ -95,7 +94,6 @@ class VISP_EXPORT vpPanda3DPostProcessFilter : public vpPanda3DBaseRenderer void getRenderBasic(vpImage &I) const; void getRenderBasic(vpImage &I) const; - virtual FrameBufferProperties getBufferProperties() const = 0; std::shared_ptr m_inputRenderer; diff --git a/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h index 43c4ae546b..06789092a3 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h @@ -78,7 +78,6 @@ class VISP_EXPORT vpPanda3DRGBRenderer : public vpPanda3DBaseRenderer, public vp */ vpPanda3DRGBRenderer(bool showSpeculars) : vpPanda3DBaseRenderer(showSpeculars ? "RGB" : "RGB-diffuse"), m_showSpeculars(showSpeculars) { } - /** * @brief Store the render resulting from calling renderFrame() into a vpImage. * @@ -96,9 +95,7 @@ class VISP_EXPORT vpPanda3DRGBRenderer : public vpPanda3DBaseRenderer, public vp bool isShowingSpeculars() const { return m_showSpeculars; } - protected: - void setupScene() VP_OVERRIDE; void setupRenderTarget() VP_OVERRIDE; @@ -114,7 +111,6 @@ class VISP_EXPORT vpPanda3DRGBRenderer : public vpPanda3DBaseRenderer, public vp NodePath m_backgroundImage; DisplayRegion *m_display2d; Texture *m_backgroundTexture; - }; END_VISP_NAMESPACE diff --git a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h index e46d55feda..49098952cf 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h @@ -151,7 +151,6 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp void enableSharedDepthBuffer(vpPanda3DBaseRenderer &sourceBuffer) VP_OVERRIDE; - /** * @brief Retrieve the first subrenderer with the specified template type. * @@ -169,6 +168,7 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp } return nullptr; } + /** * @brief Retrieve the subrenderer with the specified template type and the given name. * @@ -196,6 +196,7 @@ class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vp renderer->beforeFrameRendered(); } } + void afterFrameRendered() VP_OVERRIDE { for (std::shared_ptr &renderer: m_subRenderers) { diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp index 5dfa155c1d..948d999f7a 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp @@ -48,9 +48,6 @@ const vpHomogeneousMatrix vpPanda3DBaseRenderer::VISP_T_PANDA({ }); const vpHomogeneousMatrix vpPanda3DBaseRenderer::PANDA_T_VISP(vpPanda3DBaseRenderer::VISP_T_PANDA.inverse()); - - - void vpPanda3DBaseRenderer::initFramework() { if (m_framework.use_count() > 0) { @@ -93,7 +90,6 @@ void vpPanda3DBaseRenderer::initFromParent(const vpPanda3DBaseRenderer &renderer initFromParent(renderer.m_framework, renderer.m_window); } - void vpPanda3DBaseRenderer::setupScene() { m_renderRoot = m_window->get_render().attach_new_node(m_name); @@ -185,7 +181,6 @@ vpHomogeneousMatrix vpPanda3DBaseRenderer::getNodePose(const std::string &name) vpHomogeneousMatrix vpPanda3DBaseRenderer::getNodePose(NodePath &object) { - const LPoint3 pos = object.get_pos(); const LQuaternion quat = object.get_quat(); const vpTranslationVector t(pos[0], pos[1], pos[2]); @@ -227,7 +222,6 @@ void vpPanda3DBaseRenderer::computeNearAndFarPlanesFromNode(const std::string &n nearV = minZ; farV = maxZ; - } else { const BoundingVolume *volume = object.node()->get_bounds(); @@ -283,7 +277,6 @@ NodePath vpPanda3DBaseRenderer::loadObject(const std::string &nodeName, const st NodePath model = m_window->load_model(m_framework->get_models(), modelPath); for (int i = 0; i < model.get_num_children(); ++i) { model.get_child(i).clear_transform(); - } model.detach_node(); diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp index 02a739fafb..41edf3e0f0 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp @@ -34,7 +34,6 @@ #include - BEGIN_VISP_NAMESPACE const char *vpPanda3DGeometryRenderer::SHADER_VERT_NORMAL_AND_DEPTH_CAMERA = R"shader( #version 330 @@ -70,7 +69,6 @@ uniform mat4 p3d_ModelViewProjectionMatrix; out vec3 oNormal; out float distToCamera; - void main() { gl_Position = p3d_ModelViewProjectionMatrix * p3d_Vertex; @@ -114,13 +112,13 @@ void vpPanda3DGeometryRenderer::setupScene() PT(Shader) shader; if (m_renderType == OBJECT_NORMALS) { shader = Shader::make(Shader::ShaderLanguage::SL_GLSL, - SHADER_VERT_NORMAL_AND_DEPTH_OBJECT, - SHADER_FRAG_NORMAL_AND_DEPTH); + SHADER_VERT_NORMAL_AND_DEPTH_OBJECT, + SHADER_FRAG_NORMAL_AND_DEPTH); } else if (m_renderType == CAMERA_NORMALS) { shader = Shader::make(Shader::ShaderLanguage::SL_GLSL, - SHADER_VERT_NORMAL_AND_DEPTH_CAMERA, - SHADER_FRAG_NORMAL_AND_DEPTH); + SHADER_VERT_NORMAL_AND_DEPTH_CAMERA, + SHADER_FRAG_NORMAL_AND_DEPTH); } m_renderRoot.set_shader(shader); } @@ -172,7 +170,6 @@ void vpPanda3DGeometryRenderer::setupRenderTarget() void vpPanda3DGeometryRenderer::getRender(vpImage &normals, vpImage &depth) const { - normals.resize(m_normalDepthTexture->get_y_size(), m_normalDepthTexture->get_x_size()); depth.resize(m_normalDepthTexture->get_y_size(), m_normalDepthTexture->get_x_size()); if (m_normalDepthTexture->get_component_type() != Texture::T_float) { @@ -197,7 +194,6 @@ void vpPanda3DGeometryRenderer::getRender(vpImage &normals, vpImage &normals, vpImage &depth, const vpRect &bb, unsigned int h, unsigned w) const { normals.resize(h, w); @@ -231,7 +227,6 @@ void vpPanda3DGeometryRenderer::getRender(vpImage &normals, vpImage &normals) const { normals.resize(m_normalDepthTexture->get_y_size(), m_normalDepthTexture->get_x_size()); diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp index 8b6b4bb3b4..a59b481812 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp @@ -43,7 +43,6 @@ vpPanda3DRendererSet::vpPanda3DRendererSet(const vpPanda3DRenderParameters &rend load_prc_file_data("", "no-singular-invert"); } - void vpPanda3DRendererSet::initFramework() { if (m_framework.use_count() > 0) { @@ -86,7 +85,6 @@ void vpPanda3DRendererSet::initFromParent(const vpPanda3DBaseRenderer &renderer) } } - void vpPanda3DRendererSet::setCameraPose(const vpHomogeneousMatrix &wTc) { for (std::shared_ptr &renderer: m_subRenderers) { @@ -196,7 +194,6 @@ void vpPanda3DRendererSet::enableSharedDepthBuffer(vpPanda3DBaseRenderer &source } } - #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_ar.a(vpPanda3DRendererSet.cpp.o) has no symbols void dummy_vpPanda3DRendererSet() { }; diff --git a/modules/core/include/visp3/core/vpImage.h b/modules/core/include/visp3/core/vpImage.h index 20f678c217..59177f746f 100644 --- a/modules/core/include/visp3/core/vpImage.h +++ b/modules/core/include/visp3/core/vpImage.h @@ -316,7 +316,7 @@ template class vpImage //! Copy operator vpImage &operator=(const vpImage &other); #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher -//! move constructor + //! move constructor vpImage &operator=(vpImage &&other); #endif diff --git a/modules/core/include/visp3/core/vpImageFilter.h b/modules/core/include/visp3/core/vpImageFilter.h index 758d6c34a5..66868f7e2b 100644 --- a/modules/core/include/visp3/core/vpImageFilter.h +++ b/modules/core/include/visp3/core/vpImageFilter.h @@ -566,8 +566,8 @@ class VISP_EXPORT vpImageFilter * * @tparam FilterType Image and filter types: double or float * @param I The input image - * @param row the row coordinate where the filter should be applied - * @param col the column coordinate where the filter shoud be applied + * @param row The row coordinate where the filter should be applied + * @param col The column coordinate where the filter should be applied * @param M the filter */ template diff --git a/modules/gui/include/visp3/gui/vpDisplayFactory.h b/modules/gui/include/visp3/gui/vpDisplayFactory.h index b9bd4434d4..beddf9418a 100644 --- a/modules/gui/include/visp3/gui/vpDisplayFactory.h +++ b/modules/gui/include/visp3/gui/vpDisplayFactory.h @@ -199,9 +199,9 @@ std::shared_ptr createDisplay(vpImage &I, const int winx = -1, con return std::shared_ptr(nullptr); #endif } + namespace impl { - struct GridSettings { unsigned int rows; @@ -220,7 +220,6 @@ unsigned int maxRowHeightPixel) if (currRow != settings.rows -1 && currCol != settings.cols - 1) { throw vpException(vpException::dimensionError, "Too few images for the grid size"); } - } template @@ -245,7 +244,6 @@ const std::string &name, vpImage &I, Args&... args) makeDisplayGridHelper(res, settings, currRow, currCol + 1, currentPixelX + I.getWidth() + settings.paddingX, currentPixelY, std::max(maxRowHeightPixel, I.getHeight()), args...); } } - } /** @@ -258,14 +256,14 @@ const std::string &name, vpImage &I, Args&... args) * The name should always come before the image. The image can be vpImage or vpImage * \param rows Number of rows in the grid * \param cols Number of columns in the grid - * \param startX the starting left position of the grid - * \param startY the starting top localization of the grid + * \param startX The starting left position of the grid + * \param startY The starting top localization of the grid * \param paddingX Horizontal padding between windows * \param paddingY Vertical padding between windows - * \param args the name => image => name sequence + * \param args The name => image => name sequence * \return std::vector> The allocated displays. * - * \throws if the grid dimensions and number of images do not match + * \throws If the grid dimensions and number of images do not match * */ template @@ -285,9 +283,7 @@ Args&... args) makeDisplayGridHelper(res, settings, 0, 0, settings.startX, settings.startY, 0, args...); return res; } - #endif - } END_VISP_NAMESPACE diff --git a/modules/io/include/visp3/io/vpJsonArgumentParser.h b/modules/io/include/visp3/io/vpJsonArgumentParser.h index 61defd5949..a95bfdf2a7 100644 --- a/modules/io/include/visp3/io/vpJsonArgumentParser.h +++ b/modules/io/include/visp3/io/vpJsonArgumentParser.h @@ -55,6 +55,7 @@ nlohmann::json convertCommandLineArgument(const std::string &arg) nlohmann::json j = nlohmann::json::parse(arg); return j; } + /** * @brief Specialization of command line parsing for strings: a shell may eat the quotes, which would be necessary for JSON parsing to work. * This function thus directly converts the string to a JSON representation: no parsing is performed. @@ -170,7 +171,6 @@ class VISP_EXPORT vpJsonArgumentParser */ std::string help() const; - /** * @brief Add an argument that can be provided by the user, either via command line or through the json file. * @@ -189,7 +189,7 @@ class VISP_EXPORT vpJsonArgumentParser vpJsonArgumentParser &addArgument(const std::string &name, T ¶meter, const bool required = true, const std::string &help = "No description") { argumentType[name] = WITH_FIELD; - const auto getter = [name, this](nlohmann::json &j, bool create) -> nlohmann::json *{ + const auto getter = [name, this](nlohmann::json &j, bool create) -> nlohmann::json * { size_t pos = 0; nlohmann::json *f = &j; std::string token; @@ -255,7 +255,8 @@ class VISP_EXPORT vpJsonArgumentParser } /** - * @brief Add an argument that acts as a flag when specified on the command line. When this flag is specified, the boolean passed in argument will be inverted. + * @brief Add an argument that acts as a flag when specified on the command line. + * When this flag is specified, the boolean passed in argument will be inverted. * * @param name Name of the flag. * @param parameter The boolean to modify when the flag is specified @@ -272,7 +273,6 @@ class VISP_EXPORT vpJsonArgumentParser */ void parse(int argc, const char *argv[]); - private: std::string description; // Program description std::string jsonFileArgumentName; // Name of the argument that points to the json file: ./program --config settings.json. Here jsonFileArgumentName == "--config" diff --git a/modules/python/generator/visp_python_bindgen/methods.py b/modules/python/generator/visp_python_bindgen/methods.py index e1362aeb32..5e3a064bf5 100644 --- a/modules/python/generator/visp_python_bindgen/methods.py +++ b/modules/python/generator/visp_python_bindgen/methods.py @@ -212,8 +212,6 @@ def parameter_can_have_default_value(parameter: types.Parameter, specs, env_mapp else: type_name = '' - - if GeneratorConfig.is_forbidden_default_argument_type(type_name): return False @@ -344,8 +342,6 @@ def make_keep_alive_str(values) -> str: else: pybind_options = [method_doc.documentation] + pybind_options - - # If a function has refs to immutable params, we need to return them. # Also true if user has specified input cpp params as output python params should_wrap_for_tuple_return = param_is_output is not None and any(param_is_output) @@ -364,7 +360,6 @@ def to_argument_name(type: str, name: str) -> str: else: return type + ' ' + name - params_with_names = [to_argument_name(t, name) for t, name in zip(input_param_types, input_param_names)] # Params that are only outputs: they should be declared in function. Assume that they are default constructible diff --git a/modules/tracker/mbt/src/vpMbGenericTracker.cpp b/modules/tracker/mbt/src/vpMbGenericTracker.cpp index 61e4218109..760296c065 100644 --- a/modules/tracker/mbt/src/vpMbGenericTracker.cpp +++ b/modules/tracker/mbt/src/vpMbGenericTracker.cpp @@ -253,7 +253,7 @@ double vpMbGenericTracker::computeCurrentProjectionError(const vpImage & const vpCameraParameters &_cam) { vpImage I; - vpImageConvert::convert(I_color, I); // FS: Shoudn't we use here m_I that was converted in track() ? + vpImageConvert::convert(I_color, I); // FS: Shouldn't we use here m_I that was converted in track() ? return computeCurrentProjectionError(I, _cMo, _cam); } diff --git a/modules/tracker/me/include/visp3/me/vpMeSite.h b/modules/tracker/me/include/visp3/me/vpMeSite.h index e09ee25ba1..fbff2e46d0 100644 --- a/modules/tracker/me/include/visp3/me/vpMeSite.h +++ b/modules/tracker/me/include/visp3/me/vpMeSite.h @@ -239,7 +239,8 @@ class VISP_EXPORT vpMeSite /*! * Similar to the \ref track function, but stores the best numCandidates hypotheses in \ref outputHypotheses. - * The best matching hypotheses (if it is not suppressed) is assigned to *this* and is stored as the first element of \ref outputHypotheses. + * The best matching hypotheses (if it is not suppressed) is assigned to *this* and is stored as the first + * element of \ref outputHypotheses. * The hypotheses are sorted from best to worst match in the vector. * A match may be in the vector but mark as suppressed. If this is undesired, you should filter them afterwards. * diff --git a/modules/tracker/me/src/moving-edges/vpMeSite.cpp b/modules/tracker/me/src/moving-edges/vpMeSite.cpp index 7b1e230ac9..cc252715c0 100644 --- a/modules/tracker/me/src/moving-edges/vpMeSite.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeSite.cpp @@ -57,7 +57,6 @@ struct vpMeSiteHypothesis vpMeSite *site; double likelihood; double contrast; - }; static bool horsImage(int i, int j, int half, int rows, int cols) diff --git a/tutorial/ar/tutorial-panda3d-renderer.cpp b/tutorial/ar/tutorial-panda3d-renderer.cpp index 46de94959a..4dc70df4a9 100644 --- a/tutorial/ar/tutorial-panda3d-renderer.cpp +++ b/tutorial/ar/tutorial-panda3d-renderer.cpp @@ -193,7 +193,6 @@ int main(int argc, const char **argv) renderer.initFramework(); //! [Adding subrenderers] - //! [Scene configuration] NodePath object = renderer.loadObject(objectName, modelPath); renderer.addNodeToScene(object); @@ -208,7 +207,6 @@ int main(int argc, const char **argv) renderer.addLight(dlight); //! [Scene configuration] - if (!backgroundPath.empty()) { vpImage background; vpImageIo::read(background, backgroundPath); From a4bec8dbc1253f2422be12fcc9912f0ac36d72ce Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 3 Sep 2024 08:57:02 +0200 Subject: [PATCH 22/25] Fix warning: lambda capture 'parameter' is not used --- modules/io/src/tools/vpJsonArgumentParser.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/io/src/tools/vpJsonArgumentParser.cpp b/modules/io/src/tools/vpJsonArgumentParser.cpp index a1ca1efd86..b8d5baea35 100644 --- a/modules/io/src/tools/vpJsonArgumentParser.cpp +++ b/modules/io/src/tools/vpJsonArgumentParser.cpp @@ -97,7 +97,7 @@ std::string vpJsonArgumentParser::help() const vpJsonArgumentParser &vpJsonArgumentParser::addFlag(const std::string &name, bool ¶meter, const std::string &help) { argumentType[name] = FLAG; - const auto getter = [name, this](nlohmann::json &j, bool create) -> nlohmann::json *{ + const auto getter = [name, this](nlohmann::json &j, bool create) -> nlohmann::json * { size_t pos = 0; nlohmann::json *f = &j; std::string token; @@ -127,13 +127,13 @@ vpJsonArgumentParser &vpJsonArgumentParser::addFlag(const std::string &name, boo parsers[name] = [¶meter, getter, name](nlohmann::json &j) { const nlohmann::json *field = getter(j, false); - const bool fieldHasNoValue = field == nullptr || (field != nullptr && field->is_null()); - if (!fieldHasNoValue && field->type() == json::value_t::boolean && (*field) == true) { + const bool fieldHasNoValue = ((field == nullptr) || (field != nullptr && field->is_null())); + if (!fieldHasNoValue && (field->type() == json::value_t::boolean && (*field) == true)) { parameter = !parameter; } }; - updaters[name] = [getter, parameter](nlohmann::json &j, const std::string &) { + updaters[name] = [getter](nlohmann::json &j, const std::string &) { nlohmann::json *field = getter(j, true); *field = true; }; From c0b228bc0e43e5129f87572234e919864ee7d11f Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 3 Sep 2024 09:09:06 +0200 Subject: [PATCH 23/25] Fix warning: unused vars --- .../gui/include/visp3/gui/vpDisplayFactory.h | 41 +++++++++++-------- 1 file changed, 25 insertions(+), 16 deletions(-) diff --git a/modules/gui/include/visp3/gui/vpDisplayFactory.h b/modules/gui/include/visp3/gui/vpDisplayFactory.h index beddf9418a..59585ed1c6 100644 --- a/modules/gui/include/visp3/gui/vpDisplayFactory.h +++ b/modules/gui/include/visp3/gui/vpDisplayFactory.h @@ -175,7 +175,8 @@ std::shared_ptr createDisplay() */ template std::shared_ptr createDisplay(vpImage &I, const int winx = -1, const int winy = -1, - const std::string &title = "", const vpDisplay::vpScaleType &scaleType = vpDisplay::SCALE_DEFAULT) + const std::string &title = "", + const vpDisplay::vpScaleType &scaleType = vpDisplay::SCALE_DEFAULT) { #if defined(VISP_HAVE_DISPLAY) #ifdef VISP_HAVE_X11 @@ -213,35 +214,43 @@ struct GridSettings }; void makeDisplayGridHelper(std::vector> &res, const GridSettings &settings, -unsigned int currRow, unsigned int currCol, -unsigned int currentPixelX, unsigned int currentPixelY, -unsigned int maxRowHeightPixel) + unsigned int currRow, unsigned int currCol, + unsigned int currentPixelX, unsigned int currentPixelY, + unsigned int maxRowHeightPixel) { - if (currRow != settings.rows -1 && currCol != settings.cols - 1) { + if (currRow != (settings.rows - 1) && (currCol != settings.cols - 1)) { throw vpException(vpException::dimensionError, "Too few images for the grid size"); } + (void)res; + (void)settings; + (void)currRow; + (void)currCol; + (void)currentPixelX; + (void)currentPixelY; + (void)maxRowHeightPixel; } template -void makeDisplayGridHelper(std::vector> &res, -const GridSettings &settings, -unsigned int currRow, unsigned int currCol, -unsigned int currentPixelX, unsigned int currentPixelY, -const unsigned int maxRowHeightPixel, -const std::string &name, vpImage &I, Args&... args) +void makeDisplayGridHelper(std::vector> &res, const GridSettings &settings, + unsigned int currRow, unsigned int currCol, + unsigned int currentPixelX, unsigned int currentPixelY, + const unsigned int maxRowHeightPixel, + const std::string &name, vpImage &I, Args&... args) { if (currRow >= settings.rows) { throw vpException(vpException::dimensionError, "Too many images for the grid size"); } if (currCol == settings.cols) { - makeDisplayGridHelper(res, settings, currRow + 1, 0, settings.startX, currentPixelY + maxRowHeightPixel + settings.paddingY, 0, name, I, args...); + makeDisplayGridHelper(res, settings, currRow + 1, 0, settings.startX, + currentPixelY + maxRowHeightPixel + settings.paddingY, 0, name, I, args...); } else { std::shared_ptr display = vpDisplayFactory::createDisplay(I, currentPixelX, currentPixelY, name); vpDisplay::display(I); vpDisplay::flush(I); res.push_back(display); - makeDisplayGridHelper(res, settings, currRow, currCol + 1, currentPixelX + I.getWidth() + settings.paddingX, currentPixelY, std::max(maxRowHeightPixel, I.getHeight()), args...); + makeDisplayGridHelper(res, settings, currRow, currCol + 1, currentPixelX + I.getWidth() + settings.paddingX, + currentPixelY, std::max(maxRowHeightPixel, I.getHeight()), args...); } } } @@ -268,9 +277,9 @@ const std::string &name, vpImage &I, Args&... args) */ template std::vector> makeDisplayGrid(unsigned int rows, unsigned int cols, -unsigned int startX, unsigned int startY, -unsigned int paddingX, unsigned int paddingY, -Args&... args) + unsigned int startX, unsigned int startY, + unsigned int paddingX, unsigned int paddingY, + Args&... args) { std::vector> res; impl::GridSettings settings; From f025d37dadfc6706fd79942000b6e7efcafc26e5 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 3 Sep 2024 09:32:49 +0200 Subject: [PATCH 24/25] Doxygen warnings fixes and rendering with Panda3D tutorial updates --- doc/tutorial/rendering/tutorial-panda3d.dox | 150 ++++++++++-------- .../tracker/me/include/visp3/me/vpMeSite.h | 6 +- tutorial/ar/tutorial-panda3d-renderer.cpp | 1 - 3 files changed, 89 insertions(+), 68 deletions(-) diff --git a/doc/tutorial/rendering/tutorial-panda3d.dox b/doc/tutorial/rendering/tutorial-panda3d.dox index febdcdd360..66ab3a0941 100644 --- a/doc/tutorial/rendering/tutorial-panda3d.dox +++ b/doc/tutorial/rendering/tutorial-panda3d.dox @@ -138,7 +138,7 @@ vpPanda3DBaseRenderer, which implements basic functions for a panda renderer. - Now to build ViSP with Panda3D support when `.dmg` file `Panda3D-1.11.0-py3.9.dmg` is installed, you can just run cmake as usual. Note that PCL is not compatible with Panda3D, that's why we disable here PCL usage - (see \ref tutorial-panda3d-issue-macOS). + (see \ref tutorial-panda3d-issue-segfault-macOS). \code{.sh} $ cd $VISP_WS/visp-build $ cmake ../visp -DUSE_PCL=OFF @@ -183,7 +183,6 @@ A panda3D renderer should be instanciated with a vpPanda3DRenderParameters objec The creation of the renderer set can be found below \snippet tutorial-panda3d-renderer.cpp Renderer set - To actually render color, normals, etc., we need to define subrenderers: \snippet tutorial-panda3d-renderer.cpp Subrenderers init @@ -252,67 +251,90 @@ The full code of tutorial-panda3d-renderer.cpp is given below. \endhtmlonly \section tutorial-panda3d-issue Known issues -\subsection tutorial-panda3d-issue-macOS Known issue on macOS - -- Segfault: `:framework(error): Unable to create window` - ``` - % ./tutorial-panda3d-renderer - Initializing Panda3D rendering framework - Known pipe types: - CocoaGLGraphicsPipe - (all display modules loaded.) - :framework(error): Unable to create window. - zsh: segmentation fault ./tutorial-panda3d-renderer - ``` - This issue is probably due to `EIGEN_MAX_ALIGN_BYTES` and `HAVE_PNG` macro redefinition that occurs when building ViSP with Panda3D support: - ``` - $ cd visp-build - $ make - ... - [100%] Building CXX object tutorial/ar/CMakeFiles/tutorial-panda3d-renderer.dir/tutorial-panda3d-renderer.cpp.o - In file included from $VISP_WS/visp/tutorial/ar/tutorial-panda3d-renderer.cpp:17: - In file included from $VISP_WS/visp/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h:39: - In file included from $VISP_WS/visp/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h:42: - In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/pandaFramework.h:17: - In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/pandabase.h:21: - In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/dtoolbase.h:22: - $VISP_WS/3rdparty/panda3d/panda3d/built/include/dtool_config.h:40:9: warning: 'HAVE_PNG' macro redefined [-Wmacro-redefined] - #define HAVE_PNG 1 - ^ - /opt/homebrew/include/pcl-1.14/pcl/pcl_config.h:53:9: note: previous definition is here - #define HAVE_PNG - ^ - In file included from $VISP_WS/visp/tutorial/ar/tutorial-panda3d-renderer.cpp:17: - In file included from $VISP_WS/visp/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h:39: - In file included from $VISP_WS/visp/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h:42: - In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/pandaFramework.h:17: - In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/pandabase.h:21: - In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/dtoolbase.h:22: - $VISP_WS/3rdparty/panda3d/panda3d/built/include/dtool_config.h:64:9: warning: 'HAVE_ZLIB' macro redefined [-Wmacro-redefined] - #define HAVE_ZLIB 1 - ^ - /opt/homebrew/include/pcl-1.14/pcl/pcl_config.h:55:9: note: previous definition is here - #define HAVE_ZLIB +\subsection tutorial-panda3d-issue-library-macOS Library not loaded: libpanda.1.11.dylib + +This error occurs on macOS. + +``` +% cd $VISP_WS/visp-build/tutorial/ar/ +% ./tutorial-panda3d-renderer +dyld[1795]: Library not loaded: @loader_path/../lib/libpanda.1.11.dylib + Referenced from: <0D61FFE0-73FA-3053-8D8D-8912BFF16E36> /Users/fspindle/soft/visp/visp_ws/test-pr/visp-SamFlt/visp-build/tutorial/ar/tutorial-panda3d-renderer + Reason: tried: '/Users/fspindle/soft/visp/visp_ws/test-pr/visp-SamFlt/visp-build/tutorial/ar/../lib/libpanda.1.11.dylib' (no such file) +zsh: abort ./tutorial-panda3d-renderer +``` + +It occurs when you didn't follow carefully the instructions mentionned in \ref tutorial-panda3d-install-macos section. + +A quick fix is to add the path to the library in `DYLD_LIBRARY_PATH` env var: +``` +$ export DYLD_LIBRARY_PATH=/Library/Developer/Panda3D/lib:$DYLD_LIBRARY_PATH +``` + +\subsection tutorial-panda3d-issue-segfault-macOS Segfault: :framework(error): Unable to create window + +This error occurs on macOS. + +``` +% cd $VISP_WS/visp-build/tutorial/ar/ +% ./tutorial-panda3d-renderer +Initializing Panda3D rendering framework +Known pipe types: + CocoaGLGraphicsPipe +(all display modules loaded.) +:framework(error): Unable to create window. +zsh: segmentation fault ./tutorial-panda3d-renderer +``` +This issue is probably due to `EIGEN_MAX_ALIGN_BYTES` and `HAVE_PNG` macro redefinition that occurs when building ViSP with Panda3D support: +``` +$ cd visp-build +$ make +... +[100%] Building CXX object tutorial/ar/CMakeFiles/tutorial-panda3d-renderer.dir/tutorial-panda3d-renderer.cpp.o +In file included from $VISP_WS/visp/tutorial/ar/tutorial-panda3d-renderer.cpp:17: +In file included from $VISP_WS/visp/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h:39: +In file included from $VISP_WS/visp/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h:42: +In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/pandaFramework.h:17: +In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/pandabase.h:21: +In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/dtoolbase.h:22: +$VISP_WS/3rdparty/panda3d/panda3d/built/include/dtool_config.h:40:9: warning: 'HAVE_PNG' macro redefined [-Wmacro-redefined] +#define HAVE_PNG 1 + ^ +/opt/homebrew/include/pcl-1.14/pcl/pcl_config.h:53:9: note: previous definition is here +#define HAVE_PNG + ^ +In file included from $VISP_WS/visp/tutorial/ar/tutorial-panda3d-renderer.cpp:17: +In file included from $VISP_WS/visp/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h:39: +In file included from $VISP_WS/visp/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h:42: +In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/pandaFramework.h:17: +In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/pandabase.h:21: +In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/dtoolbase.h:22: +$VISP_WS/3rdparty/panda3d/panda3d/built/include/dtool_config.h:64:9: warning: 'HAVE_ZLIB' macro redefined [-Wmacro-redefined] +#define HAVE_ZLIB 1 + ^ +/opt/homebrew/include/pcl-1.14/pcl/pcl_config.h:55:9: note: previous definition is here +#define HAVE_ZLIB + ^ +In file included from $VISP_WS/visp/tutorial/ar/tutorial-panda3d-renderer.cpp:17: +In file included from $VISP_WS/visp/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h:39: +In file included from $VISP_WS/visp/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h:42: +In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/pandaFramework.h:17: +In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/pandabase.h:21: +$VISP_WS/3rdparty/panda3d/panda3d/built/include/dtoolbase.h:432:9: warning: 'EIGEN_MAX_ALIGN_BYTES' macro redefined [-Wmacro-redefined] +#define EIGEN_MAX_ALIGN_BYTES MEMORY_HOOK_ALIGNMENT + ^ +/opt/homebrew/include/eigen3/Eigen/src/Core/util/ConfigureVectorization.h:175:11: note: previous definition is here + #define EIGEN_MAX_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES ^ - In file included from $VISP_WS/visp/tutorial/ar/tutorial-panda3d-renderer.cpp:17: - In file included from $VISP_WS/visp/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h:39: - In file included from $VISP_WS/visp/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h:42: - In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/pandaFramework.h:17: - In file included from $VISP_WS/3rdparty/panda3d/panda3d/built/include/pandabase.h:21: - $VISP_WS/3rdparty/panda3d/panda3d/built/include/dtoolbase.h:432:9: warning: 'EIGEN_MAX_ALIGN_BYTES' macro redefined [-Wmacro-redefined] - #define EIGEN_MAX_ALIGN_BYTES MEMORY_HOOK_ALIGNMENT - ^ - /opt/homebrew/include/eigen3/Eigen/src/Core/util/ConfigureVectorization.h:175:11: note: previous definition is here - #define EIGEN_MAX_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES - ^ - 3 warnings generated. - [100%] Linking CXX executable tutorial-panda3d-renderer - [100%] Built target tutorial-panda3d-renderer - ``` - The work around consists in disabling `PCL` usage during ViSP configuration - ``` - $ cd $VISP_WS/visp-build - $ cmake ../visp -DUSE_PCL=OFF - $ make -j$(sysctl -n hw.logicalcpu) - ``` +3 warnings generated. +[100%] Linking CXX executable tutorial-panda3d-renderer +[100%] Built target tutorial-panda3d-renderer +``` +The work around consists in disabling `PCL` usage during ViSP configuration +``` +$ cd $VISP_WS/visp-build +$ cmake ../visp -DUSE_PCL=OFF +$ make -j$(sysctl -n hw.logicalcpu) +``` + */ diff --git a/modules/tracker/me/include/visp3/me/vpMeSite.h b/modules/tracker/me/include/visp3/me/vpMeSite.h index fbff2e46d0..e5f94c5dbb 100644 --- a/modules/tracker/me/include/visp3/me/vpMeSite.h +++ b/modules/tracker/me/include/visp3/me/vpMeSite.h @@ -238,13 +238,13 @@ class VISP_EXPORT vpMeSite void track(const vpImage &I, const vpMe *me, const bool &test_contrast = true); /*! - * Similar to the \ref track function, but stores the best numCandidates hypotheses in \ref outputHypotheses. + * Similar to the track() function, but stores the best numCandidates hypotheses in `outputHypotheses`. * The best matching hypotheses (if it is not suppressed) is assigned to *this* and is stored as the first - * element of \ref outputHypotheses. + * element of `outputHypotheses`. * The hypotheses are sorted from best to worst match in the vector. * A match may be in the vector but mark as suppressed. If this is undesired, you should filter them afterwards. * - * \throws if \ref numCandidates is superior to me.getRange() * 2 + 1 + * \throws If `numCandidates` is superior to me.getRange() * 2 + 1. * * \warning To display the moving edges graphics a call to vpDisplay::flush() is needed after this function. */ diff --git a/tutorial/ar/tutorial-panda3d-renderer.cpp b/tutorial/ar/tutorial-panda3d-renderer.cpp index 4dc70df4a9..39cf1d8aba 100644 --- a/tutorial/ar/tutorial-panda3d-renderer.cpp +++ b/tutorial/ar/tutorial-panda3d-renderer.cpp @@ -205,7 +205,6 @@ int main(int argc, const char **argv) vpPanda3DDirectionalLight dlight("Directional", vpRGBf(2.0f), vpColVector({ 1.0, 1.0, 0.0 })); renderer.addLight(dlight); - //! [Scene configuration] if (!backgroundPath.empty()) { vpImage background; From 983d55893b85bb57a0eba0d3d4c1914472f8efc2 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Thu, 5 Sep 2024 16:30:17 +0200 Subject: [PATCH 25/25] Using Panda's managed pointer for texture and buffers to avoid memory leaks --- .../include/visp3/ar/vpPanda3DGeometryRenderer.h | 8 ++++---- .../include/visp3/ar/vpPanda3DPostProcessFilter.h | 12 ++++-------- modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h | 14 +++++++++----- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h index a33c599553..c4540b969a 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h @@ -61,7 +61,7 @@ class VISP_EXPORT vpPanda3DGeometryRenderer : public vpPanda3DBaseRenderer }; vpPanda3DGeometryRenderer(vpRenderType renderType); - ~vpPanda3DGeometryRenderer() { } + ~vpPanda3DGeometryRenderer() = default; /** * @brief Get render results into ViSP readable structures @@ -85,7 +85,7 @@ class VISP_EXPORT vpPanda3DGeometryRenderer : public vpPanda3DBaseRenderer */ void getRender(vpImage &depth) const; - GraphicsOutput *getMainOutputBuffer() VP_OVERRIDE { return m_normalDepthBuffer; } + GraphicsOutput *getMainOutputBuffer() VP_OVERRIDE { return (GraphicsOutput *)m_normalDepthBuffer; } protected: @@ -94,8 +94,8 @@ class VISP_EXPORT vpPanda3DGeometryRenderer : public vpPanda3DBaseRenderer private: vpRenderType m_renderType; - Texture *m_normalDepthTexture; - GraphicsOutput *m_normalDepthBuffer; + PointerTo m_normalDepthTexture; + PointerTo m_normalDepthBuffer; static const char *SHADER_VERT_NORMAL_AND_DEPTH_OBJECT; static const char *SHADER_VERT_NORMAL_AND_DEPTH_CAMERA; diff --git a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h index abb27399ff..079e4e96a4 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h +++ b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h @@ -62,18 +62,14 @@ class VISP_EXPORT vpPanda3DPostProcessFilter : public vpPanda3DBaseRenderer m_renderOrder = m_inputRenderer->getRenderOrder() + 1; } - ~vpPanda3DPostProcessFilter() - { - // delete m_texture; - // delete m_buffer; - } + virtual ~vpPanda3DPostProcessFilter() = default; bool isRendering3DScene() const VP_OVERRIDE { return false; } - GraphicsOutput *getMainOutputBuffer() VP_OVERRIDE { return m_buffer; } + GraphicsOutput *getMainOutputBuffer() VP_OVERRIDE { return (GraphicsOutput *)m_buffer; } void afterFrameRendered() VP_OVERRIDE { @@ -100,8 +96,8 @@ class VISP_EXPORT vpPanda3DPostProcessFilter : public vpPanda3DBaseRenderer bool m_isOutput; //! Whether this filter is an output to be used and should be copied to ram std::string m_fragmentShader; PointerTo m_shader; - Texture *m_texture; - GraphicsOutput *m_buffer; + PointerTo m_texture; + PointerTo m_buffer; static const char *FILTER_VERTEX_SHADER; }; diff --git a/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h index 06789092a3..cadb5c021e 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h @@ -39,6 +39,8 @@ #include #include +#include "pointerTo.h" + BEGIN_VISP_NAMESPACE /** * \ingroup group_ar_renderer_panda3d_3d @@ -78,6 +80,8 @@ class VISP_EXPORT vpPanda3DRGBRenderer : public vpPanda3DBaseRenderer, public vp */ vpPanda3DRGBRenderer(bool showSpeculars) : vpPanda3DBaseRenderer(showSpeculars ? "RGB" : "RGB-diffuse"), m_showSpeculars(showSpeculars) { } + virtual ~vpPanda3DRGBRenderer() = default; + /** * @brief Store the render resulting from calling renderFrame() into a vpImage. * @@ -91,7 +95,7 @@ class VISP_EXPORT vpPanda3DRGBRenderer : public vpPanda3DBaseRenderer, public vp void setBackgroundImage(const vpImage &background); - GraphicsOutput *getMainOutputBuffer() VP_OVERRIDE { return m_colorBuffer; } + GraphicsOutput *getMainOutputBuffer() VP_OVERRIDE { return (GraphicsOutput *)m_colorBuffer; } bool isShowingSpeculars() const { return m_showSpeculars; } @@ -103,14 +107,14 @@ class VISP_EXPORT vpPanda3DRGBRenderer : public vpPanda3DBaseRenderer, public vp private: bool m_showSpeculars; - Texture *m_colorTexture; - GraphicsOutput *m_colorBuffer; + PointerTo m_colorTexture; + PointerTo m_colorBuffer; static const char *COOK_TORRANCE_VERT; static const char *COOK_TORRANCE_FRAG; NodePath m_backgroundImage; - DisplayRegion *m_display2d; - Texture *m_backgroundTexture; + PointerTo m_display2d; + PointerTo m_backgroundTexture; }; END_VISP_NAMESPACE